Pinocchio: Meaning of joint-configuration::dIntegrate

Created on 8 Apr 2019 · 13Comments · Source: stack-of-tasks/pinocchio

I'm a bit confused about the meaning of the dIntegrate_dq and dIntegrate_dv functions. I read https://github.com/stack-of-tasks/pinocchio/issues/542, but I still feel there is a dimension error: as you write it, if

M_{next} = f(M, v)

then f takes values in SE3 (i.e. dim nq), and so does \dot{f} - however dIntegrate takes values in se3 (i.e. dim nv). From what I understand, dIntegrate_dq represents the partial derivative of f over q, and thus should have dimension nq x nq, not nv x nv.

To give some perspective, I'm trying to implement a second-order joint configuration integrate function, taking into account the acceleration. Indeed, the current integrate function performs first-order Euler integration:
$q(t + dt) = q(t) + dt \dot{q}(t) = integrate(q, v * dt)$

I want to implement the second-order integration

$q(t + dt) = q(t) + dt \dot{q}(t) + \frac{dt^2}{2} \ddot{q}(t)$

If \dot{q}(t) = g(q, v) then this writes

$q(t + dt) = q(t) + dt g(q, v) + \frac{dt^2}{2} (\frac{\partial g}{\partial q} g(q, v) + \frac{\partial g}{\partial v}\dot{v})$

and I'm trying to get the two partial derivatives of g.

question

Source

matthieuvigne

Most helpful comment

Ah I think I understand now - I was confused because I was looking for a jacobian in terms of vector space, whereas in fact you consider only the configuration as part of a Lie group - so instead of using the vector space difference operator, you use the Lie group \ominus (pinocchio::difference).

As such, dIntegrate_dv satisfies the following identity:

integrate(q, v + dv) \ominus integrate(q, v) = J dv + o(dv)

which looks similar to a vector space Jacobian that would satisfy

f(q, v + dv) - f(q, v) = J dv + o(dv)

except that the first equation is entirely in the tangent space, not the original vector space.

Do we agree ? I think it could be worth being more explicitly in the documentation about what this "Jacobian" is: writing the above two formulas would greatly help IMHO. I can make a proposal for a pull request.

matthieuvigne on 8 Apr 2019

👍2

All 13 comments

With Pinocchio, you are always working in the tangent of the configuration space when dealing with derivatives. The dimension of the configuration is always the dimension of its tangent.

Therefore, you need to play with the integrate (exponential map) and the differentiate (log map) operations, and their Jacobians.
In your context, dIntegrate_dq is the partial derivative of the integral operators (integrate(model,q,v)) with respect to the first argument q.

jcarpent on 8 Apr 2019

If you want to also include the acceleration value in the integration scheme, you should rely on the classic semi-explicit Euler integrator rules.

jcarpent on 8 Apr 2019

In your context, dIntegrate_dq is the partial derivative of the integral operators (integrate(model,q,v)) with respect to the first argument q.

That's the part I don't understand. For me integrate goes from the tangent space to the configuration space (dim nq), so it's jacobian (in the mathematical sense) shoud have nq lines.

The semi-explicit Euler scheme is obviously a solution, and what we have been doing so far. But I thought that, if the jacobians are made available, I could get something more accurate by using the full second-order explicit scheme (while the semi-explicit Euler scheme is first-order).
This is mostly mathematical curiosity however - from experience the semi-explicit scheme seems to work just fine for our purposes.

matthieuvigne on 8 Apr 2019

That is correct, but the derivative will always be written in the
tangent space, i.e the derivative of integrate with respect to q is
written as a function of the displacement (tangent) applied on q.

On 4/8/19 12:07 PM, Matthieu Vigne wrote:
>

In your context, dIntegrate_dq is the partial derivative of the
integral operators (integrate(model,q,v)) with respect to the
first argument q.
That's the part I don't understand. For me integrate goes from the
tangent space to the configuration space (dim nq), so it's jacobian
(in the mathematical sense) shoud have nq lines.

The semi-explicit Euler scheme is obviously a solution, and what we
have been doing so far. But I thought that, if the jacobians are made
available, I could get something more accurate by using the full
second-order explicit scheme (while the semi-explicit Euler scheme is
first-order).
This is mostly mathematical curiosity however - from experience the
semi-explicit scheme seems to work just fine for our purposes.

—
You are receiving this because you are subscribed to this thread.
Reply to this email directly, view it on GitHub
https://github.com/stack-of-tasks/pinocchio/issues/759#issuecomment-480769698,
or mute the thread
https://github.com/notifications/unsubscribe-auth/AAb-CFGRxSFo6rAgBBGZ7Lgyd-kr4a7Zks5vexTygaJpZM4chuLs.

nmansard on 8 Apr 2019

@matthieuvigne You won't earn so much by using the second-order version of integration wrt semi-explicit. For both, you will obtain an O(dt^2) error.
I would say, keep it simple, if you don't any trouble with simple things. If it is not the case, then we can discuss offline them and find a solution if you need help.

jcarpent on 8 Apr 2019

Actually, if you do integration using the first two terms of the Taylor serie, you get an error that is a o(dt^2) - or a O(dt^3) (in other words, a second-order scheme). For the trivial case where v is just the term by term derivative of q (i.e. 1DoF joints), the semi-explicit scheme yields an error wrt to Taylor serie of dt^2/2 dv. So one can argue that the first method is more accurate than the second one - but by a second-order term only, which hopefully has little impact. In fact, currently we are doing a mix between both schemes, where we use the second-order Taylor serie for 1DoF joints and a semi-explicit scheme for multi-DoF joints. But that makes our code a bit hard to maintain - I think I will try the full semi-explicit scheme at first, for simplicity, and come back to this if needed.

@nmansard I'm sorry, but I still don't see how this jacobian is meant to be used. Your comment helped me understand https://github.com/stack-of-tasks/pinocchio/issues/542#issuecomment-428578395: if you define alpha = f(q, v), then the velocity (tangent vector) associated to the time-varying configuraiton alpha is given by the formula in https://github.com/stack-of-tasks/pinocchio/issues/542#issuecomment-428578395. Is this correct ?

matthieuvigne on 8 Apr 2019

Did you check the unit-test of this derivative against finite difference ?

jmirabel on 8 Apr 2019

As such, dIntegrate_dv satisfies the following identity:

integrate(q, v + dv) \ominus integrate(q, v) = J dv + o(dv)

which looks similar to a vector space Jacobian that would satisfy

f(q, v + dv) - f(q, v) = J dv + o(dv)

except that the first equation is entirely in the tangent space, not the original vector space.

matthieuvigne on 8 Apr 2019

👍2

Yes, integrate and difference and their Jacobians must be understood as functions exactly as you wrote it. Feel free to propose a clearer doc.

jmirabel on 8 Apr 2019

@matthieuvigne Do you think this issue can be closed now?
Are you willing to suggest some modifications of the documentation?

jcarpent on 10 Apr 2019

@jcarpent Sorry I didn't take the time to write the documentation earlier.
Here is a proposal, feel free to discuss and reword it in the PR. For me the issue is solved, but I think it would make sense to close it when the PR is integrated.

Btw, the documentation on https://gepettoweb.laas.fr/doc/stack-of-tasks/pinocchio/master/doxygen-html/ is a bit old.

matthieuvigne on 10 Apr 2019