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 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 testPyHelpers(self):
pos = [1.0, 2.0, 3.0]
p1 = iDynTree.VectorDynSize.fromPyList(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 get_solution(self) -> IKSolution:
# Lazy import iDynTree
import iDynTree
# Initialize buffers
base_transform = iDynTree.Transform.Identity()
joint_positions = iDynTree.VectorDynSize(
self._ik.model().getNrOfJoints())
# Get the solution
self._ik.getFullJointsSolution(base_transform, joint_positions)
# Convert to numpy objects
joint_positions = joint_positions.toNumPy()
base_position = base_transform.getPosition().toNumPy()
base_quaternion = base_transform.getRotation().asQuaternion().toNumPy()
return IKSolution(base_position=base_position,
base_quaternion=base_quaternion,
joint_configuration=joint_positions)
from iDynTree import ModelLoader
URDF_FILE = '/home/username/path/robot.urdf';
dynComp = KinDynComputations();
mdlLoader = ModelLoader();
mdlLoader.loadModelFromFile(URDF_FILE);
dynComp.loadRobotModel(mdlLoader.model());
print "The loaded model has", dynComp.model().getNrOfDOFs(), \
"internal degrees of freedom and",dynComp.model().getNrOfLinks(),"links."
dofs = dynComp.model().getNrOfDOFs();
s = iDynTree.VectorDynSize(dofs);
ds = iDynTree.VectorDynSize(dofs);
dds = iDynTree.VectorDynSize(dofs);
for dof in range(dofs):
# For the sake of the example, we fill the joints vector with gibberish data (remember in any case
# that all quantities are expressed in radians-based units
s.setVal(dof, 1.0);
ds.setVal(dof, 0.4);
dds.setVal(dof, 0.3);
# The gravity acceleration is a 3d acceleration vector.
gravity = iDynTree.Vector3();
gravity.zero();
gravity.setVal(2, -9.81);
dynComp.setRobotState(s,ds,gravity);
def getRandomRegressor(self, n_samples=None):
"""
Utility function for generating a random regressor for numerical base parameter calculation
Given n_samples, the Y (n_samples*getNrOfOutputs() X getNrOfParameters() ) regressor is
obtained by stacking the n_samples generated regressors
This function returns Y^T Y (getNrOfParameters() X getNrOfParameters() ) (that share the row space with Y)
(partly ported from iDynTree)
"""
if self.opt['identifyGravityParamsOnly']:
regr_filename = self.urdf_file + '.gravity_regressor.npz'
else:
regr_filename = self.urdf_file + '.regressor.npz'
generate_new = False
fb = self.opt['floatingBase']
try:
regr_file = np.load(regr_filename)
R = regr_file['R'] #regressor matrix
Q = regr_file['Q'] #QR decomposition
RQ = regr_file['RQ']
PQ = regr_file['PQ']
n = regr_file['n'] #number of samples that were used
fbase = regr_file['fb'] #floating base flag
grav = regr_file['grav_only']
fric = regr_file['fric']
fric_sym = regr_file['fric_sym']
if self.opt['verbose']:
print("loaded random structural regressor from {}".format(regr_filename))
if n != n_samples or fbase != fb or R.shape[0] != self.num_identified_params or \
self.opt['identifyGravityParamsOnly'] != grav or \
fric != self.opt['identifyFriction'] or fric_sym != self.opt['identifySymmetricVelFriction']:
generate_new = True
#TODO: save and check timestamp of urdf file, if newer regenerate
except (IOError, KeyError):
generate_new = True
if generate_new:
if not n_samples:
n_samples = self.num_dofs * 1000
if self.opt['verbose']:
print("(re-)generating structural regressor ({} random positions)".format(n_samples))
R = np.array((self.N_OUT, self.num_model_params))
regressor = iDynTree.MatrixDynSize(self.N_OUT, self.num_model_params)
knownTerms = iDynTree.VectorDynSize(self.N_OUT)
if len(self.limits) > 0:
jn = self.jointNames
q_lim_pos = [self.limits[jn[n]]['upper'] for n in range(self.num_dofs)]
q_lim_neg = [self.limits[jn[n]]['lower'] for n in range(self.num_dofs)]
dq_lim = [self.limits[jn[n]]['velocity'] for n in range(self.num_dofs)]
q_range = (np.array(q_lim_pos) - np.array(q_lim_neg)).tolist()
for i in self.progress(range(0, n_samples)):
# set random system state
if len(self.limits) > 0:
rnd = np.random.rand(self.num_dofs) #0..1
q = iDynTree.VectorDynSize.fromList((q_lim_neg+q_range*rnd))
if self.opt['identifyGravityParamsOnly']:
#set vel and acc to zero for static case
vel = np.zeros(self.num_dofs)
acc = np.zeros(self.num_dofs)
else:
vel = ((np.random.rand(self.num_dofs)-0.5)*2*dq_lim)
acc = ((np.random.rand(self.num_dofs)-0.5)*2*np.pi)
dq = iDynTree.VectorDynSize.fromList(vel.tolist())
ddq = iDynTree.VectorDynSize.fromList(acc.tolist())
else:
q = iDynTree.VectorDynSize.fromList(((np.random.ranf(self.num_dofs)*2-1)*np.pi).tolist())
dq = iDynTree.VectorDynSize.fromList(((np.random.ranf(self.num_dofs)*2-1)*np.pi).tolist())
ddq = iDynTree.VectorDynSize.fromList(((np.random.ranf(self.num_dofs)*2-1)*np.pi).tolist())
# TODO: make work with fixed dofs (set vel and acc to zero, look at iDynTree method)
if self.opt['floatingBase']:
base_vel = np.pi*np.random.rand(6)
base_acc = np.pi*np.random.rand(6)
if self.opt['identifyGravityParamsOnly']:
#set vel and acc to zero for static case (reduces resulting amount of base dependencies)
base_vel[:] = 0.0
base_acc[:] = 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)
self.generator.setRobotState(q,dq,ddq, world_T_base, base_velocity, base_acceleration, self.gravity_twist)
else:
self.generator.setRobotState(q,dq,ddq, self.gravity_twist)
# get regressor
if not self.generator.computeRegressor(regressor, knownTerms):
print("Error during numeric computation of regressor")
A = regressor.toNumPy()
#the base forces are expressed in the base frame for the regressor, so rotate them
if self.opt['floatingBase']:
to_world = np.fromstring(world_T_base.getRotation().toString(), sep=' ').reshape((3,3))
A[0:3, :] = to_world.dot(A[0:3, :])
A[3:6, :] = to_world.dot(A[3:6, :])
if self.opt['identifyGravityParamsOnly']:
#delete inertia param columns
A = np.delete(A, self.inertia_params, 1)
if self.opt['identifyFriction']:
# append unitary matrix to regressor for offsets/constant friction
sign = 1 #np.sign(dq.toNumPy())
static_diag = np.identity(self.num_dofs)*sign
offset_regressor = np.vstack( (np.zeros((fb*6, self.num_dofs)), static_diag))
A = np.concatenate((A, offset_regressor), axis=1)
if not self.opt['identifyGravityParamsOnly']:
if self.opt['identifySymmetricVelFriction']:
# just use velocity directly
vel_diag = np.identity(self.num_dofs)*dq.toNumPy()
friction_regressor = np.vstack( (np.zeros((fb*6, self.num_dofs)), vel_diag)) # add base dynamics rows
else:
# append positive/negative velocity matrix for velocity dependent asymmetrical friction
dq_p = dq.toNumPy().copy()
dq_p[dq_p < 0] = 0 #set to zero where v < 0
dq_m = dq.toNumPy().copy()
dq_m[dq_m > 0] = 0 #set to zero where v > 0
vel_diag = np.hstack((np.identity(self.num_dofs)*dq_p, np.identity(self.num_dofs)*dq_m))
friction_regressor = np.vstack( (np.zeros((fb*6, self.num_dofs*2)), vel_diag)) # add base dynamics rows
A = np.concatenate((A, friction_regressor), axis=1)
# add to previous regressors, linear dependencies don't change
# (if too many, saturation or accuracy problems?)
if i==0:
R = A.T.dot(A)
else:
R += A.T.dot(A)
# get column space dependencies
Q,RQ,PQ = sla.qr(R, pivoting=True, mode='economic')
np.savez(regr_filename, R=R, Q=Q, RQ=RQ, PQ=PQ, n=n_samples, fb=self.opt['floatingBase'], grav_only=self.opt['identifyGravityParamsOnly'], fric=self.opt['identifyFriction'], fric_sym=self.opt['identifySymmetricVelFriction'])
if 'showRandomRegressor' in self.opt and self.opt['showRandomRegressor']:
import matplotlib.pyplot as plt
plt.imshow(R, interpolation='nearest')
plt.show()
return R, Q,RQ,PQ
def computeRegressors(self, data, only_simulate=False):
# type: (Model, Data, bool) -> (None)
""" compute regressors from measurements for each time step of the measurement data
and stack them vertically. also stack measured torques and get simulation data.
for floating base, get estimated base forces (6D wrench) and add to torque measure stack
"""
self.data = data
num_time = 0
simulate_time = 0
#extra regressor rows for floating base
if self.opt['floatingBase']: fb = 6
else: fb = 0
self.regressor_stack = np.zeros(shape=((self.num_dofs+fb)*data.num_used_samples, self.num_identified_params))
self.torques_stack = np.zeros(shape=((self.num_dofs+fb)*data.num_used_samples))
self.sim_torq_stack = np.zeros(shape=((self.num_dofs+fb)*data.num_used_samples))
self.torquesAP_stack = np.zeros(shape=((self.num_dofs+fb)*data.num_used_samples))
num_contacts = len(data.samples['contacts'].item(0).keys()) if 'contacts' in data.samples else 0
self.contacts_stack = np.zeros(shape=(num_contacts, (self.num_dofs+fb)*data.num_used_samples))
self.contactForcesSum = np.zeros(shape=((self.num_dofs+fb)*data.num_used_samples))
"""loop over measurement data, optionally skip some values
- get the regressor for each time step
- if necessary, calculate inverse dynamics to get simulated torques
- if necessary, get torques from contact wrenches and add them to the torques
- stack the torques, regressors and contacts into matrices
"""
contacts = {}
for sample_index in self.progress(range(data.num_used_samples)):
m_idx = sample_index*(self.opt['skipSamples'])+sample_index
with helpers.Timer() as t:
# read samples
pos = data.samples['positions'][m_idx]
vel = data.samples['velocities'][m_idx]
acc = data.samples['accelerations'][m_idx]
torq = data.samples['torques'][m_idx]
if 'contacts' in data.samples:
for frame in data.samples['contacts'].item(0).keys():
contacts[frame] = data.samples['contacts'].item(0)[frame][m_idx]
if self.opt['identifyGravityParamsOnly']:
#set vel and acc to zero (should be almost zero already) to remove noise
vel[:] = 0.0
acc[:] = 0.0
# system state for iDynTree
q = iDynTree.VectorDynSize.fromList(pos)
dq = iDynTree.VectorDynSize.fromList(vel)
ddq = iDynTree.VectorDynSize.fromList(acc)
# in case that we simulate the torque measurements, need torque estimation for a priori parameters
# or that we need to simulate the base reaction forces for floating base
if self.opt['simulateTorques'] or self.opt['useAPriori'] or self.opt['floatingBase']:
if self.opt['useRBDL']:
#TODO: make sure joint order of torques is the same as iDynTree!
sim_torques = self.simulateDynamicsRBDL(data.samples, m_idx)
else:
sim_torques = self.simulateDynamicsIDynTree(data.samples, m_idx)
if self.opt['useAPriori']:
# torques sometimes contain nans, just a very small C number that gets converted to nan?
torqAP = np.nan_to_num(sim_torques)
if not self.opt['useRegressorForSimulation']:
if self.opt['simulateTorques']:
torq = np.nan_to_num(sim_torques)
else:
# write estimated base forces to measured torq vector from file (usually
# can't be measured so they are simulated from the measured base motion,
# contacts are added further down)
if self.opt['floatingBase']:
if len(torq) < (self.num_dofs + fb):
torq = np.concatenate((np.nan_to_num(sim_torques[0:6]), torq))
else:
torq[0:6] = np.nan_to_num(sim_torques[0:6])
simulate_time += t.interval
#...still looping over measurement samples
row_index = (self.num_dofs+fb)*sample_index # index for current row in stacked matrices
if not only_simulate:
# get numerical regressor (std)
with helpers.Timer() as t:
if self.opt['floatingBase']:
base_vel = data.samples['base_velocity'][m_idx]
base_acc = data.samples['base_acceleration'][m_idx]
# get transform from base to world
rpy = data.samples['base_rpy'][m_idx]
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)
self.generator.setRobotState(q,dq,ddq, world_T_base, base_velocity, base_acceleration,
self.gravity_twist)
else:
self.generator.setRobotState(q,dq,ddq, self.gravity_twist)
# get (standard) regressor
regressor = iDynTree.MatrixDynSize(self.N_OUT, self.num_model_params)
knownTerms = iDynTree.VectorDynSize(self.N_OUT) # what are known terms useable for?
if not self.generator.computeRegressor(regressor, knownTerms):
print("Error during numeric computation of regressor")
regressor = regressor.toNumPy()
if self.opt['floatingBase']:
# the base forces are expressed in the base frame for the regressor, so
# rotate them to world frame (inverse dynamics use world frame)
to_world = world_T_base.getRotation().toNumPy()
regressor[0:3, :] = to_world.dot(regressor[0:3, :])
regressor[3:6, :] = to_world.dot(regressor[3:6, :])
if self.opt['identifyGravityParamsOnly']:
#delete inertia param columns
regressor = np.delete(regressor, self.inertia_params, 1)
if self.opt['identifyFriction']:
# append unitary matrix to regressor for offsets/constant friction
sign = 1 #np.sign(dq.toNumPy()) #TODO: dependent on direction or always constant?
static_diag = np.identity(self.num_dofs)*sign
offset_regressor = np.vstack( (np.zeros((fb, self.num_dofs)), static_diag))
regressor = np.concatenate((regressor, offset_regressor), axis=1)
if not self.opt['identifyGravityParamsOnly']:
if self.opt['identifySymmetricVelFriction']:
# just use velocity directly
vel_diag = np.identity(self.num_dofs)*dq.toNumPy()
friction_regressor = np.vstack( (np.zeros((fb, self.num_dofs)), vel_diag)) # add base dynamics rows
else:
# append positive/negative velocity matrix for velocity dependent asymmetrical friction
dq_p = dq.toNumPy().copy()
dq_p[dq_p < 0] = 0 #set to zero where v < 0
dq_m = dq.toNumPy().copy()
dq_m[dq_m > 0] = 0 #set to zero where v > 0
vel_diag = np.hstack((np.identity(self.num_dofs)*dq_p, np.identity(self.num_dofs)*dq_m))
friction_regressor = np.vstack( (np.zeros((fb, self.num_dofs*2)), vel_diag)) # add base dynamics rows
regressor = np.concatenate((regressor, friction_regressor), axis=1)
# simulate with regressor
if self.opt['useRegressorForSimulation'] and (self.opt['simulateTorques'] or
self.opt['useAPriori'] or self.opt['floatingBase']):
torques = regressor.dot(self.xStdModel[self.identified_params])
if self.opt['simulateTorques']:
torq = torques
else:
# write estimated base forces to measured torq vector from file (usually
# can't be measured so they are simulated from the measured base motion,
# contacts are added further down)
if self.opt['floatingBase']:
if len(torq) < (self.num_dofs + fb):
torq = np.concatenate((np.nan_to_num(torques[0:6]), torq))
else:
torq[0:6] = np.nan_to_num(torques[0:6])
torques_simulated = np.nan_to_num(torques)
# stack on previous regressors
np.copyto(self.regressor_stack[row_index:row_index+self.num_dofs+fb], regressor)
num_time += t.interval
# stack results onto matrices of previous time steps
np.copyto(self.torques_stack[row_index:row_index+self.num_dofs+fb], torq)
if self.opt['useAPriori']:
np.copyto(self.torquesAP_stack[row_index:row_index+self.num_dofs+fb], torqAP)
if len(contacts.keys()):
#convert contact wrenches into torque contribution
for c in range(num_contacts):
frame = list(contacts.keys())[c]
dim = self.num_dofs+fb
# get jacobian and contact wrench for each contact frame and measurement sample
jacobian = iDynTree.MatrixDynSize(6, dim)
if not self.dynComp.getFrameJacobian(str(frame), jacobian):
continue
jacobian = jacobian.toNumPy()
# mul each sample of measured contact wrenches with frame jacobian
contacts_torq = np.empty(dim)
contacts_torq = jacobian.T.dot(contacts[frame])
contact_idx = (sample_index*dim)
np.copyto(self.contacts_stack[c][contact_idx:contact_idx+dim], contacts_torq[-dim:])
# finished looping over samples
# sum over (contact torques) for each contact frame
self.contactForcesSum = np.sum(self.contacts_stack, axis=0)
if self.opt['floatingBase']:
if self.opt['simulateTorques']:
# add measured contact wrench to torque estimation from iDynTree
if self.opt['addContacts']:
self.torques_stack = self.torques_stack + self.contactForcesSum
else:
# if not simulating, measurements of joint torques already contain contact contribution,
# so only add it to the (always simulated) base force estimation
torques_stack_2dim = np.reshape(self.torques_stack, (data.num_used_samples, self.num_dofs+fb))
self.contactForcesSum_2dim = np.reshape(self.contactForcesSum, (data.num_used_samples, self.num_dofs+fb))
if self.opt['addContacts']:
torques_stack_2dim[:, :6] += self.contactForcesSum_2dim[:, :6]
self.torques_stack = torques_stack_2dim.flatten()
if self.opt['addContacts']:
self.sim_torq_stack = self.sim_torq_stack + self.contactForcesSum
if len(contacts.keys()) or self.opt['simulateTorques']:
# write back torques to data object when simulating or contacts were added
self.data.samples['torques'] = np.reshape(self.torques_stack, (data.num_used_samples, self.num_dofs+fb))
with helpers.Timer() as t:
if self.opt['useAPriori']:
# get torque delta to identify with
self.tau = self.torques_stack - self.torquesAP_stack
else:
self.tau = self.torques_stack
simulate_time+=t.interval
self.YStd = self.regressor_stack
# if difference between random regressor (that was used for base projection) and regressor
# from the data is too big, the base regressor can still have linear dependencies.
# for these cases, it seems to be better to get the base columns directly from the data regressor matrix
if not self.opt['useStructuralRegressor'] and not only_simulate:
if self.opt['verbose']:
print('Getting independent base columns again from data regressor')
self.computeRegressorLinDepsQR(self.YStd)
if self.opt['useBasisProjection']:
self.YBase = np.dot(self.YStd, self.B) # project regressor to base regressor
else:
self.YBase = np.dot(self.YStd, self.Pb) # regressor following Sousa, 2014
if self.opt['filterRegressor']:
order = 5 # Filter order
fs = self.data.samples['frequency'] # Sampling freq
fc = self.opt['filterRegCutoff'] # Cut-off frequency (Hz)
b, a = signal.butter(order, fc / (fs / 2), btype='low', analog=False)
for j in range(0, self.num_base_inertial_params):
for i in range(0, self.num_dofs):
self.YBase[i::self.num_dofs, j] = signal.filtfilt(b, a, self.YBase[i::self.num_dofs, j])
self.sample_end = data.samples['positions'].shape[0]
if self.opt['skipSamples'] > 0: self.sample_end -= (self.opt['skipSamples'])
# keep absolute torques (self.tau can be relative)
self.tauMeasured = np.reshape(self.torques_stack, (data.num_used_samples, self.num_dofs+fb))
self.T = data.samples['times'][0:self.sample_end:self.opt['skipSamples']+1]
if self.opt['showTiming']:
print('(simulation for regressors took %.03f sec.)' % simulate_time)
print('(getting regressors took %.03f sec.)' % num_time)
if self.opt['verbose'] == 2:
print("YStd: {}".format(self.YStd.shape), end=' ')
print("YBase: {}, cond: {}".format(self.YBase.shape, la.cond(self.YBase)))
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 __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()
@author: adelpret
"""
import iDynTree
from iDynTree import DynamicsComputations
URDF_FILE = '/home/username/path/robot.urdf'
dynComp = DynamicsComputations()
dynComp.loadRobotModelFromFile(URDF_FILE)
print "The loaded model has", dynComp.getNrOfDegreesOfFreedom(), \
"internal degrees of freedom and",dynComp.getNrOfLinks(),"links."
dofs = dynComp.getNrOfDegreesOfFreedom()
q = iDynTree.VectorDynSize(dofs)
dq = iDynTree.VectorDynSize(dofs)
ddq = iDynTree.VectorDynSize(dofs)
for dof in range(dofs):
# For the sake of the example, we fill the joints vector with gibberish data (remember in any case
# that all quantities are expressed in radians-based units
q.setVal(dof, 1.0)
dq.setVal(dof, 0.4)
ddq.setVal(dof, 0.3)
# The spatial acceleration is a 6d acceleration vector.
# For all 6d quantities, we use the linear-angular serialization
# (the first three value are for the linear quantity, the
# the last three values are for the angular quantity)
gravity = iDynTree.SpatialAcc()
gravity.zero()
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
# TODO : put in the regressor only the relevant parameters params = regrGen.getNrOfParameters() # Get the number of outputs of the regressor # Given that we are considering only the base dynamics # of a subtree, we will have just 6 outputs (3 force, 3 torques) outs = regrGen.getNrOfOutputs() # We can now create the input for the regressor: # joint position, velocities and acceleration # and gravity acceleration at the base frame # (you can get the base link with the regrGen.getBaseLink() method) # If you are unsure of the assumed serialization of the DOFs, you can use # the regrGen.getDescriptionOfDegreesOfFreedom() method qj = iDynTree.VectorDynSize(dof) dqj = iDynTree.VectorDynSize(dof) ddqj = iDynTree.VectorDynSize(dof) gravity = iDynTree.Twist() # Currently the smooth conversion between Matlab and iDynTree vector and # matrices is still a TODO, so for now we have to rely on the setVal/getVal # methods gravity.setVal(0, 0.0) gravity.setVal(1, 0.0) gravity.setVal(2, -9.81) # Here we should fill the q/dqj/ddqj with measured values # Note that we are using the "fixed base" version of setRobotState regrGen.setRobotState(qj, dqj, ddqj, gravity)
def __init__(self, opt, urdf_file, urdf_file_real, measurements_files, regressor_file, validation_file):
# type: (Dict, str, str, str, str, str) -> None
self.opt = opt
# some additional options (experiments)
# in order ot get regressor and base equations, use basis projection matrix. Otherwise use
# permutation from QR directly (Gautier/Sousa method)
self.opt['useBasisProjection'] = 0
# in case projection is used, orthogonalize the basis matrix (SDP estimation seem to work
# more stable that way)
self.opt['orthogonalizeBasis'] = 1
# add regularization term to SDP identification that minimized CAD distance for non-identifiable params
self.opt['useRegressorRegularization'] = 1
self.opt['regularizationFactor'] = 1000.0 #proportion of distance term
# if using fixed base dynamics, remove first link that is the fixed base which should completely
# not be identifiable and not be part of equations (as it does not move)
self.opt['deleteFixedBase'] = 1
# end additional config flags
# load model description and initialize
self.model = Model(self.opt, urdf_file, regressor_file)
# load measurements
self.data = Data(self.opt)
if measurements_files:
self.data.init_from_files(measurements_files)
self.paramHelpers = helpers.ParamHelpers(self.model, self.opt)
self.urdfHelpers = helpers.URDFHelpers(self.paramHelpers, self.model, self.opt)
self.sdp = sdp.SDP(self)
if self.opt['constrainUsingNL']:
from identification.nlopt import NLOPT
self.nlopt = NLOPT(self)
self.tauEstimated = None # type: np._ArrayLike
self.res_error = 100 # last residual error in percent
self.urdf_file_real = urdf_file_real
if self.urdf_file_real:
dc = iDynTree.DynamicsRegressorGenerator()
if not dc.loadRobotAndSensorsModelFromFile(urdf_file_real):
sys.exit()
tmp = iDynTree.VectorDynSize(self.model.num_model_params)
#set regressor, otherwise getModelParameters segfaults
dc.loadRegressorStructureFromString(self.model.regrXml)
dc.getModelParameters(tmp)
self.xStdReal = tmp.toNumPy()
#add some zeros for friction
self.xStdReal = np.concatenate((self.xStdReal, np.zeros(self.model.num_all_params-self.model.num_model_params)))
if self.opt['identifyFriction']:
self.paramHelpers.addFrictionFromURDF(self.model, self.urdf_file_real, self.xStdReal)
self.validation_file = validation_file
progress_inst = helpers.Progress(opt)
self.progress = progress_inst.progress
def _to_idyntree_dyn_vector(array: np.ndarray):
import iDynTree
dyn_vector = iDynTree.VectorDynSize()
dyn_vector = dyn_vector.FromPython(array)
return dyn_vector
def objectiveFunc(self, x, test=False):
self.iter_cnt += 1
print("call #{}/{}".format(self.iter_cnt, self.iter_max))
wf, q, a, b = self.vecToParams(x)
if self.config['verbose']:
print('wf {}'.format(wf))
print('a {}'.format(np.round(a,5).tolist()))
print('b {}'.format(np.round(b,5).tolist()))
print('q {}'.format(np.round(q,5).tolist()))
#input vars out of bounds, skip call
if not self.testBounds(x):
# give penalty obj value for out of bounds (because we shouldn't get here)
# TODO: for some algorithms (with augemented lagrangian added bounds) this should
# not be very high as it is added again anyway)
f = 1000.0
if self.config['minVelocityConstraint']:
g = [10.0]*self.num_constraints
else:
g = [10.0]*self.num_constraints
fail = 1.0
return f, g, fail
self.trajectory.initWithParams(a,b,q, self.nf, wf)
old_verbose = self.config['verbose']
self.config['verbose'] = 0
#old_floatingBase = self.config['floatingBase']
#self.config['floatingBase'] = 0
trajectory_data, data = self.sim_func(self.config, self.trajectory, model=self.model)
self.config['verbose'] = old_verbose
#self.config['floatingBase'] = old_floatingBase
self.last_trajectory_data = trajectory_data
if self.config['showOptimizationTrajs']:
plotter(self.config, data=trajectory_data)
f = np.linalg.cond(self.model.YBase)
#f = np.log(np.linalg.det(self.model.YBase.T.dot(self.model.YBase))) #fisher information matrix
#xBaseModel = np.dot(model.Binv | K, model.xStdModel)
#f = np.linalg.cond(model.YBase.dot(np.diag(xBaseModel))) #weighted with CAD params
f1 = 0
# add constraints (later tested for all: g(n) <= 0)
g = [1e10]*self.num_constraints
jn = self.model.jointNames
for n in range(self.num_dofs):
# check for joint limits
# joint pos lower
print("minma of trajectory",np.min(trajectory_data['positions'][:, n]))
print("maxima of trajectory",np.max(trajectory_data['positions'][:, n]))
if len(self.config['ovrPosLimit'])>n and self.config['ovrPosLimit'][n]:
g[n] = np.deg2rad(self.config['ovrPosLimit'][n][0]) - np.min(trajectory_data['positions'][:, n])
else:
g[n] = self.limits[jn[n]]['lower'] - np.min(trajectory_data['positions'][:, n])
# joint pos upper
if len(self.config['ovrPosLimit'])>n and self.config['ovrPosLimit'][n]:
g[self.num_dofs+n] = np.max(trajectory_data['positions'][:, n]) - np.deg2rad(self.config['ovrPosLimit'][n][1])
else:
g[self.num_dofs+n] = np.max(trajectory_data['positions'][:, n]) - self.limits[jn[n]]['upper']
# max joint vel
#g[2*self.num_dofs+n] = np.max(np.abs(trajectory_data['velocities'][:, n])) - self.limits[jn[n]]['velocity']
# max torques
# g[3*self.num_dofs+n] = np.nanmax(np.abs(data.samples['torques'][:, n])) - self.limits[jn[n]]['torque']
'''if self.config['minVelocityConstraint']:
# max joint vel of trajectory should at least be 10% of joint limit
g[3*self.num_dofs+n] = self.limits[jn[n]]['velocity']*self.config['minVelocityPercentage'] - \
np.max(np.abs(trajectory_data['velocities'][:, n]))'''
# highest joint torque should at least be 10% of joint limit
'''g[5*self.num_dofs+n] = self.limits[jn[n]]['torque']*0.1 - np.max(np.abs(data.samples['torques'][:, n]))
f_tmp = self.limits[jn[n]]['torque']*0.1 - np.max(np.abs(data.samples['torques'][:, n]))
if f_tmp > 0:
f1+=f_tmp'''
print(g)
# check collision constraints
# (for whole trajectory but only get closest distance as constraint value)
c_s = self.num_constraints - self.num_coll_constraints # start where collision constraints start
traversal = iDynTree.Traversal()
success = self.idyn_model.computeFullTreeTraversal(traversal)
rotation = iDynTree.Rotation(1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0)
position = iDynTree.Position(0.0, 0.0, 0.0)
worldHbase = iDynTree.Transform(rotation, position)
jointPosition = iDynTree.VectorDynSize(5)
if self.config['verbose'] > 1:
print('checking collisions')
for p in range(0, trajectory_data['positions'].shape[0], 10):
g_cnt1 = 0
g_cnt2 = 1
if self.config['verbose'] > 1:
print("Sample {}".format(p))
q = trajectory_data['positions'][p]
for i in range(len(q)):
jointPosition[i] = q[i]
linkPositions = iDynTree.LinkPositions(self.idyn_model)
iDynTree.ForwardPositionKinematics(self.idyn_model, traversal, worldHbase,jointPosition, linkPositions)
pose_link5 = linkPositions(5)
z = pose_link5.getPosition().getVal(2)
origin = (0, 0 , 0)
end_eff = (pose_link5.getPosition().getVal(0), pose_link5.getPosition().getVal(1), (pose_link5.getPosition().getVal(2)))
dist = distance.euclidean(origin, end_eff)
if(z < 0.35 ):
if (z < g[c_s+g_cnt1]):
g[c_s+g_cnt1] = z
if(dist < 0.25):
if (dist < g[c_s+g_cnt2]):
g[c_s+g_cnt2] = dist
'''for l0 in range(self.model.num_links + len(self.world_links)):
for l1 in range(self.model.num_links + len(self.world_links)):
l0_name = (self.model.linkNames + self.world_links)[l0]
l1_name = (self.model.linkNames + self.world_links)[l1]
if (l0 >= l1): # don't need, distance is the same in both directions; same link never collides
continue
if l0_name in self.config['ignoreLinksForCollision'] \
or l1_name in self.config['ignoreLinksForCollision']:
continue
if [l0_name, l1_name] in self.config['ignoreLinkPairsForCollision'] or \
[l1_name, l0_name] in self.config['ignoreLinkPairsForCollision']:
continue
# neighbors can't collide with a proper joint range, so ignore--
if l0 < self.model.num_links and l1 < self.model.num_links:
if l0_name in self.neighbors[l1_name]['links'] or l1_name in self.neighbors[l0_name]['links']:
continue
if l0 < l1:
d = self.getLinkDistance(l0_name, l1_name, q)
if d < g[c_s+g_cnt]:
print("l0: {} l1: {}".format(l0,l1))
g[c_s+g_cnt] = d
g_cnt += 1'''
self.last_g = g
#add min join torques as second objective
if f1 > 0:
f+= f1
print("added cost: {}".format(f1))
c = self.testConstraints(g)
if c:
print(Fore.GREEN, end=' ')
else:
print(Fore.YELLOW, end=' ')
print("objective function value: {} (last best: {} + {})".format(f,
self.last_best_f-self.last_best_f_f1,
self.last_best_f_f1), end=' ')
print(Fore.RESET)
if self.config['verbose']:
if self.opt_prob.is_gradient:
print("(Gradient evaluation)")
if self.mpi_rank == 0 and not self.opt_prob.is_gradient and self.config['showOptimizationGraph']:
self.xar.append(self.iter_cnt)
self.yar.append(f)
self.x_constr.append(c)
self.updateGraph()
self.showVisualizerTrajectory(self.trajectory)
#keep last best solution (some solvers don't keep it)
if c and f < self.last_best_f:
self.last_best_f = f
self.last_best_f_f1 = f1
self.last_best_sol = x
print("\n\n")
fail = 0.0
#funcs = {}
#funcs['opt'] = f
#funcs['con'] = g
#return funcs, fail
return f, g, fail
from identification import helpers
if __name__ == '__main__':
#urdf_file = '../model/centauro.urdf'
urdf_file = '../model/walkman_orig.urdf'
dynamics = iDynTree.DynamicsComputations()
dynamics.loadRobotModelFromFile(urdf_file)
dynamics.setFloatingBase('LFoot')
n_dofs = dynamics.getNrOfDegreesOfFreedom()
jointNames = []
for i in range(n_dofs):
jointNames.append(dynamics.getJointName(i))
limits = helpers.URDFHelpers.getJointLimits(urdf_file, use_deg=False)
#for each joint, sweep through all possible joint angles and get mass matrix
q = iDynTree.VectorDynSize(n_dofs)
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)