def testWrenchPythonConstructor(self): pos = [-1.0, 2, 3.0, 4.0, 5.5, 6.9] p1 = iDynTree.Wrench(pos) p2 = iDynTree.Wrench() p2.setVal(0, pos[0]) p2.setVal(1, pos[1]) p2.setVal(2, pos[2]) p2.setVal(3, pos[3]) p2.setVal(4, pos[4]) p2.setVal(5, pos[5]) self.checkVectorEqual( p1, p2, "helper does not properly create vector (vectors are not equal)")
def testTransformInverse(self, nrOfTests=5): print("Running test testTransformInverse") for i in range(0, nrOfTests): #RTF.testReport("Running test testTransformInverse for position"); p = self.randomPosition() T = self.randomTransform() pZero = iDynTree.Position() pZero.zero() self.checkPointEqual(p - p, pZero, "testTransformInverse failed") self.checkPointEqual(T.inverse() * T * p, p, "testTransformInverse failed") #RTF.testReport("Running test testTransformInverse for Twist"); v = self.randomTwist() vZero = iDynTree.Twist() vZero.zero() self.checkSpatialVectorEqual(v - v, vZero, "v-v is not zero") self.checkSpatialVectorEqual(T.inverse() * T * v, v, "T.inverse()*T*v is not v") #RTF.testReport("Running test testTransformInverse for Wrench"); f = self.randomWrench() fZero = iDynTree.Wrench() fZero.zero() self.checkSpatialVectorEqual(f - f, fZero, "f-f is not zero") self.checkSpatialVectorEqual(T.inverse() * T * f, f, "T.inverse()*T*f is not f")
def testDynComp(self): dynComp = iDynTree.DynamicsComputations() ok = dynComp.loadRobotModelFromFile('./model.urdf') if not (ok): print( "Skipping testDynComp because iDynTree is compiled with IDYNTREE_USES_KDL to OFF" ) #dynComp.getFloatingBase() # set state dofs = dynComp.getNrOfDegreesOfFreedom() print "dofs: {}".format(dofs) q = iDynTree.VectorDynSize.FromPython( [random.random() for i in range(0, dofs)]) dq = iDynTree.VectorDynSize.FromPython( [random.random() for i in range(0, dofs)]) ddq = iDynTree.VectorDynSize.FromPython( [random.random() for i in range(0, dofs)]) # set gravity grav = iDynTree.SpatialAcc.FromPython([0.0, 0.0, -9.81, 0.0, 0.0, 0.0]) dynComp.setRobotState(q, dq, ddq, grav) torques = iDynTree.VectorDynSize(dofs + 6) baseReactionForce = iDynTree.Wrench() # compute id with inverse dynamics dynComp.inverseDynamics(torques, baseReactionForce) # compute id with regressors nrOfParams = 10 * dynComp.getNrOfLinks() regr = iDynTree.MatrixDynSize(6 + dofs, nrOfParams) params = iDynTree.VectorDynSize(nrOfParams) dynComp.getDynamicsRegressor(regr) dynComp.getModelDynamicsParameters(params) np_reg = regr.toNumPy() np_params = params.toNumPy() generalizedForces = np.dot(np_reg, np_params) print 'Result of inverse dynamics:' #print 'baseReactionForce: {} \nTorques: {}'.format(baseReactionForce,torques) #print 'Generalized Forces: {}'.format(generalizedForces) # check consistency self.assertTrue( np.allclose(np.hstack( (baseReactionForce.toNumPy(), torques.toNumPy())), generalizedForces, rtol=1e-04, atol=1e-04)) print 'Test of DynamicsComputations completed successfully.'
def testWrenchFromNumPy(self): pos = np.array([-1.0, 2, 3.0, 4.0, 5.5, 6.9]) p1 = iDynTree.Wrench.FromPython(pos) p2 = iDynTree.Wrench() p2.setVal(0, pos[0]) p2.setVal(1, pos[1]) p2.setVal(2, pos[2]) p2.setVal(3, pos[3]) p2.setVal(4, pos[4]) p2.setVal(5, pos[5]) self.checkVectorEqual( p1, p2, "helper does not properly create vector (vectors are not equal)")
def randomWrench(self): ''' Return a random wrench ''' res = iDynTree.Wrench() for i in range(0, 6): res.setVal(i, random.uniform(-10, 10)) return res