def callback(request):
global HANDLES
HANDLES = []
xyzs, _ = pc2xyzrgb(request.point_cloud)
new_mins = xyzs.reshape(-1, 3).min(axis=0)
new_maxes = xyzs.reshape(-1, 3).max(axis=0)
new_obj_xyz = (new_mins + new_maxes) / 2
new_obj_dim = new_maxes - new_mins
f.fit(np.atleast_2d(obj_xyz), np.atleast_2d(new_obj_xyz), 0.001, 0.001)
new_xyzs, new_mats = f.transform_frames(old_xyzs, conversions.quats2mats(old_quats))
new_quats = conversions.mats2quats(new_mats)
show_object_and_trajectory(new_xyzs, new_obj_xyz, obj_quat, new_obj_dim, NEW_ID)
show_object_and_trajectory(xyzquat[:, :3], obj_xyz, obj_quat, obj_dim, OLD_ID)
Globals.pr2.update_rave()
joint_positions, inds = trajectories.make_joint_traj(
new_xyzs, new_quats, Globals.pr2.rarm.manip, "base_link", "r_wrist_roll_link", filter_options=18
)
response = PushResponse()
if joint_positions is not None:
Globals.pr2.rarm.goto_joint_positions(joint_positions[0])
# current_joints = Globals.pr2.rarm.get_joint_positions()[None,:]
joint_positions, joint_velocities, times = retiming.make_traj_with_limits(
joint_positions, Globals.pr2.rarm.vel_limits, Globals.pr2.rarm.acc_limits, smooth=True
)
# joint_velocities = kinematics_utils.get_velocities(joint_positions, times, tol=.001)
Globals.pr2.rarm.follow_timed_joint_trajectory(joint_positions, joint_velocities, times)
response.success = True
else:
response.success = False
return response
def follow_joint_trajectory(self, positions):
positions = np.r_[np.atleast_2d(self.get_joint_positions()), positions]
positions[:,4] = np.unwrap(positions[:,4])
positions[:,6] = np.unwrap(positions[:,6])
positions, velocities, times = make_traj_with_limits(positions, self.vel_limits, self.acc_limits,smooth=True)
self.follow_timed_joint_trajectory(positions, velocities, times)
