iDynTree Jacobian feature elicitation

traversaro commented 10 years ago

I have opened this issue to clear the necessary features for Jacobian calculations, for fitting them in a clear API.

For example, discussing with @iron76 and @serena-ivaldi they expressed the need for a method that returned the jacobian between a link $i$ and a link $j$ such that: ${}^iv_i={}^iJ_{i,j}\dot{q}+{}^jv_j$ where $v_i$ and $v_i$ are expressed in local coordinates, and $\dot{q}$ is the vector of the real joint speeds.

While @andreadelprete expressed the need for a method that returned the jacobian of a link with respect to the world reference frame: ${}^wv_i={}^wJ_{i,w}\tilde{\dot{q}}$ where $v_i$ is expressed in the world frame and $\tilde{\dot{q}}$ is the vector of speeds considering also the base velocity.

P.s. : Methods currently implemented in iDynTree library are available here:

http://wiki.icub.org/codyco/dox/html/classiCub_1_1iDynTree_1_1DynTree.html

The documentation is not clear for some methods, in the next days I'll improve it.

mingxing-liu commented 10 years ago

Hello pegua, Thanks for opening this issus. I'm with @serena-ivaldi at ISIR. In fact, what we need is a method that can return the Jacobian for the whole-body control of : 1) a robot with n joints and a fixed base link. The Jacobian should relate whole-body joint velocities to end-effector velocities: where is the velocity of an arbitrary body frame i, such as the hand, the foot, the center of mass..., expressed in the i-th body frame; and qdot2 is the vector of joint angles of the whole body. 2) a free-floating robot with a base link of 6 Dofs and n joints. The Jacobian relate generalized velocities to end-effector velocities: with qdot3 , where the first 6 elements in qdot represent the twist of the base link. This jacobian and the one required by @andreadelprete can be easily transferred from one to another if the Adjoint operator is implemented: vivw

traversaro commented 10 years ago

Thank you for the feedback @mingxing-liu

traversaro commented 10 years ago

I have another question for @mingxing-liu (and also for @andreadelprete ): the first 6 elements in the floating base $\dot{q}$ are the twist of the base link, but this twist is expressed in the base link frame, or in the global (world) frame ?

mingxing-liu commented 10 years ago

For me the twist of the base link is expressed in the base link frame.

2013/7/30 Silvio Traversaro notifications@github.com

I have another question for @mingxing-liuhttps://github.com/mingxing-liu(and also for @andreadelprete https://github.com/andreadelprete ): the first 6 elements in the floating base [image: equation]https://github-camo.global.ssl.fastly.net/bd8cebb0b57627696aa50765fd4dccb080e02829/687474703a2f2f6c617465782e636f6465636f67732e636f6d2f6769662e6c617465783f253543646f7425374271253744are the twist of the base link, but this twist is expressed in the base link frame, or in the global (world) frame ?

— Reply to this email directly or view it on GitHubhttps://github.com/robotology/codyco/issues/1#issuecomment-21775339 .

Cordialement,

Mingxing

traversaro commented 10 years ago

Another ambiguity that we have to solve for defining unambiguously the Jacobians is the the serialization order of the twists (and wrenches) as elements of $\mathbb{R}^6$ . Changing the order of linear/serial components changes the order of the rows of the jacobian matrix.

The linear-angular serialization ( $\begin{bmatrix} v \\\omega \end{bmatrix}$ ) appear to be used by (listing software libraries or books):

iKin , the library used for cartesian control on the iCub (http://wiki.icub.org/iCub_documentation/group__iKin.html)
KDL , the Orocos library on which iDynTree is based
Murray's "A Mathematical Introduction to Robotic Manipulation" http://www.cds.caltech.edu/~murray/mlswiki/index.php/Main_Page
L. Sciavicco, B. Siciliano, "Modelling and Control of Robot Manipulators"
ROS geometry_msgs ( http://ros.org/wiki/geometry_msgs )

While the angular-linear serialization ( $\begin{bmatrix}\omega \\ v \end{bmatrix}$ ) is used by:

R. Featherstone "Rigid Body Dynamics Algorithms"
A. Jain "Robot and Multibody Dynamics: Analysis and Algorithms"
Eigen unsupported LGSM (Lie Group for Solid Mechanics), which as far as I could understand is the library used by ISIR controllers)

The Jacobian implementation that I am preparing uses the linear-angular serialization, if needed I can easily provide some utility functions that converts a jacobian expressed using one convention in to the other convention.

mingxing-liu commented 10 years ago

Yes angular-linear serialization was used by ISIR Controllers now. It would be better to have functions to convert between the two serializations. Thanks.

andreadelprete commented 10 years ago

Hi, sorry I'm late but I was on vacation.

It seems to me that @mingxing-liu expresses the velocity of each body in its own reference frame, while I express all velocities in the world reference frame. My reason for doing that is that, typically, tasks (such as reaching an object, walking to a point) are naturally expressed in world reference frame. May I ask you @mingxing-liu what's your reason for expressing everything in its own reference frame?

About the representation of the base twist, linear-angular makes more sense to me, but it's no big deal anyway.

traversaro commented 10 years ago

Just for giving an update: the initial implementation of the Jacobians is implemented (and tested!): http://wiki.icub.org/codyco/dox/html/classiCub_1_1iDynTree_1_1DynTree.html#ac20e45d78a65326f2b0f4fda3c3593d7

The twist serialization is linear angular (because both iDyn and KDL use this convention), then the conversion in angular linear for ISIR controller should addressed in the glue code between iDynTree and ISIR controllers. Regarding ISIR controller, I understood that they are difficult to compile, but they are available somewhere in source form? This could be helpful in developing functions that could reduce the friction between iDynTree and the ISIR controllers data structures. For example: are the Jacobian in ISIR controller rappresented by a Eigen::MatrixXd object, or perhaps by a Eigen::Matrix<double,6,Eigen::Dynamic> one?

Regarding velocities, I was thinking of having for example a method called getVel(int link_index, bool global = false) that outputs ${}^iv_i$ (the twist of the link expressed in local frame) if global is false or ${}^wv_i$ (the velocity expressed in the world frame) if global is true. Obviously we can change the default value for global depending on which seems the more natural choice.

mingxing-liu commented 10 years ago

It's just because many functions that ISIR controllers are using are based on velocities in body frame. As long as we have Adjoint, we can then express it in any world reference frame according to the setting of the world scene. And yes, it's no big deal anyway.

2013/8/2 Andrea Del Prete notifications@github.com

Hi, sorry I'm late but I was on vacation.

It seems to me that @mingxing-liu https://github.com/mingxing-liuexpresses the velocity of each body in its own reference frame, while I express all velocities in the world reference frame. My reason for doing that is that, typically, tasks (such as reaching an object, walking to a point) are naturally expressed in world reference frame. May I ask you @mingxing-liu https://github.com/mingxing-liu what's your reason for expressing everything in its own reference frame?

About the representation of the base twist, linear-angular makes more sense to me, but it's no big deal anyway.

— Reply to this email directly or view it on GitHubhttps://github.com/robotology/codyco/issues/1#issuecomment-21994400 .

Cordialement,

Mingxing

mingxing-liu commented 10 years ago

Hi Silvio, I'm not sure where you can get ISIR controller because everyone who knows the answer is on vacation. I'll let you know as soon as I get an answer from them. However, to see the data Structure of ISIR Controller, here are some functions regarding the whole-body model of iCub, which we need to implement for ISIR Controller (for example, the Jacobian function is: virtual const Eigen::Matrix<double,6,Eigen::Dynamic>& getSegmentJacobian(int index) const;):

virtual const Eigen::VectorXd&      getJointPositions()     const;
virtual const Eigen::VectorXd&      getJointVelocities()    const;
virtual const Eigen::Displacementd& getFreeFlyerPosition()  const;
virtual const Eigen::Twistd&        getFreeFlyerVelocity()  const;

virtual const Eigen::VectorXd&   getActuatedDofs()      const;
virtual const Eigen::VectorXd&   getJointLowerLimits()  const;
virtual const Eigen::VectorXd&   getJointUpperLimits()  const;

//CoM
virtual double

getMass() const; virtual const Eigen::Vector3d& getCoMPosition() const; virtual const Eigen::Vector3d& getCoMVelocity() const; virtual const Eigen::Vector3d& getCoMJdotQdot() const; virtual const Eigen::Matrix<double,3,Eigen::Dynamic>& getCoMJacobian() const; virtual const Eigen::Matrix<double,3,Eigen::Dynamic>& getCoMJacobianDot() const;

//dynamic/static equation terms
virtual const Eigen::MatrixXd&   getInertiaMatrix()         const;
virtual const Eigen::MatrixXd&   getInertiaMatrixInverse()  const;
virtual const Eigen::MatrixXd&   getDampingMatrix()         const;
virtual const Eigen::VectorXd&   getNonLinearTerms()        const;
virtual const Eigen::VectorXd&   getLinearTerms()           const;
virtual const Eigen::VectorXd&   getGravityTerms()          const;

//segment data
virtual const Eigen::Displacementd&

getSegmentPosition(int index) const; virtual const Eigen::Twistd& getSegmentVelocity(int index) const; virtual const Eigen::Matrix<double,6,Eigen::Dynamic>& getSegmentJacobian(int index) const; virtual const Eigen::Matrix<double,6,Eigen::Dynamic>& getSegmentJdot(int index) const; virtual const Eigen::Twistd& getSegmentJdotQdot(int index) const; virtual const Eigen::Matrix<double,6,Eigen::Dynamic>& getJointJacobian(int index) const; virtual double getSegmentMass(int index) const; virtual const Eigen::Vector3d& getSegmentCoM(int index) const; virtual const Eigen::Matrix<double, 6, 6>& getSegmentMassMatrix(int index) const; virtual const Eigen::Vector3d& getSegmentMomentsOfInertia(int index) const; virtual const Eigen::Rotation3d& getSegmentInertiaAxes(int index) const;

2013/8/2 Silvio Traversaro notifications@github.com

Just for giving an update: the initial implementation of the Jacobians is implemented (and tested!):

http://wiki.icub.org/codyco/dox/html/classiCub_1_1iDynTree_1_1DynTree.html#ac20e45d78a65326f2b0f4fda3c3593d7

The twist serialization is linear angular (because both iDyn and KDL use this convention), then the conversion in angular linear for ISIR controller should addressed in the glue code between iDynTree and ISIR controllers. Regarding ISIR controller, I understood that they are difficult to compile, but they are available somewhere in source form? This could be helpful in developing functions that could reduce the friction between iDynTree and the ISIR controllers data structures. For example: are the Jacobian in ISIR controller rappresented by a Eigen::MatrixXd object, or perhaps by a Eigen::Matrix one?

Regarding velocities, I was thinking of having for example a method called getVel(int link_index, bool global = false) that outputs [image: equation]https://github-camo.global.ssl.fastly.net/d2c45c4795bb75c50d88025c23b2c96d9b703feb/687474703a2f2f7777772e736369776561766572732e6f72672f74657832696d672e7068703f65713d25374225374425354569765f692662633d57686974652666633d426c61636b26696d3d6a70672666733d31322666663d6172657626656469743d30(the twist of the link expressed in local frame) if global is false or [image: equation]https://github-camo.global.ssl.fastly.net/24e5a92026d8e3cbc0db63a49f2765e7afc16f51/687474703a2f2f7777772e736369776561766572732%20e6f72672f74657832696d672e7068703f65713d25374225374425354577765f692662633d57686974652666633d426c61636b26696d3d6a70672666733d31322666663d6172657626656469743d30(the velocity expressed in the world frame) if global is true. Obviously we can change the default value for global depending on which seems the more natural choice.

— Reply to this email directly or view it on GitHubhttps://github.com/robotology/codyco/issues/1#issuecomment-21998312 .

Cordialement,

Mingxing

serena-ivaldi commented 10 years ago

I'm working now on providing the interface to ISIR controller in a proper way. Give me few days and I should be done with that.

traversaro commented 8 years ago

Thanks everyone for the feedback, it has been useful.

robotology-legacy / codyco-modules

iDynTree Jacobian feature elicitation #1