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')
#dynComp.getFloatingBase()
# set state
dofs = dynComp.getNrOfDegreesOfFreedom()
print "dofs: {}".format(dofs)
q = iDynTree.VectorDynSize.fromPyList(
[random.random() for i in range(0, dofs)])
dq = iDynTree.VectorDynSize.fromPyList(
[random.random() for i in range(0, dofs)])
ddq = iDynTree.VectorDynSize.fromPyList(
[random.random() for i in range(0, dofs)])
# set gravity
grav = iDynTree.SpatialAcc.fromPyList([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 simulateDynamicsIDynTree(self, samples, sample_idx, dynComp=None, xStdModel=None):
# type: (Dict[str, np._ArrayLike], int, iDynTree.DynamicsComputations, np._ArrayLike[float]) -> np._ArrayLike[float]
""" compute torques for one time step of measurements """
if not dynComp:
dynComp = self.dynComp
if xStdModel is None:
xStdModel = self.xStdModel
world_gravity = iDynTree.SpatialAcc.fromList(self.gravity)
# read sample data
pos = samples['positions'][sample_idx]
vel = samples['velocities'][sample_idx]
acc = samples['accelerations'][sample_idx]
# system state for iDynTree
q = iDynTree.VectorDynSize.fromList(pos)
dq = iDynTree.VectorDynSize.fromList(vel)
ddq = iDynTree.VectorDynSize.fromList(acc)
# calc torques and forces with iDynTree dynamicsComputation class
if self.opt['floatingBase']:
base_vel = samples['base_velocity'][sample_idx]
base_acc = samples['base_acceleration'][sample_idx]
rpy = samples['base_rpy'][sample_idx]
# get the homogeneous transformation that transforms vectors expressed
# in the base reference frame to frames expressed in the world
# reference frame, i.e. pos_world = world_T_base*pos_base
# for identification purposes, the position does not matter but rotation is taken
# from IMU estimation. The gravity, base velocity and acceleration all need to be
# expressed in world frame
rot = iDynTree.Rotation.RPY(rpy[0], rpy[1], rpy[2])
pos = iDynTree.Position.Zero()
world_T_base = iDynTree.Transform(rot, pos).inverse()
'''
# rotate base vel and acc to world frame
to_world = world_T_base.getRotation().toNumPy()
base_vel[0:3] = to_world.dot(base_vel[0:3])
base_vel[3:] = to_world.dot(base_vel[3:])
base_acc[0:3] = to_world.dot(base_acc[0:3])
base_acc[3:] = to_world.dot(base_acc[3:])
'''
# The twist (linear, angular velocity) of the base, expressed in the world
# orientation frame and with respect to the base origin
base_velocity = iDynTree.Twist.fromList(base_vel)
# The 6d classical acceleration (linear, angular acceleration) of the base
# expressed in the world orientation frame and with respect to the base origin
base_acceleration = iDynTree.ClassicalAcc.fromList(base_acc)
dynComp.setRobotState(q, dq, ddq, world_T_base, base_velocity, base_acceleration,
world_gravity)
else:
dynComp.setRobotState(q, dq, ddq, world_gravity)
# compute inverse dynamics
torques = iDynTree.VectorDynSize(self.num_dofs)
baseReactionForce = iDynTree.Wrench()
dynComp.inverseDynamics(torques, baseReactionForce)
torques = torques.toNumPy()
if self.opt['identifyFriction']:
# add friction torques
# constant
sign = 1 #np.sign(vel)
p_constant = range(self.friction_params_start, self.friction_params_start+self.num_dofs)
torques += sign*xStdModel[p_constant]
# vel dependents
if not self.opt['identifyGravityParamsOnly']:
# (take only first half of params as they are not direction dependent in urdf anyway)
p_vel = range(self.friction_params_start+self.num_dofs, self.friction_params_start+self.num_dofs*2)
torques += xStdModel[p_vel]*vel
if self.opt['floatingBase']:
return np.concatenate((baseReactionForce.toNumPy(), torques))
else:
return torques
def test_regressors():
#get some random state values and compare inverse dynamics torques with torques
#obtained with regressor and parameter vector
idyn_model = iDynTree.Model()
iDynTree.modelFromURDF(urdf_file, idyn_model)
# create generator instance and load model
generator = iDynTree.DynamicsRegressorGenerator()
generator.loadRobotAndSensorsModelFromFile(urdf_file)
regrXml = '''
<regressor>
<baseLinkDynamics/>
<jointTorqueDynamics>
<allJoints/>
</jointTorqueDynamics>
</regressor>'''
generator.loadRegressorStructureFromString(regrXml)
num_model_params = generator.getNrOfParameters()
num_out = generator.getNrOfOutputs()
n_dofs = generator.getNrOfDegreesOfFreedom()
num_samples = 100
xStdModel = iDynTree.VectorDynSize(num_model_params)
generator.getModelParameters(xStdModel)
xStdModel = xStdModel.toNumPy()
gravity_twist = iDynTree.Twist.fromList([0, 0, -9.81, 0, 0, 0])
gravity_acc = iDynTree.SpatialAcc.fromList([0, 0, -9.81, 0, 0, 0])
dynComp = iDynTree.DynamicsComputations()
dynComp.loadRobotModelFromFile(urdf_file)
regressor_stack = np.zeros(shape=((n_dofs + 6) * num_samples,
num_model_params))
idyn_torques = np.zeros(shape=((n_dofs + 6) * num_samples))
contactForceSum = np.zeros(shape=((n_dofs + 6) * num_samples))
for sample_index in range(0, num_samples):
q = iDynTree.VectorDynSize.fromList(
((np.random.ranf(n_dofs) * 2 - 1) * np.pi).tolist())
dq = iDynTree.VectorDynSize.fromList(
((np.random.ranf(n_dofs) * 2 - 1) * np.pi).tolist())
ddq = iDynTree.VectorDynSize.fromList(
((np.random.ranf(n_dofs) * 2 - 1) * np.pi).tolist())
base_vel = np.pi * np.random.rand(6)
base_acc = np.pi * np.random.rand(6)
#rpy = [0,0,0]
rpy = np.random.ranf(3) * 0.1
rot = iDynTree.Rotation.RPY(rpy[0], rpy[1], rpy[2])
pos = iDynTree.Position.Zero()
world_T_base = iDynTree.Transform(rot, pos).inverse()
# rotate base vel and acc to world frame
to_world = world_T_base.getRotation().toNumPy()
base_vel[0:3] = to_world.dot(base_vel[0:3])
base_vel[3:] = to_world.dot(base_vel[3:])
base_acc[0:3] = to_world.dot(base_acc[0:3])
base_acc[3:] = to_world.dot(base_acc[3:])
base_velocity = iDynTree.Twist.fromList(base_vel)
base_acceleration = iDynTree.Twist.fromList(base_acc)
base_acceleration_acc = iDynTree.ClassicalAcc.fromList(base_acc)
# regressor
generator.setRobotState(q, dq, ddq, world_T_base, base_velocity,
base_acceleration, gravity_twist)
regressor = iDynTree.MatrixDynSize(num_out, num_model_params)
knownTerms = iDynTree.VectorDynSize(num_out)
if not generator.computeRegressor(regressor, knownTerms):
print("Error during numeric computation of regressor")
#the base forces are expressed in the base frame for the regressor, so transform them
to_world = np.fromstring(world_T_base.getRotation().toString(),
sep=' ').reshape((3, 3))
regressor = regressor.toNumPy()
regressor[0:3, :] = to_world.dot(regressor[0:3, :])
regressor[3:6, :] = to_world.dot(regressor[3:6, :])
row_index = (
n_dofs + 6
) * sample_index # index for current row in stacked regressor matrix
np.copyto(regressor_stack[row_index:row_index + n_dofs + 6], regressor)
# inverse dynamics
#dynComp.setFloatingBase('base_link')
dynComp.setRobotState(q, dq, ddq, world_T_base, base_velocity,
base_acceleration_acc, gravity_acc)
torques = iDynTree.VectorDynSize(n_dofs)
baseReactionForce = iDynTree.Wrench()
dynComp.inverseDynamics(torques, baseReactionForce)
torques = np.concatenate(
(baseReactionForce.toNumPy(), torques.toNumPy()))
np.copyto(idyn_torques[row_index:row_index + n_dofs + 6], torques)
# contacts
dim = n_dofs + 6
contact = np.array([0, 0, 10, 0, 0, 0])
jacobian = iDynTree.MatrixDynSize(6, dim)
dynComp.getFrameJacobian(contactFrame, jacobian)
jacobian = jacobian.toNumPy()
contactForceSum[sample_index * dim:(sample_index + 1) *
dim] = jacobian.T.dot(contact)
regressor_torques = np.dot(regressor_stack, xStdModel) + contactForceSum
idyn_torques += contactForceSum
error = np.reshape(regressor_torques - idyn_torques, (num_samples, dim))
#plots = plt.plot(range(0, num_samples), error)
#plt.legend(plots, ['f_x', 'f_y', 'f_z', 'm_x', 'm_y', 'm_z', 'j_0'])
#plt.show()
error_norm = la.norm(error)
print(error_norm)
assert error_norm <= 0.01
# Note that we are using the "fixed base" version of setRobotState
regrGen.setRobotState(qj, dqj, ddqj, gravity)
# TODO: set sensor values
# Now we can compute the regressor
regressor = iDynTree.MatrixDynSize(outs, params)
knownTerms = iDynTree.VectorDynSize(outs)
cadParams = iDynTree.VectorDynSize(params)
# We can get the current model parameters (probably extracted from CAD)
regrGen.getModelParameters(cadParams)
# We want to set the measure for the `l_arm_ft_sensor`
sensorMeasure = iDynTree.Wrench()
sensorMeasure.setVal(0, 0.0)
sensorMeasure.setVal(1, 0.0)
sensorMeasure.setVal(2, 10.0)
sensorMeasure.setVal(3, 0.4)
sensorMeasure.setVal(4, 0.6)
sensorMeasure.setVal(5, 0.5)
sensorIndex = \
regrGen.getSensorsModel().getSensorIndex(iDynTree.SIX_AXIS_FORCE_TORQUE,'r_arm_ft_sensor')
regrGen.getSensorsMeasurements().setMeasurement(iDynTree.SIX_AXIS_FORCE_TORQUE,
sensorIndex, sensorMeasure)
regrGen.computeRegressor(regressor, knownTerms)
# Convert data to numpy
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
q.zero()
dq = iDynTree.VectorDynSize(n_dofs)
#dq.fromList([1.0]*n_dofs)
dq.zero()
ddq = iDynTree.VectorDynSize(n_dofs)
ddq.zero()
world_gravity = iDynTree.SpatialAcc.fromList([0, 0, -9.81, 0, 0, 0])
base_velocity = iDynTree.Twist()
base_velocity.zero()
base_acceleration = iDynTree.ClassicalAcc()
base_acceleration.zero()
rot = iDynTree.Rotation.RPY(0, 0, 0)
pos = iDynTree.Position.Zero()
world_T_base = iDynTree.Transform(rot, pos)
torques = iDynTree.VectorDynSize(n_dofs)
baseReactionForce = iDynTree.Wrench()
m = iDynTree.MatrixDynSize(n_dofs, n_dofs)
maxima = [0]*n_dofs
minima = [99999]*n_dofs
getMaxima = 1
for i in range(n_dofs):
for pos in np.arange(limits[jointNames[i]]['lower'], limits[jointNames[i]]['upper'], 0.01):
q.zero()
q[i] = pos
if getMaxima:
# saggital = pitch, transversal = yaw, lateral = roll
if i == jointNames.index('LHipYaw'):
# lift right leg up 90 deg