def main():
'''
open a urdf file and add noise to each parameter. Can be used for testing, but noisy params are usually not consistent, so may be of limited use.
'''
parser = argparse.ArgumentParser(description='Load measurements and URDF model to get inertial parameters.')
parser.add_argument('--urdf_input', required=True, type=str, help='the file to load the robot model from')
parser.add_argument('--urdf_output', required=True, type=str, help='the file to save the noisy robot model to')
parser.add_argument('--noise', required=False, type=float, help='scale of noise (default 0.01)')
parser.set_defaults(noise=0.01)
args = parser.parse_args()
model = iDynTree.Model()
iDynTree.modelFromURDF(args.urdf_input, model)
link_names = []
for i in range(0, model.getNrOfLinks()):
link_names.append(model.getLinkName(i))
n_params = model.getNrOfLinks()*10
dynComp = iDynTree.DynamicsComputations()
dynComp.loadRobotModelFromFile(args.urdf_input)
xStdModel = iDynTree.VectorDynSize(n_params)
dynComp.getModelDynamicsParameters(xStdModel)
xStdModel = xStdModel.toNumPy()
# percentage noise
#for p in range(0, len(xStdModel)):
# xStdModel[p] += np.random.randn()*args.noise*xStdModel[p]
# additive noise
xStdModel += np.random.randn(n_params)*args.noise
helpers = identificationHelpers.IdentificationHelpers(n_params)
helpers.replaceParamsInURDF(input_urdf=args.urdf_input, output_urdf=args.urdf_output, \
new_params=xStdModel, link_names=link_names)
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 __init__(self, opt, urdf_file, regressor_file=None, regressor_init=True):
# (Dict[str, Any, str, str]) -> None
self.urdf_file = urdf_file
self.opt = opt
progress_inst = helpers.Progress(opt)
self.progress = progress_inst.progress
# set these options in case model was not instanciated from Identification
if 'orthogonalizeBasis' not in self.opt:
self.opt['orthogonalizeBasis'] = 1
if 'useBasisProjection' not in self.opt:
self.opt['useBasisProjection'] = 0
# debug options
self.opt['useRegressorForSimulation'] = 0
self.opt['addContacts'] = 1
# create generator instance and load model
self.generator = iDynTree.DynamicsRegressorGenerator()
ret = self.generator.loadRobotAndSensorsModelFromFile(urdf_file)
if not ret:
sys.exit()
# load also with new model class for some functions
#self.idyn_model = iDynTree.Model()
#iDynTree.modelFromURDF(urdf_file, self.idyn_model)
#viz = iDynTree.Visualizer()
#viz.addModel(self.idyn_model, 'model')
#for i in range(0,30):
#model_inst = viz.modelViz('model')
#model_inst.setPositions(world_H_base, VectorDynSize jointPos)
# viz.draw()
#viz.close()
if self.opt['verbose']:
print('loaded model {}'.format(urdf_file))
# define what regressor type
if regressor_file:
with open(regressor_file, 'r') as filename:
regrXml = filename.read()
self.jointNames = [] # type: List[str]
import xml.etree.ElementTree as ET
tree = ET.fromstring(regrXml)
for l in tree.iter():
if l.tag == 'joint':
self.jointNames.append(l.text)
self.num_dofs = len(self.jointNames)
else:
# (default for all joints)
if self.opt['floatingBase']:
regrXml = '''
<regressor>
<baseLinkDynamics/>
<jointTorqueDynamics>
<allJoints/>
</jointTorqueDynamics>
</regressor>'''
else:
regrXml = '''
<regressor>
<jointTorqueDynamics>
<allJoints/>
</jointTorqueDynamics>
</regressor>'''
self.generator.loadRegressorStructureFromString(regrXml)
self.regrXml = regrXml
if not regressor_file:
import re
self.jointNames = re.sub(r"DOF Index: \d+ Name: ", "", self.generator.getDescriptionOfDegreesOfFreedom()).split()
self.num_dofs = self.generator.getNrOfDegreesOfFreedom()
# TODO: reported dofs and links are not dependent on joints specified in regressor (but
# uses all from model file)
# dynComp simulates with all joints regardless of regressor, regressor rows should be as specified
# (worked around ATM by reading from XML directly)
if self.opt['verbose']:
print('# DOFs: {}'.format(self.num_dofs))
print('Joints: {}'.format(self.jointNames))
# Get the number of outputs of the regressor
# (should eq #dofs + #base vals)
self.N_OUT = self.generator.getNrOfOutputs()
if self.opt['verbose']:
if self.opt['floatingBase']:
print('# regressor outputs: {} (DOFs + 6 base)'.format(self.N_OUT))
else:
print('# regressor outputs: {}'.format(self.N_OUT))
self.num_links = self.generator.getNrOfLinks()-self.generator.getNrOfFakeLinks()
if self.opt['verbose']:
print('# links: {} (+ {} fake)'.format(self.num_links, self.generator.getNrOfFakeLinks()))
self.inertia_params = list() # type: List[int]
self.mass_params = list() # type: List[int]
for i in range(self.num_links):
self.mass_params.append(i*10)
self.inertia_params.extend([i*10+4, i*10+5, i*10+6, i*10+7, i*10+8, i*10+9])
#self.linkNames = self.generator.getDescriptionOfLinks().split()
self.linkNames = [] # type: List[str]
import re
for d in self.generator.getDescriptionOfParameters().strip().split("\n"):
link = re.findall(r"of link (.*)", d)[0]
if link not in self.linkNames:
self.linkNames.append(link)
if self.opt['verbose']:
print('{}'.format({i: self.linkNames[i] for i in range(self.num_links)}))
self.limits = helpers.URDFHelpers.getJointLimits(self.urdf_file, use_deg=False)
# get amount of initial inertia params (from urdf) (full params, no friction, no removed columns)
self.num_model_params = self.num_links*10
self.num_all_params = self.num_model_params
# add N offset params (offsets or constant friction) and 2*N velocity dependent friction params
# (velocity +- for asymmetrical friction)
if self.opt['identifyFriction']:
self.num_identified_params = self.num_model_params + self.num_dofs
self.num_all_params += self.num_dofs
if not self.opt['identifyGravityParamsOnly']:
if self.opt['identifySymmetricVelFriction']:
self.num_identified_params += self.num_dofs
self.num_all_params += self.num_dofs
else:
self.num_identified_params += 2*self.num_dofs
self.num_all_params += 2*self.num_dofs
else:
self.num_identified_params = self.num_model_params
self.friction_params_start = self.num_model_params
if self.opt['identifyGravityParamsOnly']:
self.num_identified_params = self.num_identified_params - len(self.inertia_params)
self.friction_params_start = self.num_model_params - len(self.inertia_params)
if self.opt['verbose']:
print('# params: {} ({} will be identified)'.format(self.num_model_params, self.num_identified_params))
self.baseNames = ['base f_x', 'base f_y', 'base f_z', 'base m_x', 'base m_y', 'base m_z']
self.gravity = [0,0,-9.81,0,0,0]
self.gravity_twist = iDynTree.Twist.fromList(self.gravity)
if self.opt['useRBDL']:
import rbdl
self.rbdlModel = rbdl.loadModel(self.urdf_file, floating_base=self.opt['floatingBase'], verbose=False)
self.rbdlModel.gravity = np.array(self.gravity[0:3])
self.dynComp = iDynTree.DynamicsComputations()
self.dynComp.loadRobotModelFromFile(self.urdf_file)
# get model parameters
xStdModel = iDynTree.VectorDynSize(self.generator.getNrOfParameters())
self.generator.getModelParameters(xStdModel)
self.xStdModel = xStdModel.toNumPy()
if self.opt['identifyFriction']:
self.xStdModel = np.concatenate((self.xStdModel, np.zeros(self.num_dofs)))
if not self.opt['identifyGravityParamsOnly']:
if self.opt['identifySymmetricVelFriction']:
self.xStdModel = np.concatenate((self.xStdModel, np.zeros(self.num_dofs)))
else:
self.xStdModel = np.concatenate((self.xStdModel, np.zeros(2*self.num_dofs)))
helpers.ParamHelpers.addFrictionFromURDF(self, self.urdf_file, self.xStdModel)
if opt['estimateWith'] == 'urdf':
self.xStd = self.xStdModel
if regressor_init:
# get model dependent projection matrix and linear column dependencies (i.e. base
# groupings)
self.computeRegressorLinDepsQR()
required=False,
type=str,
help='the file to load world links from')
args = parser.parse_args()
import yaml
with open(args.config, 'r') as stream:
try:
config = yaml.load(stream)
except yaml.YAMLError as exc:
print(exc)
import iDynTree
iDynTree.init_helpers()
iDynTree.init_numpy_helpers()
dynComp = iDynTree.DynamicsComputations()
dynComp.loadRobotModelFromFile(args.model)
world_gravity = iDynTree.SpatialAcc.fromList([0, 0, -9.81, 0, 0, 0])
n_dof = dynComp.getNrOfDegreesOfFreedom()
config['num_dofs'] = n_dof
config['urdf'] = args.model
g_model = Model(config,
args.model,
regressor_file=None,
regressor_init=False)
linkNames = g_model.linkNames
# get bounding boxes for model
from identification.helpers import URDFHelpers, ParamHelpers
paramHelpers = ParamHelpers(None, config)
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
def estimateValidationTorques(self):
""" calculate torques of trajectory from validation measurements and identified params """
# TODO: don't duplicate simulation code
# TODO: get identified params directly into idyntree (new KinDynComputations class does not
# have inverse dynamics yet, so we have to go over a new urdf file for now)
import os
v_data = np.load(self.validation_file)
dynComp = iDynTree.DynamicsComputations()
if self.opt['estimateWith'] == 'urdf':
params = self.model.xStdModel
else:
params = self.model.xStd
outfile = self.model.urdf_file + '.tmp.urdf'
self.urdfHelpers.replaceParamsInURDF(input_urdf=self.model.urdf_file,
output_urdf=outfile,
new_params=params)
if self.opt['useRBDL']:
import rbdl
self.model.rbdlModel = rbdl.loadModel(outfile,
floating_base=self.opt['floatingBase'],
verbose=False)
self.model.rbdlModel.gravity = np.array(self.model.gravity)
else:
dynComp.loadRobotModelFromFile(outfile)
os.remove(outfile)
old_skip = self.opt['skipSamples']
self.opt['skipSamples'] = 8
self.tauEstimatedValidation = None # type: np._ArrayLike
for m_idx in self.progress(range(0, v_data['positions'].shape[0], self.opt['skipSamples'] + 1)):
if self.opt['useRBDL']:
torques = self.model.simulateDynamicsRBDL(v_data, m_idx, None, params)
else:
torques = self.model.simulateDynamicsIDynTree(v_data, m_idx, dynComp, params)
if self.tauEstimatedValidation is None:
self.tauEstimatedValidation = torques
else:
self.tauEstimatedValidation = np.vstack((self.tauEstimatedValidation, torques))
if self.opt['skipSamples'] > 0:
self.tauMeasuredValidation = v_data['torques'][::self.opt['skipSamples'] + 1]
self.Tv = v_data['times'][::self.opt['skipSamples'] + 1]
else:
self.tauMeasuredValidation = v_data['torques']
self.Tv = v_data['times']
# add simulated base forces also to measurements
if self.opt['floatingBase']:
self.tauMeasuredValidation = \
np.concatenate((self.tauEstimatedValidation[:, :6], self.tauMeasuredValidation), axis=1)
#TODO: add contact forces to estimation, so far validation is only correct for fixed-base!
print(Fore.RED+'No proper validation for floating base yet!'+Fore.RESET)
self.opt['skipSamples'] = old_skip
self.val_error = sla.norm(self.tauEstimatedValidation - self.tauMeasuredValidation) \
* 100 / sla.norm(self.tauMeasuredValidation)
print("Relative validation error: {}%".format(self.val_error))
self.val_residual = np.mean(sla.norm(self.tauEstimatedValidation-self.tauMeasuredValidation, axis=1))
print("Absolute validation error: {} Nm".format(self.val_residual))
torque_limits = []
for joint in self.model.jointNames:
torque_limits.append(self.model.limits[joint]['torque'])
self.val_nrms = helpers.getNRMSE(self.tauMeasuredValidation, self.tauEstimatedValidation, limits=torque_limits)
print("NRMS validation error: {}%".format(self.val_nrms))
def loadModel(urdf_file):
# type: (AnyStr) -> (iDynTree.DynamicsComputations)
dynComp = iDynTree.DynamicsComputations()
dynComp.loadRobotModelFromFile(urdf_file)
return dynComp