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
......@@ -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;
......
......@@ -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.
......
......@@ -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
{
......
Supports Markdown
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