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

revert code formatting and add some requested changes in PR

parent 81e1f4cc
......@@ -2,11 +2,10 @@
// 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/bindings/python/utils/std-vector.hpp"
#include "pinocchio/algorithm/model.hpp"
namespace pinocchio
{
......@@ -14,7 +13,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,
......@@ -31,23 +30,21 @@ 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 std::vector<GeometryModel> &list_of_geom_models,
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 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> reduced_geom_models;
buildReducedModel(model,list_of_geom_models,list_of_joints_to_lock,
reference_configuration,reduced_model,reduced_geom_models);
Model reduced_model; GeometryModel reduced_geom_model;
return bp::make_tuple(reduced_model,reduced_geom_models);
buildReducedModel(model,geom_model,list_of_joints_to_lock,
reference_configuration,reduced_model,reduced_geom_model);
return bp::make_tuple(reduced_model,reduced_geom_model);
}
template <typename Scalar, int Options,
......@@ -55,17 +52,16 @@ namespace pinocchio
typename ConfigVectorType>
bp::tuple buildReducedModel(
const ModelTpl<Scalar, Options, JointCollectionTpl> &model,
const GeometryModel &geom_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;
typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
std::vector<GeometryModel> reduced_geom_models;
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
buildReducedModel(model,geom_models,list_of_joints_to_lock,
reference_configuration,reduced_model,reduced_geom_models);
return bp::make_tuple(reduced_model,reduced_geom_models.front());
buildReducedModel(model, list_of_geom_models, list_of_joints_to_lock,
reference_configuration, reduced_model,
reduced_geom_models);
return bp::make_tuple(reduced_model, reduced_geom_models);
}
void exposeModelAlgo()
......@@ -108,43 +104,37 @@ 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 reduced 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 reduced 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");
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 reduced 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 model 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
......
......@@ -468,16 +468,15 @@ 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) {
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<GeometryModel> temp_reduced_geom_models;
......@@ -498,68 +497,65 @@ namespace pinocchio
const Eigen::MatrixBase<ConfigVectorType> &reference_configuration,
ModelTpl<Scalar, Options, JointCollectionTpl> &reduced_model,
std::vector<GeometryModel> &list_of_reduced_geom_models) {
typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
buildReducedModel(input_model, list_of_joints_to_lock,
reference_configuration, reduced_model);
buildReducedModel(input_model, list_of_joints_to_lock, reference_configuration, reduced_model);
// for all GeometryModels
for (auto &input_geom_model : list_of_geom_models) {
for (const GeometryModel &input_geom_model : list_of_geom_models) {
GeometryModel reduced_geom_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;
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];
_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)) {
if(reduced_model.existJointName(parent_joint_name))
{
reduced_joint_id = reduced_model.getJointId(parent_joint_name);
} else // The joint is now a frame
}
else // The joint is now a frame
{
const FrameIndex reduced_frame_id =
reduced_model.getFrameId(parent_joint_name);
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.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);
}
#endif
list_of_reduced_geom_models.push_back(reduced_geom_model);
#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
list_of_reduced_geom_models.push_back(reduced_geom_model);
}
}
} // 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