bindings_joint_composite.py

Go to the documentation of this file.

import unittest
import pinocchio as pin
import numpy as np
 
 
class TestJointCompositeBindings(unittest.TestCase):
    def test_basic(self):
        jc = pin.JointModelComposite()
        self.assertTrue(hasattr(jc, "joints"))
        self.assertTrue(hasattr(jc, "njoints"))
        self.assertTrue(hasattr(jc, "jointPlacements"))
 
    def test_empty_constructor(self):
        jc = pin.JointModelComposite()
        self.assertTrue(jc.nq == 0)
        self.assertTrue(len(jc.joints) == 0)
        self.assertTrue(jc.njoints == len(jc.joints))
 
    def test_reserve_constructor(self):
        jc = pin.JointModelComposite(2)
        self.assertTrue(jc.nq == 0)
        self.assertTrue(len(jc.joints) == 0)
        self.assertTrue(jc.njoints == len(jc.joints))
 
    def test_joint_constructor(self):
        j1 = pin.JointModelRX()
        self.assertTrue(j1.nq == 1)
 
        jc1 = pin.JointModelComposite(j1)
        self.assertTrue(jc1.nq == 1)
        self.assertTrue(len(jc1.joints) == 1)
        self.assertTrue(jc1.njoints == len(jc1.joints))
 
        j2 = pin.JointModelRX()
        self.assertTrue(j2.nq == 1)
 
        jc2 = pin.JointModelComposite(j1, pin.SE3.Random())
        self.assertTrue(jc2.nq == 1)
        self.assertTrue(len(jc2.joints) == 1)
        self.assertTrue(jc2.njoints == len(jc2.joints))
 
    def test_add_joint(self):
        j1 = pin.JointModelRX()
        self.assertTrue(j1.nq == 1)
        j2 = pin.JointModelRY()
        self.assertTrue(j2.nq == 1)
        j3 = pin.JointModelRZ()
        self.assertTrue(j3.nq == 1)
 
        jc = pin.JointModelComposite(2)
        self.assertTrue(jc.nq == 0)
        self.assertTrue(len(jc.joints) == 0)
        self.assertTrue(jc.njoints == len(jc.joints))
 
        jc.addJoint(j1)
        self.assertTrue(jc.nq == 1)
        self.assertTrue(len(jc.joints) == 1)
        self.assertTrue(jc.njoints == len(jc.joints))
 
        jc.addJoint(j2)
        self.assertTrue(jc.nq == 2)
        self.assertTrue(len(jc.joints) == 2)
        self.assertTrue(jc.njoints == len(jc.joints))
 
        jc.addJoint(j3, pin.SE3.Random())
        self.assertTrue(jc.nq == 3)
        self.assertTrue(jc.njoints == len(jc.joints))
 
    def test_add_joint_return(self):
        jc1 = pin.JointModelComposite()
        jc2 = jc1.addJoint(pin.JointModelRX())
        jc3 = jc2.addJoint(pin.JointModelRY())
        jc4 = jc1.addJoint(pin.JointModelRZ())
        self.assertTrue(jc1.njoints == 3)
        self.assertTrue(jc2.njoints == 3)
        self.assertTrue(jc3.njoints == 3)
        self.assertTrue(jc4.njoints == 3)
 
        del jc1
        del jc3
        del jc4
        self.assertTrue(jc2.njoints == 3)
 
    def test_add_joint_concat(self):
        j1 = pin.JointModelRX()
        self.assertTrue(j1.nq == 1)
        j2 = pin.JointModelRY()
        self.assertTrue(j2.nq == 1)
        j3 = pin.JointModelRZ()
        self.assertTrue(j3.nq == 1)
 
        jc = pin.JointModelComposite(j1).addJoint(j2, pin.SE3.Random()).addJoint(j3)
 
        self.assertTrue(jc.nq == 3)
        self.assertTrue(len(jc.joints) == 3)
        self.assertTrue(jc.njoints == len(jc.joints))
 
 
if __name__ == "__main__":
    unittest.main()