'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'))
start = Time.now()
integrator.run(int_factor,
1200,
logging=False,
real_time=False,
show_progress=True)
total_dur.append((Time.now() - start).to_sec())
n_iter.append(integrator.current_iteration + 1)
except Exception as e:
print(f'Exception during integration:\n{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('{}/{}_sandbox_{}_plots.png'.format(plot_dir, robot, part))
if PANDA_LOGGING:
rec_w, rec_b, rec_c, recs = convert_qp_builder_log(
integrator.qp_builder)
rec_c.title = 'Joint Velocity Commands'
rec_c.data = {
s.split(SPS)[1]: d
for s, d in rec_c.data.items()
if s.split(SPS)[1] in start_poses['pr2']
}
print(rec_c.data)
color_gen = ColorGenerator(v=1.0, s_lb=0.75)
rec_c.colors = {
s: color_gen.get_color_hex()
for s in rec_c.data.keys()
}
rec_c.data_lim = {}
rec_c.compute_limits()
rec_c.set_xtitle('Iteration')