Unverified Commit 5e07b288 authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #752 from jmirabel/devel

[C++] Add algo appendModel to merge two Models.
parents 9babff77 1be8b48a
......@@ -89,10 +89,10 @@ IF(BUILD_WITH_URDF_SUPPORT)
ADD_REQUIRED_DEPENDENCY("urdfdom >= 0.2.0")
ENDIF(BUILD_WITH_URDF_SUPPORT)
IF(BUILD_WITH_HPP_FCL_SUPPORT)
IF(BUILD_WITH_COLLISION_SUPPORT)
find_package(catkin QUIET COMPONENTS hpp-fcl) # compatability with catkin-packaged hpp-fcl
ADD_REQUIRED_DEPENDENCY("hpp-fcl >= 1.0.0")
ENDIF(BUILD_WITH_HPP_FCL_SUPPORT)
ENDIF(BUILD_WITH_COLLISION_SUPPORT)
IF(BUILD_WITH_AUTODIFF_SUPPORT)
ADD_REQUIRED_DEPENDENCY("cppad >= 20180000.0")
......
//
// Copyright (c) 2019 CNRS
//
#ifndef __pinocchio_algorithm_model_hpp__
#define __pinocchio_algorithm_model_hpp__
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/geometry.hpp"
namespace pinocchio
{
/**
* \brief Append a child model into a parent model, after a specific frame.
* the model given as reference argument.
*
* \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.
* \param[out] model the resulting model
*
*/
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
void
appendModel(const ModelTpl<Scalar,Options,JointCollectionTpl> & modelA,
const ModelTpl<Scalar,Options,JointCollectionTpl> & modelB,
FrameIndex frameInModelA,
const SE3Tpl<Scalar, Options>& aMb,
ModelTpl<Scalar,Options,JointCollectionTpl>& 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);
*
* \param[in] geomModelA the parent geometry model
* \param[in] geomModelB the child geometry model
* \param[out] geomModel
*/
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
void
appendModel(const ModelTpl<Scalar,Options,JointCollectionTpl> & modelA,
const ModelTpl<Scalar,Options,JointCollectionTpl> & modelB,
const GeometryModel& geomModelA,
const GeometryModel& geomModelB,
FrameIndex frameInModelA,
const SE3Tpl<Scalar, Options>& aMb,
ModelTpl<Scalar,Options,JointCollectionTpl>& model,
GeometryModel& geomModel);
} // namespace pinocchio
#include "pinocchio/algorithm/model.hxx"
#endif // ifndef __pinocchio_algorithm_model_hpp__
//
// Copyright (c) 2019 CNRS
//
#ifndef __pinocchio_algorithm_model_hxx__
#define __pinocchio_algorithm_model_hxx__
namespace pinocchio
{
namespace details {
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
void appendUniverseToModel (const ModelTpl<Scalar,Options,JointCollectionTpl> & modelAB,
const GeometryModel& geomModelAB,
FrameIndex parentFrame,
const SE3Tpl<Scalar, Options>& pfMAB,
ModelTpl<Scalar,Options,JointCollectionTpl> & model,
GeometryModel& geomModel)
{
typedef FrameTpl<Scalar, Options> Frame;
const Frame& pframe = model.frames[parentFrame];
JointIndex jid = pframe.parent;
assert (jid < model.joints.size());
// If inertia is not NaN, add it.
if (modelAB.inertias[0] == modelAB.inertias[0])
model.appendBodyToJoint (jid, modelAB.inertias[0], pframe.placement * pfMAB);
// Add all frames whose parent is this joint.
for (FrameIndex fid = 1; fid < modelAB.frames.size(); ++fid) {
Frame frame = modelAB.frames[fid];
if (frame.parent == 0) {
frame.parent = jid;
if (frame.previousFrame != 0) {
frame.previousFrame = model.getFrameId (
modelAB.frames[frame.previousFrame].name,
modelAB.frames[frame.previousFrame].type);
} else {
frame.previousFrame = parentFrame;
}
// Modify frame placement
frame.placement = pframe.placement * pfMAB * frame.placement;
model.addFrame (frame);
}
}
// Add all geometries whose parent is this joint.
for (GeomIndex gid = 0; gid < geomModelAB.geometryObjects.size(); ++gid) {
GeometryObject go = geomModelAB.geometryObjects[gid];
if (go.parentJoint == 0) {
go.parentJoint = jid;
if (go.parentFrame != 0) {
go.parentFrame = model.getFrameId (
modelAB.frames[go.parentFrame].name,
modelAB.frames[go.parentFrame].type);
} else {
go.parentFrame = parentFrame;
}
go.placement = pframe.placement * pfMAB * go.placement;
geomModel.addGeometryObject (go);
}
}
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
struct AppendJointOfModelAlgoTpl : public fusion::JointVisitorBase< AppendJointOfModelAlgoTpl<Scalar,Options,JointCollectionTpl> >
{
typedef boost::fusion::vector<
const ModelTpl<Scalar,Options,JointCollectionTpl> &,
const GeometryModel&,
JointIndex,
const SE3Tpl<Scalar, Options>&,
ModelTpl<Scalar,Options,JointCollectionTpl> &,
GeometryModel& > ArgsType;
template <typename JointModel>
static void algo (const JointModelBase<JointModel> & jmodel,
const ModelTpl<Scalar,Options,JointCollectionTpl> & modelAB,
const GeometryModel& geomModelAB,
JointIndex parentId,
const SE3Tpl<Scalar, Options>& pMi,
ModelTpl<Scalar,Options,JointCollectionTpl> & model,
GeometryModel& geomModel)
{
// If old parent is universe, use what's provided in the input.
// otherwise, get the parent from modelAB.
if (modelAB.parents[jmodel.id()] > 0)
parentId = model.getJointId(modelAB.names[modelAB.parents[jmodel.id()]]);
JointIndex jid = model.addJoint (
parentId,
jmodel,
pMi * modelAB.jointPlacements[jmodel.id()],
modelAB.names[jmodel.id()],
jmodel.jointVelocitySelector(modelAB.effortLimit),
jmodel.jointVelocitySelector(modelAB.velocityLimit),
jmodel.jointConfigSelector(modelAB.lowerPositionLimit),
jmodel.jointConfigSelector(modelAB.upperPositionLimit));
assert (jid < model.joints.size());
model.appendBodyToJoint (jid, modelAB.inertias[jmodel.id()]);
// Add all frames whose parent is this joint.
for (FrameIndex fid = 1; fid < modelAB.frames.size(); ++fid) {
FrameTpl<Scalar, Options> frame = modelAB.frames[fid];
if (frame.parent == jmodel.id()) {
frame.parent = jid;
assert (frame.previousFrame > 0 || frame.type == JOINT);
if (frame.previousFrame != 0) {
frame.previousFrame = model.getFrameId (
modelAB.frames[frame.previousFrame].name,
modelAB.frames[frame.previousFrame].type);
}
model.addFrame (frame);
}
}
// Add all geometries whose parent is this joint.
for (GeomIndex gid = 0; gid < geomModelAB.geometryObjects.size(); ++gid) {
GeometryObject go = geomModelAB.geometryObjects[gid];
if (go.parentJoint == jmodel.id()) {
go.parentJoint = jid;
assert (go.parentFrame > 0);
if (go.parentFrame != 0) {
go.parentFrame = model.getFrameId (
modelAB.frames[go.parentFrame].name,
modelAB.frames[go.parentFrame].type);
}
geomModel.addGeometryObject (go);
}
}
}
};
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
void
appendModel(const ModelTpl<Scalar,Options,JointCollectionTpl> & modelA,
const ModelTpl<Scalar,Options,JointCollectionTpl> & modelB,
FrameIndex frameInModelA,
const SE3Tpl<Scalar, Options>& aMb,
ModelTpl<Scalar,Options,JointCollectionTpl>& model)
{
GeometryModel geomModelA, geomModelB, geomModel;
appendModel(modelA, modelB, geomModelA, geomModelB, frameInModelA, aMb,
model, geomModel);
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
void
appendModel(const ModelTpl<Scalar,Options,JointCollectionTpl> & modelA,
const ModelTpl<Scalar,Options,JointCollectionTpl> & modelB,
const GeometryModel& geomModelA,
const GeometryModel& geomModelB,
FrameIndex frameInModelA,
const SE3Tpl<Scalar, Options>& aMb,
ModelTpl<Scalar,Options,JointCollectionTpl>& model,
GeometryModel& geomModel)
{
typedef details::AppendJointOfModelAlgoTpl<Scalar, Options, JointCollectionTpl> AppendJointOfModelAlgo;
typedef typename AppendJointOfModelAlgo::ArgsType ArgsType;
typedef SE3Tpl<Scalar, Options> SE3;
const FrameTpl<Scalar, Options>& frame = modelA.frames[frameInModelA];
static const SE3 id = SE3::Identity();
int njoints = modelA.njoints + modelB.njoints - 1;
model.names .reserve (njoints);
model.joints .reserve (njoints);
model.jointPlacements .reserve (njoints);
model.parents .reserve (njoints);
model.inertias .reserve (njoints);
int nframes = modelA.nframes + modelB.nframes - 1;
model.frames .reserve (nframes);
geomModel.geometryObjects.reserve (geomModelA.ngeoms + geomModelB.ngeoms);
// Copy modelA joints until frame.parentJoint
details::appendUniverseToModel (modelA, geomModelA, 0, id, model, geomModel);
for (JointIndex jid = 1; jid <= frame.parent; ++jid) {
ArgsType args (modelA, geomModelA, 0, id, model, geomModel);
AppendJointOfModelAlgo::run (modelA.joints[jid], args);
}
// Copy modelB joints
details::appendUniverseToModel (modelB, geomModelB,
model.getFrameId (frame.name, frame.type), aMb, model, geomModel);
for (JointIndex jid = 1; jid < modelB.joints.size(); ++jid) {
SE3 pMi = (jid == 1 ? frame.placement * aMb : id);
ArgsType args (modelB, geomModelB, frame.parent, pMi, model, geomModel);
AppendJointOfModelAlgo::run (modelB.joints[jid], args);
}
// Copy remaining joints of modelA
for (JointIndex jid = frame.parent+1; jid < modelA.joints.size(); ++jid) {
ArgsType args (modelA, geomModelA, 0, id, model, geomModel);
AppendJointOfModelAlgo::run (modelA.joints[jid], args);
}
// Add collision pairs of geomModelA and geomModelB
geomModel.collisionPairs.reserve(geomModelA.collisionPairs.size()
+ geomModelB.collisionPairs.size()
+ geomModelA.geometryObjects.size() * geomModelB.geometryObjects.size());
for (std::size_t icp = 0; icp < geomModelA.collisionPairs.size(); ++icp) {
const CollisionPair& cp (geomModelA.collisionPairs[icp]);
GeomIndex go1 = geomModel.getGeometryId(geomModelA.geometryObjects[cp.first ].name);
GeomIndex go2 = geomModel.getGeometryId(geomModelA.geometryObjects[cp.second].name);
geomModel.addCollisionPair (CollisionPair (go1, go2));
}
for (std::size_t icp = 0; icp < geomModelB.collisionPairs.size(); ++icp) {
const CollisionPair& cp (geomModelB.collisionPairs[icp]);
GeomIndex go1 = geomModel.getGeometryId(geomModelB.geometryObjects[cp.first ].name);
GeomIndex go2 = geomModel.getGeometryId(geomModelB.geometryObjects[cp.second].name);
geomModel.addCollisionPair (CollisionPair (go1, go2));
}
// add the collision pairs between geomModelA and geomModelB.
for (Index i = 0; i < geomModelA.geometryObjects.size(); ++i) {
GeomIndex go1 = geomModel.getGeometryId(geomModelA.geometryObjects[i].name);
for (Index j = 0; j < geomModelB.geometryObjects.size(); ++j) {
GeomIndex go2 = geomModel.getGeometryId(geomModelB.geometryObjects[j].name);
if ( geomModel.geometryObjects[go1].parentJoint
!= geomModel.geometryObjects[go2].parentJoint)
geomModel.addCollisionPair(CollisionPair(go1, go2));
}
}
}
} // namespace pinocchio
#endif // ifndef __pinocchio_algorithm_model_hxx__
......@@ -48,7 +48,7 @@ namespace pinocchio
os << "Nb joints = " << model.njoints << " (nq="<< model.nq<<",nv="<<model.nv<<")" << std::endl;
for(Index i=0;i<(Index)(model.njoints);++i)
{
os << " Joint "<< model.names[i] << ": parent=" << model.parents[i] << std::endl;
os << " Joint " << i << " " << model.names[i] << ": parent=" << model.parents[i] << std::endl;
}
return os;
......
......@@ -337,6 +337,7 @@ namespace pinocchio
jff.addJoint(typename JC::JointModelSphericalZYX());
ffidx = model.addJoint(0,jff,SE3::Identity(),"freeflyer_joint");
}
model.appendBodyToJoint(ffidx,Ijoint);
model.addJointFrame(ffidx);
/* --- Lower limbs --- */
......
......@@ -3,6 +3,7 @@
//
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/algorithm/model.hpp"
#include "pinocchio/parsers/sample-models.hpp"
#include <boost/test/unit_test.hpp>
......@@ -43,4 +44,110 @@ BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
BOOST_CHECK(model.cast<long double>().cast<double>() == model);
}
#ifdef PINOCCHIO_WITH_HPP_FCL
struct AddPrefix {
std::string p;
std::string operator() (const std::string& n) { return p + n; }
Frame operator() (const Frame& _f) { Frame f (_f); f.name = p + f.name; return f; }
AddPrefix (const char* _p) : p(_p) {}
};
BOOST_AUTO_TEST_CASE(append)
{
Model manipulator, humanoid, model;
GeometryModel geomManipulator, geomHumanoid, geomModel;
buildModels::manipulator(manipulator);
buildModels::manipulatorGeometries(manipulator, geomManipulator);
// Add prefix to joint and frame names
AddPrefix addManipulatorPrefix ("manipulator/");
std::transform (++manipulator.names.begin(), manipulator.names.end(),
++manipulator.names.begin(), addManipulatorPrefix);
std::transform (++manipulator.frames.begin(), manipulator.frames.end(),
++manipulator.frames.begin(), addManipulatorPrefix);
BOOST_MESSAGE(manipulator);
buildModels::humanoid(humanoid);
buildModels::humanoidGeometries(humanoid, geomHumanoid);
// Add prefix to joint and frame names
AddPrefix addHumanoidPrefix ("humanoid/");
std::transform (++humanoid.names.begin(), humanoid.names.end(),
++humanoid.names.begin(), addHumanoidPrefix);
std::transform (++humanoid.frames.begin(), humanoid.frames.end(),
++humanoid.frames.begin(), addHumanoidPrefix);
BOOST_MESSAGE(humanoid);
//TODO fix inertia of the base
manipulator.inertias[0].setRandom();
SE3 aMb = SE3::Random();
FrameIndex fid = humanoid.addFrame (Frame ("humanoid/add_manipulator",
humanoid.getJointId("humanoid/chest2_joint"),
humanoid.getFrameId("humanoid/chest2_joint"), aMb,
OP_FRAME));
appendModel (humanoid, manipulator, geomHumanoid, geomManipulator, fid,
SE3::Identity(), model, geomModel);
BOOST_MESSAGE(model);
// Check the model
BOOST_CHECK_EQUAL(model.getJointId("humanoid/chest2_joint"),
model.parents[model.getJointId("manipulator/shoulder1_joint")]);
// check the joint order and the inertias
JointIndex chest2 = model.getJointId("humanoid/chest2_joint");
for (JointIndex jid = 1; jid < chest2; ++jid) {
BOOST_MESSAGE("Checking joint " << jid << " " << model.names[jid]);
BOOST_CHECK_EQUAL(model.names[jid], humanoid.names[jid]);
BOOST_CHECK_EQUAL(model.inertias[jid], humanoid.inertias[jid]);
BOOST_CHECK_EQUAL(model.jointPlacements[jid], humanoid.jointPlacements[jid]);
}
BOOST_MESSAGE("Checking joint " << chest2 << " " << model.names[chest2]);
BOOST_CHECK_EQUAL(model.names[chest2], humanoid.names[chest2]);
BOOST_CHECK_MESSAGE(model.inertias[chest2].isApprox(manipulator.inertias[0].se3Action(aMb) + humanoid.inertias[chest2]),
model.inertias[chest2] << " != " << manipulator.inertias[0].se3Action(aMb) + humanoid.inertias[chest2]);
BOOST_CHECK_EQUAL(model.jointPlacements[chest2], humanoid.jointPlacements[chest2]);
for (JointIndex jid = 1; jid < manipulator.joints.size(); ++jid) {
BOOST_MESSAGE("Checking joint " << chest2+jid << " " << model.names[chest2+jid]);
BOOST_CHECK_EQUAL(model.names[chest2+jid], manipulator.names[jid]);
BOOST_CHECK_EQUAL(model.inertias[chest2+jid], manipulator.inertias[jid]);
if (jid==1)
BOOST_CHECK_EQUAL(model.jointPlacements[chest2+jid], aMb*manipulator.jointPlacements[jid]);
else
BOOST_CHECK_EQUAL(model.jointPlacements[chest2+jid], manipulator.jointPlacements[jid]);
}
for (JointIndex jid = chest2+1; jid < humanoid.joints.size(); ++jid) {
BOOST_MESSAGE("Checking joint " << jid+manipulator.joints.size()-1 << " " << model.names[jid+manipulator.joints.size()-1]);
BOOST_CHECK_EQUAL(model.names[jid+manipulator.joints.size()-1], humanoid.names[jid]);
BOOST_CHECK_EQUAL(model.inertias[jid+manipulator.joints.size()-1], humanoid.inertias[jid]);
BOOST_CHECK_EQUAL(model.jointPlacements[jid+manipulator.joints.size()-1], humanoid.jointPlacements[jid]);
}
// Check the frames
for (FrameIndex fid = 1; fid < humanoid.frames.size(); ++fid) {
const Frame& frame = humanoid.frames[fid],
parent = humanoid.frames[frame.previousFrame];
BOOST_CHECK(model.existFrame (frame.name, frame.type));
const Frame& nframe = model.frames[model.getFrameId(frame.name, frame.type)],
nparent = model.frames[nframe.previousFrame];
BOOST_CHECK_EQUAL(parent.name, nparent.name);
BOOST_CHECK_EQUAL(frame.placement, nframe.placement);
}
for (FrameIndex fid = 1; fid < manipulator.frames.size(); ++fid) {
const Frame& frame = manipulator.frames[fid],
parent = manipulator.frames[frame.previousFrame];
BOOST_CHECK(model.existFrame (frame.name, frame.type));
const Frame& nframe = model.frames[model.getFrameId(frame.name, frame.type)],
nparent = model.frames[nframe.previousFrame];
if (frame.previousFrame > 0) {
BOOST_CHECK_EQUAL(parent.name, nparent.name);
BOOST_CHECK_EQUAL(frame.placement, nframe.placement);
}
}
}
#endif
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