Commit a2bf3a88 authored by Justin Carpentier's avatar Justin Carpentier

algo/model: add new signatures appendModel and buildReducedModel

parent 55407f2a
......@@ -31,6 +31,34 @@ namespace pinocchio
const SE3Tpl<Scalar,Options> & aMb,
ModelTpl<Scalar,Options,JointCollectionTpl> & model);
/**
* \brief Append a child model into a parent model, after a specific frame.
*
* \param[in] modelA the parent model.
* \param[in] modelB the child model.
* \param[in] geomModelA the parent geometry model.
* \param[in] geomModelB the child geometry model.
* \param[in] frameInModelA index of the frame of modelA where to append modelB.
* \param[in] aMb pose of modelB universe joint (index 0) in frameInModelA.
*
* \return A new model containing the fusion of modelA and modelB.
*
*/
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
ModelTpl<Scalar,Options,JointCollectionTpl>
appendModel(const ModelTpl<Scalar,Options,JointCollectionTpl> & modelA,
const ModelTpl<Scalar,Options,JointCollectionTpl> & modelB,
const FrameIndex frameInModelA,
const SE3Tpl<Scalar,Options> & aMb)
{
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
Model model;
appendModel(modelA,modelB,frameInModelA,aMb,model);
return model;
}
/**
* \copydoc pinocchio::appendModel(const ModelTpl<Scalar,Options,JointCollectionTpl>&, const ModelTpl<Scalar,Options,JointCollectionTpl> & modelB, FrameIndex frameInModelA, const SE3Tpl<Scalar, Options>& aMb, ModelTpl<Scalar,Options,JointCollectionTpl>& model);
*
......@@ -55,7 +83,24 @@ namespace pinocchio
*
* \param[in] model the input model to reduce.
* \param[in] list_of_joints list of joints to lock in the input model.
* \param[in] reference_configuration reference configuration
* \param[in] reference_configuration reference configuration.
* \param[out] reduced_model the reduced model.
*
*/
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
void
buildReducedModel(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
std::vector<JointIndex> list_of_joints_to_lock,
const Eigen::MatrixBase<ConfigVectorType> & reference_configuration,
ModelTpl<Scalar,Options,JointCollectionTpl> & reduced_model);
/**
*
* \brief Build a reduce model from a given input model and a list of joint to lock.
*
* \param[in] model the input model to reduce.
* \param[in] list_of_joints list of joints to lock in the input model.
* \param[in] reference_configuration reference configuration.
*
* \returns A reduce model of the input model.
*
......@@ -63,8 +108,16 @@ namespace pinocchio
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
ModelTpl<Scalar,Options,JointCollectionTpl>
buildReducedModel(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
std::vector<JointIndex> list_of_joints_to_lock,
const Eigen::MatrixBase<ConfigVectorType> & reference_configuration);
const std::vector<JointIndex> & list_of_joints_to_lock,
const Eigen::MatrixBase<ConfigVectorType> & reference_configuration)
{
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
Model reduced_model;
buildReducedModel(model,list_of_joints_to_lock,reference_configuration,reduced_model);
return reduced_model;
}
} // namespace pinocchio
......
......@@ -277,10 +277,11 @@ namespace pinocchio
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
ModelTpl<Scalar,Options,JointCollectionTpl>
void
buildReducedModel(const ModelTpl<Scalar,Options,JointCollectionTpl> & input_model,
std::vector<JointIndex> list_of_joints_to_lock,
const Eigen::MatrixBase<ConfigVectorType> & reference_configuration)
const Eigen::MatrixBase<ConfigVectorType> & reference_configuration,
ModelTpl<Scalar,Options,JointCollectionTpl> & reduced_model)
{
PINOCCHIO_CHECK_INPUT_ARGUMENT(reference_configuration.size() == input_model.nq,
"The configuration vector is not of right size");
......@@ -293,8 +294,6 @@ namespace pinocchio
typedef typename Model::Frame Frame;
typedef typename Model::SE3 SE3;
Model reduced_model;
// Sort indexes
std::sort(list_of_joints_to_lock.begin(),list_of_joints_to_lock.end());
......@@ -452,11 +451,8 @@ namespace pinocchio
reduced_model.addFrame(reduced_frame);
}
}
return reduced_model;
}
} // 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