Commit 9b97f7c1 authored by Justin Carpentier's avatar Justin Carpentier

algo/model: add buildReducedModel overload for GeometryModel

parent a9e58c96
......@@ -119,6 +119,29 @@ namespace pinocchio
return reduced_model;
}
/**
*
* \brief Build a reduced model and a rededuced geometry model from a given input model, a given input geometry model and a list of joint to lock.
*
* \param[in] model the input model to reduce.
* \param[in] geom_model the input geometry model to reduce.
* \param[in] list_of_joints list of joints to lock in the input model.
* \param[in] reference_configuration reference configuration.
* \param[out] reduced_model the reduced model.
* \param[out] reduced_geom_model the reduced geometry model.
*
* \remarks All the joints that have been set to be fixed in the new reduced_model now appear in the kinematic tree as a Frame as FIXED_JOINT.
*
*/
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
void
buildReducedModel(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const GeometryModel & geom_model,
const std::vector<JointIndex> & list_of_joints_to_lock,
const Eigen::MatrixBase<ConfigVectorType> & reference_configuration,
ModelTpl<Scalar,Options,JointCollectionTpl> & reduced_model,
GeometryModel & reduced_geom_model);
} // namespace pinocchio
#include "pinocchio/algorithm/model.hxx"
......
......@@ -453,6 +453,65 @@ namespace pinocchio
}
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
void
buildReducedModel(const ModelTpl<Scalar,Options,JointCollectionTpl> & input_model,
const GeometryModel & input_geom_model,
const std::vector<JointIndex> & list_of_joints_to_lock,
const Eigen::MatrixBase<ConfigVectorType> & reference_configuration,
ModelTpl<Scalar,Options,JointCollectionTpl> & reduced_model,
GeometryModel & reduced_geom_model)
{
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
buildReducedModel(input_model,list_of_joints_to_lock,reference_configuration,reduced_model);
// Add all the geometries
typedef GeometryModel::GeometryObject GeometryObject;
typedef GeometryModel::GeometryObjectVector GeometryObjectVector;
for(GeometryObjectVector::const_iterator it = input_geom_model.geometryObjects.begin();
it != input_geom_model.geometryObjects.end(); ++it)
{
const GeometryModel::GeometryObject & geom = *it;
const JointIndex joint_id_in_input_model = geom.parentJoint;
PINOCCHIO_CHECK_INPUT_ARGUMENT(joint_id_in_input_model < (JointIndex)input_model.njoints,
"Invalid joint parent index for the geometry with name " + geom.name);
const std::string & parent_joint_name = input_model.names[joint_id_in_input_model];
JointIndex reduced_joint_id = (JointIndex)-1;
typedef typename Model::SE3 SE3;
SE3 relative_placement = SE3::Identity();
if(reduced_model.existJointName(parent_joint_name))
{
reduced_joint_id = reduced_model.getJointId(parent_joint_name);
}
else // The joint is now a frame
{
const FrameIndex reduced_frame_id = reduced_model.getFrameId(parent_joint_name);
reduced_joint_id = reduced_model.frames[reduced_frame_id].parent;
relative_placement = reduced_model.frames[reduced_frame_id].placement;
}
GeometryObject reduced_geom(geom);
reduced_geom.parentJoint = reduced_joint_id;
reduced_geom.placement = relative_placement * geom.placement;
reduced_geom_model.addGeometryObject(reduced_geom);
}
#ifdef PINOCCHIO_WITH_HPP_FCL
// Add all the collision pairs - the index of the geometry objects should have not changed
typedef GeometryModel::CollisionPairVector CollisionPairVector;
for(CollisionPairVector::const_iterator it = input_geom_model.collisionPairs.begin();
it != input_geom_model.collisionPairs.end(); ++it)
{
const CollisionPair & cp = *it;
reduced_geom_model.addCollisionPair(cp);
}
#endif
}
} // namespace pinocchio
#endif // ifndef __pinocchio_algorithm_model_hxx__
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment