def plate_grasps(plate_pose_stamped, grasp_reference_frame): plate_pose = tl.transform_pose(grasp_reference_frame, plate_pose_stamped.header.frame_id, plate_pose_stamped.pose) #this is the object in the frame of the gripper grasp_pose = Pose() grasps = [] rospy.loginfo('plate pose = \n' + str(plate_pose)) origin = Pose() origin.orientation.w = 1.0 grasp_pose.position.x = GRASP_DIST for i in range(NPLATEGRASPS): alpha = 2.0 * np.pi / NPLATEGRASPS * i grasp_pose.orientation = gt.multiply_quaternions( Quaternion(x=0.707, w=0.707), Quaternion(z=np.sin(alpha / 2.0), w=np.cos(alpha / 2.0))) grasp = om.Grasp() grasp.grasp_pose = gt.transform_pose( gt.inverse_transform_pose(origin, grasp_pose), plate_pose) grasp.min_approach_distance = 0.0 grasp.desired_approach_distance = 0.3 grasps.append(grasp) return grasps
def get_gripper(arm, obj, goal): ''' finds the pose of the gripper for an object in its hand at the given goal pose @type obj: arm_navigation_msgs.msg.attached_collision_object @param obj: the object being held by the gripper @type goal: PoseStamped @param goal: goal pose for the object in torso_lift_link @rtype PoseStamped @returns pose of the gripper for the given object at the given goal ''' # print 'Getting the gripper. Oh yeah' # first get the object's current pose obj_pose = tf.transform_pose("/torso_lift_link", obj.object.header.frame_id, \ obj.object.poses[0]) obj_pose_stamped = PoseStamped() obj_pose_stamped.header.frame_id = "/torso_lift_link" obj_pose_stamped.pose = obj_pose # self.make_marker(obj_pose_stamped, namespace='obj_pose', color=(1.0,1.0,1.0,0.8)) # next get the gripper's current pose gripper_pose_stamped = arm.get_hand_frame_pose(\ frame_id="/torso_lift_link") gripper_pose = gripper_pose_stamped.pose # self.make_marker(obj_pose_stamped, namespace='gripper_pose', color=(1.0,0.0,0.0,0.8), mtype=Marker.ARROW) # the position of the object when the wrist is the origin # grasp = gripper_pose^{-1}*obj_pose grasp = gt.inverse_transform_pose(obj_pose, gripper_pose) #we need the position of the gripper when the object is at goal #gripper_goal = goal*grasp^{-1} origin = Pose() origin.orientation.w = 1 #this is grasp^{-1} grasp_inv = gt.inverse_transform_pose(origin, grasp) #this is goal*grasp_inv gripper_goal = gt.transform_pose(grasp_inv, goal.pose) gripper_ps = PoseStamped() gripper_ps.header.frame_id = goal.header.frame_id gripper_ps.pose = gripper_goal return gripper_ps
def transform_pose(self, new_frame_id, old_frame_id, pose, robot_state): """ Transforms a pose defined in old_frame_id into new_frame_id according to robot state. **Args:** **new_frame_id (string):** new frame id **old_frame_id (string):** old frame id **pose (geometry_msgs.msg.Pose):** pose defined in old_frame_id **robot_state (arm_navigation_msg.msg.RobotState):** Robot state to use for transformation **Returns:** A geometry_msgs.msg.Pose defined in new_frame_id **Raises:** **exceptions.TransformStateError:** if there is an error getting the transform """ trans = self.get_transform(old_frame_id, new_frame_id, robot_state) return gt.transform_pose(pose, conv.transform_to_pose(trans.transform))
def transform_pose(self, new_frame_id, old_frame_id, pose, robot_state): ''' Transforms a pose defined in old_frame_id into new_frame_id according to robot state. **Args:** **new_frame_id (string):** new frame id **old_frame_id (string):** old frame id **pose (geometry_msgs.msg.Pose):** pose defined in old_frame_id **robot_state (arm_navigation_msg.msg.RobotState):** Robot state to use for transformation **Returns:** A geometry_msgs.msg.Pose defined in new_frame_id **Raises:** **exceptions.TransformStateError:** if there is an error getting the transform ''' trans = self.get_transform(old_frame_id, new_frame_id, robot_state) return gt.transform_pose(pose, conv.transform_to_pose(trans.transform))
def free_spots_from_dimensions(table, object_dims, blocking_objs, res=0.1): ''' Currently works only with a single shape Assumes table is in xy plane ''' #find the lower left corner of the table in the header frame #we want everything relative to this point table_corners = gt.bounding_box_corners(table.shapes[0]) lower_left = Pose() lower_left.position = gt.list_to_point(table_corners[0]) lower_left.orientation.w = 1.0 #rospy.loginfo('Table corners are:') #for c in table_corners: rospy.loginfo('\n'+str(c)) #rospy.loginfo('Lower left =\n'+str(lower_left)) #rospy.loginfo('Table header =\n'+str(table.header)) #rospy.loginfo('Table pose =\n'+str(table.poses[0])) #this is the position of the minimum x, minimum y point in the table's header frame table_box_origin = gt.transform_pose(lower_left, table.poses[0]) tr = gt.inverse_transform_list(gt.transform_list(table_corners[-1], table.poses[0]), table_box_origin) table_dims = (tr[0], tr[1]) tbos = PoseStamped() tbos.header = table.header tbos.pose = table_box_origin marray.markers.append(vt.marker_at(tbos, ns='table_origin', mtype=Marker.CUBE, r=1.0)) max_box = PointStamped() max_box.header = table.header max_box.point = gt.list_to_point(gt.transform_list([table_dims[0], table_dims[1], 0], table_box_origin)) marray.markers.append(vt.marker_at_point(max_box, ns='table_max', mtype=Marker.CUBE, r=1.0)) #rospy.loginfo('Table box origin is '+str(table_box_origin)+' dimensions are '+str(table_dims)) locs_on_table = _get_feasible_locs(table_dims, object_dims, res) for i, l in enumerate(locs_on_table): pt = Point() pt.x = l[0] pt.y = l[1] mpt = PointStamped() mpt.header = table.header mpt.point = gt.transform_point(pt, table_box_origin) marray.markers.append(vt.marker_at_point(mpt, mid=i, ns='locations', r=1.0, g=1.0, b=0.0)) feasible_locs = [] #check that these points really are on the table for i, l in enumerate(locs_on_table): pt = Point() pt.x = l[0] pt.y = l[1] #this point is now defined relative to the origin of the table (rather than its minimum x, minimum y point) table_pt = gt.inverse_transform_point(gt.transform_point(pt, table_box_origin), table.poses[0]) if point_on_table(table_pt, table.shapes[0]): feasible_locs.append(l) marray.markers[i+2].color.r = 0.0 marray.markers[i+2].color.b = 1.0 rospy.loginfo('Testing '+str(len(feasible_locs))+' locations') if not feasible_locs: return feasible_locs forbidden=[] for i, o in enumerate(blocking_objs): ofcs = gt.bounding_box_corners(o.shapes[0]) objpose = tl.transform_pose(table.header.frame_id, o.header.frame_id, o.poses[0]) hfcs = [gt.transform_list(c, objpose) for c in ofcs] tfcs = [gt.inverse_transform_list(c, table_box_origin) for c in hfcs] oxmax = max([c[0] for c in tfcs]) oxmin = min([c[0] for c in tfcs]) oymax = max([c[1] for c in tfcs]) oymin = min([c[1] for c in tfcs]) forbidden.append(((oxmin, oymin), (oxmax - oxmin, oymax - oymin))) #rospy.loginfo('\n'+str(forbidden[-1])) ps = PoseStamped() ps.header = table.header ps.pose = objpose ps = PoseStamped() ps.header = table.header ps.pose.position = gt.list_to_point(gt.transform_list([oxmin, oymin, 0], table_box_origin)) ps.pose.orientation.w = 1.0 marray.markers.append(vt.marker_at(ps, ns='forbidden', mid=i, r=1.0, b=0.0)) # Remove forbidden rectangles for (bottom_left, dims) in forbidden: _remove_feasible_locs(feasible_locs, object_dims, bottom_left, _add(bottom_left, dims), res) rospy.loginfo('There are '+str(len(feasible_locs))+' possible locations') obj_poses = [] for i, fl in enumerate(feasible_locs): table_frame_pose = Pose() table_frame_pose.position.x = fl[0] + object_dims[0]/2.0 table_frame_pose.position.y = fl[1] + object_dims[1]/2.0 table_frame_pose.orientation.w = 1.0 pose = PoseStamped() pose.header = copy.deepcopy(table.header) pose.pose = gt.transform_pose(table_frame_pose, table_box_origin) obj_poses.append(pose) pt = PointStamped() pt.header = table.header pt.point = pose.pose.position marray.markers.append(vt.marker_at_point(pt, mid=i, ns='final_locations', g=1.0, b=0.0)) #rospy.loginfo('Object poses are:') #for op in obj_poses: rospy.loginfo(str(op)) for i in range(10): vpub.publish(marray) rospy.sleep(0.1) return obj_poses
def add_spatula(self, arm): spatula = CollisionObject() spatula.id = "spatula" spatula.header.frame_id = self.wi.world_frame spatula.operation.operation = spatula.operation.ADD paddle = Shape() handle = Shape() paddle.type = paddle.BOX paddle.dimensions = [0.11, 0.12, 0.005] handle.type = handle.CYLINDER handle.dimensions = [0.02, 0.24] paddle_pose = Pose() handle_pose = Pose() paddle_pose.position.y = paddle.dimensions[1] / 2.0 paddle_pose.orientation.w = 1.0 angle = np.pi / 5.0 handle_pose.position.y = -1.0 * handle.dimensions[1] / 2.0 * np.sin( np.pi / 2.0 - angle) handle_pose.position.z = handle.dimensions[1] / 2.0 * np.cos( np.pi / 2.0 - angle) handle_pose.orientation.x = np.sin((np.pi / 2.0 - angle) / 2.0) handle_pose.orientation.w = np.cos((np.pi / 2.0 - angle) / 2.0) spatula.shapes = [paddle, handle] spatula.poses = [paddle_pose, handle_pose] #this is the grasp transformation pos_on_handle = handle.dimensions[1] - 0.1 inv_grasp = Transform() grasp = RigidGrasp() #really should be calculating this... inv_grasp.translation.y = GRIPPER_LENGTH inv_grasp.translation.z = pos_on_handle / 2.0 #flip 90 degrees inv_grasp.rotation.z = np.sin(-1.0 * np.pi / 4.0) inv_grasp.rotation.w = np.cos(-1.0 * np.pi / 4.0) g = gt.transform_pose(transform_to_pose(inv_grasp), handle_pose) origin = Pose() origin.orientation.w = 1.0 grasp.transform = pose_to_transform( gt.inverse_transform_pose(origin, g)) grasp.touch_links = [arm[0] + '_end_effector'] grasp.attach_link = arm[0] + '_gripper_r_finger_tip_link' grasp.min_approach_distance = 0 grasp.desired_approach_distance = 0.15 grasp.min_distance_from_surface = -1 spat_p = copy.deepcopy(spatula) wtrans = Pose() wtrans.orientation.x = np.sin(angle / 2.0) wtrans.orientation.w = np.cos(angle / 2.0) if self.world == -1: wtrans.position.x = 3 wtrans.position.y = -2.8 wtrans.position.z = FAR_TABLE_HEIGHT + 0.02 + handle.dimensions[0] ss = ['far_corner'] elif self.world == -2 or self.world == -7 or self.world == -9 or self.world == -5: wtrans.position.x = -1.7 wtrans.position.y = 2 wtrans.position.z = DOOR_TABLE_HEIGHT + 0.02 + handle.dimensions[0] ss = ['door_table'] else: wtrans.position.x = 0.6 wtrans.position.y = -0.3 wtrans.position.z = CENTER_TABLE_HEIGHT + 0.02 + handle.dimensions[ 0] ss = ['center_table'] if self.world == -4 or self.world == -5: wtrans.position.y = 0 rot = Quaternion() rot.z = np.sin(np.pi / 2.0) rot.w = np.cos(np.pi / 2.0) wtrans.orientation = gt.multiply_quaternions( rot, wtrans.orientation) if self.world == -5: wtrans.position.x = 0 for i in range(len(spat_p.poses)): spat_p.poses[i] = gt.transform_pose(spat_p.poses[i], wtrans) self.wi.add_object(spat_p) return ObjectType(type="SpatulaObject", collision_object=spatula, parameters=ss, numeric_parameters=paddle.dimensions + handle.dimensions + [angle]), [grasp]