text: import unittest
  from test_case import PinocchioTestCase as TestCase
  
  import pinocchio as pin
  import numpy as np
  
  class TestJointsAlgo(TestCase):
  
      def setUp(self):
          self.model = pin.buildSampleModelHumanoidRandom()
  
          qmax = np.full((self.model.nq,1),np.pi)
          self.q = pin.randomConfiguration(self.model,-qmax,qmax)
          self.v = np.random.rand(self.model.nv)
  
      def test_basic(self):
          model = self.model
  
          q_ones = np.ones((model.nq))
          self.assertFalse(pin.isNormalized(model, q_ones))
          self.assertFalse(pin.isNormalized(model, q_ones, 1e-8))
          self.assertTrue(pin.isNormalized(model, q_ones, 1e2))
  
          q_rand = np.random.rand((model.nq))
          q_rand = pin.normalize(model,q_rand)
          self.assertTrue(pin.isNormalized(model, q_rand))
          self.assertTrue(pin.isNormalized(model, q_rand, 1e-8))
  
          self.assertTrue(abs(np.linalg.norm(q_rand[3:7])-1.) <= 1e-8)
  
          q_next = pin.integrate(model,self.q,np.zeros((model.nv)))
          self.assertApprox(q_next,self.q)
  
          v_diff = pin.difference(model,self.q,q_next)
          self.assertApprox(v_diff,np.zeros((model.nv)))
  
          q_next = pin.integrate(model,self.q,self.v)
          q_int = pin.interpolate(model,self.q,q_next,0.5)
  
          self.assertApprox(q_int,q_int)
  
          value = pin.squaredDistance(model,self.q,self.q)
          self.assertTrue((value <= 1e-8).all())
  
          dist = pin.distance(model,self.q,self.q)
          self.assertTrue(dist <= 1e-8)
  
          q_neutral = pin.neutral(model)
          self.assertApprox(q_neutral,q_neutral)
  
          q_rand1 = pin.randomConfiguration(model)
          q_rand2 = pin.randomConfiguration(model,-np.ones((model.nq)),np.ones((model.nq)))
  
          self.assertTrue(pin.isSameConfiguration(model,self.q,self.q,1e-8))
  
          self.assertFalse(pin.isSameConfiguration(model,q_rand1,q_rand2,1e-8))
  
      def test_derivatives(self):
          model = self.model
  
          q = self.q
          v = self.v
  
          J0, J1 = pin.dIntegrate(model,q,v)
          res_0 = pin.dIntegrate(model,q,v,pin.ArgumentPosition.ARG0)
          res_1 = pin.dIntegrate(model,q,v,pin.ArgumentPosition.ARG1)
  
          self.assertApprox(J0,res_0)
          self.assertApprox(J1,res_1)
  
          q_next = pin.integrate(model,q,v)
  
          J0, J1 = pin.dDifference(model,q,q_next)
          res_0 = pin.dDifference(model,q,q_next,pin.ARG0)
          res_1 = pin.dDifference(model,q,q_next,pin.ARG1)
  
          self.assertApprox(J0,res_0)
          self.assertApprox(J1,res_1)
  
      def test_transport(self):
          model = self.model
          Jq, Jv = pin.dIntegrate(model, self.q, self.v)
          mat = np.random.randn(model.nv, 2)
  
          mat_transported_q = pin.dIntegrateTransport(model, self.q, self.v, mat, pin.ARG0)
          mat_transported_v = pin.dIntegrateTransport(model, self.q, self.v, mat, pin.ARG1)
  
          self.assertApprox(mat_transported_q, np.dot(Jq, mat))
          self.assertApprox(mat_transported_v, np.dot(Jv, mat))
  
  if __name__ == '__main__':
      unittest.main()
  