ps.header.frame_id = 'base_link'
rviz.draw_marker(
ps,
id=1,
type=Marker.CUBE,
rgba = (0,1,0,1),
scale = asarray(traj["object_dimension"]))
pose_array = conversions.array_to_pose_array(asarray(traj["gripper_positions"]), 'base_link')
rviz.draw_curve(
pose_array,
id=0)
n_waypoints = 20
xyzquat = np.c_[traj["gripper_positions"],traj["gripper_orientations"]]
xyzquat_rs = kinematics_utils.unif_resample(xyzquat, n_waypoints, weights = np.ones(7), tol=.001)
times = np.linspace(0,10,n_waypoints)
pr2.torso.go_up()
pr2.join_all()
pr2.update_rave()
joint_positions,_ = trajectories.make_joint_traj(xyzquat_rs[:,0:3], xyzquat_rs[:,3:7], pr2.rarm.manip, 'base_link', 'r_wrist_roll_link', filter_options = 18)
joint_velocities = kinematics_utils.get_velocities(joint_positions, times, tol=.001)
pr2.rarm.follow_timed_joint_trajectory(joint_positions, joint_velocities, times)
#for xyzq in xyzquat_rs:
#xyz = xyzq[:3]
#quat = xyzq[3:]
#hmat = conversions.trans_rot_to_hmat(xyz,quat)
#try:
#pr2.rarm.goto_pose_matrix(hmat, 'base_link', 'r_wrist_roll_link')
ps.header.frame_id = "base_footprint"
HANDLES.append(Globals.rviz.draw_marker(ps, type=Marker.CYLINDER, rgba=COLORS[id], scale=asarray(obj_dim)))
pose_array = conversions.array_to_pose_array(asarray(traj_xyz), "base_footprint")
HANDLES.append(Globals.rviz.draw_curve(pose_array, rgba=COLORS[id]))
if __name__ == "__main__":
HANDLES = []
rospy.init_node("push_service")
Globals.setup()
traj = h5py.File(args.file, "r")[args.group]
n_waypoints = 20
xyzquat = np.c_[traj["gripper_positions"], traj["gripper_orientations"]]
xyzquat = kinematics_utils.unif_resample(
xyzquat, n_waypoints, weights=np.r_[1, 1, 1, 0.25, 0.25, 0.25, 0.25], tol=0.001
)
old_xyzs, old_quats = xyzquat[:, :3], xyzquat[:, 3:]
times = np.linspace(0, 10, n_waypoints)
Globals.pr2.update_rave()
f = registration.ThinPlateSpline()
obj_xyz = asarray(traj["object_pose"])
obj_quat = asarray(traj["object_orientation"])
obj_dim = asarray(traj["object_dimension"])
def callback(request):
global HANDLES
HANDLES = []
xyzs, _ = pc2xyzrgb(request.point_cloud)
*traj["object_orientation"])))
ps.header.frame_id = 'base_link'
rviz.draw_marker(ps,
id=1,
type=Marker.CUBE,
rgba=(0, 1, 0, 1),
scale=asarray(traj["object_dimension"]))
pose_array = conversions.array_to_pose_array(
asarray(traj["gripper_positions"]), 'base_link')
rviz.draw_curve(pose_array, id=0)
n_waypoints = 20
xyzquat = np.c_[traj["gripper_positions"], traj["gripper_orientations"]]
xyzquat_rs = kinematics_utils.unif_resample(xyzquat,
n_waypoints,
weights=np.ones(7),
tol=.001)
times = np.linspace(0, 10, n_waypoints)
pr2.torso.go_up()
pr2.join_all()
pr2.update_rave()
joint_positions, _ = trajectories.make_joint_traj(xyzquat_rs[:, 0:3],
xyzquat_rs[:, 3:7],
pr2.rarm.manip,
'base_link',
'r_wrist_roll_link',
filter_options=18)
joint_velocities = kinematics_utils.get_velocities(joint_positions,
times,
tol=.001)