Add test for new createReducedModel argument

src/model/tests/ModelUnitTest.cpp

@@ -97,20 +97,20 @@ void getRandomJointPositonsForJointsNotInReducedModels(const Model & fullModel,
  std::unordered_map<std::string, double>& removedJointPositions,
  FreeFloatingPos& fullModelPos)
 {
- for(auto jntName: subsetOfJointsInReducedModel)
- {
- std::cerr << " " << jntName;
- }
- std::cerr << std::endl;
  for(JointIndex jntIndex = 0; jntIndex < fullModel.getNrOfJoints(); jntIndex++)
  {
  // Check if joint is in reduced model
  std::string jointName = fullModel.getJointName(jntIndex);
+
+ // Only set non-zero position if the DOF size is exactly 1
  if (!isStringInVector(jointName, subsetOfJointsInReducedModel))
  {
- double jointConf = iDynTree::getRandomDouble();
- removedJointPositions[jointName] = jointConf;
- fullModelPos.jointPos()(fullModel.getJoint(jntIndex)->getPosCoordsOffset()) = jointConf;
+ if (fullModel.getJoint(jntIndex)->getNrOfDOFs() == 1)
+ {
+ double jointConf = iDynTree::getRandomDouble();
+ removedJointPositions[jointName] = jointConf;
+ fullModelPos.jointPos()(fullModel.getJoint(jntIndex)->getPosCoordsOffset()) = jointConf;
+ }
  }
  }
 }
@@ -217,6 +217,8 @@ void checkReducedModel(const Model & model)
  // is giving the same results
  for(size_t jnts=0; jnts < model.getNrOfJoints(); jnts += 5)
  {
+ FreeFloatingPos fullPos(model);
+
  std::vector<std::string> jointInReducedModel;
  getRandomSubsetOfJoints(model,jnts,jointInReducedModel);
 
@@ -225,7 +227,7 @@ void checkReducedModel(const Model & model)
  getRandomJointPositonsForJointsNotInReducedModels(model, jointInReducedModel, removedJointPositions, fullPos);
 
  Model reducedModel;
- bool ok = createReducedModel(model,jointInReducedModel,reducedModel);
+ bool ok = createReducedModel(model, jointInReducedModel, reducedModel, removedJointPositions);
 
  ASSERT_EQUAL_DOUBLE(ok,1.0);
 
@@ -253,7 +255,6 @@ void checkReducedModel(const Model & model)
  reducedAcc.baseAcc() = getRandomTwist();
  getRandomVector(reducedAcc.jointAcc());
 
- FreeFloatingPos fullPos(model);
  FreeFloatingVel fullVel(model);
  FreeFloatingAcc fullAcc(model);
  FreeFloatingGeneralizedTorques fullTrqs(model);