示例#1
0
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))
示例#2
0
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
示例#3
0
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
示例#4
0
 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])
示例#5
0
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([])
示例#6
0
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([])