Skip to content
Snippets Groups Projects
Commit 818f7aae authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

[C++] Add Frame::previousFrame and update class Model

parent 21bb5b85
No related branches found
No related tags found
No related merge requests found
...@@ -58,12 +58,14 @@ namespace se3 ...@@ -58,12 +58,14 @@ namespace se3
/// ///
/// \param[in] name Name of the frame. /// \param[in] name Name of the frame.
/// \param[in] parent Index of the parent joint in the kinematic tree. /// \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] placement Placement of the frame wrt the parent joint frame.
/// \param[in] type The type of the frame, see the enum FrameType /// \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) : name(name)
, parent(parent) , parent(parent)
, previousFrame(previousFrame)
, placement(frame_placement) , placement(frame_placement)
, type(type) , type(type)
{} {}
...@@ -76,6 +78,7 @@ namespace se3 ...@@ -76,6 +78,7 @@ namespace se3
bool operator == (const Frame & other) const bool operator == (const Frame & other) const
{ {
return name == other.name && parent == other.parent return name == other.name && parent == other.parent
&& previousFrame == other.previousFrame
&& placement == other.placement && placement == other.placement
&& type == other.type ; && type == other.type ;
} }
...@@ -86,6 +89,9 @@ namespace se3 ...@@ -86,6 +89,9 @@ namespace se3
/// \brief Index of the parent joint. /// \brief Index of the parent joint.
JointIndex parent; JointIndex parent;
/// \brief Index of the previous frame.
FrameIndex previousFrame;
/// \brief Placement of the frame wrt the parent joint. /// \brief Placement of the frame wrt the parent joint.
SE3 placement; SE3 placement;
......
...@@ -124,13 +124,15 @@ namespace se3 ...@@ -124,13 +124,15 @@ namespace se3
, gravity( gravity981,Eigen::Vector3d::Zero() ) , gravity( gravity981,Eigen::Vector3d::Zero() )
{ {
names[0] = "universe"; // Should be "universe joint (trivial)" names[0] = "universe"; // Should be "universe joint (trivial)"
addFrame(Frame("universe", 0, 0, SE3::Identity(), FIXED_JOINT));
} }
~Model() {} // std::cout << "Destroy model" << std::endl; } ~Model() {} // std::cout << "Destroy model" << std::endl; }
/// ///
/// \brief Add a joint to the kinematic tree. /// \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. /// \remark The inertia supported by the joint is set to Zero.
/// ///
/// \tparam JointModelDerived The type of the joint model. /// \tparam JointModelDerived The type of the joint model.
...@@ -146,7 +148,7 @@ namespace se3 ...@@ -146,7 +148,7 @@ namespace se3
/// ///
/// \return The index of the new joint. /// \return The index of the new joint.
/// ///
/// \sa Model::appendBodyToJoint /// \sa Model::appendBodyToJoint, Model::addJointFrame
/// ///
template<typename JointModelDerived> template<typename JointModelDerived>
JointIndex addJoint(const JointIndex parent, const JointModelBase<JointModelDerived> & joint_model, const SE3 & joint_placement, JointIndex addJoint(const JointIndex parent, const JointModelBase<JointModelDerived> & joint_model, const SE3 & joint_placement,
...@@ -157,6 +159,18 @@ namespace se3 ...@@ -157,6 +159,18 @@ namespace se3
const Eigen::VectorXd & max_config = Eigen::VectorXd::Constant(JointModelDerived::NQ,std::numeric_limits<double>::max()) 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. /// \brief Append a body to a given joint of the kinematic tree.
/// ///
...@@ -165,13 +179,27 @@ namespace se3 ...@@ -165,13 +179,27 @@ namespace se3
/// \param[in] joint_index Index of the supporting joint. /// \param[in] joint_index Index of the supporting joint.
/// \param[in] Y Spatial inertia of the body. /// \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_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 /// \sa Model::addJoint
/// ///
void appendBodyToJoint(const JointIndex joint_index, const Inertia & Y, void appendBodyToJoint(const JointIndex joint_index, const Inertia & Y,
const SE3 & body_placement = SE3::Identity(), const SE3 & body_placement = SE3::Identity());
const std::string & body_name = "");
///
/// \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. /// \brief Return the index of a body given by its name.
......
...@@ -87,15 +87,41 @@ namespace se3 ...@@ -87,15 +87,41 @@ namespace se3
return idx; 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, inline void Model::appendBodyToJoint(const Model::JointIndex joint_index,
const Inertia & Y, const Inertia & Y,
const SE3 & body_placement, const SE3 & body_placement)
const std::string & body_name)
{ {
const Inertia & iYf = Y.se3Action(body_placement); const Inertia & iYf = Y.se3Action(body_placement);
inertias[joint_index] += iYf; inertias[joint_index] += iYf;
nbody++; 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 inline Model::JointIndex Model::getBodyId (const std::string & name) const
{ {
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment