Commit e1c1ba0c by fvalenza

### 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 inertias; // Spatial inertias of the body in the supporting joint frame std::vector jointPlacements; // Placement (SE3) of the input of joint in parent joint output
• JointModelVector joints; // Model of joint std::vector parents; // Joint parent of joint , denoted
• (li==parents[i]) std::vector names; // Name of joint std::vector bodyNames; // Name of the body attached to the output of joint std::vector hasVisual; // True iff body has a visual mesh. /// \brief Spatial inertias of the body expressed in the supporting joint frame . std::vector inertias; /// \brief Placement (SE3) of the input of joint regarding to the parent joint output
• . std::vector jointPlacements; /// \brief Model of joint . JointModelVector joints; /// \brief Joint parent of joint , denoted
• (li==parents[i]). std::vector parents; /// \brief Name of joint std::vector names; /// \brief Name of the body attached to the output of joint . std::vector bodyNames; /// \brief True iff body has a visual mesh. std::vector hasVisual; std::vector fix_lmpMi; // Fixed-body relative placement (wrt last moving parent) std::vector fix_lastMovingParent; // Fixed-body index of the last moving parent std::vector fix_hasVisual; // True iff fixed-body has a visual mesh. std::vector fix_bodyNames;// Name of fixed-joint /// \brief Fixed-body relative placement (wrt last moving parent) std::vector fix_lmpMi; /// \brief Fixed-body index of the last moving parent std::vector fix_lastMovingParent; /// \brief True iff fixed-body has a visual mesh. std::vector fix_hasVisual; /// \brief Name of fixed-joint . std::vector fix_bodyNames; /// \brief Vector of operational frames registered on the model. std::vector 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 JointIndex addBody( JointIndex parent,const JointModelBase & j,const SE3 & placement, const Inertia & Y, const std::string & jointName = "", const std::string & bodyName = "", bool visual = false); JointIndex addBody(JointIndex parent, const JointModelBase & 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 JointIndex addBody( JointIndex parent,const JointModelBase & 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 & 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 Matrix6x; /// \brief The 3d jacobian type (temporary) typedef Eigen::Matrix 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 Fcrb; // Spatial forces set, used in CRBA and CCRBA /// \brief Spatial forces set, used in CRBA and CCRBA std::vector Fcrb; std::vector lastChild; // Index of the last child (for CRBA) std::vector nvSubtree; // Dimension of the subtree motion space (for CRBA) /// \brief Index of the last child (for CRBA) std::vector lastChild; /// \brief Dimension of the subtree motion space (for CRBA) std::vector 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 parents_fromRow; // First previous non-zero row in M (used in Cholesky) std::vector 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 parents_fromRow; /// \brief Subtree of the current row index (used in Cholesky Decomposition). std::vector 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::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