API for extracting generalized inertia, velocity, contact Jacobians from Gazebo

[osrf-migration]

Original report (archived issue) by John Hsu (Bitbucket: hsu, GitHub: hsu).

---

[osrf-migration]

Original comment by Isura (Bitbucket: isura).

---

Which physics engines do you plan to use to support this?

[osrf-migration]

Original comment by John Hsu (Bitbucket: hsu, GitHub: hsu).

---

• set assignee_account_id to "557058:f3968cd3-4910-4384-8349-482a6c7889ec"
• set assignee to "hsu (Bitbucket: hsu, GitHub: hsu)"

[osrf-migration]

Original comment by John Hsu (Bitbucket: hsu, GitHub: hsu).

---

A rough draft of propose API for gazebo::physics:Model class functions:

GetIndependentVelocity(const Pose& P, Vector& v)  // get the velocities of the m degrees-of-freedom of the model (for floating bases, velocity is computed in frame P)

GetIndependentForces(const Pose& P, Vector& f)  // get the forces of the m degrees-of-freedom of the model (for floating bases, force is computed in frame P); forces will include gravity and centrifugal/Coriolis forces

GetIndependentInertia(const Pose& P, Matrix& m)  // get the m x m inertia matrix of the model (for floating bases, inertia is computed in frame P)

GetJacobian(math::Vector3& p, Link startLink, Link endEffector, const Pose& P, Matrix& m) // gets the Jacobian from the start link to the end-effector with respect to point p and defined in pose P

ComputeInverseDynamics(const Vector& qdd, Vector& tau) // computes the inverse dynamics (solves for tau) of the robot under gravity, centrifugal/Coriolis forces, and contact forces such that the robot's acceleration at the joints will be qdd

where

Layout of independent coordinates (assuming n joint degrees-of-freedom + 6 DOF base = m DOF total)

Each Jacobian (J) is a 6xq matrix, where q is determined by a chain specified by a startLink and an endEffector.

A contact Jacobian would be composed by multiplying [ n s t; zeros(3)] * J, where n, s, and t are the contact normal and the two contact tangents.

@isura I think we'll add the function calls in gazebo first, and start with lowest hanging fruits.

[osrf-migration]

Original comment by Michael Sherman (Bitbucket: sherm1).

---

For max performance you may want to think of these matrices primarily as operators that are applied to vectors. So rather than M, think of M*v. That way if a matrix is sparse (common) or if a matrix-vector product can be computed in an efficient matrix-free manner (also common) then the underlying implementation has a shot at doing it efficiently. Mass matrix, inverse mass matrix, constraint Jacobian, and task Jacobians for multiple tasks would all likely benefit from being considered operators. Getting the full matrix is also very useful, esp. during development, but likely to be suboptimal. It is always possible to create the matrix from the operator, although a special-purpose method that known it is returning the full matrix can possibly do it faster.

[osrf-migration]

Original comment by Steve Peters (Bitbucket: Steven Peters, GitHub: scpeters).

---

@sherm1 Perhaps we can define the useful operations (left multiply by Jacobian or Jacobian transpose) as virtual functions and make a default implementation that calls GetJacobian and does non-sparse computations. The max performance can be obtained by over-loading those methods but would not be required.

How does that sound?

Also, gazebo::math doesn't have a generic Matrix class yet.

[osrf-migration]

Original comment by Michael Sherman (Bitbucket: sherm1).

---

Yes, great idea. Define the API so that it is possible to make it efficient; then provide a default implementation that gets the job done however.

The matrix-returning methods are sufficient, if suboptimal. As long as those are provided, they can be used in default implementations for the operators.

It might be worth thinking (briefly) about sparsity. My inclination would be always to return full matrices and only exploit sparsity in the operator methods. However, depending on the features of the available Matrix class you might be able to return a generic Matrix that might be full or sparse as appropriate. I think it would be overkill to have a separate set of methods that returns sparse matrices.

I should have said that -- the operators are an optimization. To get started, all that's needed are methods that return whole matrices.

[osrf-migration]

Original comment by Michael Sherman (Bitbucket: sherm1).

---

Adding @chrisdembia to this discussion.

[osrf-migration]

Original comment by John Hsu (Bitbucket: hsu, GitHub: hsu).

---

starting work in jacobian_gazebo_4.0 branch (0eef057ce32da164c45d0da343b45ced45b35a4c)

[osrf-migration]

Original comment by Steve Peters (Bitbucket: Steven Peters, GitHub: scpeters).

---

I'm going to start a gazebo_design document for creating a gazebo::math::Matrix class that should implement these methods and allow them to be overloaded.

[osrf-migration]

Original comment by Jeongseok Lee (Bitbucket: jlee02, GitHub: jslee02).

---

Added (tentative) implementation of the API for dart.

To make it work:

• general size of vector and matrix need to be added to gazebo
• dart should implement API for Jacobian between arbitrary links.

[osrf-migration]

Original comment by Nate Koenig (Bitbucket: Nathan Koenig).

---

• set version to "all"