diff --git a/src/multibody/frame.hpp b/src/multibody/frame.hpp
index 6634f40cab536814685657cfc515593cc8c47ed6..b2fec585ba5174620f731d930dda2f79e24d5d51 100644
--- a/src/multibody/frame.hpp
+++ b/src/multibody/frame.hpp
@@ -58,12 +58,14 @@ namespace se3
///
/// \param[in] name Name of the frame.
/// \param[in] parent Index of the parent joint in the kinematic tree.
+ /// \param[in] previousFrame Index of the parent frame in the kinematic tree.
/// \param[in] placement Placement of the frame wrt the parent joint frame.
/// \param[in] type The type of the frame, see the enum FrameType
///
- Frame(const std::string & name, const JointIndex parent, const SE3 & frame_placement, const FrameType type)
+ Frame(const std::string & name, const JointIndex parent, const FrameIndex previousFrame, const SE3 & frame_placement, const FrameType type)
: name(name)
, parent(parent)
+ , previousFrame(previousFrame)
, placement(frame_placement)
, type(type)
{}
@@ -76,6 +78,7 @@ namespace se3
bool operator == (const Frame & other) const
{
return name == other.name && parent == other.parent
+ && previousFrame == other.previousFrame
&& placement == other.placement
&& type == other.type ;
}
@@ -86,6 +89,9 @@ namespace se3
/// \brief Index of the parent joint.
JointIndex parent;
+ /// \brief Index of the previous frame.
+ FrameIndex previousFrame;
+
/// \brief Placement of the frame wrt the parent joint.
SE3 placement;
diff --git a/src/multibody/model.hpp b/src/multibody/model.hpp
index 93f784fd36b54dc4eaf0cff884ae12d408dbfca7..b1e69611cdb121807c6540038ac5ca915dd54c8d 100644
--- a/src/multibody/model.hpp
+++ b/src/multibody/model.hpp
@@ -124,13 +124,15 @@ namespace se3
, gravity( gravity981,Eigen::Vector3d::Zero() )
{
names[0] = "universe"; // Should be "universe joint (trivial)"
+ addFrame(Frame("universe", 0, 0, SE3::Identity(), FIXED_JOINT));
}
~Model() {} // std::cout << "Destroy model" << std::endl; }
///
/// \brief Add a joint to the kinematic tree.
///
- /// \remark This method also adds a Frame of same name to the vector of frames.
+ /// \remark This method does not add a Frame of same name to the vector of frames.
+ /// Use Model::addJointFrame.
/// \remark The inertia supported by the joint is set to Zero.
///
/// \tparam JointModelDerived The type of the joint model.
@@ -146,7 +148,7 @@ namespace se3
///
/// \return The index of the new joint.
///
- /// \sa Model::appendBodyToJoint
+ /// \sa Model::appendBodyToJoint, Model::addJointFrame
///
template<typename JointModelDerived>
JointIndex addJoint(const JointIndex parent, const JointModelBase<JointModelDerived> & joint_model, const SE3 & joint_placement,
@@ -157,6 +159,18 @@ namespace se3
const Eigen::VectorXd & max_config = Eigen::VectorXd::Constant(JointModelDerived::NQ,std::numeric_limits<double>::max())
);
+ ///
+ /// \brief Add a joint to the frame tree.
+ ///
+ /// \param[in] jointIndex Index of the joint.
+ /// \param[in] frameIndex Index of the parent frame. If negative,
+ /// the parent frame is the frame of the parent joint.
+ ///
+ /// \return The index of the new frame.
+ ///
+ FrameIndex addJointFrame (const JointIndex& jointIndex,
+ int frameIndex = -1);
+
///
/// \brief Append a body to a given joint of the kinematic tree.
///
@@ -165,13 +179,27 @@ namespace se3
/// \param[in] joint_index Index of the supporting joint.
/// \param[in] Y Spatial inertia of the body.
/// \param[in] body_placement The relative placement of the body regarding to the parent joint. Set default to the Identity placement.
- /// \param[in] body_name Name of the body. If empty, the name is random.
///
/// \sa Model::addJoint
///
void appendBodyToJoint(const JointIndex joint_index, const Inertia & Y,
- const SE3 & body_placement = SE3::Identity(),
- const std::string & body_name = "");
+ const SE3 & body_placement = SE3::Identity());
+
+ ///
+ /// \brief Add a body to the frame tree.
+ ///
+ /// \param[in] body_name Name of the body.
+ /// \param[in] parentJoint Index of the parent joint.
+ /// \param[in] body_placement The relative placement of the body regarding to the parent joint. Set default to the Identity placement.
+ /// \param[in] previousFrame Index of the parent frame. If negative,
+ /// the parent frame is the frame of the parent joint.
+ ///
+ /// \return The index of the new frame.
+ ///
+ FrameIndex addBodyFrame (const std::string& body_name,
+ const JointIndex& parentJoint,
+ const SE3 & body_placement = SE3::Identity(),
+ int previousFrame = -1);
///
/// \brief Return the index of a body given by its name.
diff --git a/src/multibody/model.hxx b/src/multibody/model.hxx
index d3c827c08e54ffef8bd062a379e6e05cc562718e..344e1e46017442ca374eee16230f82f963fea76b 100644
--- a/src/multibody/model.hxx
+++ b/src/multibody/model.hxx
@@ -87,15 +87,41 @@ namespace se3
return idx;
}
+ inline FrameIndex Model::addJointFrame (const JointIndex& jidx,
+ int fidx)
+ {
+ if (fidx < 0) {
+ fidx = getFrameId(names[parents[jidx]]);
+ }
+ if (fidx >= frames.size())
+ throw std::invalid_argument ("Frame not found");
+ // Add a the joint frame attached to itself to the frame vector - redundant information but useful.
+ addFrame(Frame(names[jidx],jidx,fidx,SE3::Identity(),JOINT));
+ return frames.size() - 1;
+ }
+
inline void Model::appendBodyToJoint(const Model::JointIndex joint_index,
const Inertia & Y,
- const SE3 & body_placement,
- const std::string & body_name)
+ const SE3 & body_placement)
{
const Inertia & iYf = Y.se3Action(body_placement);
inertias[joint_index] += iYf;
nbody++;
}
+
+ inline FrameIndex Model::addBodyFrame (const std::string& body_name,
+ const JointIndex& parentJoint,
+ const SE3 & body_placement,
+ int previousFrame)
+ {
+ if (previousFrame < 0) {
+ previousFrame = getFrameId(names[parentJoint]);
+ }
+ if (previousFrame >= frames.size())
+ throw std::invalid_argument ("Frame not found");
+ addFrame(Frame(body_name, parentJoint, previousFrame, body_placement, BODY));
+ return frames.size() - 1;
+ }
inline Model::JointIndex Model::getBodyId (const std::string & name) const
{