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