Skip to content

Commit

Permalink
fixup
Browse files Browse the repository at this point in the history
  • Loading branch information
@traversaro
traversaro committed Dec 5, 2024
1 parent a6ec2dd commit 667453a
Show file tree
Hide file tree
Showing 2 changed files with 54 additions and 11 deletions.
37 changes: 27 additions & 10 deletions src/model/src/ModelTransformers.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@
#include <algorithm>
#include <cassert>
#include <unordered_map>
#include <optional>
#include <set>
#include <vector>

Expand Down Expand Up @@ -282,7 +283,7 @@ void reducedModelAddAdditionalFrames(const Model& fullModel,
const FreeFloatingPos& pos,
LinkPositions& subModelBase_X_link,
const std::unordered_map<std::string, iDynTree::Transform>& newLink_H_oldLink,
const std::optional<std::vector<std::string>>>& allowedAdditionalFrames)
const std::optional<std::vector<std::string>>& allowedAdditionalFrames)
{
// First compute the transform between each link in the submodel and the submodel base
computeTransformToTraversalBaseWithAdditionalTransform(fullModel,linkSubModel,pos.jointPos(),subModelBase_X_link,newLink_H_oldLink);
Expand Down Expand Up @@ -326,13 +327,13 @@ void reducedModelAddAdditionalFrames(const Model& fullModel,
subModelBase_H_visitedLink*visitedLink_H_additionalFrame;
}

bool shouldWeAddTheAdditionalFrame = true;
bool shouldWeAddTheAdditionalFrame = true;

// Check if we need to add the additional frame or not
if (allowedAdditionalFrames.has_value())
{
// If allowedAdditionalFrames has a value, we only need to add additional frames specified ther
if (std::find(vec.begin(), vec.end(), value) == vec.end())
// If allowedAdditionalFrames has a value, we only need to add additional frames specified ther
if (std::find(allowedAdditionalFrames.value().begin(), allowedAdditionalFrames.value().end(), additionalFrameName) == allowedAdditionalFrames.value().end())
{
shouldWeAddTheAdditionalFrame = false;
}
Expand All @@ -342,6 +343,8 @@ void reducedModelAddAdditionalFrames(const Model& fullModel,
{
reducedModel.addAdditionalFrameToLink(linkInReducedModel,additionalFrameName,
subModelBase_H_additionalFrame);
} else {
std::cerr << " ==================> skipping frame " << additionalFrameName << std::endl;
}
}
}
Expand Down Expand Up @@ -405,11 +408,11 @@ void reducedModelAddSolidShapes(const Model& fullModel,
// * createReducedModel : function to create a reduced model given the specified joints
// * moveLinkFramesToBeCompatibleWithURDFWithGivenBaseLink: function to make sure a model is URDF compatible
// * removeAdditionalFramesFromModel: function to remove additional frames from a URDF
//
//
// The logic is similar to the createReducedModel, but with additional options:
// * std::vector<iDynTree::Transform> newLink_H_oldLink vector (of size fullModel.getNrOfLinks() that can be used
// to specify an optional additional transform of the final link used in the "reduced model"
// * std::optional<std::vector<std::string>>> allowedAdditionalFrames : If allowedAdditionalFrames.has_value is False,
// * std::optional<std::vector<std::string>> allowedAdditionalFrames : If allowedAdditionalFrames.has_value is False,
// all the additional frames are of the input model are copied in the reduced model, if allowedAdditionalFrames.has_value
// is True only the additional frames with the name contained in allowedAdditionalFrames are copied to the reduce model
bool createReducedModelAndChangeLinkFrames(const Model& fullModel,
Expand All @@ -418,7 +421,7 @@ bool createReducedModelAndChangeLinkFrames(const Model& fullModel,
const std::unordered_map<std::string, double>& removedJointPositions,
const std::unordered_map<std::string, iDynTree::Transform>& newLink_H_oldLink,
bool addOriginalLinkFrameWith_original_frame_suffix,
const std::optional<std::vector<std::string>>>& allowedAdditionalFrames)
const std::optional<std::vector<std::string>>& allowedAdditionalFrames)
{
// We use the default traversal for deciding the base links of the reduced model
Traversal fullModelTraversal;
Expand Down Expand Up @@ -537,7 +540,7 @@ bool createReducedModelAndChangeLinkFrames(const Model& fullModel,
// As this quantity is influenced by newLink_H_oldLink, this is passed along
reducedModelAddAdditionalFrames(fullModel,reducedModel,
linkName,subModels.getTraversal(linkInReducedModel),
jointPos,subModelBase_X_link,newLink_H_oldLink);
jointPos,subModelBase_X_link,newLink_H_oldLink,{});

// Lump the visual and collision shapes in the new model
reducedModelAddSolidShapes(fullModel,reducedModel,
Expand Down Expand Up @@ -843,7 +846,7 @@ bool createReducedModel(const Model& fullModel,
// We do not want to move the link frames in createReducedModel
std::unordered_map<std::string, iDynTree::Transform> newLink_H_oldLink;
bool addOriginalLinkFrameWith_original_frame_suffix = false;
return createReducedModelAndChangeLinkFrames(fullModel, jointsInReducedModel, reducedModel, removedJointPositions, newLink_H_oldLink, addOriginalLinkFrameWith_original_frame_suffix);
return createReducedModelAndChangeLinkFrames(fullModel, jointsInReducedModel, reducedModel, removedJointPositions, newLink_H_oldLink, addOriginalLinkFrameWith_original_frame_suffix, {});
}

bool createReducedModel(const Model& fullModel,
Expand Down Expand Up @@ -1103,7 +1106,7 @@ bool moveLinkFramesToBeCompatibleWithURDFWithGivenBaseLink(const iDynTree::Model

bool addOriginalLinkFrameWith_original_frame_suffix = true;
bool okReduced = createReducedModelAndChangeLinkFrames(inputModel, consideredJoints, outputModel,
removedJointPositions, newLink_H_oldLink, addOriginalLinkFrameWith_original_frame_suffix);
removedJointPositions, newLink_H_oldLink, addOriginalLinkFrameWith_original_frame_suffix, {});

if (okReduced)
{
Expand All @@ -1122,6 +1125,20 @@ bool removeAdditionalFramesFromModel(const Model& modelWithAllAdditionalFrames,
Model& modelWithOnlyAllowedAdditionalFrames,
const std::vector<std::string> allowedAdditionalFrames)
{
// Get list of all joints to pass as considered joints
std::vector<std::string> consideredJoints;
for(iDynTree::JointIndex jntIdx=0; jntIdx < modelWithAllAdditionalFrames.getNrOfJoints(); jntIdx++)
{
consideredJoints.push_back(modelWithAllAdditionalFrames.getJointName(jntIdx));
}

return createReducedModelAndChangeLinkFrames(modelWithAllAdditionalFrames,
consideredJoints,
modelWithOnlyAllowedAdditionalFrames,
std::unordered_map<std::string, double>(),
std::unordered_map<std::string, iDynTree::Transform>(),
false,
allowedAdditionalFrames);

}

Expand Down
28 changes: 27 additions & 1 deletion src/model/tests/ModelTransformersUnitTest.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@
#include <iDynTree/Link.h>

#include <iDynTree/TestUtils.h>
#include <iDynTree/ModelTestUtils.h>

#include <algorithm>
#include <cassert>
Expand Down Expand Up @@ -43,12 +44,37 @@ void checkThatOneSphereGetsAName()
ASSERT_IS_TRUE(oneSphereModelWithValidName.collisionSolidShapes().getLinkSolidShapes()[0][0]->getName() == "link0_collision");
}

void check
void checkRemoveAdditionalFramesFromModel()
{
// Create random model with 10 links and 10 additional frames
iDynTree::Model modelWithAllAdditionalFrames = getRandomModel(10, 10);

// Create an allow list of three additional frames
std::vector<std::string> allowedAdditionalFrames;
allowedAdditionalFrames.push_back(modelWithAllAdditionalFrames.getFrameName(modelWithAllAdditionalFrames.getNrOfLinks() + 7));
allowedAdditionalFrames.push_back(modelWithAllAdditionalFrames.getFrameName(modelWithAllAdditionalFrames.getNrOfLinks() + 1));
allowedAdditionalFrames.push_back(modelWithAllAdditionalFrames.getFrameName(modelWithAllAdditionalFrames.getNrOfLinks() + 3));

// Create a model with only the allowed additional frames
iDynTree::Model modelWithOnlyAllowedAdditionalFrames;
ASSERT_IS_TRUE(removeAdditionalFramesFromModel(modelWithAllAdditionalFrames, modelWithOnlyAllowedAdditionalFrames, allowedAdditionalFrames));

// Check that the model with only the allowed additional frames has the correct number of links and additional frames
ASSERT_IS_TRUE(modelWithOnlyAllowedAdditionalFrames.getNrOfLinks() == modelWithAllAdditionalFrames.getNrOfLinks());
ASSERT_IS_TRUE(modelWithOnlyAllowedAdditionalFrames.getNrOfFrames() == modelWithOnlyAllowedAdditionalFrames.getNrOfLinks() + allowedAdditionalFrames.size());

// Check that the additional frames contained in the modelWithOnlyAllowedAdditionalFrames are the one specified in modelWithOnlyAllowedAdditionalFrames
for (size_t i = 0; i < allowedAdditionalFrames.size(); i++)
{
ASSERT_IS_TRUE(modelWithOnlyAllowedAdditionalFrames.isFrameNameUsed(allowedAdditionalFrames[i]));
}
}


int main()
{
checkThatOneSphereGetsAName();
checkRemoveAdditionalFramesFromModel();

return EXIT_SUCCESS;
}

0 comments on commit 667453a

Please sign in to comment.