Skip to content

GitLab

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
Hide whitespace changes
Inline Side-by-side
bindings/python/algorithm/expose-model.cpp
View file @ aa2ee503
......@@ -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
......
src/algorithm/model.hxx
View file @ aa2ee503
......@@ -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