'distance': dist.expr,
'gaze_align': look_goal.expr,
'location_x': roomba_joint.x_pos,
'location_y': roomba_joint.y_pos,
'location_z': roomba_joint.z_pos,
'rotation_a': roomba_joint.a_pos
})
# RUN
int_factor = 0.02
integrator.restart('Fetch Cartesian Goal Example')
integrator.run(int_factor)
# DRAW
draw_recorders([integrator.recorder] +
split_recorders([integrator.sym_recorder]), 4.0 / 9.0, 8,
4).savefig('{}/fetch_sandbox_plots.png'.format(plot_dir))
trajectory = {
Path(j)[-1][:-2]: [0.0] * len(integrator.recorder.data.values()[0])
for j in joint_symbols
}
trajectory.update({
Path(se.Symbol(j))[-1][:-2]: d
for j, d in integrator.recorder.data.items()
})
base_trajectory = list(
zip(integrator.sym_recorder.data['location_x'],
integrator.sym_recorder.data['location_y'],
integrator.sym_recorder.data['location_z'],
integrator.sym_recorder.data['rotation_a']))
'l_forearm_roll_joint': 0.0,
'l_wrist_flex_joint': -1.05,
'r_elbow_flex_joint': -2.1213,
'r_shoulder_lift_joint': 1.2963,
'r_shoulder_pan_joint': 0.0,
'r_upper_arm_roll_joint': 0.0,
'r_forearm_roll_joint': 0.0,
'r_wrist_flex_joint': -1.05,
'torso_lift_joint': 0.16825
}
closer = ObsessiveObjectCloser(
Path('iai_oven_area'),
Path('pr2/links/r_gripper_r_finger_tip_link'),
Path('pr2/links/head_mount_link'),
'/tracked/state',
'/pr2',
resting_pose=resting_pose)
while not rospy.is_shutdown():
rospy.sleep(1000)
if closer.pushing_controller is not None and PANDA_LOGGING:
recW, recB, recC, recs = convert_qp_builder_log(
closer.pushing_controller)
draw_recorders([recW, recB, recC] + [
r for _, r in sorted(recs.items())
], 1, 12, 6).savefig(
res_pkg_path(
'package://kineverse_experiment_world/test/keep_contact.png'))
'is_unlocked': is_unlocked,
'handle_position': handle_position,
'door_position': door_position
},
printed_exprs={'is_unlocked': is_unlocked})
try:
integrator.restart(title='Door opening generator')
integrator.run(dt=0.05,
max_iterations=500,
logging=True,
show_progress=False,
real_time=True)
draw_recorders([integrator.sym_recorder], 2, 8, 4).savefig(
res_pkg_path(
'package://kineverse_experiment_world/plots/door_opening_terms.png'
))
if PANDA_LOGGING:
rec_w, rec_b, rec_c, recs = convert_qp_builder_log(
integrator.qp_builder)
draw_recorders([rec_b, rec_c] + [
r for _, r in sorted(recs.items())
], 1, 8, 4).savefig(
res_pkg_path(
'package://kineverse_experiment_world/plots/door_opening.png'
))
except Exception as e:
traceback.print_exception(type(e), e, e.__traceback__)
print(f'Motion generation crashed:\n{e}')
rec_iter.data = {
iter_columns[0]: df_performance[iter_columns[0]].to_list()
}
rec_iter.data[iter_columns[1]] = (df_performance[iter_columns[1]] *
1000).to_list()
rec_iter.compute_limits()
rec_iter.set_grid(True)
# rec_iter.set_xspace(-0.1, df_performance['DoF'].max() + 0.1)
rec_iter.set_ytitle('n iterations | ms')
rec_iter.set_xtitle('Degrees of Freedom')
rec_iter.set_xlabels([str(int(x)) for x in df_performance['DoF']])
columns = ['Mean Error', 'SD Error', 'Min Error', 'Max Error']
rec_err = ValueRecorder('Error Evaluation', *columns)
rec_err.data = {x: df_error[x].to_list() for x in columns}
rec_err.compute_limits()
rec_err.set_grid(True)
rec_err.set_xspace(-0.1, len(df_error) - 0.9)
rec_err.set_xlabels([
'{:.3f} m linear $\\sigma$ \n{:.3f} rad angular $\\sigma$'.format(
x, y)
for x, y in zip(df_error['Linear SD'], df_error['Angular SD'])
])
rec_err.set_marker('.')
# rec_err.set_xlabels([]) # ['{:.3f} m linear $\\sigma$ \n{:.3f} rad angular $\\sigma$'.format(x, y) for x, y in df.iloc[:,:2].to_numpy()])
#rec_iter.set_xlabels(['{:.3f} m linear $\\sigma$ \n{:.3f} rad angular $\\sigma$'.format(x, y) for x, y in df.iloc[:,:2].to_numpy()])
draw_recorders([rec_err, rec_iter], 1.0, 8,
3).savefig('{}.png'.format(args[0][:-4]))
matrix = matrix.col_join(f)
x_labels.append(len(matrix.free_symbols))
cython_m = llvm.speed_up(matrix, matrix.free_symbols)
avg_d = 0.0
avg_l = 0.0
for i in range(iterations):
state = {s: random.random() for s in matrix.free_symbols}
start = time.time()
matrix.subs(state)
avg_d += time.time() - start
start = time.time()
cython_m(**{str(s): v for s, v in state.items()})
avg_l += time.time() - start
rec_d.log_data('direct', avg_d / iterations)
rec_d.log_data('llvm', avg_l / iterations)
# rec_d.set_xspace(x_labels[0], x_labels[-1])
rec_d.set_xspace(-5, 105)
rec_d.set_xtitle('Number of Transformation Matrices')
rec_d.set_ytitle('Time (s)')
draw_recorders([rec_d], ratio=1, plot_width=8, plot_height=4).savefig('llvm_speedtest.png')
controlled_symbols.union({button_position}))
print('----\n{}'.format('\n'.join([str(x) for x in constraints.values()])))
cvs, constraints = generate_controlled_values(constraints,
controlled_symbols)
print('Retrieved Constraints:\n{}'.format('\n'.join(
['{}:\n {}'.format(k, c) for k, c in constraints.items()])))
integrator = CommandIntegrator(TQPB(
constraints, {'goal': SC(-0.1, -0.1, 1, button_position)}, cvs),
start_state={button_position: 0},
recorded_terms={
'lock_lower': lock_constraint.lower,
'lock_upper': lock_constraint.upper,
'spring_lower': spring_constraint.lower,
})
integrator.restart('Door spring')
integrator.run(dt=0.05)
integrator.recorder.add_threshold('Door release', point_of_release, 'b')
plot_dir = res_pkg_path('package://kineverse/test/plots')
draw_recorders([integrator.recorder] +
split_recorders([integrator.sym_recorder]), 4.0 / 9.0, 8,
4).savefig('{}/microwave_spring.png'.format(plot_dir))
traj_vis.visualize(integrator.recorder.data)
traj_vis.shutdown()
rec_err = ValueRecorder('Error Evaluation', *columns)
rec_err.data = {
x: v
for x, v in zip(columns,
np.vstack(estimate_data_points).T)
}
rec_err.compute_limits()
rec_err.set_grid(True)
rec_err.set_xspace(-0.1, len(estimate_data_points) - 0.9)
rec_err.set_xlabels(noise_labels)
rec_err.set_marker('.')
np_p = df_s.to_numpy() * 1000
iter_columns = ['Mean Duration',
'SD Duration'] # , 'Min Duration', 'Max Duration']
rec_iter = ValueRecorder('Iterations', *iter_columns)
rec_iter.data = {
iter_columns[0]: np_p.mean(axis=0),
iter_columns[1]: np_p.std(axis=0),
}
# iter_columns[2]: np_p.min(axis=0),
# iter_columns[3]: np_p.max(axis=0)}
rec_iter.compute_limits()
rec_iter.set_grid(True)
# rec_iter.set_xspace(-0.1, df_performance['DoF'].max() + 0.1)
rec_iter.set_ytitle('ms')
rec_iter.set_xtitle('Degrees of Freedom')
rec_iter.set_xlabels([str(int(x)) for x in range(1, np_p.shape[1] + 1)])
draw_recorders([rec_err, rec_iter], 1.0, 8, 3).savefig(args.output)
) in symbolic_types and u_const_name not in generated_constraints:
new_goals[u_const_name] = SoftConstraint(
less_than(c.upper, 0), 1000, goal.weight, c.upper)
generated_constraints.add(u_const_name)
elif diff_sign[s] < 0 and type(
c.lower
) in symbolic_types and l_const_name not in generated_constraints:
new_goals[l_const_name] = SoftConstraint(
-1000, -greater_than(c.lower, 0), goal.weight,
c.lower)
generated_constraints.add(l_const_name)
final_goals.update(
lock_explorer(km, state, new_goals, generated_constraints))
return final_goals
if __name__ == '__main__':
scenario = LockboxOpeningGenerator()
scenario.run(0.19)
draw_recorders([
scenario.value_recorder, scenario.vel_recorder,
scenario.symbol_recorder
], 4.0 / 9.0, 10, 3).savefig(
res_pkg_path(
'package://kineverse_experiment_world/test/plots/lockbox_opening.png'
))
# RUN
int_factor = 0.1
integrator.restart('Fetch Cartesian Goal Example')
# print('\n'.join(['{}: {}'.format(k, v) for k, v in integrator.integration_rules.items()]))
# exit(0)
try:
integrator.run(int_factor, 500)
except Exception as e:
print(e)
# DRAW
print('Drawing recorders')
draw_recorders([
filter_contact_symbols(integrator.recorder, integrator.qp_builder),
integrator.sym_recorder
], 4.0 / 9.0, 8, 4).savefig('{}/fetch_sandbox_{}_plots.png'.format(
plot_dir, part))
if PANDA_LOGGING:
rec_w, rec_b, rec_c, recs = convert_qp_builder_log(
integrator.qp_builder)
draw_recorders(
[rec_b, rec_c] + [r for _, r in sorted(recs.items())], 1, 8,
4).savefig('{}/fetch_sandbox_{}_constraints.png'.format(
plot_dir, part))
if False:
traj_vis.visualize(integrator.recorder.data, hz=50)
pass
traj_vis.shutdown()