Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
def write(self, message):
self.terminal.write(message)
self.log += message
def flush(self):
self.terminal.flush()
sys.stdout = Logger() # type: ignore
logger = sys.stdout
#for ipython, reset this with
#import sys
#sys.stdout = sys.__stdout__
idf = Identification(config, args.model, args.model_real, args.measurements, args.regressor, args.validation)
if idf.opt['selectBlocksFromMeasurements']:
idf.opt['selectingBlocks'] = 1
old_essential_option = idf.opt['useEssentialParams']
idf.opt['useEssentialParams'] = 0
old_feasible_option = idf.opt['constrainToConsistent']
idf.opt['constrainToConsistent'] = 0
# loop over input blocks and select good ones
while 1:
idf.estimateParameters()
idf.data.getBlockStats(idf.model)
idf.estimateRegressorTorques()
oc = OutputConsole(idf)
oc.render(summary_only=True)
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]))
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))