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
with open(args.config, 'r') as stream:
try:
config = yaml.load(stream)
except yaml.YAMLError as exc:
print(exc)
config['useDeg'] = 0
config['model'] = args.model
if args.robot == 'centauro':
out = readCentauroCSV(args.measurements, config, args.plot)
sampleTime = np.mean(np.diff(out['times']))
elif args.robot == 'walkman':
sampleTime = 0.005 #200 Hz
out = readWalkmanCSV(args.measurements, config, args.plot)
out['frequency'] = 1.0 / sampleTime
data = Data(config)
#init empty arrays for preprocess
out['torques_raw'] = np.zeros_like(out['torques'])
#filter, diff, integrate
if args.robot == 'walkman':
out['IMUlinVel'] = np.zeros(
(out['times'].shape[0], 3)) #IMU linear velocity
out['IMUrotAcc'] = np.zeros(
(out['times'].shape[0], 3)) #IMU rotational acceleration
data.preprocess(Q=out['positions'],
V=out['velocities'],
Vdot=out['accelerations'],
Tau=out['torques'],
Tau_raw=out['torques_raw'],
def simulateTrajectory(config, trajectory, model=None, measurements=None):
# type: (Dict, Trajectory, Model, np._ArrayLike) -> Tuple[Dict, Data]
# generate data arrays for simulation and regressor building
old_sim = config['simulateTorques']
config['simulateTorques'] = True
if config['floatingBase']: fb = 6
else: fb = 0
if not model:
if 'urdf_real' in config and config['urdf_real']:
print('Simulating using "real" model parameters.')
urdf = config['urdf_real']
else:
urdf = config['urdf']
model = Model(config, urdf)
data = Data(config)
trajectory_data = {} # type: Dict[str, Union[List, np._ArrayLike]]
trajectory_data['target_positions'] = []
trajectory_data['target_velocities'] = []
trajectory_data['target_accelerations'] = []
trajectory_data['torques'] = []
trajectory_data['times'] = []
freq = config['excitationFrequency']
#f= open("trajectory3.txt","a+")
for t in range(0, int(trajectory.getPeriodLength() * freq)):
trajectory.setTime(t / freq)
q = np.array(
[trajectory.getAngle(d) for d in range(config['num_dofs'])])
if config['useDeg']:
q = np.deg2rad(q)
trajectory_data['target_positions'].append(q)
#f.write("%s\n" % q)
qdot = np.array(
[trajectory.getVelocity(d) for d in range(config['num_dofs'])])
if config['useDeg']:
qdot = np.deg2rad(qdot)
trajectory_data['target_velocities'].append(qdot)
qddot = np.array(
[trajectory.getAcceleration(d) for d in range(config['num_dofs'])])
if config['useDeg']:
qddot = np.deg2rad(qddot)
trajectory_data['target_accelerations'].append(qddot)
trajectory_data['times'].append(t / freq)
trajectory_data['torques'].append(np.zeros(config['num_dofs'] + fb))
#f.write("next iteration \n")
#f.close()
num_samples = len(trajectory_data['times'])
#convert lists to numpy arrays
trajectory_data['target_positions'] = np.array(
trajectory_data['target_positions'])
trajectory_data['positions'] = trajectory_data['target_positions']
trajectory_data['target_velocities'] = np.array(
trajectory_data['target_velocities'])
trajectory_data['velocities'] = trajectory_data['target_velocities']
trajectory_data['target_accelerations'] = np.array(
trajectory_data['target_accelerations'])
trajectory_data['accelerations'] = trajectory_data['target_accelerations']
trajectory_data['torques'] = np.array(trajectory_data['torques'])
trajectory_data['times'] = np.array(trajectory_data['times'])
trajectory_data['measured_frequency'] = freq
trajectory_data['base_velocity'] = np.zeros((num_samples, 6))
trajectory_data['base_acceleration'] = np.zeros((num_samples, 6))
trajectory_data['base_rpy'] = np.zeros((num_samples, 3))
# add static contact force
contacts = 1
if config['floatingBase'] and contacts:
contactFrame = 'contact_ft'
# get base acceleration that results from acceleration at contact frame
#contact_wrench = np.zeros(6)
#contact_wrench[2] = 9.81 * 3.0 # -g * mass
contact_wrench = np.random.rand(6) * 10
trajectory_data['base_rpy'] = np.random.rand(
(3 * num_samples)).reshape((num_samples, 3)) * 0.5
"""
contact_wrench[2] = 9.81 # * 139.122814
len_contact = la.norm(contact_wrench[0:3])
# get vector from contact frame to robot center of mass
model_com = iDynTree.Position()
model_com = model.dynComp.getWorldTransform(contactFrame).inverse()*model.dynComp.getCenterOfMass()
model_com = model_com.toNumPy()
# rotate contact wrench to be in line with COM (so that base link is not rotationally accelerated)
contact_wrench[0:3] = (-model_com) / la.norm(model_com) * len_contact
# rotate base accordingly (ore rather the whole robot) so gravity is parallel to contact force
a_xz = np.array([0,9.81]) #xz of non-rotated contact force
b_xz = contact_wrench[[0,2]] #rotated contact force vec projected to xz
pitch = np.arccos( (a_xz.dot(b_xz))/(la.norm(a_xz)*la.norm(b_xz)) ) #angle in xz layer
a_yz = np.array([0,9.81]) #yz of non-rotated contact force
b_yz = contact_wrench[[1,2]] #rotated contact force vec projected to yz
roll = np.arccos( (a_yz.dot(b_yz))/(la.norm(a_yz)*la.norm(b_yz)) ) #angle in yz layer
yaw = 0
trajectory_data['base_rpy'][:] += np.array([roll, pitch, yaw])
"""
trajectory_data['contacts'] = np.array(
{contactFrame: np.tile(contact_wrench, (num_samples, 1))})
else:
#TODO: add proper simulated contacts (from e.g. gazebo) for floating-base
trajectory_data['contacts'] = np.array({})
if measurements:
trajectory_data['positions'] = measurements['Q']
trajectory_data['velocities'] = measurements['V']
trajectory_data['accelerations'] = measurements['Vdot']
trajectory_data['measured_frequency'] = measurements[
'measured_frequency']
old_skip = config['skipSamples']
config['skipSamples'] = 0
old_offset = config['startOffset']
config['startOffset'] = 0
data.init_from_data(trajectory_data)
model.computeRegressors(data)
trajectory_data['torques'][:, :] = data.samples['torques'][:, :]
'''
if config['floatingBase']:
# add force of contact to keep robot fixed in space (always accelerate exactly against gravity)
# floating base orientation has to be rotated so that accelerations resulting from hanging
# are zero, i.e. the vector COM - contact point is parallel to gravity.
if contacts:
# get jacobian of contact frame at current posture
dim = model.num_dofs+fb
jacobian = iDynTree.MatrixDynSize(6, dim)
model.dynComp.getFrameJacobian(contactFrame, jacobian)
jacobian = jacobian.toNumPy()
# get base link vel and acc and torques that result from contact force / acceleration
contacts_torq = np.zeros(dim)
contacts_torq = jacobian.T.dot(contact_wrench)
trajectory_data['base_acceleration'] += contacts_torq[0:6] # / 139.122
data.samples['base_acceleration'][:,:] = trajectory_data['base_acceleration'][:,:]
# simulate again with proper base acceleration
model.computeRegressors(data, only_simulate=True)
trajectory_data['torques'][:,:] = data.samples['torques'][:,:]
'''
config['skipSamples'] = old_skip
config['startOffset'] = old_offset
config['simulateTorques'] = old_sim
return trajectory_data, data
