compareInverseAndForwardDynamics NOT working when baseOrientation is changed

[lucasrm25]

Dear @erwincoumans and PyBullet contributors,

I am trying to obtain joint accelerations (Forward Dynamics) from Inverse Dynamics and compare them to what is obtained from finite differences (dq-dq_old)/dt (similar to #2513).

The difference is that now I am changing the robot baseOrientation.

The problem is that IFF I change baseOrientation, the joint acceleration qdd_FinDiff obtained from finite differences DOES NOT match anymore the acceleration ddq_InvDyn obtained from inverse dynamics.

Do you know what could be the cause or how to fix this issue?

If I could guess... this could be a problem in the calculateJacobian method. I noticed that it actually returns a Jacobian in robot base frame coordinates, i.e. B_x_dot = J_t * q_dot is in robot base frame coordinates (B) and not in world coordinates. (If you want I can also post here a sample code that confirms this). Btw, this is also not described in the documentation.

I thank you very much.

Bellow follows a MWE (pyBullet Version: 2.7.9):

import pybullet as p
import pybullet_data
import os, time, copy
import numpy as np
np.set_printoptions(precision=5, suppress=True,linewidth=500)


#################       CHANGE HERE      #############################
KUKABASEORIENTATION = p.getQuaternionFromEuler([0,0.3,0.3])       # DOES NOT WORK
# KUKABASEORIENTATION = p.getQuaternionFromEuler([0,0,0])         # WORKS
######################################################################


dt = 1./240
clid = p.connect(p.GUI)
p.setGravity(0,0,-9.8)
p.setTimeStep(dt)
p.setRealTimeSimulation(0)

p.setAdditionalSearchPath(pybullet_data.getDataPath())
urdf_path = "kuka_iiwa/model.urdf"

# load Kuka IIWA
kukaId = p.loadURDF(
    "kuka_iiwa/model.urdf", 
    useFixedBase    = True,
    basePosition    = [0,0,1.2],
    baseOrientation = KUKABASEORIENTATION,
    flags           = p.URDF_USE_INERTIA_FROM_FILE
)
numJoints = p.getNumJoints(kukaId)

# change initial robot joint position configuration
for i in range(numJoints):
    p.resetJointState(kukaId, i, [-2.4+0.2,-0.47,-2.45,-1.43,-2.07,1.43,1.83][i])

# setting joint dampings to zero
for link_idx in range(numJoints+1):
    p.changeDynamics(kukaId, link_idx, linearDamping=0.0, angularDamping=0.0, maxJointVelocity=1000) #, jointDamping=0.0) # 


# deactivating standard pyBullet control
p.setJointMotorControlArray(
    bodyIndex=kukaId,
    jointIndices=range(numJoints),
    controlMode=p.VELOCITY_CONTROL,
    forces=np.zeros(numJoints)
)


def getCurrentJointPosVel():
    jointStates = p.getJointStates(kukaId, range(numJoints))
    q, dq, _, _ = list(zip(*jointStates))
    return q, dq

def computeGravityCompensation():
    [q,dq] = getCurrentJointPosVel()
    tau = p.calculateInverseDynamics(
        kukaId, 
        objPositions     = q, 
        objVelocities    = [0] * numJoints, 
        objAccelerations = [0] * numJoints
    )
    return np.array(tau)

def forwardDynamics(q, dq, tau, jointDampCoeff=0):
    eta = np.array(p.calculateInverseDynamics(
        kukaId,
        objPositions     = q.tolist(),
        objVelocities    = dq.tolist(),
        objAccelerations = [0.0]*numJoints
    ))
    M = np.vstack( p.calculateMassMatrix(kukaId, objPositions=q.tolist()) )
    ddq = np.linalg.solve( M, tau - eta - jointDampCoeff * dq )
    return ddq


prev_qd = [0.0]*numJoints
time_count = 0
tau = None
while True:
    [q, qd] = getCurrentJointPosVel()

    # estimate joint acceleration ddq with Finite Differences
    qdd_FinDiff = list((np.array(qd) - np.array(prev_qd))/dt)

    print('qdd_FinDiff = ', np.array(qdd_FinDiff))
    print('time_count  = ', time_count)
    print('')

    if time_count > 2:
        assert(np.allclose(np.array(qd), (np.array(q) - np.array(prev_q))/dt, atol=1e-6))

    # calculate joint torques to be applied
    tau = computeGravityCompensation() * 0.8 # reduce torque to see robot moving

    # apply torques
    p.setJointMotorControlArray(
        bodyIndex    = kukaId,
        jointIndices = range(numJoints),
        controlMode  = p.TORQUE_CONTROL,
        forces       = tau.tolist()
    )

    time.sleep(0.5)

    # estimate joint acceleration ddq from inverse dynamics
    ddq_InvDyn = forwardDynamics(np.array(q), np.array(qd), tau, jointDampCoeff=[0.5]*7)
    print('ddq_InvDyn  = ', ddq_InvDyn)

    prev_q = copy.deepcopy(q)
    prev_qd = copy.deepcopy(qd)
    time_count += 1
    
    p.stepSimulation()

[erwincoumans]

Thanks for the feedback. We didn't spend much time exposing the inverseDynamics and skipped the base coordinates. Indeed, you may need to transform the queries in base frame coordinates first. In fact, for the deep_mimic implementation, we use a separate implementation of inverse dynamics/mass matrix (using flags=1) for floating base and spherical joints.