def plot_gripper_xyzs_from_poses(lr, gripper_poses):
manip = get_manipulator(lr)
gripper_xyzs, gripper_quats = [],[]
for pose in gripper_poses:
xyz, quat = conversions.pose_to_trans_rot(pose)
gripper_xyzs.append(xyz)
gripper_quats.append(quat)
make_verb_traj.plot_curve(gripper_xyzs, (1, 1, 0, 1))
def do_traj_ik_default(lr, gripper_poses):
gripper_xyzs, gripper_quats = [], []
for pose in gripper_poses:
xyz, quat = conversions.pose_to_trans_rot(pose)
gripper_xyzs.append(xyz)
gripper_quats.append(quat)
manip = get_manipulator(lr)
joint_positions, inds = trajectories.make_joint_traj(gripper_xyzs, gripper_quats, manip, "base_footprint", "%s_gripper_tool_frame"%lr, filter_options = 1+18)
return joint_positions
def do_traj_ik_default(pr2, lr, gripper_poses):
gripper_xyzs, gripper_quats = [], []
for pose in gripper_poses:
xyz, quat = juc.pose_to_trans_rot(pose)
gripper_xyzs.append(xyz)
gripper_quats.append(quat)
manip = get_manipulator(pr2, lr)
joint_positions, inds = trajectories.make_joint_traj(gripper_xyzs, gripper_quats, manip, "base_footprint", "%s_gripper_tool_frame"%lr, filter_options = 1+18)
return joint_positions
def draw_axes(self, pose_array, rgba = (0,1,0,1), arrow_scale = .1, ns = "default_ns", duration=0):
marker_array = MarkerArray()
for pose in pose_array.poses:
pose_trans, pose_rot = conversions.pose_to_trans_rot(pose)
x_arrow_pose = conversions.trans_rot_to_pose(pose_trans, (0,0,0,1))
x_arrow = self.make_arrow_marker(pose_array.header, x_arrow_pose, rgba, arrow_scale, ns)
marker_array.markers.append(x_arrow)
y_arrow_pose = conversions.trans_rot_to_pose(pose_trans, (0,0,-2**0.5/2,2**0.5/2))
y_arrow = self.make_arrow_marker(pose_array.header, y_arrow_pose, rgba, arrow_scale, ns)
marker_array.markers.append(y_arrow)
z_arrow_pose = conversions.trans_rot_to_pose(pose_trans, (0,-2**0.5/2,0,2**0.5/2))
z_arrow = self.make_arrow_marker(pose_array.header, z_arrow_pose, rgba, arrow_scale, ns)
marker_array.markers.append(z_arrow)
self.array_pub.publish(marker_array)
return MarkerListHandle([MarkerHandle(marker, self.pub) for marker in marker_array.markers])
def do_traj_ik_opt(lr, gripper_poses):
plan_traj_req = PlanTrajRequest()
plan_traj_req.manip = "rightarm" if lr == 'r' else "leftarm"
plan_traj_req.link = "%s_gripper_tool_frame"%lr
plan_traj_req.task = "follow_cart"
#set table bounds
table_bounds = map(float, rospy.get_param("table_bounds").split())
xmin, xmax, ymin, ymax, zmin, zmax = table_bounds
plan_traj_req.xmlstring = \
"""
<Environment>
<KinBody name="%s">
<Body type="static">
<Geom type="box">
<Translation> %f %f %f </Translation>
<extents> %f %f %f </extents>
</Geom>
</Body>
</KinBody>
</Environment>
""" \
% ("table",
(xmin+xmax)/2, (ymin+ymax)/2, (zmin+zmax)/2,
(xmax-xmin)/2, (ymax-ymin)/2, (zmax-zmin)/2)
plan_traj_req.robot_joints = Globals.pr2.robot.GetDOFValues()
pose_vals = []
for pose in gripper_poses:
xyz, quat = conversions.pose_to_trans_rot(pose)
pose_vals.append(np.concatenate((quat, xyz)))
plan_traj_req.goal = np.array(pose_vals)[::5].flatten()
plan_traj_service_name = "plan_traj"
rospy.wait_for_service(plan_traj_service_name)
plan_traj_service_proxy = rospy.ServiceProxy(plan_traj_service_name, PlanTraj)
try:
plan_traj_resp = plan_traj_service_proxy(plan_traj_req)
except rospy.ServiceException, e:
print "Service did not process request: %s"%str(e)
return np.array([])
def do_traj_ik_opt(pr2, lr, gripper_poses):
plan_traj_req = PlanTrajRequest()
plan_traj_req.manip = "rightarm" if lr == 'r' else "leftarm"
plan_traj_req.link = "%s_gripper_tool_frame"%lr
plan_traj_req.task = "follow_cart"
#set table bounds
table_bounds = map(float, rospy.get_param("table_bounds").split())
xmin, xmax, ymin, ymax, zmin, zmax = table_bounds
plan_traj_req.xmlstring = \
"""
<Environment>
<KinBody name="%s">
<Body type="static">
<Geom type="box">
<Translation> %f %f %f </Translation>
<extents> %f %f %f </extents>
</Geom>
</Body>
</KinBody>
</Environment>
""" \
% ("table",
(xmin+xmax)/2, (ymin+ymax)/2, (zmin+zmax)/2,
(xmax-xmin)/2, (ymax-ymin)/2, (zmax-zmin)/2)
plan_traj_req.robot_joints = pr2.robot.GetDOFValues()
pose_vals = []
for pose in gripper_poses:
xyz, quat = juc.pose_to_trans_rot(pose)
pose_vals.append(np.concatenate((quat, xyz)))
plan_traj_req.goal = np.array(pose_vals)[::5].flatten()
plan_traj_service_name = "plan_traj"
rospy.wait_for_service(plan_traj_service_name)
plan_traj_service_proxy = rospy.ServiceProxy(plan_traj_service_name, PlanTraj)
try:
plan_traj_resp = plan_traj_service_proxy(plan_traj_req)
except rospy.ServiceException, e:
print "Service did not process request: %s"%str(e)
return np.array([])
