diff --git a/src/multibody/model.hpp b/src/multibody/model.hpp
index 3adcd15466fd7ad769be5d5ac69a1e2b3bec81c7..a617ec2c71dccf89f5cd098533165824a12b1148 100644
--- a/src/multibody/model.hpp
+++ b/src/multibody/model.hpp
@@ -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,47 +126,220 @@ 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);
};
class Data
{
public:
+ /// \brief The 6d jacobian type (temporary)
typedef Eigen::Matrix<double,6,Eigen::Dynamic> Matrix6x;
+ /// \brief The 3d jacobian type (temporary)
typedef Eigen::Matrix<double,3,Eigen::Dynamic> Matrix3x;
typedef SE3::Vector3 Vector3;
@@ -191,25 +397,41 @@ namespace se3
Eigen::VectorXd u; // Joint Inertia
// CCRBA return quantities
- /// \brief Centroidal Momentum Matrix (map the joint velocity set to the centroidal momentum hg=Ag*v)
+ /// \brief Centroidal Momentum Matrix
+ /// \note \f$ hg = Ag \dot{q}\f$ maps the joint velocity set to the centroidal momentum.
Matrix6x Ag;
- /// \brief Centroidal momentum (expressed in the frame centered at the CoM and aligned with the inertial frame)
+ /// \brief Centroidal momentum quantity.
+ /// \note The centroidal momentum is expressed in the frame centered at the CoM and aligned with the inertial frame.
+ ///
Force hg;
- /// \brief Centroidal Composite Rigid Body Inertia
+ /// \brief Centroidal Composite Rigid Body Inertia.
+ /// \note \f$ hg = Ig v_{\text{mean}}\f$ map a mean velocity to the current centroil momentum quantity.
Inertia Ig;
- std::vector<Matrix6x> Fcrb; // Spatial forces set, used in CRBA and CCRBA
+ /// \brief Spatial forces set, used in CRBA and CCRBA
+ std::vector<Matrix6x> Fcrb;
- std::vector<int> lastChild; // Index of the last child (for CRBA)
- std::vector<int> nvSubtree; // Dimension of the subtree motion space (for CRBA)
+ /// \brief Index of the last child (for CRBA)
+ std::vector<int> lastChild;
+ /// \brief Dimension of the subtree motion space (for CRBA)
+ std::vector<int> nvSubtree;
- Eigen::MatrixXd U; // Joint Inertia square root (upper triangle)
- Eigen::VectorXd D; // Diagonal of UDUT inertia decomposition
- Eigen::VectorXd tmp; // Temporary of size NV used in Cholesky
- std::vector<int> parents_fromRow; // First previous non-zero row in M (used in Cholesky)
- std::vector<int> nvSubtree_fromRow; //
+ /// \brief Joint space intertia matrix square root (upper trianglular part) computed with a Cholesky Decomposition.
+ Eigen::MatrixXd U;
+
+ /// \brief Diagonal of the joint space intertia matrix obtained by a Cholesky Decomposition.
+ Eigen::VectorXd D;
+
+ /// \brief Temporary of size NV used in Cholesky Decomposition.
+ Eigen::VectorXd tmp;
+
+ /// \brief First previous non-zero row in M (used in Cholesky Decomposition).
+ std::vector<int> parents_fromRow;
+
+ /// \brief Subtree of the current row index (used in Cholesky Decomposition).
+ std::vector<int> nvSubtree_fromRow;
/// \brief Jacobian of joint placements.
/// \note The columns of J corresponds to the basis of the spatial velocities of each joint and expressed at the origin of the inertial frame. In other words, if \f$ v_{J_{i}} = S_{i} \dot{q}_{i}\f$ is the relative velocity of the joint i regarding to its parent, then \f$J = \begin{bmatrix} ^{0}X_{1} S_{1} & \cdots & ^{0}X_{i} S_{i} & \cdots & ^{0}X_{\text{nj}} S_{\text{nj}} \end{bmatrix} \f$. This Jacobian has no special meaning. To get the jacobian of a precise joint, you need to call se3::getJacobian
@@ -234,18 +456,27 @@ namespace se3
/// \note This Jacobian maps the joint velocity vector to the velocity of the center of mass, expressed in the inertia frame. In other words, \f$ v_{\text{CoM}} = J_{\text{CoM}} \dot{q}\f$.
Matrix3x Jcom;
- Eigen::VectorXd effortLimit; // Joint max effort
- Eigen::VectorXd velocityLimit; // Joint max velocity
+ /// \brief Vector of maximal joint torques
+ Eigen::VectorXd effortLimit;
+ /// \brief Vector of maximal joint velocities
+ Eigen::VectorXd velocityLimit;
- Eigen::VectorXd lowerPositionLimit; // limit for joint lower position
- Eigen::VectorXd upperPositionLimit; // limit for joint upper position
+ /// \brief Lower joint configuration limit
+ Eigen::VectorXd lowerPositionLimit;
+ /// \brief Upper joint configuration limit
+ Eigen::VectorXd upperPositionLimit;
- double kinetic_energy; // kinetic energy of the model
- double potential_energy; // potential energy of the model
-
+ /// \brief Kinetic energy of the model.
+ double kinetic_energy;
+
+ /// \brief Potential energy of the model.
+ double potential_energy;
+
+ ///
/// \brief Default constructor of se3::Data from a se3::Model.
///
/// \param[in] model The model structure of the rigid body system.
+ ///
Data (const Model & model);
private:
diff --git a/src/multibody/model.hxx b/src/multibody/model.hxx
index 8444cff6debfb5c9ad52888ea894b2dff840eef8..0cb390bec7b94bf54382d7490fdbf62ad6a98ea7 100644
--- a/src/multibody/model.hxx
+++ b/src/multibody/model.hxx
@@ -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;
}
diff --git a/travis_custom/custom_before_install b/travis_custom/custom_before_install
index 138aaefb66fa9b8bf1adeba3603e536032b76804..14164f0a342fe72931ca618ad743e099bb63c0d0 100755
--- a/travis_custom/custom_before_install
+++ b/travis_custom/custom_before_install
@@ -7,13 +7,18 @@ set -v
# Add robotpkg
sudo sh -c "echo \"deb http://robotpkg.openrobots.org/packages/debian precise robotpkg\" >> /etc/apt/sources.list "
+curl http://robotpkg.openrobots.org/packages/debian/robotpkg.key | sudo apt-key add -
# show memory usage before install
sudo free -m -t
# Setup environment variables.
-export APT_DEPENDENCIES="doxygen libboost-dev libboost-doc libboost-thread-dev libboost-system-dev libboost-test-dev libboost-filesystem-dev libboost-python-dev libboost-program-options-dev libeigen3-dev libtinyxml-dev robotpkg-urdfdom "
-curl http://robotpkg.openrobots.org/packages/debian/robotpkg.key | sudo apt-key add -
+export APT_DEPENDENCIES="doxygen libboost-dev libboost-doc libboost-thread-dev libboost-system-dev libboost-test-dev libboost-filesystem-dev libboost-python-dev libboost-program-options-dev libeigen3-dev libtinyxml-dev robotpkg-urdfdom"
+# Add Python dependency
+export APT_DEPENDENCIES=$APT_DEPENDENCIES" python-numpy robotpkg-eigenpy"
+# Add Geometry dependencies
+#export APT_DEPENDENCIES=$APT_DEPENDENCIES" robotpkg-hpp-fcl robotpkg-assimp robotpkg-octomap"
+
# When this script is called the current directory is ./custom_travis
. ./.travis/run ../.travis/before_install