From ff1046028268cba5355a6c75107d30dc27315e84 Mon Sep 17 00:00:00 2001
From: Valenza Florian <fvalenza@laas.fr>
Date: Tue, 21 Jun 2016 13:09:05 +0200
Subject: [PATCH] [C++] bodies are now a subpart of the Frames : some Frames
are of type BODY and when referring to getBodyId/Name in Model it will look
in its frames
---
src/multibody/model.hpp | 30 +++---
src/multibody/model.hxx | 30 ++++--
src/multibody/parser/sample-models.cpp | 90 ++++++++--------
src/multibody/parser/urdf-with-geometry.hxx | 4 +-
src/multibody/parser/urdf.hxx | 2 +-
src/python/model.hpp | 12 ---
unittest/dynamics.cpp | 24 ++---
unittest/geom.cpp | 4 +-
unittest/jacobian.cpp | 8 +-
unittest/joint-configurations.cpp | 108 ++++++++++----------
unittest/joints.cpp | 8 +-
11 files changed, 162 insertions(+), 158 deletions(-)
diff --git a/src/multibody/model.hpp b/src/multibody/model.hpp
index 988542aef..7fedd7e27 100644
--- a/src/multibody/model.hpp
+++ b/src/multibody/model.hpp
@@ -75,24 +75,15 @@ namespace se3
/// \brief Placement (SE3) of the input of joint <i> regarding to the parent joint output <li>.
std::vector<SE3> jointPlacements;
- /// \brief Placement (SE3) of the body <i> regarding to the parent joint output <li>.
- std::vector<SE3> bodyPlacements;
-
/// \brief Model of joint <i>.
JointModelVector joints;
/// \brief Joint parent of joint <i>, denoted <li> (li==parents[i]).
std::vector<JointIndex> parents;
- /// \brief Joint parent of body <i>
- std::vector<JointIndex> bodyParents;
-
/// \brief Name of joint <i>
std::vector<std::string> names;
- /// \brief Name of the body attached to the output of joint <i>.
- std::vector<std::string> bodyNames;
-
/// \brief Vector of maximal joint torques
Eigen::VectorXd effortLimit;
/// \brief Vector of maximal joint velocities
@@ -122,13 +113,10 @@ namespace se3
, jointPlacements(1)
, joints(1)
, parents(1)
- , bodyParents(1)
, names(1)
- , bodyNames(1)
, gravity( gravity981,Eigen::Vector3d::Zero() )
{
names[0] = "universe"; // Should be "universe joint (trivial)"
- bodyNames[0] = "universe";
}
~Model() {} // std::cout << "Destroy model" << std::endl; }
@@ -319,6 +307,24 @@ namespace se3
/// \return
///
JointIndex getFrameParent(const FrameIndex index) const;
+
+ ///
+ /// \brief Get the type of the frame given by its index.
+ ///
+ /// \param[in] name Name of the frame.
+ ///
+ /// \return
+ ///
+ FrameType getFrameType(const std::string & name) const;
+
+ ///
+ /// \brief Get the type of the frame given by its index.
+ ///
+ /// \param[in] index Index of the frame.
+ ///
+ /// \return
+ ///
+ FrameType getFrameType(const FrameIndex index) const;
///
/// \brief Return the relative placement between a frame and its supporting joint.
diff --git a/src/multibody/model.hxx b/src/multibody/model.hxx
index afb44446f..4126cadcc 100644
--- a/src/multibody/model.hxx
+++ b/src/multibody/model.hxx
@@ -140,9 +140,6 @@ namespace se3
inertias[parent] += iYf;
addFrame((bodyName!="")?bodyName:random(8), parent, bodyPlacement, BODY);
- bodyParents.push_back(parent);
- bodyPlacements.push_back(bodyPlacement);
- bodyNames.push_back( (bodyName!="")?bodyName:random(8));
nbody ++;
}
@@ -153,22 +150,18 @@ namespace se3
inline Model::JointIndex Model::getBodyId (const std::string & name) const
{
- std::vector<std::string>::iterator::difference_type
- res = std::find(bodyNames.begin(),bodyNames.end(),name) - bodyNames.begin();
- assert( (res<INT_MAX) && "Id superior to int range. Should never happen.");
- assert( (res>=0)&&(res<nbody) && "The body name you asked does not exist" );
- return Model::JointIndex(res);
+ return getFrameId(name);
}
inline bool Model::existBodyName (const std::string & name) const
{
- return (bodyNames.end() != std::find(bodyNames.begin(),bodyNames.end(),name));
+ return existFrame(name);
}
inline const std::string& Model::getBodyName (const Model::JointIndex index) const
{
assert( index < (Model::Index)nbody );
- return bodyNames[index];
+ return getFrameName(index);
}
inline Model::JointIndex Model::getJointId (const std::string & name) const
@@ -227,6 +220,23 @@ namespace se3
return operational_frames[index].parent;
}
+ inline FrameType Model::getFrameType( const std::string & name ) const
+ {
+ assert(existFrame(name) && "The Frame you requested does not exist");
+ std::vector<Frame>::const_iterator it = std::find_if( operational_frames.begin()
+ , operational_frames.end()
+ , boost::bind(&Frame::name, _1) == name
+ );
+
+ std::vector<Frame>::iterator::difference_type it_diff = it - operational_frames.begin();
+ return getFrameType(Model::JointIndex(it_diff));
+ }
+
+ inline FrameType Model::getFrameType( const FrameIndex index ) const
+ {
+ return operational_frames[index].type;
+ }
+
inline const SE3 & Model::getFramePlacement( const std::string & name) const
{
assert(existFrame(name) && "The Frame you requested does not exist");
diff --git a/src/multibody/parser/sample-models.cpp b/src/multibody/parser/sample-models.cpp
index 8c409456e..d9af943db 100644
--- a/src/multibody/parser/sample-models.cpp
+++ b/src/multibody/parser/sample-models.cpp
@@ -29,34 +29,34 @@ namespace se3
void humanoid2d(Model& model)
{
- model.addJointAndBody(model.getBodyId("universe"),JointModelRX(),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("universe"),JointModelRX(),SE3::Identity(),Inertia::Random(),
"ff1_joint", "ff1_body");
- model.addJointAndBody(model.getBodyId("ff1_body"),JointModelRY(),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("ff1_joint"),JointModelRY(),SE3::Identity(),Inertia::Random(),
"root_joint", "root_body");
- model.addJointAndBody(model.getBodyId("root_body"),JointModelRZ(),SE3::Random(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("root_joint"),JointModelRZ(),SE3::Random(),Inertia::Random(),
"lleg1_joint", "lleg1_body");
- model.addJointAndBody(model.getBodyId("lleg1_body"),JointModelRY(),SE3::Random(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("lleg1_joint"),JointModelRY(),SE3::Random(),Inertia::Random(),
"lleg2_joint", "lleg2_body");
- model.addJointAndBody(model.getBodyId("root_body"),JointModelRZ(),SE3::Random(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("root_joint"),JointModelRZ(),SE3::Random(),Inertia::Random(),
"rleg1_joint", "rleg1_body");
- model.addJointAndBody(model.getBodyId("rleg1_body"),JointModelRY(),SE3::Random(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("rleg1_joint"),JointModelRY(),SE3::Random(),Inertia::Random(),
"rleg2_joint", "rleg2_body");
- model.addJointAndBody(model.getBodyId("root_body"),JointModelRY(),SE3::Random(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("root_joint"),JointModelRY(),SE3::Random(),Inertia::Random(),
"torso1_joint", "torso1_body");
- model.addJointAndBody(model.getBodyId("torso1_body"),JointModelRZ(),SE3::Random(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("torso1_joint"),JointModelRZ(),SE3::Random(),Inertia::Random(),
"chest_joint", "chest_body");
- model.addJointAndBody(model.getBodyId("chest_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("chest_joint"),JointModelRX(),SE3::Random(),Inertia::Random(),
"rarm1_joint", "rarm1_body");
- model.addJointAndBody(model.getBodyId("rarm1_body"),JointModelRZ(),SE3::Random(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("rarm1_joint"),JointModelRZ(),SE3::Random(),Inertia::Random(),
"rarm2_joint", "rarm2_body");
- model.addJointAndBody(model.getBodyId("chest_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("chest_joint"),JointModelRX(),SE3::Random(),Inertia::Random(),
"larm1_joint", "larm1_body");
- model.addJointAndBody(model.getBodyId("larm1_body"),JointModelRZ(),SE3::Random(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("larm1_joint"),JointModelRZ(),SE3::Random(),Inertia::Random(),
"larm2_joint", "larm2_body");
}
@@ -64,22 +64,22 @@ namespace se3
{
if(! usingFF )
{
- model.addJointAndBody(model.getBodyId("universe"),JointModelRX(),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("universe"),JointModelRX(),SE3::Identity(),Inertia::Random(),
"ff1_joint", "ff1_body");
- model.addJointAndBody(model.getBodyId("ff1_body"),JointModelRY(),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("ff1_joint"),JointModelRY(),SE3::Identity(),Inertia::Random(),
"ff2_joint", "ff2_body");
- model.addJointAndBody(model.getBodyId("ff2_body"),JointModelRZ(),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("ff2_joint"),JointModelRZ(),SE3::Identity(),Inertia::Random(),
"ff3_joint", "ff3_body");
- model.addJointAndBody(model.getBodyId("ff3_body"),JointModelRZ(),SE3::Random(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("ff3_joint"),JointModelRZ(),SE3::Random(),Inertia::Random(),
"ff4_joint", "ff4_body");
- model.addJointAndBody(model.getBodyId("ff4_body"),JointModelRY(),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("ff4_joint"),JointModelRY(),SE3::Identity(),Inertia::Random(),
"ff5_joint", "ff5_body");
- model.addJointAndBody(model.getBodyId("ff5_body"),JointModelRX(),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("ff5_joint"),JointModelRX(),SE3::Identity(),Inertia::Random(),
"root_joint", "root_body");
}
else
{
- model.addJointAndBody(model.getBodyId("universe"),JointModelFreeFlyer(),
+ model.addJointAndBody(model.getJointId("universe"),JointModelFreeFlyer(),
SE3::Identity(),Inertia::Random(),
Eigen::VectorXd::Zero(6), 1e3 * (Eigen::VectorXd::Random(6).array() + 1),
1e3 * (Eigen::VectorXd::Random(7).array() - 1),
@@ -87,111 +87,111 @@ namespace se3
"root_joint", "root_body");
}
- model.addJointAndBody(model.getBodyId("root_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("root_joint"),JointModelRX(),SE3::Random(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"lleg1_joint", "lleg1_body");
- model.addJointAndBody(model.getBodyId("lleg1_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("lleg1_joint"),JointModelRX(),SE3::Random(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"lleg2_joint", "lleg2_body");
- model.addJointAndBody(model.getBodyId("lleg2_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("lleg2_joint"),JointModelRX(),SE3::Random(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"lleg3_joint", "lleg3_body");
- model.addJointAndBody(model.getBodyId("lleg3_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("lleg3_joint"),JointModelRX(),SE3::Random(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"lleg4_joint", "lleg4_body");
- model.addJointAndBody(model.getBodyId("lleg4_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("lleg4_joint"),JointModelRX(),SE3::Random(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"lleg5_joint", "lleg5_body");
- model.addJointAndBody(model.getBodyId("lleg5_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("lleg5_joint"),JointModelRX(),SE3::Random(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"lleg6_joint", "lleg6_body");
- model.addJointAndBody(model.getBodyId("root_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("root_joint"),JointModelRX(),SE3::Random(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"rleg1_joint", "rleg1_body");
- model.addJointAndBody(model.getBodyId("rleg1_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("rleg1_joint"),JointModelRX(),SE3::Random(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"rleg2_joint", "rleg2_body");
- model.addJointAndBody(model.getBodyId("rleg2_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("rleg2_joint"),JointModelRX(),SE3::Random(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"rleg3_joint", "rleg3_body");
- model.addJointAndBody(model.getBodyId("rleg3_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("rleg3_joint"),JointModelRX(),SE3::Random(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"rleg4_joint", "rleg4_body");
- model.addJointAndBody(model.getBodyId("rleg4_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("rleg4_joint"),JointModelRX(),SE3::Random(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"rleg5_joint", "rleg5_body");
- model.addJointAndBody(model.getBodyId("rleg5_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("rleg5_joint"),JointModelRX(),SE3::Random(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"rleg6_joint", "rleg6_body");
- model.addJointAndBody(model.getBodyId("root_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("root_joint"),JointModelRX(),SE3::Random(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"torso1_joint", "torso1_body");
- model.addJointAndBody(model.getBodyId("torso1_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("torso1_joint"),JointModelRX(),SE3::Random(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"chest_joint", "chest_body");
- model.addJointAndBody(model.getBodyId("chest_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("chest_joint"),JointModelRX(),SE3::Random(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"rarm1_joint", "rarm1_body");
- model.addJointAndBody(model.getBodyId("rarm1_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("rarm1_joint"),JointModelRX(),SE3::Random(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"rarm2_joint", "rarm2_body");
- model.addJointAndBody(model.getBodyId("rarm2_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("rarm2_joint"),JointModelRX(),SE3::Random(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"rarm3_joint", "rarm3_body");
- model.addJointAndBody(model.getBodyId("rarm3_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("rarm3_joint"),JointModelRX(),SE3::Random(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"rarm4_joint", "rarm4_body");
- model.addJointAndBody(model.getBodyId("rarm4_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("rarm4_joint"),JointModelRX(),SE3::Random(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"rarm5_joint", "rarm5_body");
- model.addJointAndBody(model.getBodyId("rarm5_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("rarm5_joint"),JointModelRX(),SE3::Random(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"rarm6_joint", "rarm6_body");
- model.addJointAndBody(model.getBodyId("chest_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("chest_joint"),JointModelRX(),SE3::Random(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"larm1_joint", "larm1_body");
- model.addJointAndBody(model.getBodyId("larm1_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("larm1_joint"),JointModelRX(),SE3::Random(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"larm2_joint", "larm2_body");
- model.addJointAndBody(model.getBodyId("larm2_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("larm2_joint"),JointModelRX(),SE3::Random(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"larm3_joint", "larm3_body");
- model.addJointAndBody(model.getBodyId("larm3_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("larm3_joint"),JointModelRX(),SE3::Random(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"larm4_joint", "larm4_body");
- model.addJointAndBody(model.getBodyId("larm4_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("larm4_joint"),JointModelRX(),SE3::Random(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"larm5_joint", "larm5_body");
- model.addJointAndBody(model.getBodyId("larm5_body"),JointModelRX(),SE3::Random(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("larm5_joint"),JointModelRX(),SE3::Random(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"larm6_joint", "larm6_body");
diff --git a/src/multibody/parser/urdf-with-geometry.hxx b/src/multibody/parser/urdf-with-geometry.hxx
index edd08a87b..9cd4e7d3c 100644
--- a/src/multibody/parser/urdf-with-geometry.hxx
+++ b/src/multibody/parser/urdf-with-geometry.hxx
@@ -125,7 +125,7 @@ namespace se3
fcl::CollisionObject collision_object = retrieveCollisionGeometry((*i)->geometry, package_dirs, mesh_path);
SE3 geomPlacement = convertFromUrdf((*i)->origin);
std::string collision_object_name = link_name;
- geom_model.addCollisionObject(model.bodyParents[model.getBodyId(link_name)], collision_object, geomPlacement, collision_object_name, mesh_path);
+ geom_model.addCollisionObject(model.getFrameParent(link_name), collision_object, geomPlacement, collision_object_name, mesh_path);
}
} // if(link->collision)
@@ -144,7 +144,7 @@ namespace se3
fcl::CollisionObject visual_object = retrieveCollisionGeometry((*i)->geometry, package_dirs, mesh_path);
SE3 geomPlacement = convertFromUrdf((*i)->origin);
std::string visual_object_name = link_name;
- geom_model.addVisualObject(model.bodyParents[model.getBodyId(link_name)], visual_object, geomPlacement, visual_object_name, mesh_path);
+ geom_model.addVisualObject(model.getFrameParent(link_name), visual_object, geomPlacement, visual_object_name, mesh_path);
}
} // if(link->visual)
diff --git a/src/multibody/parser/urdf.hxx b/src/multibody/parser/urdf.hxx
index 54ff468f6..afe200f14 100644
--- a/src/multibody/parser/urdf.hxx
+++ b/src/multibody/parser/urdf.hxx
@@ -371,7 +371,7 @@ namespace se3
// Add a frame in the model to keep trace of this fixed joint
- model.addFrame(joint->name, parent_joint_id, nextPlacementOffset);
+ model.addFrame(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.
diff --git a/src/python/model.hpp b/src/python/model.hpp
index 92dc9a685..9857cdb26 100644
--- a/src/python/model.hpp
+++ b/src/python/model.hpp
@@ -110,24 +110,15 @@ namespace se3
.add_property("jointPlacements",
bp::make_function(&ModelPythonVisitor::jointPlacements,
bp::return_internal_reference<>()) )
- .add_property("bodyPlacements",
- bp::make_function(&ModelPythonVisitor::bodyPlacements,
- bp::return_internal_reference<>()) )
.add_property("joints",
bp::make_function(&ModelPythonVisitor::joints,
bp::return_internal_reference<>()) )
.add_property("parents",
bp::make_function(&ModelPythonVisitor::parents,
bp::return_internal_reference<>()) )
- .add_property("bodyParents",
- bp::make_function(&ModelPythonVisitor::bodyParents,
- bp::return_internal_reference<>()) )
.add_property("names",
bp::make_function(&ModelPythonVisitor::names,
bp::return_internal_reference<>()) )
- .add_property("bodyNames",
- bp::make_function(&ModelPythonVisitor::bodyNames,
- bp::return_internal_reference<>()) )
.def("addJointAndBody",&ModelPythonVisitor::addJointAndBodyToModel)
@@ -162,12 +153,9 @@ namespace se3
static int nbody( ModelHandler & m ) { return m->nbody; }
static std::vector<Inertia> & inertias( ModelHandler & m ) { return m->inertias; }
static std::vector<SE3> & jointPlacements( ModelHandler & m ) { return m->jointPlacements; }
- static std::vector<SE3> & bodyPlacements( ModelHandler & m ) { return m->bodyPlacements; }
static JointModelVector & joints( ModelHandler & m ) { return m->joints; }
static std::vector<Model::JointIndex> & parents( ModelHandler & m ) { return m->parents; }
- static std::vector<Model::JointIndex> & bodyParents( ModelHandler & m ) { return m->bodyParents; }
static std::vector<std::string> & names ( ModelHandler & m ) { return m->names; }
- static std::vector<std::string> & bodyNames ( ModelHandler & m ) { return m->bodyNames; }
static Model::Index addJointAndBodyToModel(ModelHandler & modelPtr,
Model::JointIndex idx, bp::object joint,
diff --git a/unittest/dynamics.cpp b/unittest/dynamics.cpp
index 9df57886d..56534de3f 100644
--- a/unittest/dynamics.cpp
+++ b/unittest/dynamics.cpp
@@ -49,15 +49,15 @@ BOOST_AUTO_TEST_CASE ( test_FD )
VectorXd v = VectorXd::Ones(model.nv);
VectorXd tau = VectorXd::Zero(model.nv);
- const std::string RF = "rleg6_body";
- const std::string LF = "lleg6_body";
+ const std::string RF = "rleg6_joint";
+ const std::string LF = "lleg6_joint";
Data::Matrix6x J_RF (6, model.nv);
J_RF.setZero();
- getJacobian <true> (model, data, model.getBodyId(RF), J_RF);
+ getJacobian <true> (model, data, model.getJointId(RF), J_RF);
Data::Matrix6x J_LF (6, model.nv);
J_LF.setZero();
- getJacobian <true> (model, data, model.getBodyId(LF), J_LF);
+ getJacobian <true> (model, data, model.getJointId(LF), J_LF);
Eigen::MatrixXd J (12, model.nv);
J.setZero();
@@ -111,15 +111,15 @@ BOOST_AUTO_TEST_CASE ( test_ID )
VectorXd v_before = VectorXd::Ones(model.nv);
- const std::string RF = "rleg6_body";
- const std::string LF = "lleg6_body";
+ const std::string RF = "rleg6_joint";
+ const std::string LF = "lleg6_joint";
Data::Matrix6x J_RF (6, model.nv);
J_RF.setZero();
- getJacobian <true> (model, data, model.getBodyId(RF), J_RF);
+ getJacobian <true> (model, data, model.getJointId(RF), J_RF);
Data::Matrix6x J_LF (6, model.nv);
J_LF.setZero();
- getJacobian <true> (model, data, model.getBodyId(LF), J_LF);
+ getJacobian <true> (model, data, model.getJointId(LF), J_LF);
Eigen::MatrixXd J (12, model.nv);
J.setZero();
@@ -183,13 +183,13 @@ BOOST_AUTO_TEST_CASE (timings_fd_llt)
VectorXd v = VectorXd::Ones(model.nv);
VectorXd tau = VectorXd::Zero(model.nv);
- const std::string RF = "rleg6_body";
- const std::string LF = "lleg6_body";
+ const std::string RF = "rleg6_joint";
+ const std::string LF = "lleg6_joint";
Data::Matrix6x J_RF (6, model.nv);
- getJacobian <true> (model, data, model.getBodyId(RF), J_RF);
+ getJacobian <true> (model, data, model.getJointId(RF), J_RF);
Data::Matrix6x J_LF (6, model.nv);
- getJacobian <true> (model, data, model.getBodyId(LF), J_LF);
+ getJacobian <true> (model, data, model.getJointId(LF), J_LF);
Eigen::MatrixXd J (12, model.nv);
J.topRows<6> () = J_RF;
diff --git a/unittest/geom.cpp b/unittest/geom.cpp
index 0fbbebd6e..82ac54f9c 100644
--- a/unittest/geom.cpp
+++ b/unittest/geom.cpp
@@ -149,9 +149,9 @@ BOOST_AUTO_TEST_CASE ( simple_boxes )
using namespace se3;
- model.addJointAndBody(model.getBodyId("universe"),JointModelPlanar(),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("universe"),JointModelPlanar(),SE3::Identity(),Inertia::Random(),
"planar1_joint", "planar1_body");
- model.addJointAndBody(model.getBodyId("universe"),JointModelPlanar(),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("universe"),JointModelPlanar(),SE3::Identity(),Inertia::Random(),
"planar2_joint", "planar2_body");
boost::shared_ptr<fcl::Box> Sample(new fcl::Box(1));
diff --git a/unittest/jacobian.cpp b/unittest/jacobian.cpp
index 995d323be..1197d4691 100644
--- a/unittest/jacobian.cpp
+++ b/unittest/jacobian.cpp
@@ -43,7 +43,7 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
VectorXd q = VectorXd::Zero(model.nq);
computeJacobians(model,data,q);
- Model::Index idx = model.existBodyName("rarm2")?model.getBodyId("rarm2"):(Model::Index)(model.nbody-1);
+ Model::Index idx = model.existJointName("rarm2")?model.getJointId("rarm2"):(Model::Index)(model.nbody-1);
Data::Matrix6x Jrh(6,model.nv); Jrh.fill(0);
getJacobian<false>(model,data,idx,Jrh);
@@ -110,7 +110,7 @@ BOOST_AUTO_TEST_CASE ( test_timings )
if( flag >> 1 & 1 )
{
computeJacobians(model,data,q);
- Model::Index idx = model.existBodyName("rarm6")?model.getBodyId("rarm6"):(Model::Index)(model.nbody-1);
+ Model::Index idx = model.existJointName("rarm6")?model.getJointId("rarm6"):(Model::Index)(model.nbody-1);
Data::Matrix6x Jrh(6,model.nv); Jrh.fill(0);
timer.tic();
@@ -125,7 +125,7 @@ BOOST_AUTO_TEST_CASE ( test_timings )
if( flag >> 2 & 1 )
{
computeJacobians(model,data,q);
- Model::Index idx = model.existBodyName("rarm6")?model.getBodyId("rarm6"):(Model::Index)(model.nbody-1);
+ Model::Index idx = model.existJointName("rarm6")?model.getJointId("rarm6"):(Model::Index)(model.nbody-1);
Data::Matrix6x Jrh(6,model.nv); Jrh.fill(0);
timer.tic();
@@ -140,7 +140,7 @@ BOOST_AUTO_TEST_CASE ( test_timings )
if( flag >> 3 & 1 )
{
computeJacobians(model,data,q);
- Model::Index idx = model.existBodyName("rarm6")?model.getBodyId("rarm6"):(Model::Index)(model.nbody-1);
+ Model::Index idx = model.existJointName("rarm6")?model.getJointId("rarm6"):(Model::Index)(model.nbody-1);
Data::Matrix6x Jrh(6,model.nv); Jrh.fill(0);
timer.tic();
diff --git a/unittest/joint-configurations.cpp b/unittest/joint-configurations.cpp
index ddc6b6314..eda2868dd 100644
--- a/unittest/joint-configurations.cpp
+++ b/unittest/joint-configurations.cpp
@@ -70,23 +70,23 @@ BOOST_AUTO_TEST_CASE ( integration_test )
using namespace se3;
- model.addJointAndBody(model.getBodyId("universe"),JointModelFreeFlyer(),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("universe"),JointModelFreeFlyer(),SE3::Identity(),Inertia::Random(),
"freeflyer_joint", "freeflyer_body");
- model.addJointAndBody(model.getBodyId("freeflyer_body"),JointModelSpherical(),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("freeflyer_joint"),JointModelSpherical(),SE3::Identity(),Inertia::Random(),
"spherical_joint", "spherical_body");
- model.addJointAndBody(model.getBodyId("spherical_body"),JointModelRX(),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("spherical_joint"),JointModelRX(),SE3::Identity(),Inertia::Random(),
"revolute_joint", "revolute_body");
- model.addJointAndBody(model.getBodyId("revolute_body"),JointModelPX(),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("revolute_joint"),JointModelPX(),SE3::Identity(),Inertia::Random(),
"px_joint", "px_body");
- model.addJointAndBody(model.getBodyId("px_body"),JointModelPrismaticUnaligned(Eigen::Vector3d(1,0,0)),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("px_joint"),JointModelPrismaticUnaligned(Eigen::Vector3d(1,0,0)),SE3::Identity(),Inertia::Random(),
"pu_joint", "pu_body");
- model.addJointAndBody(model.getBodyId("pu_body"),JointModelRevoluteUnaligned(Eigen::Vector3d(0,0,1)),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("pu_joint"),JointModelRevoluteUnaligned(Eigen::Vector3d(0,0,1)),SE3::Identity(),Inertia::Random(),
"ru_joint", "ru_body");
- model.addJointAndBody(model.getBodyId("ru_body"),JointModelSphericalZYX(),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("ru_joint"),JointModelSphericalZYX(),SE3::Identity(),Inertia::Random(),
"sphericalZYX_joint", "sphericalZYX_body");
- model.addJointAndBody(model.getBodyId("sphericalZYX_body"),JointModelTranslation(),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("sphericalZYX_joint"),JointModelTranslation(),SE3::Identity(),Inertia::Random(),
"translation_joint", "translation_body");
- model.addJointAndBody(model.getBodyId("translation_body"),JointModelPlanar(),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("translation_joint"),JointModelPlanar(),SE3::Identity(),Inertia::Random(),
"planar_joint", "planar_body");
se3::Data data(model);
@@ -122,23 +122,23 @@ BOOST_AUTO_TEST_CASE ( interpolation_test )
using namespace se3;
- model.addJointAndBody(model.getBodyId("universe"),JointModelFreeFlyer(),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("universe"),JointModelFreeFlyer(),SE3::Identity(),Inertia::Random(),
"freeflyer_joint", "freeflyer_body");
- model.addJointAndBody(model.getBodyId("freeflyer_body"),JointModelSpherical(),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("freeflyer_joint"),JointModelSpherical(),SE3::Identity(),Inertia::Random(),
"spherical_joint", "spherical_body");
- model.addJointAndBody(model.getBodyId("spherical_body"),JointModelRX(),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("spherical_joint"),JointModelRX(),SE3::Identity(),Inertia::Random(),
"revolute_joint", "revolute_body");
- model.addJointAndBody(model.getBodyId("revolute_body"),JointModelPX(),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("revolute_joint"),JointModelPX(),SE3::Identity(),Inertia::Random(),
"px_joint", "px_body");
- model.addJointAndBody(model.getBodyId("px_body"),JointModelPrismaticUnaligned(Eigen::Vector3d(1,0,0)),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("px_joint"),JointModelPrismaticUnaligned(Eigen::Vector3d(1,0,0)),SE3::Identity(),Inertia::Random(),
"pu_joint", "pu_body");
- model.addJointAndBody(model.getBodyId("pu_body"),JointModelRevoluteUnaligned(Eigen::Vector3d(0,0,1)),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("pu_joint"),JointModelRevoluteUnaligned(Eigen::Vector3d(0,0,1)),SE3::Identity(),Inertia::Random(),
"ru_joint", "ru_body");
- model.addJointAndBody(model.getBodyId("ru_body"),JointModelSphericalZYX(),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("ru_joint"),JointModelSphericalZYX(),SE3::Identity(),Inertia::Random(),
"sphericalZYX_joint", "sphericalZYX_body");
- model.addJointAndBody(model.getBodyId("sphericalZYX_body"),JointModelTranslation(),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("sphericalZYX_joint"),JointModelTranslation(),SE3::Identity(),Inertia::Random(),
"translation_joint", "translation_body");
- model.addJointAndBody(model.getBodyId("translation_body"),JointModelPlanar(),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("translation_joint"),JointModelPlanar(),SE3::Identity(),Inertia::Random(),
"planar_joint", "planar_body");
se3::Data data(model);
@@ -233,23 +233,23 @@ BOOST_AUTO_TEST_CASE ( differentiation_test )
using namespace se3;
- model.addJointAndBody(model.getBodyId("universe"),JointModelFreeFlyer(),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("universe"),JointModelFreeFlyer(),SE3::Identity(),Inertia::Random(),
"freeflyer_joint", "freeflyer_body");
- model.addJointAndBody(model.getBodyId("freeflyer_body"),JointModelSpherical(),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("freeflyer_joint"),JointModelSpherical(),SE3::Identity(),Inertia::Random(),
"spherical_joint", "spherical_body");
- model.addJointAndBody(model.getBodyId("spherical_body"),JointModelRX(),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("spherical_joint"),JointModelRX(),SE3::Identity(),Inertia::Random(),
"revolute_joint", "revolute_body");
- model.addJointAndBody(model.getBodyId("revolute_body"),JointModelPX(),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("revolute_joint"),JointModelPX(),SE3::Identity(),Inertia::Random(),
"px_joint", "px_body");
- model.addJointAndBody(model.getBodyId("px_body"),JointModelPrismaticUnaligned(Eigen::Vector3d(1,0,0)),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("px_joint"),JointModelPrismaticUnaligned(Eigen::Vector3d(1,0,0)),SE3::Identity(),Inertia::Random(),
"pu_joint", "pu_body");
- model.addJointAndBody(model.getBodyId("pu_body"),JointModelRevoluteUnaligned(Eigen::Vector3d(0,0,1)),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("pu_joint"),JointModelRevoluteUnaligned(Eigen::Vector3d(0,0,1)),SE3::Identity(),Inertia::Random(),
"ru_joint", "ru_body");
- model.addJointAndBody(model.getBodyId("ru_body"),JointModelSphericalZYX(),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("ru_joint"),JointModelSphericalZYX(),SE3::Identity(),Inertia::Random(),
"sphericalZYX_joint", "sphericalZYX_body");
- model.addJointAndBody(model.getBodyId("sphericalZYX_body"),JointModelTranslation(),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("sphericalZYX_joint"),JointModelTranslation(),SE3::Identity(),Inertia::Random(),
"translation_joint", "translation_body");
- model.addJointAndBody(model.getBodyId("translation_body"),JointModelPlanar(),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("translation_joint"),JointModelPlanar(),SE3::Identity(),Inertia::Random(),
"planar_joint", "planar_body");
se3::Data data(model);
@@ -310,23 +310,23 @@ BOOST_AUTO_TEST_CASE ( distance_computation_test )
using namespace se3;
- model.addJointAndBody(model.getBodyId("universe"),JointModelFreeFlyer(),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("universe"),JointModelFreeFlyer(),SE3::Identity(),Inertia::Random(),
"freeflyer_joint", "freeflyer_body");
- model.addJointAndBody(model.getBodyId("freeflyer_body"),JointModelSpherical(),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("freeflyer_joint"),JointModelSpherical(),SE3::Identity(),Inertia::Random(),
"spherical_joint", "spherical_body");
- model.addJointAndBody(model.getBodyId("spherical_body"),JointModelRX(),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("spherical_joint"),JointModelRX(),SE3::Identity(),Inertia::Random(),
"revolute_joint", "revolute_body");
- model.addJointAndBody(model.getBodyId("revolute_body"),JointModelPX(),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("revolute_joint"),JointModelPX(),SE3::Identity(),Inertia::Random(),
"px_joint", "px_body");
- model.addJointAndBody(model.getBodyId("px_body"),JointModelPrismaticUnaligned(Eigen::Vector3d(1,0,0)),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("px_joint"),JointModelPrismaticUnaligned(Eigen::Vector3d(1,0,0)),SE3::Identity(),Inertia::Random(),
"pu_joint", "pu_body");
- model.addJointAndBody(model.getBodyId("pu_body"),JointModelRevoluteUnaligned(Eigen::Vector3d(0,0,1)),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("pu_joint"),JointModelRevoluteUnaligned(Eigen::Vector3d(0,0,1)),SE3::Identity(),Inertia::Random(),
"ru_joint", "ru_body");
- model.addJointAndBody(model.getBodyId("ru_body"),JointModelSphericalZYX(),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("ru_joint"),JointModelSphericalZYX(),SE3::Identity(),Inertia::Random(),
"sphericalZYX_joint", "sphericalZYX_body");
- model.addJointAndBody(model.getBodyId("sphericalZYX_body"),JointModelTranslation(),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("sphericalZYX_joint"),JointModelTranslation(),SE3::Identity(),Inertia::Random(),
"translation_joint", "translation_body");
- model.addJointAndBody(model.getBodyId("translation_body"),JointModelPlanar(),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("translation_joint"),JointModelPlanar(),SE3::Identity(),Inertia::Random(),
"planar_joint", "planar_body");
se3::Data data(model);
@@ -393,41 +393,41 @@ BOOST_AUTO_TEST_CASE ( uniform_sampling_test )
using namespace se3;
- model.addJointAndBody(model.getBodyId("universe"),JointModelFreeFlyer(),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("universe"),JointModelFreeFlyer(),SE3::Identity(),Inertia::Random(),
Eigen::VectorXd::Zero(6), 1e3 * (Eigen::VectorXd::Random(6).array() + 1),
1e3 * (Eigen::VectorXd::Random(7).array() - 1),
1e3 * (Eigen::VectorXd::Random(7).array() + 1),
"freeflyer_joint", "freeflyer_body");
- model.addJointAndBody(model.getBodyId("freeflyer_body"),JointModelSpherical(),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("freeflyer_joint"),JointModelSpherical(),SE3::Identity(),Inertia::Random(),
Eigen::VectorXd::Zero(3), 1e3 * (Eigen::VectorXd::Random(3).array() + 1),
1e3 * (Eigen::VectorXd::Random(4).array() - 1),
1e3 * (Eigen::VectorXd::Random(4).array() + 1),
"spherical_joint", "spherical_body");
- model.addJointAndBody(model.getBodyId("spherical_body"),JointModelRX(),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("spherical_joint"),JointModelRX(),SE3::Identity(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"revolute_joint", "revolute_body");
- model.addJointAndBody(model.getBodyId("revolute_body"),JointModelPX(),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("revolute_joint"),JointModelPX(),SE3::Identity(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"px_joint", "px_body");
- model.addJointAndBody(model.getBodyId("px_body"),JointModelPrismaticUnaligned(Eigen::Vector3d(1,0,0)),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("px_joint"),JointModelPrismaticUnaligned(Eigen::Vector3d(1,0,0)),SE3::Identity(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"pu_joint", "pu_body");
- model.addJointAndBody(model.getBodyId("pu_body"),JointModelRevoluteUnaligned(Eigen::Vector3d(0,0,1)),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("pu_joint"),JointModelRevoluteUnaligned(Eigen::Vector3d(0,0,1)),SE3::Identity(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"ru_joint", "ru_body");
- model.addJointAndBody(model.getBodyId("ru_body"),JointModelSphericalZYX(),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("ru_joint"),JointModelSphericalZYX(),SE3::Identity(),Inertia::Random(),
Eigen::VectorXd::Random(3).array() + 1, Eigen::VectorXd::Random(3).array() + 1,
Eigen::VectorXd::Random(3).array() - 1, Eigen::VectorXd::Random(3).array() + 1,
"sphericalZYX_joint", "sphericalZYX_body");
- model.addJointAndBody(model.getBodyId("sphericalZYX_body"),JointModelTranslation(),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("sphericalZYX_joint"),JointModelTranslation(),SE3::Identity(),Inertia::Random(),
Eigen::VectorXd::Random(3).array() + 1, Eigen::VectorXd::Random(3).array() + 1,
Eigen::VectorXd::Random(3).array() - 1, Eigen::VectorXd::Random(3).array() + 1,
"translation_joint", "translation_body");
- model.addJointAndBody(model.getBodyId("translation_body"),JointModelPlanar(),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("translation_joint"),JointModelPlanar(),SE3::Identity(),Inertia::Random(),
Eigen::VectorXd::Random(3).array() + 1, Eigen::VectorXd::Random(3).array() + 1,
Eigen::VectorXd::Random(3).array() - 1, Eigen::VectorXd::Random(3).array() + 1,
"planar_joint", "planar_body");
@@ -450,41 +450,41 @@ BOOST_AUTO_TEST_CASE ( integrate_difference_test )
using namespace se3;
- model.addJointAndBody(model.getBodyId("universe"),JointModelFreeFlyer(),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("universe"),JointModelFreeFlyer(),SE3::Identity(),Inertia::Random(),
Eigen::VectorXd::Zero(6), 1e3 * (Eigen::VectorXd::Random(6).array() + 1),
1e3 * (Eigen::VectorXd::Random(7).array() - 1),
1e3 * (Eigen::VectorXd::Random(7).array() + 1),
"freeflyer_joint", "freeflyer_body");
- model.addJointAndBody(model.getBodyId("freeflyer_body"),JointModelSpherical(),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("freeflyer_joint"),JointModelSpherical(),SE3::Identity(),Inertia::Random(),
Eigen::VectorXd::Zero(3), 1e3 * (Eigen::VectorXd::Random(3).array() + 1),
1e3 * (Eigen::VectorXd::Random(4).array() - 1),
1e3 * (Eigen::VectorXd::Random(4).array() + 1),
"spherical_joint", "spherical_body");
- model.addJointAndBody(model.getBodyId("spherical_body"),JointModelRX(),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("spherical_joint"),JointModelRX(),SE3::Identity(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"revolute_joint", "revolute_body");
- model.addJointAndBody(model.getBodyId("revolute_body"),JointModelPX(),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("revolute_joint"),JointModelPX(),SE3::Identity(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"px_joint", "px_body");
- model.addJointAndBody(model.getBodyId("px_body"),JointModelPrismaticUnaligned(Eigen::Vector3d(1,0,0)),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("px_joint"),JointModelPrismaticUnaligned(Eigen::Vector3d(1,0,0)),SE3::Identity(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"pu_joint", "pu_body");
- model.addJointAndBody(model.getBodyId("pu_body"),JointModelRevoluteUnaligned(Eigen::Vector3d(0,0,1)),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("pu_joint"),JointModelRevoluteUnaligned(Eigen::Vector3d(0,0,1)),SE3::Identity(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"ru_joint", "ru_body");
- model.addJointAndBody(model.getBodyId("ru_body"),JointModelSphericalZYX(),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("ru_joint"),JointModelSphericalZYX(),SE3::Identity(),Inertia::Random(),
Eigen::VectorXd::Random(3).array() + 1, Eigen::VectorXd::Random(3).array() + 1,
Eigen::VectorXd::Random(3).array() - 1, Eigen::VectorXd::Random(3).array() + 1,
"sphericalZYX_joint", "sphericalZYX_body");
- model.addJointAndBody(model.getBodyId("sphericalZYX_body"),JointModelTranslation(),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("sphericalZYX_joint"),JointModelTranslation(),SE3::Identity(),Inertia::Random(),
Eigen::VectorXd::Random(3).array() + 1, Eigen::VectorXd::Random(3).array() + 1,
Eigen::VectorXd::Random(3).array() - 1, Eigen::VectorXd::Random(3).array() + 1,
"translation_joint", "translation_body");
- model.addJointAndBody(model.getBodyId("translation_body"),JointModelPlanar(),SE3::Identity(),Inertia::Random(),
+ model.addJointAndBody(model.getJointId("translation_joint"),JointModelPlanar(),SE3::Identity(),Inertia::Random(),
Eigen::VectorXd::Random(3).array() + 1, Eigen::VectorXd::Random(3).array() + 1,
Eigen::VectorXd::Random(3).array() - 1, Eigen::VectorXd::Random(3).array() + 1,
"planar_joint", "planar_body");
diff --git a/unittest/joints.cpp b/unittest/joints.cpp
index a9dc9f87f..fe09c9a2d 100644
--- a/unittest/joints.cpp
+++ b/unittest/joints.cpp
@@ -386,7 +386,7 @@ BOOST_AUTO_TEST_CASE ( test_rnea )
Model model;
Inertia inertia (1., Vector3 (0.5, 0., 0.0), Matrix3::Identity ());
- model.addJointAndBody (model.getBodyId("universe"), JointModelSphericalZYX (), SE3::Identity (), inertia, "root");
+ model.addJointAndBody (model.getJointId("universe"), JointModelSphericalZYX (), SE3::Identity (), inertia, "root");
Data data (model);
@@ -428,7 +428,7 @@ BOOST_AUTO_TEST_CASE ( test_crba )
Model model;
Inertia inertia (1., Vector3 (0.5, 0., 0.0), Matrix3::Identity ());
- model.addJointAndBody (model.getBodyId("universe"), JointModelSphericalZYX (), SE3::Identity (), inertia, "root");
+ model.addJointAndBody (model.getJointId("universe"), JointModelSphericalZYX (), SE3::Identity (), inertia, "root");
Data data (model);
@@ -527,7 +527,7 @@ BOOST_AUTO_TEST_CASE ( test_rnea )
Model model;
Inertia inertia (1., Vector3 (0.5, 0., 0.0), Matrix3::Identity ());
- model.addJointAndBody (model.getBodyId("universe"), JointModelPX(), SE3::Identity (), inertia, "root");
+ model.addJointAndBody (model.getJointId("universe"), JointModelPX(), SE3::Identity (), inertia, "root");
Data data (model);
@@ -572,7 +572,7 @@ BOOST_AUTO_TEST_CASE ( test_crba )
Model model;
Inertia inertia (1., Vector3 (0.5, 0., 0.0), Matrix3::Identity ());
- model.addJointAndBody (model.getBodyId("universe"), JointModelPX (), SE3::Identity (), inertia, "root");
+ model.addJointAndBody (model.getJointId("universe"), JointModelPX (), SE3::Identity (), inertia, "root");
Data data (model);
--
GitLab