def main():
# save either optimized or random trajectory parameters to filename
if args.filename:
traj_file = args.filename
else:
traj_file = config['urdf'] + '.trajectory.npz'
if config['optimizeTrajectory']:
# find trajectory params by optimization
old_sim = config['simulateTorques']
config['simulateTorques'] = True
model = Model(config, config['urdf'])
if config['useStaticTrajectories']:
old_gravity = config['identifyGravityParamsOnly']
idf = Identification(config, config['urdf'], config['urdf_real'], measurements_files=None,
regressor_file=None, validation_file=None)
trajectoryOptimizer = PostureOptimizer(config, idf, model, simulation_func=simulateTrajectory, world=args.world)
config['identifyGravityParamsOnly'] = old_gravity
else:
idf = Identification(config, config['urdf'], urdf_file_real=None, measurements_files=None,
regressor_file=None, validation_file=None)
trajectoryOptimizer = TrajectoryOptimizer(config, idf, model, simulation_func=simulateTrajectory, world=args.world)
trajectory = trajectoryOptimizer.optimizeTrajectory()
config['simulateTorques'] = old_sim
else:
# use some random params
print("no optimized trajectory found, generating random one")
trajectory = PulsedTrajectory(config['num_dofs'], use_deg=config['useDeg']).initWithRandomParams()
print("a {}".format([t_a.tolist() for t_a in trajectory.a]))
print("b {}".format([t_b.tolist() for t_b in trajectory.b]))
print("q {}".format(trajectory.q.tolist()))
print("nf {}".format(trajectory.nf.tolist()))
print("wf {}".format(trajectory.w_f_global))
print("Saving found trajectory to {}".format(traj_file))
if config['useStaticTrajectories']:
# always saved with rad angles
np.savez(traj_file, static=True, angles=trajectory.angles)
else:
# TODO: remove degrees option
np.savez(traj_file, use_deg=trajectory.use_deg, static=False, a=trajectory.a, b=trajectory.b,
q=trajectory.q, nf=trajectory.nf, wf=trajectory.w_f_global)
config = yaml.load(stream)
except yaml.YAMLError as exc:
print(exc)
import iDynTree
iDynTree.init_helpers()
iDynTree.init_numpy_helpers()
dynComp = iDynTree.DynamicsComputations()
dynComp.loadRobotModelFromFile(args.model)
world_gravity = iDynTree.SpatialAcc.fromList([0, 0, -9.81, 0, 0, 0])
n_dof = dynComp.getNrOfDegreesOfFreedom()
config['num_dofs'] = n_dof
config['urdf'] = args.model
g_model = Model(config,
args.model,
regressor_file=None,
regressor_init=False)
linkNames = g_model.linkNames
# get bounding boxes for model
from identification.helpers import URDFHelpers, ParamHelpers
paramHelpers = ParamHelpers(None, config)
urdfHelpers = URDFHelpers(paramHelpers, None, config)
link_cuboid_hulls = {} # type: Dict[str, Tuple[List, List, List]]
for i in range(len(linkNames)):
link_name = linkNames[i]
box, pos, rot = urdfHelpers.getBoundingBox(input_urdf=args.model,
old_com=[0, 0, 0],
link_name=link_name,
scaling=False)
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'],
velocities_raw=out['velocities'],
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 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
