Skip to content
GitLab
Commit df44efb2 authored by jcarpent's avatar jcarpent
Browse files

[Doc] Complete doc of model

parent 61b9a2f4
Hide whitespace changes
Inline Side-by-side
src/multibody/model.hpp
View file @ df44efb2
......@@ -50,30 +50,63 @@ namespace se3
typedef se3::GeomIndex GeomIndex;
typedef se3::FrameIndex FrameIndex;
int nq; // Dimension of the configuration representation
int nv; // Dimension of the velocity vector space
int nbody; // Number of bodies (= number of joints + 1)
int nFixBody; // Number of fixed-bodies (= number of fixed-joints)
int nOperationalFrames; // Number of operational frames
/// \brief Dimension of the configuration vector representation.
int nq;
/// \brief Dimension of the velocity vector space.
int nv;
/// \brief Number of bodies (= number of joints + 1).
int nbody;
/// \brief Number of fixed-bodies (= number of fixed-joints).
int nFixBody;
/// \brief Number of operational frames.
int nOperationalFrames;
std::vector<Inertia> inertias; // Spatial inertias of the body <i> in the supporting joint frame <i>
std::vector<SE3> jointPlacements; // Placement (SE3) of the input of joint <i> in parent joint output <li>
JointModelVector joints; // Model of joint <i>
std::vector<JointIndex> parents; // Joint parent of joint <i>, denoted <li> (li==parents[i])
std::vector<std::string> names; // Name of joint <i>
std::vector<std::string> bodyNames; // Name of the body attached to the output of joint <i>
std::vector<bool> hasVisual; // True iff body <i> has a visual mesh.
/// \brief Spatial inertias of the body <i> expressed in the supporting joint frame <i>.
std::vector<Inertia> inertias;
/// \brief Placement (SE3) of the input of joint <i> regarding to the parent joint output <li>.
std::vector<SE3> jointPlacements;
/// \brief Model of joint <i>.
JointModelVector joints;
/// \brief Joint parent of joint <i>, denoted <li> (li==parents[i]).
std::vector<JointIndex> parents;
/// \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 True iff body <i> has a visual mesh.
std::vector<bool> hasVisual;
std::vector<SE3> fix_lmpMi; // Fixed-body relative placement (wrt last moving parent)
std::vector<Model::JointIndex> fix_lastMovingParent; // Fixed-body index of the last moving parent
std::vector<bool> fix_hasVisual; // True iff fixed-body <i> has a visual mesh.
std::vector<std::string> fix_bodyNames;// Name of fixed-joint <i>
/// \brief Fixed-body relative placement (wrt last moving parent)
std::vector<SE3> fix_lmpMi;
/// \brief Fixed-body index of the last moving parent
std::vector<Model::JointIndex> fix_lastMovingParent;
/// \brief True iff fixed-body <i> has a visual mesh.
std::vector<bool> fix_hasVisual;
/// \brief Name of fixed-joint <i>.
std::vector<std::string> fix_bodyNames;
/// \brief Vector of operational frames registered on the model.
std::vector<Frame> operational_frames;
Motion gravity; // Spatial gravity
static const Eigen::Vector3d gravity981; // Default 3D gravity (=(0,0,-9.81))
/// \brief Spatial gravity
Motion gravity;
/// \brief Default 3D gravity vector (=(0,0,-9.81)).
static const Eigen::Vector3d gravity981;
/// \brief Default constructor
Model()
: nq(0)
, nv(0)
......@@ -93,40 +126,211 @@ namespace se3
bodyNames[0] = "universe";
}
~Model() {} // std::cout << "Destroy model" << std::endl; }
///
/// \brief Add a body to the kinematic tree.
///
/// \param[in] parent Index of the parent joint.
/// \param[in] j The joint model.
/// \param[in] placement The relative placement of the joint j regarding to the parent joint.
/// \param[in] Y Spatial inertia of the body.
/// \param[in] jointName Name of the joint.
/// \param[in] bodyName Name of the body.
/// \param[in] visual Inform if the current body has a visual or not.
///
/// \return The index of the new added joint.
///
template<typename D>
JointIndex addBody( JointIndex parent,const JointModelBase<D> & j,const SE3 & placement,
const Inertia & Y,
const std::string & jointName = "", const std::string & bodyName = "",
bool visual = false);
JointIndex addBody(JointIndex parent, const JointModelBase<D> & j, const SE3 & placement,
const Inertia & Y,
const std::string & jointName = "", const std::string & bodyName = "",
bool visual = false);
///
/// \brief Add a body to the kinematic tree.
///
/// \param[in] parent Index of the parent joint.
/// \param[in] j The joint model.
/// \param[in] placement The relative placement of the joint j regarding to the parent joint.
/// \param[in] Y Spatial inertia of the body.
/// \param[in] effort Maximal joint torque.
/// \param[in] velocity Maximal joint velocity.
/// \param[in] lowPos Lower joint configuration.
/// \param[in] upPos Upper joint configuration.
/// \param[in] jointName Name of the joint.
/// \param[in] bodyName Name of the body.
/// \param[in] visual Inform if the current body has a visual or not.
///
/// \return The index of the new added joint.
///
template<typename D>
JointIndex addBody( JointIndex parent,const JointModelBase<D> & j,const SE3 & placement,
const Inertia & Y,
const Eigen::VectorXd & effort, const Eigen::VectorXd & velocity,
const Eigen::VectorXd & lowPos, const Eigen::VectorXd & upPos,
const std::string & jointName = "", const std::string & bodyName = "",
bool visual = false);
JointIndex addFixedBody( JointIndex fix_lastMovingParent,
const SE3 & placementFromLastMoving,
const std::string &jointName = "",
bool visual=false);
void mergeFixedBody(JointIndex parent, const SE3 & placement, const Inertia & Y);
JointIndex getBodyId( const std::string & name ) const;
bool existBodyName( const std::string & name ) const;
const std::string& getBodyName( const JointIndex index ) const;
JointIndex getJointId( const std::string & name ) const;
bool existJointName( const std::string & name ) const;
const std::string& getJointName( const JointIndex index ) const;
JointIndex addBody(JointIndex parent,const JointModelBase<D> & j,const SE3 & placement,
const Inertia & Y,
const Eigen::VectorXd & effort, const Eigen::VectorXd & velocity,
const Eigen::VectorXd & lowPos, const Eigen::VectorXd & upPos,
const std::string & jointName = "", const std::string & bodyName = "",
bool visual = false);
/// Need modifications
/// \brief Add a body to the kinematic tree.
///
/// \param[in] fix_lastMovingParent Index of the closest movable parent joint.
/// \param[in] placementFromLastMoving Placement regarding to the closest movable parent joint.
/// \param[in] jointName Name of the joint.
/// \param[in] visual Inform if the current body has a visual or not.
///
/// \return The index of the new added joint.
///
JointIndex addFixedBody(JointIndex fix_lastMovingParent,
const SE3 & placementFromLastMoving,
const std::string & jointName = "",
bool visual=false);
///
/// \brief Merge a body to a parent body in the kinematic tree.
///
/// \param[in] parent Index of the parent body to merge with.
/// \param[in] placement Relative placement between the body and its parent body.
/// \param[in] Y Spatial inertia of the body.
///
void mergeFixedBody(const JointIndex parent, const SE3 & placement, const Inertia & Y);
///
/// \brief Return the index of a body given by its name.
///
/// \param[in] name Name of the body.
///
/// \return Index of the body.
///
JointIndex getBodyId(const std::string & name) const;
///
/// \brief Check if a body given by its name exists.
///
/// \param[in] name Name of the body.
///
/// \return True if the body exists in the kinematic tree.
///
bool existBodyName(const std::string & name) const;
///
/// \brief Get the name of a body given by its index.
///
/// \param[in] index Index of the body.
///
/// \return Name of the body.
///
const std::string & getBodyName(const JointIndex index) const;
///
/// \brief Return the index of a joint given by its name.
///
/// \param[in] index Index of the joint.
///
/// \return Index of the joint.
///
JointIndex getJointId(const std::string & name) const;
///
/// \brief Check if a joint given by its name exists.
///
/// \param[in] name Name of the joint.
///
/// \return True if the joint exists in the kinematic tree.
///
bool existJointName(const std::string & name) const;
///
/// \brief Get the name of a joint given by its index.
///
/// \param[in] index Index of the joint.
///
/// \return Name of the joint.
///
const std::string & getJointName(const JointIndex index) const;
FrameIndex getFrameId ( const std::string & name ) const;
bool existFrame ( const std::string & name ) const;
const std::string & getFrameName ( const FrameIndex index ) const;
const JointIndex& getFrameParent( const std::string & name ) const;
const JointIndex& getFrameParent( const FrameIndex index ) const;
const SE3 & getFramePlacement( const std::string & name ) const;
const SE3 & getFramePlacement( const FrameIndex index ) const;
///
/// \brief Return the index of a frame given by its name.
///
/// \param[in] index Index of the frame.
///
/// \return Index of the frame.
///
FrameIndex getFrameId (const std::string & name) const;
///
/// \brief Check if a frame given by its name exists.
///
/// \param[in] name Name of the frame.
///
/// \return Return true if the frame exists.
///
bool existFrame (const std::string & name) const;
///
/// \brief Get the name of a frame given by its index.
///
/// \param[in] index Index of the frame.
///
/// \return The name of the frame.
///
const std::string & getFrameName (const FrameIndex index) const;
///
/// \brief Get the index of the joint supporting the frame given by its name.
///
/// \param[in] name Name of the frame.
///
/// \return
///
JointIndex getFrameParent(const std::string & name) const;
///
/// \brief Get the index of the joint supporting the frame given by its index.
///
/// \param[in] index Index of the frame.
///
/// \return
///
JointIndex getFrameParent(const FrameIndex index) const;
///
/// \brief Return the relative placement between a frame and its supporting joint.
///
/// \param[in] name Name of the frame.
///
/// \return The frame placement regarding the supporing joint.
///
const SE3 & getFramePlacement(const std::string & name) const;
///
/// \brief Return the relative placement between a frame and its supporting joint.
///
/// \param[in] index Index of the frame.
///
/// \return The frame placement regarding the supporing joint.
///
const SE3 & getFramePlacement(const FrameIndex index) const;
bool addFrame ( const Frame & frame );
bool addFrame ( const std::string & name, JointIndex index_parent, const SE3 & placement );
///
/// \brief Add a frame to the kinematic tree.
///
/// \param[in] frame The frame to add to the kinematic tree.
///
/// \return Return true if the frame has been successfully added.
///
bool addFrame(const Frame & frame);
///
/// \brief Create and add a frame to the kinematic tree.
///
/// \param[in] name Name of the frame.
/// \param[in] parent Index of the supporting joint.
/// \param[in] placement Placement of the frame regarding to the joint frame.
///
/// \return Return true if the frame has been successfully added.
///
bool addFrame(const std::string & name, const JointIndex parent, const SE3 & placement);
};
......
src/multibody/model.hxx
View file @ df44efb2
......@@ -120,7 +120,7 @@ namespace se3
return idx;
}
inline void Model::mergeFixedBody (JointIndex parent, const SE3 & placement, const Inertia & Y)
inline void Model::mergeFixedBody (const JointIndex parent, const SE3 & placement, const Inertia & Y)
{
const Inertia & iYf = Y.se3Action(placement); //TODO
inertias[parent] += iYf;
......@@ -185,7 +185,7 @@ namespace se3
return operational_frames[index].name;
}
inline const Model::JointIndex& Model::getFrameParent( const std::string & name ) const
inline Model::JointIndex Model::getFrameParent( 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()
......@@ -197,7 +197,7 @@ namespace se3
return getFrameParent(Model::JointIndex(it_diff));
}
inline const Model::JointIndex& Model::getFrameParent( const FrameIndex index ) const
inline Model::JointIndex Model::getFrameParent( const FrameIndex index ) const
{
return operational_frames[index].parent_id;
}
......
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