def testDynSizePythonConstructor(self):
pos = [-1.0, 2, 3.0]
p1 = iDynTree.VectorDynSize(pos)
p2 = iDynTree.VectorDynSize(3)
p2.setVal(0, pos[0])
p2.setVal(1, pos[1])
p2.setVal(2, pos[2])
self.checkVectorEqual(
p1, p2,
"helper does not properly create vector (vectors are not equal)")
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 testDynSizeFromNumPy(self):
pos = np.array([-1.0, 2, 3.0])
p1 = iDynTree.VectorDynSize.FromPython(pos)
p2 = iDynTree.VectorDynSize(3)
p2.setVal(0, pos[0])
p2.setVal(1, pos[1])
p2.setVal(2, pos[2])
self.checkVectorEqual(
p1, p2,
"helper does not properly create vector (vectors are not equal)")
def testToNumPy(self):
print("Running test testToNumPy")
pos = np.array([1.0, 2.0, 3.0])
p2 = iDynTree.VectorDynSize(3)
p2[0] = pos[0]
p2[1] = pos[1]
p2[2] = pos[2]
p2_asNumpy = p2.toNumPy()
if not isinstance(p2_asNumpy, np.ndarray):
raise TypeError("Conversion to NumPy failed")
self.checkVectorEqual(pos, p2_asNumpy,
"helper does not returned a well-formed array")