def _to_idyntree_transform(position: np.ndarray, quaternion: np.ndarray):
p = InverseKinematicsNLP._to_idyntree_position(position=position)
R = InverseKinematicsNLP._to_idyntree_rotation(quaternion=quaternion)
import iDynTree
H = iDynTree.Transform()
H.setPosition(p)
H.setRotation(R)
return H
def randomTransform(self):
''' Return a random Transform '''
res = iDynTree.Transform()
res.setPosition(
iDynTree.Position(random.uniform(-10, 10), random.uniform(-10, 10),
random.uniform(-10, 10)))
res.setRotation(
iDynTree.Rotation.RPY(random.uniform(-10, 10),
random.uniform(-10, 10),
random.uniform(-10, 10)))
return res
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 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
Created on Tue Jun 23 11:35:46 2015
@author: adelpret
"""
import iDynTree
from iDynTree import KinDynComputations
URDF_FILE = '/home/letrend/workspace/roboy_control/src/CARDSflow/robots/msj_platform/model.urdf'
kinDyn = KinDynComputations()
kinDyn.loadRobotModelFromFile(URDF_FILE)
print "The loaded model has", kinDyn.getNrOfDegreesOfFreedom(), \
"internal degrees of freedom and",kinDyn.getNrOfLinks(),"links."
base = iDynTree.Transform()
vel = iDynTree.Twist()
dofs = kinDyn.getNrOfDegreesOfFreedom()
q = iDynTree.VectorDynSize(dofs)
dq = 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)
# 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.Vector3()
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
#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)
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