Unverified Commit 2c44cf48 authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub

Merge pull request #1002 from jcarpent/devel

Add new signatures for buildReducedModel with geometries
parents 5527909d 2db9c3db
......@@ -15,31 +15,136 @@ namespace pinocchio
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
ModelTpl<Scalar,Options,JointCollectionTpl>
buildReducedModel_list(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const bp::list & list_of_joints_to_lock,
const Eigen::MatrixBase<ConfigVectorType> & reference_configuration)
buildReducedModel(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const bp::list & list_of_joints_to_lock,
const Eigen::MatrixBase<ConfigVectorType> & reference_configuration)
{
std::vector<JointIndex> list_of_joints_to_lock_vec = extract<JointIndex>(list_of_joints_to_lock);
return buildReducedModel(model,list_of_joints_to_lock_vec,reference_configuration);
const std::vector<JointIndex> vec_of_joints_to_lock = extract<JointIndex>(list_of_joints_to_lock);
return buildReducedModel(model,vec_of_joints_to_lock,reference_configuration);
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
bp::tuple appendModel(const ModelTpl<Scalar,Options,JointCollectionTpl> & modelA,
const ModelTpl<Scalar,Options,JointCollectionTpl> & modelB,
const GeometryModel & geomModelA,
const GeometryModel & geomModelB,
const FrameIndex frameInModelA,
const SE3Tpl<Scalar,Options> & aMb)
{
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
Model model; GeometryModel geom_model;
appendModel(modelA,modelB,geomModelA,geomModelB,frameInModelA,aMb,model,geom_model);
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)
{
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
Model reduced_model; GeometryModel reduced_geom_model;
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, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
bp::tuple
buildReducedModel(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const GeometryModel & geom_model,
const bp::list & list_of_joints_to_lock,
const Eigen::MatrixBase<ConfigVectorType> & reference_configuration)
{
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
Model reduced_model; GeometryModel reduced_geom_model;
const std::vector<JointIndex> vec_of_joints_to_lock = extract<JointIndex>(list_of_joints_to_lock);
buildReducedModel(model,geom_model,vec_of_joints_to_lock,
reference_configuration,reduced_model,reduced_geom_model);
return bp::make_tuple(reduced_model,reduced_geom_model);
}
void exposeModelAlgo()
{
using namespace Eigen;
bp::def("appendModel",
(Model (*)(const Model &, const Model &, const FrameIndex, const SE3 &))&appendModel<double,0,JointCollectionDefaultTpl>,
bp::args("modelA","modelB","frame_in_modelA","aMb"),
"Append a child model into a parent model, after a specific frame given by its index.\n\n"
" modelA: the parent model\n"
" modelB: the child model\n"
" frameInModelA: index of the frame of modelA where to append modelB\n"
" aMb: pose of modelB universe joint (index 0) in frameInModelA\n");
bp::def("appendModel",
(bp::tuple (*)(const Model &, const Model &, const GeometryModel &, const GeometryModel &, const FrameIndex, const SE3 &))&appendModel<double,0,JointCollectionDefaultTpl>,
bp::args("modelA","modelB","frame_in_modelA","aMb"),
"Append a child (geometry) model into a parent (geometry) model, after a specific frame given by its index.\n\n"
" modelA: the parent model\n"
" modelB: the child model\n"
" geomModelA: the parent geometry model\n"
" geomModelB: the child geometry model\n"
" frameInModelA: index of the frame of modelA where to append modelB\n"
" aMb: pose of modelB universe joint (index 0) in frameInModelA\n");
bp::def("buildReducedModel",
(Model (*)(const Model &, const bp::list &, const Eigen::MatrixBase<VectorXd> &))
buildReducedModel<double,0,JointCollectionDefaultTpl,VectorXd>,
bp::args("model",
"list_of_joints_to_lock",
"reference_configuration"),
"Build a reduce model from a given input model and a list of joint to lock.\n\n"
" model: input kinematic modell to reduce\n"
" list_of_joints_to_lock: list of joint indexes to lock\n"
" reference_configuration: reference configuration to compute the placement of the lock joints\n");
bp::def("buildReducedModel",
(Model (*)(const Model &, const std::vector<JointIndex> &, const Eigen::MatrixBase<VectorXd> &))
&pinocchio::buildReducedModel<double,0,JointCollectionDefaultTpl,VectorXd>,
bp::args("model",
"list_of_joints_to_lock",
"reference_configuration"),
"Build a reduce model from a given input model and a list of joint to lock.\n\n"
" model: input kinematic modell to reduce\n"
" list_of_joints_to_lock: list of joint indexes to lock\n"
" reference_configuration: reference configuration to compute the placement of the lock joints\n");
bp::def("buildReducedModel",
buildReducedModel_list<double,0,JointCollectionDefaultTpl,VectorXd>,
bp::args("model: input kinematic model",
"list_of_joints_to_lock: list of joint indexes to lock",
"reference_configuration: reference configuration to compute the placement of the lock joints"),
"Build a reduce model from a given input model and a list of joint to lock.");
(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"
" model: input kinematic modell to reduce\n"
" geom_model: input geometry model to reduce\n"
" list_of_joints_to_lock: list of joint indexes to lock\n"
" reference_configuration: reference configuration to compute the placement of the lock joints\n");
bp::def("buildReducedModel",
(Model (*)(const Model &, const std::vector<JointIndex> &, const Eigen::MatrixBase<VectorXd> &))&buildReducedModel<double,0,JointCollectionDefaultTpl,VectorXd>,
bp::args("model: input kinematic model",
"list_of_joints_to_lock: list of joint indexes to lock",
"reference_configuration: reference configuration to compute the placement of the lock joints"),
"Build a reduce model from a given input model and a list of joint to lock.");
(bp::tuple (*)(const Model &, const GeometryModel &, const bp::list &, 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"
" model: input kinematic modell to reduce\n"
" geom_model: input geometry model to reduce\n"
" list_of_joints_to_lock: list of joint indexes to lock\n"
" reference_configuration: reference configuration to compute the placement of the lock joints\n");
}
......
......@@ -84,6 +84,9 @@ namespace pinocchio
bp::args("collision_pair"),
"Return the index of a collision pair.")
#endif // PINOCCHIO_WITH_HPP_FCL
.def(bp::self == bp::self)
.def(bp::self != bp::self)
;
}
......
......@@ -11,7 +11,7 @@
namespace pinocchio
{
/**
* \brief Append a child model into a parent model, after a specific frame.
* \brief Append a child model into a parent model, after a specific frame given by its index.
* the model given as reference argument.
*
* \param[in] modelA the parent model.
......@@ -30,7 +30,7 @@ namespace pinocchio
ModelTpl<Scalar,Options,JointCollectionTpl> & model);
/**
* \brief Append a child model into a parent model, after a specific frame.
* \brief Append a child model into a parent model, after a specific frame given by its index.
*
* \param[in] modelA the parent model.
* \param[in] modelB the child model.
......@@ -75,13 +75,15 @@ namespace pinocchio
/**
*
* \brief Build a reduce model from a given input model and a list of joint to lock.
* \brief Build a reduced 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.
* \param[out] reduced_model the reduced 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
......@@ -92,7 +94,7 @@ namespace pinocchio
/**
*
* \brief Build a reduce model from a given input model and a list of joint to lock.
* \brief Build a reduced 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.
......@@ -100,6 +102,8 @@ namespace pinocchio
*
* \returns A reduce model of the input 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>
ModelTpl<Scalar,Options,JointCollectionTpl>
......@@ -115,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__
......@@ -49,6 +49,11 @@ namespace pinocchio
struct FakeCollisionGeometry
{
FakeCollisionGeometry(){};
bool operator==(const FakeCollisionGeometry & other) const
{
return true;
}
};
struct AABB
......
......@@ -29,20 +29,11 @@ namespace pinocchio
typedef SE3Tpl<Scalar,Options> SE3;
typedef ::pinocchio::GeometryObject GeometryObject;
typedef container::aligned_vector<GeometryObject> GeometryObjectVector;
typedef std::vector<CollisionPair> CollisionPairVector;
typedef pinocchio::GeomIndex GeomIndex;
/// \brief The number of GeometryObjects
Index ngeoms;
/// \brief Vector of GeometryObjects used for collision computations
GeometryObjectVector geometryObjects;
///
/// \brief Vector of collision pairs.
///
CollisionPairVector collisionPairs;
GeometryModel()
: ngeoms(0)
......@@ -141,8 +132,40 @@ namespace pinocchio
PairIndex findCollisionPair(const CollisionPair & pair) const;
#endif // PINOCCHIO_WITH_HPP_FCL
///
/// \brief Returns true if *this and other are equal.
///
bool operator==(const GeometryModel & other) const
{
return
ngeoms == other.ngeoms
&& geometryObjects == other.geometryObjects
&& collisionPairs == other.collisionPairs
;
}
///
/// \brief Returns true if *this and other are not equal.
///
bool operator!=(const GeometryModel & other) const
{
return !(*this == other);
}
friend std::ostream& operator<<(std::ostream & os, const GeometryModel & model_geom);
friend std::ostream& operator<<(std::ostream & os,
const GeometryModel & model_geom);
/// \brief The number of GeometryObjects
Index ngeoms;
/// \brief Vector of GeometryObjects used for collision computations
GeometryObjectVector geometryObjects;
///
/// \brief Vector of collision pairs.
///
CollisionPairVector collisionPairs;
}; // struct GeometryModel
struct GeometryData
......
......@@ -10,6 +10,7 @@
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/frames.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/geometry.hpp"
#include "pinocchio/parsers/sample-models.hpp"
......@@ -250,7 +251,6 @@ BOOST_AUTO_TEST_CASE(test_buildReducedModel)
BOOST_CHECK(humanoid_copy_model.names == humanoid_model.names);
BOOST_CHECK(humanoid_copy_model.joints == humanoid_model.joints);
BOOST_CHECK(humanoid_copy_model.joints == humanoid_model.joints);
BOOST_CHECK(humanoid_copy_model == humanoid_model);
std::vector<JointIndex> joints_to_lock;
......@@ -263,6 +263,10 @@ BOOST_AUTO_TEST_CASE(test_buildReducedModel)
BOOST_CHECK(reduced_humanoid_model.njoints == humanoid_model.njoints-(int)joints_to_lock.size());
BOOST_CHECK(reduced_humanoid_model != humanoid_model);
Model reduced_humanoid_model_other_signature;
buildReducedModel(humanoid_model,joints_to_lock,reference_config_humanoid,reduced_humanoid_model_other_signature);
BOOST_CHECK(reduced_humanoid_model == reduced_humanoid_model_other_signature);
// Check that forward kinematics give same results
Data data(humanoid_model), reduced_data(reduced_humanoid_model);
Eigen::VectorXd q = reference_config_humanoid;
......@@ -316,4 +320,117 @@ BOOST_AUTO_TEST_CASE(test_buildReducedModel)
}
}
#ifdef PINOCCHIO_WITH_HPP_FCL
BOOST_AUTO_TEST_CASE(test_buildReducedModel_with_geom)
{
Model humanoid_model;
buildModels::humanoid(humanoid_model);
humanoid_model.lowerPositionLimit.head<3>().fill(-1.);
humanoid_model.upperPositionLimit.head<3>().fill( 1.);
const Eigen::VectorXd reference_config_humanoid = randomConfiguration(humanoid_model);
GeometryModel humanoid_geometry;
buildModels::humanoidGeometries(humanoid_model, humanoid_geometry);
static const std::vector<JointIndex> empty_index_vector;
Model humanoid_copy_model; GeometryModel humanoid_copy_geometry;
buildReducedModel(humanoid_model,humanoid_geometry,empty_index_vector,
reference_config_humanoid,humanoid_copy_model,humanoid_copy_geometry);
BOOST_CHECK(humanoid_copy_model == humanoid_model);
BOOST_CHECK(humanoid_copy_geometry == humanoid_geometry);
std::vector<JointIndex> joints_to_lock;
const std::string joint1_to_lock = "rarm_shoulder2_joint";
joints_to_lock.push_back(humanoid_model.getJointId(joint1_to_lock));
const std::string joint2_to_lock = "larm_shoulder2_joint";
joints_to_lock.push_back(humanoid_model.getJointId(joint2_to_lock));
Model reduced_humanoid_model; GeometryModel reduced_humanoid_geometry;
buildReducedModel(humanoid_model,humanoid_geometry,joints_to_lock,
reference_config_humanoid,reduced_humanoid_model,reduced_humanoid_geometry);
BOOST_CHECK(reduced_humanoid_geometry.ngeoms == humanoid_geometry.ngeoms);
BOOST_CHECK(reduced_humanoid_geometry.collisionPairs == humanoid_geometry.collisionPairs);
BOOST_CHECK(reduced_humanoid_geometry.geometryObjects.size() == humanoid_geometry.geometryObjects.size());
for(Index i = 0; i < humanoid_geometry.geometryObjects.size(); ++i)
{
const GeometryObject & go1 = humanoid_geometry.geometryObjects[i];
const GeometryObject & go2 = reduced_humanoid_geometry.geometryObjects[i];
BOOST_CHECK(go1.name == go2.name);
BOOST_CHECK(go1.geometry == go2.geometry);
BOOST_CHECK(go1.meshPath == go2.meshPath);
BOOST_CHECK(go1.meshScale == go2.meshScale);
BOOST_CHECK(go1.overrideMaterial == go2.overrideMaterial);
BOOST_CHECK(go1.meshColor == go2.meshColor);
BOOST_CHECK(go1.meshTexturePath == go2.meshTexturePath);
}
Data data(humanoid_model), reduced_data(reduced_humanoid_model);
const Eigen::VectorXd q = reference_config_humanoid;
Eigen::VectorXd reduced_q(reduced_humanoid_model.nq);
for(JointIndex joint_id = 1; joint_id < (JointIndex)reduced_humanoid_model.njoints; ++joint_id)
{
const JointIndex reference_joint_id = humanoid_model.getJointId(reduced_humanoid_model.names[joint_id]);
reduced_humanoid_model.joints[joint_id].jointConfigSelector(reduced_q) =
humanoid_model.joints[reference_joint_id].jointConfigSelector(q);
}
framesForwardKinematics(humanoid_model,data,q);
framesForwardKinematics(reduced_humanoid_model,reduced_data,reduced_q);
for(size_t frame_id = 0; frame_id < reduced_humanoid_model.frames.size(); ++frame_id)
{
const Frame & reduced_frame = reduced_humanoid_model.frames[frame_id];
switch(reduced_frame.type)
{
case JOINT:
case FIXED_JOINT:
{
// May not be present in the original model
if(humanoid_model.existJointName(reduced_frame.name))
{
const JointIndex joint_id = humanoid_model.getJointId(reduced_frame.name);
BOOST_CHECK(data.oMi[joint_id].isApprox(reduced_data.oMf[frame_id]));
}
else if(humanoid_model.existFrame(reduced_frame.name))
{
const FrameIndex humanoid_frame_id = humanoid_model.getFrameId(reduced_frame.name);
BOOST_CHECK(data.oMf[humanoid_frame_id].isApprox(reduced_data.oMf[frame_id]));
}
else
{
BOOST_CHECK_MESSAGE(false, "The frame " << reduced_frame.name << " is not presend in the humanoid_model");
}
break;
}
default:
{
BOOST_CHECK(humanoid_model.existFrame(reduced_frame.name));
const FrameIndex humanoid_frame_id = humanoid_model.getFrameId(reduced_frame.name);
BOOST_CHECK(data.oMf[humanoid_frame_id].isApprox(reduced_data.oMf[frame_id]));
break;
}
}
}
// Test GeometryObject placements
GeometryData geom_data(humanoid_geometry), reduded_geom_data(reduced_humanoid_geometry);
updateGeometryPlacements(humanoid_model,data,humanoid_geometry,geom_data);
updateGeometryPlacements(reduced_humanoid_model,reduced_data,reduced_humanoid_geometry,reduded_geom_data);
BOOST_CHECK(geom_data.oMg.size() == reduded_geom_data.oMg.size());
for(FrameIndex i = 0; i < geom_data.oMg.size(); ++i)
{
BOOST_CHECK(geom_data.oMg[i].isApprox(reduded_geom_data.oMg[i]));
}
}
#endif // PINOCCHIO_WITH_HPP_FCL
BOOST_AUTO_TEST_SUITE_END()
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