Time derivative of jacobians

[MaximilianDio]

Hello, I have a question regarding Jacobian derivatives.

I am trying to compute the acceleration drift by multiplying the derivative of the Jacobian with the velocity. Specifically, I need to compute the derivative of the Jacobian such that it computes the acceleration drift when multiplied with the velocity, i.e., dJ*dq=a_drift.

Currently, I can obtain the acceleration drift by computing a=Jddq+dJdq using getFrameClassicalAcceleration with zero joint acceleration. However, I explicitly need the Jacobian derivative.

When I compute getJointJacobianTimeVariation, it provides me with a Jacobian derivative that computes the spacial acceleration. However, as far as I recall, this derivative does not include the w x w x r term (see https://en.wikipedia.org/wiki/Spatial_acceleration).

How can I obtain the actual dJ that computes the acceleration drift? I came across a similar issue in this Github discussion (#901), but it does not provide a sufficient answer regarding dJ.

I would appreciate your help.

Best regards,
Max

[jcarpent]

Classic and spatial accelerations just differ at the linear quantity level, where:
$a_s = a_c - \omega \times v$.
And you know that, $a_s = J_s \ddot{q} + \dot{J}_s \dot{q}, \omega = J_a \dot{q}, v = J_l \dot{q} $
This will give you something like:
$\dot{J}_c = \dot{J}_s + \omega \times J_l$

You can manually compose all these expressions to get your results.

Hope it helps you,
Best,
Justin

[MaximilianDio]

Hi,

I tried the manual composition. It works in the way that $\dot{J}_c\dot{q}=a_c$ (assuming no joint acceleration).

However, when I compare the resulting matrix with the analytical derivative from a simple example, I get different results.
I don't know what happens during the calculation of the Jacobian derivative, but the result is not the actual Jacobian time derivative, although the resulting accelerations are the same. It can be explained by $\exists x: Vx=0$ and $Bx=(A+V)x=Ax+Vx=Ax$, where in this case $x$ is the joint velocity and A and B are the Jacobian derivatives.

As I need the correct $\dot{J}_c$ I am wondering if there is another way to get the exact derivative.
Maybe using CppAD?

Kind Regards,
Max

[jcarpent]

Please provide a short but complete Python script.

[MaximilianDio]

This is a simple planar RR model:

<?xml version="1.0" encoding="utf-8"?>
<robot name="planar_RR">
    <link name="base_link">
    </link>

    <joint name="joint1" type="revolute">
        <origin xyz="0 0 0" rpy="0 0 0" />
        <parent link="base_link" />
        <child link="link1" />
        <axis xyz="1 0 0" />
        <limit lower="0" upper="0" effort="0" velocity="0" />
    </joint>

    <link name="link1">
        <inertial>
            <origin xyz="0.0 0.0 0.25" rpy="0 0 0" />
            <mass value="5.0" />
            <inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0" />
        </inertial>
    </link>

    <joint name="joint2" type="revolute">
        <origin xyz="0.0 0.0 0.5" rpy="0 0 0" />
        <parent link="link1" />
        <child link="link2" />
        <axis xyz="1 0 0" />
        <limit lower="0" upper="0" effort="0" velocity="0" />
    </joint>

    <link name="link2">
        <inertial>
            <origin xyz="0.0 0.0 0.25" rpy="0 0 0" />
            <mass value="5.0" />
            <inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0" />
        </inertial>
    </link>

    <joint name="joint_ee" type="fixed">
        <parent link="link2"/>
        <child link="link_ee"/>
        <origin xyz="0 0 0.5" rpy="0 0 0"/>
    </joint>
    <link name="link_ee">
    </link>
</robot>

To validate the kinematics, I used the following python script

import pinocchio as pin
import numpy as np

model = pin.buildModelFromUrdf("planar_RR.urdf")
data = model.createData()

q = np.random.rand(model.nv)
v = np.random.rand(model.nv)
a = np.zeros(model.nv)

print("q: ", q)
print("v: ", v)
print("a: ", a)

# run kinematics with pinocchio

pin.forwardKinematics(model, data, q, v, a)
pin.computeJointJacobians(model, data, q)
pin.computeJointJacobiansTimeVariation(model, data, q, v)
pin.updateFramePlacements(model, data)

frame_id = model.getFrameId("link_ee")

T_ee = data.oMf[frame_id]
v_ee = pin.getFrameVelocity(
    model, data, frame_id, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
)
a_ee = pin.getFrameClassicalAcceleration(
    model, data, frame_id, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
)
v_ee = np.block([v_ee.linear, v_ee.angular])
a_ee = np.block([a_ee.linear, a_ee.angular])

print("r_ee:", T_ee.translation)
print("v_ee: ", v_ee)
print("a_ee: ", a_ee)

J = pin.getFrameJacobian(model, data, frame_id, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)
dJ_s = pin.getFrameJacobianTimeVariation(
    model, data, frame_id, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
)

omega = v_ee[3:6]
skew = np.matrix(
    [[0, -omega[2], omega[1]], [omega[2], 0, -omega[0]], [-omega[1], omega[0], 0]]
)
dJ = dJ_s + np.block([[skew @ J[0:3, :]], [skew @ J[3:6, :]]])

print("J:", J)
print("dJ:", dJ)

# validate jacobian
assert np.allclose(v_ee, J @ v)
assert np.allclose(a_ee, J @ a + dJ @ v)
# Validate with analytical values


# analytical derivative
s1 = float(np.sin(q[0]))
c1 = float(np.cos(q[0]))
s12 = float(np.sin(q[0] + q[1]))
c12 = float(np.cos(q[0] + q[1]))

l1 = 0.5
l2 = 0.5

r_ee_analytical = np.matrix([0, -l1 * s1 - l2 * s12, l1 * c1 + l2 * c12])
print("r_ee:", r_ee_analytical)
assert np.allclose(r_ee_analytical, T_ee.translation)

J_analytical = np.matrix(
    [
        [0, 0],
        [-l1 * c1 - l2 * c12, -l2 * c12],
        [-l1 * s1 - l2 * s12, -l2 * s12],
        [1, 1],
        [0, 0],
        [0, 0],
    ]
)
dJ_analytical = np.matrix(
    [
        [0, 0],
        [l1 * s1 * v[0] + l2 * s12 * (v[0] + v[1]), l2 * s12 * (v[0] + v[1])],
        [-l1 * c1 * v[0] - l2 * c12 * (v[0] + v[1]), -l2 * c12 * (v[0] + v[1])],
        [0, 0],
        [0, 0],
        [0, 0],
    ]
)
v_ee_analytical = J_analytical @ v
print("v_ee:", v_ee_analytical)
a_ee_analytical = J_analytical @ a + dJ_analytical @ v
print("a_ee:", a_ee_analytical)

assert np.allclose(v_ee_analytical, v_ee)
assert np.allclose(a_ee_analytical, a_ee)

print("J:", J_analytical)
print("dJ:", dJ_analytical)

assert np.allclose(J_analytical, J)

# ! dJ is not equal to dJ_analytical
assert np.allclose(dJ_analytical, dJ)

All test run through except the comparison between the Jacobian derivatives

[wly2014]

I meet the similar question. According to this issue, I realized the numerical derivatives, and find that the J_analytical can be verified easily. However, the result of getFrameJacobianTimeVariation can be also verified using a different numerical derivative. I cannot figure out how to explain this. I used the following python script and hope it helps to understand the question.

print("---------------- numerical derivative -----------")
# numerical derivative
data_plus = model.createData()

eps = 1e-6
q_plus = pin.integrate(model,q,v*eps)
print('q_plus: ',q_plus.T)

pin.forwardKinematics(model, data_plus, q_plus)
pin.computeJointJacobians(model,data_plus, q_plus)
pin.updateFramePlacements(model, data_plus)

J_lwa_plus = pin.getFrameJacobian(model,data_plus,frame_id,pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)
J_lwa = pin.getFrameJacobian(model,data,frame_id,pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)
dJ_fd2 = (J_lwa_plus-J_lwa)/eps

print("dJ_analytical:\n", dJ_analytical)
print('dJ_fd2:\n',dJ_fd2)               # dJ_fd2 is close to the dJ_analytical


J_local_plus = pin.getFrameJacobian(model,data_plus,frame_id,pin.ReferenceFrame.LOCAL)
J_local = pin.getFrameJacobian(model,data,frame_id,pin.ReferenceFrame.LOCAL)
dJ_local = pin.getFrameJacobianTimeVariation(model, data, frame_id, pin.ReferenceFrame.LOCAL)

fMf_plus = data.oMf[frame_id].inverse() * data_plus.oMf[frame_id]   # SE3
dJ_fd = (np.matmul(fMf_plus.toActionMatrix(), J_local_plus)-J_local)/eps

print('dJ_local:\n',dJ_local)
print('dJ_fd:\n',dJ_fd)                 # However, dJ_fd is close to the dJ_local

assert np.allclose(dJ_analytical, dJ_fd2)
assert np.allclose(dJ_local, dJ_fd)