diff --git a/src/parsers/lua.cpp b/src/parsers/lua.cpp
index 48dbcbda30c27a51fb58ca20db15cf3ee2ca6d57..34451a30e0e4838d9dfe6fb07f7d9c52a4c39953 100644
--- a/src/parsers/lua.cpp
+++ b/src/parsers/lua.cpp
@@ -142,8 +142,10 @@ namespace se3
idx = model.addJoint(parent_id,jmodel,
joint_placement,joint_name);
- model.appendBodyToJoint(idx,Y,SE3::Identity(),body_name);
-
+ model.addJointFrame(idx);
+ model.appendBodyToJoint(idx,Y);
+ model.addBodyFrame(body_name, idx);
+
return idx;
}
@@ -295,7 +297,11 @@ namespace se3
}
else if (joint_type == "JointTypeFixed")
{
- model.appendBodyToJoint(parent_id, Y, global_placement, "");
+ model.appendBodyToJoint(parent_id, Y, global_placement);
+ // TODO Why not
+ // model.addBodyFrame(body_name, parent_id, global_placement);
+ // ???
+ model.addBodyFrame(randomStringGenerator(8), parent_id, global_placement);
joint_id = (Model::JointIndex)model.njoint;
diff --git a/src/parsers/sample-models.cpp b/src/parsers/sample-models.cpp
index c46a64f310d684baf2f44318bf5a6a86f7e4a493..052cde20be33617bc74dcc4e9e500bf987ed6b92 100644
--- a/src/parsers/sample-models.cpp
+++ b/src/parsers/sample-models.cpp
@@ -22,112 +22,89 @@ namespace se3
{
namespace buildModels
{
+ const SE3 Id = SE3::Identity();
+
template<typename JointModel>
static void addJointAndBody(Model & model,
const JointModelBase<JointModel> & joint,
const std::string & parent_name,
- const std::string & name)
+ const std::string & name,
+ const SE3 placement = SE3::Random(),
+ bool setRandomLimits = true)
{
typedef typename JointModel::ConfigVector_t CV;
typedef typename JointModel::TangentVector_t TV;
Model::JointIndex idx;
- idx = model.addJoint(model.getJointId(parent_name),joint,
- SE3::Random(),name + "_joint",
- TV::Random() + TV::Constant(1),
- TV::Random() + TV::Constant(1),
- CV::Random() - CV::Constant(1),
- CV::Random() + CV::Constant(1));
-
- model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity(),name + "_body");
+ if (setRandomLimits)
+ idx = model.addJoint(model.getJointId(parent_name),joint,
+ placement, name + "_joint",
+ TV::Random() + TV::Constant(1),
+ TV::Random() + TV::Constant(1),
+ CV::Random() - CV::Constant(1),
+ CV::Random() + CV::Constant(1));
+ else
+ idx = model.addJoint(model.getJointId(parent_name),joint,
+ placement, name + "_joint");
+ model.addJointFrame(idx);
+ model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity());
+ model.addBodyFrame(name + "_body", idx);
}
void humanoid2d(Model & model)
{
- Model::JointIndex idx;
-
// root
- idx = model.addJoint(model.getJointId("universe"),JointModelRX(),SE3::Identity(),"ff1_joint");
- model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity(),"ff1_body");
-
- idx = model.addJoint(model.getJointId("ff1_joint"),JointModelRY(),SE3::Identity(),"root_joint");
- model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity(),"root_body");
+ addJointAndBody(model, JointModelRX(), "universe", "ff1", Id, false);
+ addJointAndBody(model, JointModelRY(), "ff1_joint", "root", Id, false);
// lleg
- idx = model.addJoint(model.getJointId("root_joint"),JointModelRZ(),SE3::Identity(),"lleg1_joint");
- model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity(),"lleg1_body");
-
- idx = model.addJoint(model.getJointId("lleg1_joint"),JointModelRY(),SE3::Identity(),"lleg2_joint");
- model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity(),"lleg2_body");
+ addJointAndBody(model, JointModelRZ(), "root_joint", "lleg1", Id, false);
+ addJointAndBody(model, JointModelRY(), "lleg1_joint", "lleg2", Id, false);
// rlgg
- idx = model.addJoint(model.getJointId("root_joint"),JointModelRZ(),SE3::Identity(),"rleg1_joint");
- model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity(),"rleg1_body");
-
- idx = model.addJoint(model.getJointId("rleg1_joint"),JointModelRY(),SE3::Identity(),"rleg2_joint");
- model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity(),"rleg2_body");
+ addJointAndBody(model, JointModelRZ(), "root_joint", "rleg1", Id, false);
+ addJointAndBody(model, JointModelRY(), "lleg1_joint", "rleg2", Id, false);
// torso
- idx = model.addJoint(model.getJointId("root_joint"),JointModelRY(),SE3::Identity(),"torso1_joint");
- model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity(),"torso1_body");
-
- idx = model.addJoint(model.getJointId("torso1_joint"),JointModelRZ(),SE3::Identity(),"chest_joint");
- model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity(),"chest_body");
+ addJointAndBody(model, JointModelRY(), "root_joint", "torso1", Id, false);
+ addJointAndBody(model, JointModelRZ(), "torso1_joint", "chest", Id, false);
// rarm
- idx = model.addJoint(model.getJointId("chest_joint"),JointModelRX(),SE3::Identity(),"rarm1_joint");
- model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity(),"rarm1_body");
-
- idx = model.addJoint(model.getJointId("rarm1_joint"),JointModelRZ(),SE3::Identity(),"rarm2_joint");
- model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity(),"rarm2_body");
+ addJointAndBody(model, JointModelRX(), "chest_joint", "rarm1", Id, false);
+ addJointAndBody(model, JointModelRZ(), "rarm1_joint", "rarm2", Id, false);
// larm
- idx = model.addJoint(model.getJointId("chest_joint"),JointModelRX(),SE3::Identity(),"larm1_joint");
- model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity(),"larm1_body");
-
- idx = model.addJoint(model.getJointId("larm1_joint"),JointModelRZ(),SE3::Identity(),"larm2_joint");
- model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity(),"larm2_body");
+ addJointAndBody(model, JointModelRX(), "root_joint", "larm1", Id, false);
+ addJointAndBody(model, JointModelRZ(), "larm1_joint", "larm2", Id, false);
}
void humanoidSimple(Model & model, bool usingFF)
{
- Model::JointIndex idx;
-
// root
if(! usingFF )
{
- idx = model.addJoint(model.getJointId("universe"),JointModelRX(),SE3::Identity(),"ff1_joint");
- model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity(),"ff1_body");
-
- idx = model.addJoint(model.getJointId("ff1_joint"),JointModelRY(),SE3::Identity(),"ff2_joint");
- model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity(),"ff2_body");
-
- idx = model.addJoint(model.getJointId("ff2_joint"),JointModelRZ(),SE3::Identity(),"ff3_joint");
- model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity(),"ff3_body");
-
- idx = model.addJoint(model.getJointId("ff3_joint"),JointModelRZ(),SE3::Random(),"ff4_joint");
- model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity(),"ff4_body");
-
- idx = model.addJoint(model.getJointId("ff4_joint"),JointModelRY(),SE3::Identity(),"ff5_joint");
- model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity(),"ff5_body");
-
- idx = model.addJoint(model.getJointId("ff5_joint"),JointModelRX(),SE3::Identity(),"root_joint");
- model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity(),"root_body");
+ addJointAndBody(model, JointModelRX(), "universe", "ff1", Id, false);
+ addJointAndBody(model, JointModelRY(), "ff1_joint", "ff2", Id, false);
+ addJointAndBody(model, JointModelRZ(), "ff2_joint", "ff3", Id, false);
+ addJointAndBody(model, JointModelRZ(), "ff3_joint", "ff4", Id, false);
+ addJointAndBody(model, JointModelRY(), "ff4_joint", "ff5", Id, false);
+ addJointAndBody(model, JointModelRX(), "ff5_joint", "root", Id, false);
}
else
{
- typedef JointModelFreeFlyer::ConfigVector_t CV;
- typedef JointModelFreeFlyer::TangentVector_t TV;
+ // typedef JointModelFreeFlyer::ConfigVector_t CV;
+ // typedef JointModelFreeFlyer::TangentVector_t TV;
- idx = model.addJoint(model.getJointId("universe"),JointModelFreeFlyer(),
- SE3::Identity(),"root_joint",
- TV::Zero(), 1e3 * (TV::Random() + TV::Constant(1.)),
- 1e3 * (CV::Random() - CV::Constant(1)),
- 1e3 * (CV::Random() + CV::Constant(1)));
- model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity(),"root_body");
+ addJointAndBody(model, JointModelFreeFlyer(), "universe", "root", Id, false);
+ // idx = model.addJoint(model.getJointId("universe"),JointModelFreeFlyer(),
+ // SE3::Identity(),"root_joint",
+ // TV::Zero(), 1e3 * (TV::Random() + TV::Constant(1.)),
+ // 1e3 * (CV::Random() - CV::Constant(1)),
+ // 1e3 * (CV::Random() + CV::Constant(1)));
+ // model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity(),"root_body");
}
// lleg
diff --git a/src/parsers/urdf/model.cpp b/src/parsers/urdf/model.cpp
index be9d73bd8daae8b3bd7d72494090397825c0cc3c..8737fe49ef479081bb163e753cf29a707fb648fe 100644
--- a/src/parsers/urdf/model.cpp
+++ b/src/parsers/urdf/model.cpp
@@ -33,24 +33,52 @@ namespace se3
{
namespace details
{
- void appendBodyToJoint(Model& model, const Model::JointIndex jid,
+ FrameIndex getParentJointFrame(::urdf::LinkConstPtr link, Model & model)
+ {
+ assert(link!=NULL && link->getParent()!=NULL);
+
+ FrameIndex id;
+ if (link->getParent()->parent_joint==NULL) {
+ if (model.existFrame("root_joint"))
+ id = model.getFrameId ("root_joint");
+ else
+ id = 0;
+ } else {
+ if (model.existFrame(link->getParent()->parent_joint->name))
+ id = model.getFrameId (link->getParent()->parent_joint->name);
+ else
+ throw std::invalid_argument ("Model does not have any joints named "
+ + link->getParent()->parent_joint->name);
+ }
+
+ Frame& f = model.frames[id];
+ if (f.type == JOINT || f.type == FIXED_JOINT)
+ return id;
+ throw std::invalid_argument ("Parent frame is not a JOINT neither a FIXED_JOINT");
+ }
+
+ void appendBodyToJoint(Model& model, const FrameIndex fid,
const boost::shared_ptr< ::urdf::Inertial> Y,
const SE3 & placement,
const std::string & body_name)
{
- if (Y == NULL || Y->mass < Eigen::NumTraits<double>::epsilon()) {
- model.addFrame(Frame(body_name, jid, placement, BODY));
- } else {
- model.appendBodyToJoint(jid, convertFromUrdf(*Y), placement, body_name);
+ const Frame& frame = model.frames[fid];
+ const SE3& p = frame.placement * placement;
+ if (Y != NULL && Y->mass > Eigen::NumTraits<double>::epsilon()) {
+ model.appendBodyToJoint(frame.parent, convertFromUrdf(*Y), p);
}
- assert (!model.inertias[jid].lever().hasNaN() && ! model.inertias[jid].inertia().data().hasNaN());
+ model.addBodyFrame(body_name, frame.parent, p, fid);
+ // Reference to model.frames[fid] can has changed because the vector
+ // may have been reallocated.
+ assert (!model.inertias[model.frames[fid].parent].lever().hasNaN()
+ && !model.inertias[model.frames[fid].parent].inertia().data().hasNaN());
}
///
/// \brief Shortcut for adding a joint and directly append a body to it.
///
template<typename JointModel>
- void addJointAndBody(Model & model, const JointModelBase<JointModel> & jmodel, const Model::JointIndex parent_id,
+ void addJointAndBody(Model & model, const JointModelBase<JointModel> & jmodel, const FrameIndex& parentFrameId,
const SE3 & joint_placement, const std::string & joint_name,
const boost::shared_ptr< ::urdf::Inertial> Y,
const std::string & body_name,
@@ -60,29 +88,44 @@ namespace se3
const typename JointModel::ConfigVector_t & max_config = JointModel::ConfigVector_t::Constant(std::numeric_limits<double>::max()))
{
Model::JointIndex idx;
+ Frame& frame = model.frames[parentFrameId];
- idx = model.addJoint(parent_id,jmodel,
- joint_placement,joint_name,
- max_effort,max_velocity,
- min_config,max_config);
-
- appendBodyToJoint(model, idx, Y, SE3::Identity(), body_name);
+ idx = model.addJoint(frame.parent,jmodel,
+ frame.placement * joint_placement,
+ joint_name,
+ max_effort,max_velocity,min_config,max_config);
+ FrameIndex jointFrameId = model.addJointFrame(idx, parentFrameId);
+ appendBodyToJoint(model, jointFrameId, Y, SE3::Identity(), body_name);
}
///
/// \brief Handle the case of JointModelDense which is dynamic.
///
- void addJointAndBody(Model & model, const JointModelBase< JointModelDense<-1,-1> > & jmodel, const Model::JointIndex parent_id,
- const SE3 & joint_placement, const std::string & joint_name,
- const boost::shared_ptr< ::urdf::Inertial> Y,
- const std::string & body_name)
+ void addJointAndBody(Model & , const JointModelBase< JointModelDense<-1,-1> > & , const FrameIndex& ,
+ const SE3 & , const std::string & ,
+ const boost::shared_ptr< ::urdf::Inertial> ,
+ const std::string & )
+ {
+ assert(false && "Cannot add a joint of type JointModelDense");
+ }
+
+ ///
+ /// \brief Shortcut for adding a fixed joint and directly append a body to it.
+ ///
+ void addFixedJointAndBody(Model & model, const FrameIndex& parentFrameId,
+ const SE3 & joint_placement, const std::string & joint_name,
+ const boost::shared_ptr< ::urdf::Inertial> Y,
+ const std::string & body_name)
{
Model::JointIndex idx;
-
- idx = model.addJoint(parent_id,jmodel,
- joint_placement,joint_name);
-
- appendBodyToJoint(model, idx, Y, SE3::Identity(), body_name);
+ Frame& frame = model.frames[parentFrameId];
+
+ model.addFrame(
+ Frame (joint_name, frame.parent, parentFrameId,
+ frame.placement, FIXED_JOINT)
+ );
+ // TODO addFrame should return an index.
+ appendBodyToJoint(model, model.frames.size()-1, Y, SE3::Identity(), body_name);
}
///
@@ -91,17 +134,13 @@ namespace se3
///
/// \param[in] link The current URDF link.
/// \param[in] model The model where the link must be added.
- /// \param[in] placementOffset The relative placement of the link relative to the closer non fixed joint in the tree.
///
- void parseTree(::urdf::LinkConstPtr link, Model & model, const SE3 & placementOffset, bool verbose) throw (std::invalid_argument)
+ void parseTree(::urdf::LinkConstPtr link, Model & model, bool verbose) throw (std::invalid_argument)
{
// Parent joint of the current body
::urdf::JointConstPtr joint = link->parent_joint;
- // OffSet of the next link. In case we encounter a fixed joint, we need to propagate the offset placement of its attached body to next joint.
- SE3 nextPlacementOffset = SE3::Identity();
-
if(joint != NULL) // if the link is not the root of the tree
{
assert(link->getParent()!=NULL);
@@ -111,12 +150,10 @@ namespace se3
const std::string & parent_link_name = link->getParent()->name;
std::ostringstream joint_info;
- Model::JointIndex parent_joint_id = (link->getParent()->parent_joint==NULL)
- ? (model.existJointName("root_joint") ? model.getJointId("root_joint") : 0)
- : model.getJointId( link->getParent()->parent_joint->name );
+ FrameIndex parentFrameId = getParentJointFrame(link, model);
// Transformation from the parent link to the joint origin
- const SE3 & jointPlacement = placementOffset*convertFromUrdf(joint->parent_to_joint_origin_transform);
+ const SE3 jointPlacement = convertFromUrdf(joint->parent_to_joint_origin_transform);
const boost::shared_ptr< ::urdf::Inertial> Y = link->inertial;
@@ -126,7 +163,7 @@ namespace se3
{
joint_info << "joint FreeFlyer";
addJointAndBody(model,JointModelFreeFlyer(),
- parent_joint_id,jointPlacement,joint->name,
+ parentFrameId,jointPlacement,joint->name,
Y,link->name);
break;
@@ -161,7 +198,7 @@ namespace se3
{
joint_info << " along X";
addJointAndBody(model,JointModelRX(),
- parent_joint_id,jointPlacement,joint->name,
+ parentFrameId,jointPlacement,joint->name,
Y,link->name,
max_effort,max_velocity,
lower_position, upper_position);
@@ -171,7 +208,7 @@ namespace se3
{
joint_info << " along Y";
addJointAndBody(model,JointModelRY(),
- parent_joint_id,jointPlacement,joint->name,
+ parentFrameId,jointPlacement,joint->name,
Y,link->name,
max_effort,max_velocity,
lower_position, upper_position);
@@ -181,7 +218,7 @@ namespace se3
{
joint_info << " along Z";
addJointAndBody(model,JointModelRZ(),
- parent_joint_id,jointPlacement,joint->name,
+ parentFrameId,jointPlacement,joint->name,
Y,link->name,
max_effort,max_velocity,
lower_position, upper_position);
@@ -194,7 +231,7 @@ namespace se3
joint_info << " unaligned along (" << joint_axis.transpose() << ")";
addJointAndBody(model,JointModelRevoluteUnaligned(joint_axis.normalized()),
- parent_joint_id,jointPlacement,joint->name,
+ parentFrameId,jointPlacement,joint->name,
Y,link->name,
max_effort,max_velocity,
lower_position, upper_position);
@@ -236,7 +273,7 @@ namespace se3
{
joint_info << " along X";
addJointAndBody(model,JointModelRX(),
- parent_joint_id,jointPlacement,joint->name,
+ parentFrameId,jointPlacement,joint->name,
Y,link->name,
max_effort,max_velocity,
lower_position, upper_position);
@@ -246,7 +283,7 @@ namespace se3
{
joint_info << " along Y";
addJointAndBody(model,JointModelRY(),
- parent_joint_id,jointPlacement,joint->name,
+ parentFrameId,jointPlacement,joint->name,
Y,link->name,
max_effort,max_velocity,
lower_position, upper_position);
@@ -256,7 +293,7 @@ namespace se3
{
joint_info << " along Z";
addJointAndBody(model,JointModelRZ(),
- parent_joint_id,jointPlacement,joint->name,
+ parentFrameId,jointPlacement,joint->name,
Y,link->name,
max_effort,max_velocity,
lower_position, upper_position);
@@ -269,7 +306,7 @@ namespace se3
joint_info << " unaligned along (" << joint_axis.transpose() << ")";
addJointAndBody(model,JointModelRevoluteUnaligned(joint_axis.normalized()),
- parent_joint_id,jointPlacement,joint->name,
+ parentFrameId,jointPlacement,joint->name,
Y,link->name,
max_effort,max_velocity,
lower_position, upper_position);
@@ -310,7 +347,7 @@ namespace se3
{
joint_info << " along X";
addJointAndBody(model,JointModelPX(),
- parent_joint_id,jointPlacement,joint->name,
+ parentFrameId,jointPlacement,joint->name,
Y,link->name,
max_effort,max_velocity,
lower_position, upper_position);
@@ -320,7 +357,7 @@ namespace se3
{
joint_info << " along Y";
addJointAndBody(model,JointModelPY(),
- parent_joint_id,jointPlacement,joint->name,
+ parentFrameId,jointPlacement,joint->name,
Y,link->name,
max_effort,max_velocity,
lower_position, upper_position);
@@ -330,7 +367,7 @@ namespace se3
{
joint_info << " along Z";
addJointAndBody(model,JointModelPZ(),
- parent_joint_id,jointPlacement,joint->name,
+ parentFrameId,jointPlacement,joint->name,
Y,link->name,
max_effort,max_velocity,
lower_position, upper_position);
@@ -343,7 +380,7 @@ namespace se3
joint_info << " unaligned along (" << joint_axis.transpose() << ")";
addJointAndBody(model,JointModelPrismaticUnaligned(joint_axis.normalized()),
- parent_joint_id,jointPlacement,joint->name,
+ parentFrameId,jointPlacement,joint->name,
Y,link->name,
max_effort,max_velocity,
lower_position, upper_position);
@@ -378,7 +415,7 @@ namespace se3
}
addJointAndBody(model,JointModelPlanar(),
- parent_joint_id,jointPlacement,joint->name,
+ parentFrameId,jointPlacement,joint->name,
Y,link->name,
max_effort,max_velocity,
lower_position, upper_position);
@@ -396,20 +433,9 @@ namespace se3
// -add fixed body in model to display it in gepetto-viewer
joint_info << "fixed joint";
- appendBodyToJoint(model, parent_joint_id, Y, jointPlacement, link_name);
-
- //transformation of the current placement offset
- nextPlacementOffset = jointPlacement;
-
- // Add a frame in the model to keep trace of this fixed joint
- model.addFrame(Frame(joint->name, parent_joint_id, nextPlacementOffset, FIXED_JOINT));
-
- //for the children of the current link, set their parent to be
- //the the parent of the current link.
- BOOST_FOREACH(::urdf::LinkPtr child_link, link->child_links)
- {
- child_link->setParent(link->getParent() );
- }
+ addFixedJointAndBody(model, parentFrameId, jointPlacement,
+ joint_name, Y, link_name);
+
break;
}
default:
@@ -442,7 +468,7 @@ namespace se3
BOOST_FOREACH(::urdf::LinkConstPtr child,link->child_links)
{
- parseTree(child, model, nextPlacementOffset, verbose);
+ parseTree(child, model, verbose);
}
}
@@ -460,7 +486,7 @@ namespace se3
BOOST_FOREACH(::urdf::LinkPtr child, root_link->child_links)
{
- parseTree(child, model, SE3::Identity(), verbose);
+ parseTree(child, model, verbose);
}
// FIXME: check the inertias
@@ -487,7 +513,7 @@ namespace se3
BOOST_FOREACH(::urdf::LinkPtr child, root_link->child_links)
{
- parseTree(child, model, SE3::Identity(), verbose);
+ parseTree(child, model, verbose);
}
// FIXME: See fixme in previous parseRootTree definition