On dDifference for continuous joints

[cmastalli]

Dear all,

While debugging an internal development inside Crocoddyl, I noticed that the Jacobians of the different operators in a double pendulum with continuous joints are always identity matrices. Do you think this makes sense? I understand that these Jacobians are identity matrices for double pendulums with revolute or prismatic joints.

Below, I share a code that reproduces this. Just in case, there is a bug:

import numpy as np
import pinocchio
import example_robot_data

# Load a double pendulum with both continuous joints
pendulum = example_robot_data.load("double_pendulum_continuous")
nq, nv = pendulum.model.nq, pendulum.model.nv
assert (nq != nv)

# Generate random configurations for computing Jacobians
q0 = np.random.random(nq)
q1 = np.random.random(nq)
J0, J1 = pinocchio.dDifference(pendulum.model, q0, q1)

print(J0, J1) # These are identity matrices, which I don't expect to get with random numbers