Verified Commit f306ba60 authored by Nicolas Torres's avatar Nicolas Torres Committed by Justin Carpentier
Browse files

add buildReducedModel for multiple GeometryModels

parent f3e0aecc
......@@ -2,9 +2,11 @@
// Copyright (c) 2019-2020 INRIA
//
#include "pinocchio/algorithm/model.hpp"
#include "pinocchio/bindings/python/algorithm/algorithms.hpp"
#include "pinocchio/bindings/python/serialization/serializable.hpp"
#include "pinocchio/bindings/python/utils/list.hpp"
#include "pinocchio/algorithm/model.hpp"
#include "pinocchio/bindings/python/utils/std-vector.hpp"
namespace pinocchio
{
......@@ -12,7 +14,7 @@ namespace pinocchio
{
namespace bp = boost::python;
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
bp::tuple appendModel_proxy(const ModelTpl<Scalar,Options,JointCollectionTpl> & modelA,
const ModelTpl<Scalar,Options,JointCollectionTpl> & modelB,
......@@ -29,27 +31,56 @@ namespace pinocchio
return bp::make_tuple(model,geom_model);
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
bp::tuple
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)
{
template <typename Scalar, int Options,
template <typename, int> class JointCollectionTpl,
typename ConfigVectorType>
bp::tuple buildReducedModel(
const ModelTpl<Scalar, Options, JointCollectionTpl> &model,
const std::vector<GeometryModel> &list_of_geom_models,
const std::vector<JointIndex> &list_of_joints_to_lock,
const Eigen::MatrixBase<ConfigVectorType> &reference_configuration) {
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
Model reduced_model; GeometryModel reduced_geom_model;
Model reduced_model;
std::vector<GeometryModel> reduced_geom_models(list_of_geom_models.size());
std::vector<std::reference_wrapper<GeometryModel>>
reduced_geom_models_refs(reduced_geom_models.begin(),
reduced_geom_models.end());
buildReducedModel(model,list_of_geom_models,list_of_joints_to_lock,
reference_configuration,reduced_model,reduced_geom_models_refs);
buildReducedModel(model,geom_model,list_of_joints_to_lock,
reference_configuration,reduced_model,reduced_geom_model);
return bp::make_tuple(reduced_model,reduced_geom_models);
}
template <typename Scalar, int Options,
template <typename, int> class JointCollectionTpl,
typename ConfigVectorType>
bp::tuple 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) {
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
Model reduced_model;
std::vector<GeometryModel> geom_models{ geom_model };
std::vector<GeometryModel> reduced_geom_models(1); // in this case it's a single element
std::vector<std::reference_wrapper<GeometryModel>>
reduced_geom_models_refs(reduced_geom_models.begin(),
reduced_geom_models.end());
buildReducedModel(model,geom_models,list_of_joints_to_lock,
reference_configuration,reduced_model,reduced_geom_models_refs);
return bp::make_tuple(reduced_model,reduced_geom_model);
return bp::make_tuple(reduced_model,reduced_geom_models.front());
}
void exposeModelAlgo()
{
using namespace Eigen;
StdVectorPythonVisitor<GeometryModel>::expose("StdVec_GeometryModel");
bp::def("appendModel",
(Model (*)(const Model &, const Model &, const FrameIndex, const SE3 &))&appendModel<double,0,JointCollectionDefaultTpl>,
bp::args("modelA","modelB","frame_in_modelA","aMb"),
......@@ -84,21 +115,43 @@ namespace pinocchio
"\tlist_of_joints_to_lock: list of joint indexes to lock\n"
"\treference_configuration: reference configuration to compute the placement of the lock joints\n");
bp::def("buildReducedModel",
(bp::tuple (*)(const Model &, const GeometryModel &, const std::vector<JointIndex> &, const Eigen::MatrixBase<VectorXd> &))
&buildReducedModel<double,0,JointCollectionDefaultTpl,VectorXd>,
bp::args("model",
"geom_model",
"list_of_joints_to_lock",
"reference_configuration"),
"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.\n\n"
"Parameters:\n"
"\tmodel: input kinematic modell to reduce\n"
"\tgeom_model: input geometry model to reduce\n"
"\tlist_of_joints_to_lock: list of joint indexes to lock\n"
"\treference_configuration: reference configuration to compute the placement of the lock joints\n");
bp::def(
"buildReducedModel",
(bp::tuple(*)(
const Model &,
const GeometryModel &,
const std::vector<JointIndex> &,
const Eigen::MatrixBase<VectorXd> &)) &
buildReducedModel<double, 0, JointCollectionDefaultTpl, VectorXd>,
bp::args("model", "geom_model", "list_of_joints_to_lock",
"reference_configuration"),
"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.\n\n"
"Parameters:\n"
"\tmodel: input kinematic modell to reduce\n"
"\tgeom_model: input geometry model to reduce\n"
"\tlist_of_joints_to_lock: list of joint indexes to lock\n"
"\treference_configuration: reference configuration to compute the "
"placement of the lock joints\n");
bp::def(
"buildReducedModel",
(bp::tuple(*)(const Model &, const std::vector<GeometryModel> &,
const std::vector<JointIndex> &,
const Eigen::MatrixBase<VectorXd> &)) &
buildReducedModel<double, 0, JointCollectionDefaultTpl, VectorXd>,
bp::args("model", "list_of_geom_models", "list_of_joints_to_lock",
"reference_configuration"),
"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.\n\n"
"Parameters:\n"
"\tmodel: input kinematic modell to reduce\n"
"\tlist_of_geom_models: input geometry models to reduce\n"
"\tlist_of_joints_to_lock: list of joint indexes to lock\n"
"\treference_configuration: reference configuration to compute the "
"placement of the lock joints\n");
}
} // namespace python
......
......@@ -144,6 +144,31 @@ namespace pinocchio
ModelTpl<Scalar,Options,JointCollectionTpl> & reduced_model,
GeometryModel & reduced_geom_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] list_of_geom_models the input geometry model to reduce (example: visual_model, collision_model).
* \param[in] list_of_joints_to_lock list of joints to lock in the input model.
* \param[in] reference_configuration reference configuration.
* \param[out] reduced_model the reduced model.
* \param[out] list_of_reduced_geom_model the list of reduced geometry models.
*
* \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 std::vector<GeometryModel> &list_of_geom_models,
const std::vector<JointIndex> &list_of_joints_to_lock,
const Eigen::MatrixBase<ConfigVectorType> &reference_configuration,
ModelTpl<Scalar, Options, JointCollectionTpl> &reduced_model,
std::vector<std::reference_wrapper<GeometryModel>> &list_of_reduced_geom_models);
} // namespace pinocchio
#include "pinocchio/algorithm/model.hxx"
......
......@@ -6,6 +6,7 @@
#define __pinocchio_algorithm_model_hxx__
#include <algorithm>
#include <functional>
namespace pinocchio
{
......@@ -468,67 +469,105 @@ 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_2((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;
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) {
// const std::vector<GeometryModel> temp_input_geoms (input_geom_model);
// std::vector<std::reference_wrapper<GeometryModel>> temp_reduced_geoms(reduced_geom_model);
buildReducedModel(input_model,
// temp_input_geoms,
{input_geom_model},
list_of_joints_to_lock, reference_configuration,
reduced_model,
// temp_reduced_geoms
{reduced_geom_model}
);
}
template <typename Scalar, int Options,
template <typename, int> class JointCollectionTpl,
typename ConfigVectorType>
void buildReducedModel(
const ModelTpl<Scalar, Options, JointCollectionTpl> &input_model,
const std::vector<GeometryModel> &list_of_geom_models,
const std::vector<JointIndex> &list_of_joints_to_lock,
const Eigen::MatrixBase<ConfigVectorType> &reference_configuration,
ModelTpl<Scalar, Options, JointCollectionTpl> &reduced_model,
std::vector<std::reference_wrapper<GeometryModel>>
&list_of_reduced_geom_models) {
typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
buildReducedModel(input_model, list_of_joints_to_lock,
reference_configuration, reduced_model);
// for all GeometryModels
// TODO: the check below could only come up from C++,
// as the python wrapper creates list of the same size
assert(list_of_reduced_geom_models.size() == list_of_geom_models.size());
for (unsigned int i; i < list_of_reduced_geom_models.size(); ++i) {
auto &input_geom_model =
list_of_geom_models[i];
auto &reduced_geom_model =
list_of_reduced_geom_models[i];
// 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_2(
(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.parentFrame =
reduced_model.getBodyId(input_model.frames[geom.parentFrame].name);
reduced_geom.placement = relative_placement * geom.placement;
reduced_geom_model.get().addGeometryObject(reduced_geom);
}
GeometryObject reduced_geom(geom);
reduced_geom.parentJoint = reduced_joint_id;
reduced_geom.parentFrame = reduced_model.getBodyId(
input_model.frames[geom.parentFrame].name);
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);
}
// 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.get().addCollisionPair(cp);
}
#endif
}
}
} // namespace pinocchio
} // 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