bindings_kinematics_derivatives.py

Go to the documentation of this file.

import unittest
import pinocchio as pin
import numpy as np

from test_case import PinocchioTestCase

class TestFrameBindings(PinocchioTestCase):

    def setUp(self):
        self.model = pin.buildSampleModelHumanoidRandom()
        self.joint_idx = self.model.getJointId("rarm2_joint") if self.model.existJointName("rarm2_joint") else (self.model.njoints-1)

        self.data = self.model.createData()
        self.q = pin.randomConfiguration(self.model)
        self.v = np.random.rand((self.model.nv))
        self.a = np.random.rand((self.model.nv))

    def tearDown(self):
        del self.model

    def test_derivatives(self):
        model = self.model
        data = self.data

        q = self.q
        v = self.v
        a = self.a

        pin.computeForwardKinematicsDerivatives(model,data,q,v,a)

        pin.getJointVelocityDerivatives(model,data,self.joint_idx,pin.WORLD)
        pin.getJointVelocityDerivatives(model,data,self.joint_idx,pin.LOCAL)
        pin.getJointVelocityDerivatives(model,data,self.joint_idx,pin.LOCAL_WORLD_ALIGNED)

        pin.getJointAccelerationDerivatives(model,data,self.joint_idx,pin.WORLD)
        pin.getJointAccelerationDerivatives(model,data,self.joint_idx,pin.LOCAL)
        pin.getJointAccelerationDerivatives(model,data,self.joint_idx,pin.LOCAL_WORLD_ALIGNED)

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