Commit e1c1ba0c authored by fvalenza's avatar fvalenza
Browse files

Merge pull request #128 from jcarpent/devel

Improve doc of Model+Data and add python bindings in Travis
parents a9af1ee5 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,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:
......
......@@ -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;
}
......
......@@ -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
......
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