bindings_frame_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.parent_idx = self.model.getJointId("rarm2_joint") if self.model.existJointName("rarm2_joint") else (self.model.njoints-1)
        self.frame_name = self.model.names[self.parent_idx] + "_frame"
        self.frame_placement = pin.SE3.Random()
        self.frame_type = pin.FrameType.OP_FRAME
        self.model.addFrame(pin.Frame(self.frame_name, self.parent_idx, 0, self.frame_placement, self.frame_type))
        self.frame_idx = self.model.getFrameId(self.frame_name)

        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.getFrameVelocityDerivatives(model,data,self.frame_idx,pin.WORLD)
        pin.getFrameVelocityDerivatives(model,data,self.frame_idx,pin.LOCAL)
        pin.getFrameVelocityDerivatives(model,data,self.frame_idx,pin.LOCAL_WORLD_ALIGNED)

        pin.getFrameAccelerationDerivatives(model,data,self.frame_idx,pin.WORLD)
        pin.getFrameAccelerationDerivatives(model,data,self.frame_idx,pin.LOCAL)
        pin.getFrameAccelerationDerivatives(model,data,self.frame_idx,pin.LOCAL_WORLD_ALIGNED)

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