bindings_com_velocity_derivatives.py

Go to the documentation of this file.

import unittest
import pinocchio as pin
from pinocchio.utils import *
from numpy.linalg import norm

def df_dq(model,func,q,h=1e-9):
    """ Perform df/dq by num_diff. q is in the lie manifold.
    :params func: function to differentiate f : np.array -> np.array
    :params q: configuration value at which f is differentiated. type np.array
    :params h: eps
    
    :returns df/dq
    """
    dq = zero(model.nv)
    f0 = func(q)
    res = zero([len(f0),model.nv])
    for iq in range(model.nv):
        dq[iq] = h
        res[:,iq] = (func(pin.integrate(model,q,dq)) - f0)/h
        dq[iq] = 0
    return res

class TestVComDerivativesBindings(unittest.TestCase):
    def setUp(self):
        self.rmodel = rmodel = pin.buildSampleModelHumanoid()
        self.rdata  = rmodel.createData()
        self.rdata_fd  = rmodel.createData()

        self.rmodel.lowerPositionLimit[:3] = -1.
        self.rmodel.upperPositionLimit[:3] = -1.
        self.q = pin.randomConfiguration(rmodel)
        self.vq = rand(rmodel.nv)*2-1

        self.precision = 1e-8
        
    def test_numdiff(self):
        rmodel,rdata = self.rmodel,self.rdata
        rdata_fd = self.rdata_fd
        q,vq = self.q,self.vq

        
        pin.computeAllTerms(rmodel,rdata,q,vq)
        dvc_dq = pin.getCenterOfMassVelocityDerivatives(rmodel,rdata)
        
        
        def calc_vc(q,vq):
            """ Compute COM velocity """
            pin.centerOfMass(rmodel,rdata_fd,q,vq)
            return rdata_fd.vcom[0].copy()
        dvc_dqn = df_dq(rmodel,lambda _q: calc_vc(_q,vq),q)

        self.assertTrue(np.allclose(dvc_dq,dvc_dqn,atol=np.sqrt(self.precision)))

if __name__ == '__main__':
    unittest.main()