def simulateTrajectory(config, trajectory, model=None, measurements=None):
# generate data arrays for simulation and regressor building
old_sim = config['simulateTorques']
config['simulateTorques'] = True
if not model:
model = Model(config, config['model'])
data = Data(config)
trajectory_data = {}
trajectory_data['target_positions'] = []
trajectory_data['target_velocities'] = []
trajectory_data['target_accelerations'] = []
trajectory_data['torques'] = []
trajectory_data['times'] = []
freq=200.0
for t in range(0, int(trajectory.getPeriodLength()*freq)):
trajectory.setTime(t/freq)
q = [trajectory.getAngle(d) for d in range(config['N_DOFS'])]
q = np.array(q)
if config['useDeg']:
q = np.deg2rad(q)
trajectory_data['target_positions'].append(q)
qdot = [trajectory.getVelocity(d) for d in range(config['N_DOFS'])]
qdot = np.array(qdot)
if config['useDeg']:
qdot = np.deg2rad(qdot)
trajectory_data['target_velocities'].append(qdot)
qddot = [trajectory.getAcceleration(d) for d in range(config['N_DOFS'])]
qddot = np.array(qddot)
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['N_DOFS']))
num_samples = len(trajectory_data['times'])
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) )
trajectory_data['contacts'] = np.array({'dummy_sim': np.zeros( num_samples )})
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['skip_samples']
config['skip_samples'] = 0
old_offset = config['start_offset']
config['start_offset'] = 0
data.init_from_data(trajectory_data)
model.computeRegressors(data) #TODO: this needlessly also computes regressors in addition to simulation
config['skip_samples'] = old_skip
config['start_offset'] = old_offset
config['simulateTorques'] = old_sim
return trajectory_data, data, model
Vdot=out['accelerations'],
Tau=out['torques'],
Tau_raw=out['torques_raw'],
T=out['times'],
Fs=out['frequency'])
#simulate with iDynTree if we're using gazebo data
if is_gazebo:
#use all data
old_skip = config['skipSamples']
config['skipSamples'] = 0
old_offset = config['startOffset']
config['startOffset'] = 0
old_sim = config['simulateTorques']
config['simulateTorques'] = 1
data.init_from_data(out)
model = Model(config, config['model'], args.regressor)
if config['verbose']:
print('simulating torques for motion data')
model.computeRegressors(data)
out['torques'] = out['torques_raw'] = model.data.samples['torques']
config['skipSamples'] = old_skip
config['startOffset'] = old_offset
config['simulateTorques'] = old_sim
if config['floatingBase']:
np.savez(args.outfile,
positions=out['positions'],
positions_raw=out['positions'],
target_positions=out['target_positions'],
velocities=out['velocities'],
out['base_velocity'] = np.hstack( (out['IMUlinVel'], out['IMUrotVel']) )
out['base_acceleration'] = np.hstack( (out['IMUlinAcc'], out['IMUrotAcc']) )
out['contacts'] = np.array({'r_leg_ft': out['FTright']})
out['base_rpy'] = out['IMUrpy']
#simulate with iDynTree if we're using gazebo data
if not is_hw:
#use all data
old_skip = config['skip_samples']
config['skip_samples'] = 0
old_offset = config['start_offset']
config['start_offset'] = 0
old_sim = config['simulateTorques']
config['simulateTorques'] = 1
data.init_from_data(out)
model = Model(config, config['model'])
model.computeRegressors(data)
out['torques'] = out['torques_raw'] = model.data.samples['torques']
config['skip_samples'] = old_skip
config['start_offset'] = old_offset
config['simulateTorques'] = old_sim
np.savez(args.outfile, positions=out['positions'], positions_raw=out['positions'],
velocities=out['velocities'], velocities_raw=out['velocities'],
accelerations=out['accelerations'], torques=out['torques'],
torques_raw=out['torques_raw'], base_velocity=out['base_velocity'],
base_acceleration=out['base_acceleration'], base_rpy=out['base_rpy'],
contacts=out['contacts'], times=out['times'], frequency=out['frequency'])
print("Saved csv data as {}".format(args.outfile))
print("Samples: {}, Time: {}s, Frequency: {} Hz".format(out['times'].shape[0], out['times'][-1], out['frequency']))
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
