Ejemplo n.º 1
0
class ObjectChooser(QWidget):
    """
    Display the list of found objects in a table. Make PickUp calls to
    manipulator action server on selected objects.
    """
    def __init__(self, parent, gui, title):
        QWidget.__init__(self)
        self.gui = gui
        self.grasp = None
        self.title = QLabel()
        self.title.setText(title)
        self.draw_functions = draw_functions.DrawFunctions('grasp_markers')
        self.pickup_result = None
        self.listener = tf.TransformListener()
        self.grasp_display_publisher = rospy.Publisher("/grasp_display", Grasp)
        self.pickupservice = Execution()

    def draw(self):
        self.frame = QFrame(self)

        self.tree = QTreeWidget()
        self.connect(self.tree,
                     SIGNAL('itemDoubleClicked (QTreeWidgetItem *, int)'),
                     self.double_click)
        self.tree.setHeaderLabels(["Object Name", "Maker", "tags"])
        self.tree.resizeColumnToContents(0)
        self.tree.resizeColumnToContents(1)
        self.tree.resizeColumnToContents(2)

        self.layout = QVBoxLayout()
        self.layout.addWidget(self.title)
        self.layout.addWidget(self.tree)

        self.frame.setLayout(self.layout)
        layout = QVBoxLayout()
        layout.addWidget(self.frame)
        self.frame.show()
        self.setLayout(layout)
        self.show()

    def double_click(self, item, value):
        self.object_name = str(item.text(0))
        self.object = self.gui.found_objects[self.object_name]

        graspable_object = self.object.graspable_object

        # draw a bounding box around the selected object
        (box_pose, box_dims) = self.call_find_cluster_bounding_box(
            graspable_object.cluster)
        if box_pose == None:
            return

        box_mat = pose_to_mat(box_pose.pose)
        rospy.logerr("box_pose %f %f %f  q: %f %f %f %f",
                     box_pose.pose.position.x, box_pose.pose.position.y,
                     box_pose.pose.position.z, box_pose.pose.orientation.x,
                     box_pose.pose.orientation.y, box_pose.pose.orientation.z,
                     box_pose.pose.orientation.w)
        box_ranges = [[-box_dims.x / 2, -box_dims.y / 2, -box_dims.z / 2],
                      [box_dims.x / 2, box_dims.y / 2, box_dims.z / 2]]
        self.box_pose = box_pose

        self.draw_functions.draw_rviz_box(box_mat,
                                          box_ranges,
                                          '/world',
                                          ns='bounding box',
                                          color=[0, 0, 1],
                                          opaque=0.25,
                                          duration=60)

        # call the pickup service
        res = self.pickup(graspable_object, self.object.graspable_object_name,
                          self.object_name)

        if res == 0:  #correctly picked up
            #TODO: set up place_location
            executed_grasp = self.pickup_result.grasp
            initial_pose = PoseStamped()
            initial_pose.header.stamp = rospy.get_rostime()
            initial_pose.header.frame_id = "/world"
            initial_pose.pose.position.x = self.box_pose.pose.position.x + 0.1
            initial_pose.pose.position.y = self.box_pose.pose.position.y - 0.3
            initial_pose.pose.position.z = self.box_pose.pose.position.z - box_dims.z / 2  # graspable object is from bottom but bounding box is at center !
            q = transformations.quaternion_about_axis(-0.05, (0, 0, 1))
            initial_pose.pose.orientation.x = self.box_pose.pose.orientation.x  #q[0]
            initial_pose.pose.orientation.y = self.box_pose.pose.orientation.y  #q[1]
            initial_pose.pose.orientation.z = self.box_pose.pose.orientation.z  #q[2]
            initial_pose.pose.orientation.w = self.box_pose.pose.orientation.w  #q[3]

            self.list_of_poses = self.compute_list_of_poses(
                initial_pose, graspable_object, executed_grasp)
            #print "list of pose",self.list_of_poses

            self.place_object(graspable_object,
                              self.object.graspable_object_name,
                              self.object_name, self.list_of_poses)

    def place_click(self):

        if (self.pickup_result):
            grasp = self.pickup_result.grasp
            self.pickupservice.pre_grasp_exec(grasp, 10.0)
        else:
            self.pickupservice.grasp_release_exec(10.0)

        initial_pose = PoseStamped()
        initial_pose.header.stamp = rospy.get_rostime()
        initial_pose.header.frame_id = "/world"  #"/fixed"

        #fake hand to world pose
        initial_pose.pose.position.x = 0.4  #self.box_pose.pose.position.x+0.0
        initial_pose.pose.position.y = 0.091  # self.box_pose.pose.position.y-0.2
        initial_pose.pose.position.z = 1.25  #self.box_pose.pose.position.z+0.05
        q = transformations.quaternion_about_axis(-0.05, (0, 0, 1))
        initial_pose.pose.orientation.x = 0.41  #self.box_pose.pose.orientation.x#q[0]
        initial_pose.pose.orientation.y = 0.695  #self.box_pose.pose.orientation.y#q[1]
        initial_pose.pose.orientation.z = 0.52  #self.box_pose.pose.orientation.z#q[2]
        initial_pose.pose.orientation.w = 0.284  #self.box_pose.pose.orientation.w#q[3]

        executed_grasp = Grasp()
        executed_grasp.grasp_pose = copy.deepcopy(initial_pose.pose)

        #fake obj world origin pose
        initial_pose.pose.position.x = 0.43
        initial_pose.pose.position.y = 0.149
        initial_pose.pose.position.z = 1.088
        initial_pose.pose.orientation.x = 0.0  #self.box_pose.pose.orientation.x#q[0]
        initial_pose.pose.orientation.y = 0.0  #self.box_pose.pose.orientation.y#q[1]
        initial_pose.pose.orientation.z = 0.567  #self.box_pose.pose.orientation.z#q[2]
        initial_pose.pose.orientation.w = 0.824  #self.box_pose.pose.orientation.w#q[3]

        #fake graspable object
        graspable_object = GraspableObject()
        graspable_object.reference_frame_id = "/world"
        mypotentialmodels = DatabaseModelPose()
        mypotentialmodels.model_id = 18744
        mypotentialmodels.confidence = 1.0
        mypotentialmodels.pose = copy.deepcopy(initial_pose)
        graspable_object.potential_models.append(mypotentialmodels)

        #fake obj world destination pose
        initial_pose.pose.position.x = 0.43
        initial_pose.pose.position.y = 0.0
        initial_pose.pose.position.z = 1.088
        initial_pose.pose.orientation.x = 0.0  #self.box_pose.pose.orientation.x#q[0]
        initial_pose.pose.orientation.y = 0.0  #self.box_pose.pose.orientation.y#q[1]
        initial_pose.pose.orientation.z = 0.567  #self.box_pose.pose.orientation.z#q[2]
        initial_pose.pose.orientation.w = 0.824  #self.box_pose.pose.orientation.w#q[3]

        list_of_poses = self.compute_list_of_poses(initial_pose,
                                                   graspable_object,
                                                   executed_grasp)
        #print "list of pose",list_of_poses

        #self.place_object(self.object.graspable_object, self.object.graspable_object_name, self.object_name, self.list_of_poses)

    def pickup(self, graspable_object, graspable_object_name, object_name):
        """
        Try to pick up the given object. Sends a message (PickupGoal from
        actionlib_msgs) to the manipularior action server on
        /object_manipulator/object_manipulator_pickup/goal
        """
        info_tmp = "Picking up " + object_name
        rospy.loginfo(info_tmp)
        pickup_goal = PickupGoal()
        pickup_goal.target = graspable_object
        pickup_goal.collision_object_name = graspable_object_name
        pickup_goal.collision_support_surface_name = self.gui.collision_support_surface_name

        #pickup_goal.additional_link_padding
        pickup_goal.ignore_collisions = True

        pickup_goal.arm_name = "right_arm"
        #pickup_goal.desired_approach_distance = 0.08 This does not exist anymore in the message
        #pickup_goal.min_approach_distance = 0.02 This does not exist anymore in the message

        direction = Vector3Stamped()
        direction.header.stamp = rospy.get_rostime()
        direction.header.frame_id = "/base_link"
        direction.vector.x = 0
        direction.vector.y = 0
        direction.vector.z = 1
        pickup_goal.lift.direction = direction
        #request a vertical lift of 15cm after grasping the object
        pickup_goal.lift.desired_distance = 0.1
        pickup_goal.lift.min_distance = 0.07
        #do not use tactile-based grasping or tactile-based lift
        pickup_goal.use_reactive_lift = True
        pickup_goal.use_reactive_execution = True

        self.pickup_result = self.pickupservice.pick(pickup_goal)

        #pickup_client = actionlib.SimpleActionClient('/object_manipulator/object_manipulator_pickup', PickupAction)
        #pickup_client.wait_for_server()
        #rospy.loginfo("Pickup server ready")

        #pickup_client.send_goal(pickup_goal)
        #TODO: change this when using the robot
        #pickup_client.wait_for_result(timeout=rospy.Duration.from_sec(3600.0))
        loginfo("Got Pickup results")
        #self.pickup_result = pickup_client.get_result()

        #print "Pickup result: "+str(self.pickup_result)
        '''
        if pickup_client.get_state() != GoalStatus.SUCCEEDED:
            rospy.logerr("The pickup action has failed: " + str(self.pickup_result.manipulation_result.value) )
            QMessageBox.warning(self, "Warning",
                    "Pickup action failed: "+str(self.pickup_result.manipulation_result.value))
            for tested_grasp,tested_grasp_result in zip(self.pickup_result.attempted_grasps,self.pickup_result.attempted_grasp_results):
                if tested_grasp_result.result_code==7:
                  self.grasp_display_publisher.publish(tested_grasp)
            return -1
        '''

        if self.pickup_result.manipulation_result.value == ManipulationResult.SUCCESS:
            loginfo("Pick succeeded, now lifting")
            self.pickupservice.lift(0.07)

        else:
            loginfo("Pick failed")
            return 1
        return 0

    def place_object(self, graspable_object, graspable_object_name,
                     object_name, list_of_poses):
        """
        Place the given object in the given pose
        """
        if self.pickup_result == None:
            rospy.logwarn(
                "No objects where picked up. Aborting place object action.")
            return

        info_tmp = "Placing " + object_name
        rospy.loginfo(info_tmp)
        place_goal = PlaceGoal()

        #place at the prepared location
        place_goal.place_locations = list_of_poses

        place_goal.collision_object_name = graspable_object_name
        place_goal.collision_support_surface_name = self.gui.collision_support_surface_name
        print "collision support surface name: ", self.gui.collision_support_surface_name

        #information about which grasp was executed on the object,
        #returned by the pickup action
        place_goal.grasp = self.pickup_result.grasp
        #use the right rm to place
        place_goal.arm_name = "right_arm"
        #padding used when determining if the requested place location
        #would bring the object in collision with the environment
        place_goal.place_padding = 0.01
        #how much the gripper should retreat after placing the object
        place_goal.desired_retreat_distance = 0.1
        place_goal.min_retreat_distance = 0.05
        #we will be putting down the object along the "vertical" direction
        #which is along the z axis in the fixed frame
        direction = Vector3Stamped()
        direction.header.stamp = rospy.get_rostime()
        direction.header.frame_id = "/world"
        direction.vector.x = 0
        direction.vector.y = 0
        direction.vector.z = 1
        place_goal.approach.direction = direction
        place_goal.approach.min_distance = 0.01
        place_goal.approach.desired_distance = 0.03
        #request a vertical put down motion of 10cm before placing the object
        place_goal.desired_retreat_distance = 0.1
        place_goal.min_retreat_distance = 0.05
        #we are not using tactile based placing
        place_goal.use_reactive_place = False

        placeresult = self.pickupservice.place(place_goal)
        '''place_client = actionlib.SimpleActionClient('/object_manipulator/object_manipulator_place', PlaceAction)
        place_client.wait_for_server()
        rospy.loginfo("Place server ready")

        place_client.send_goal(place_goal)
        #timeout after 1sec
        #TODO: change this when using the robot
        place_client.wait_for_result(timeout=rospy.Duration.from_sec(3600.0))
        rospy.loginfo("Got Place results")

        place_result = place_client.get_result()

        if place_client.get_state() != GoalStatus.SUCCEEDED:
            rospy.logerr("The place action has failed: " + str(place_result.manipulation_result.value) )
        print place_result
        '''

        if placeresult.manipulation_result.value == ManipulationResult.SUCCESS:
            loginfo("Place succeeded, now retreating")
            self.pickupservice.retreat(0.07)
        else:
            loginfo("Place failed")
            return 1
        return 0

    def compute_list_of_poses(self, destination_pose, graspable_object,
                              executed_grasp):

        list_of_poses = []

        # find hand to object transform
        #  executed_grasp is hand pose in world frame
        #  graspable_object is object in world frame
        hand_world_pose = executed_grasp.grasp_pose
        #print "hand_world", hand_world_pose
        object_world_pose = graspable_object.potential_models[0].pose.pose
        # print "object world" , object_world_pose
        tmathandworld = pose_to_mat(hand_world_pose)
        tmatobjworld = pose_to_mat(object_world_pose)
        hand_object_pose = mat_to_pose(
            np.dot(np.linalg.inv(tmatobjworld), tmathandworld))
        #print "hand_object_pose" , hand_object_pose

        # generate rotational_symmetric hand to object frames
        hand_object_poses = self.generate_rotational_symmetric_poses(
            hand_object_pose, 16)

        target_pose = PoseStamped()
        target_pose = copy.deepcopy(destination_pose)
        target_pose.header.stamp = rospy.get_rostime()
        #print "target_object_world pose" , target_pose
        tmattarget_world = pose_to_mat(target_pose.pose)

        for hand_object_pose in hand_object_poses:
            tmathandobj = pose_to_mat(hand_object_pose)
            target_hand_posestamped_world = PoseStamped()
            target_hand_posestamped_world.header.frame_id = "/world"
            target_hand_posestamped_world.pose = mat_to_pose(
                np.dot(tmattarget_world, tmathandobj))
            target_hand_posestamped_world.header.stamp = rospy.get_rostime()
            list_of_poses.append(target_hand_posestamped_world)

        self.draw_place_area(list_of_poses, graspable_object)

        return list_of_poses

    def generate_rotational_symmetric_poses(self, hand_object_pose, step=8):
        '''
        Generate a list of 20 pregrasps around the z axis of the object
        '''
        generated_poses = []
        pose = hand_object_pose
        #We'll generate 20 poses centered in 0,0,0 with orientations in 360 degrees around z

        for i in range(0, step):
            angle = (6.28318532 * i) / step
            q = tf.transformations.quaternion_from_euler(0.0, 0.0, angle)
            rotation_pose = Pose(Point(0, 0, 0),
                                 Quaternion(0.0, 0.0, 0.0, 1.0))
            rotation_pose.orientation = Quaternion(*q)

            tmatrotz = pose_to_mat(rotation_pose)
            # premultiply by the rotation matrix
            tmatpose = pose_to_mat(pose)
            rotated_pose = mat_to_pose(np.dot(tmatrotz, tmatpose))

            generated_poses.append(rotated_pose)

        return generated_poses

    def draw_place_area(self, list_of_poses, graspable_object):
        '''
        Displays all the possible placing locations that are going to be tried.
        '''
        #try:
        (trans_palm,
         rot_palm) = self.listener.lookupTransform('/world', '/palm',
                                                   rospy.Time(0))
        #except:
        #    return

        for index, pose_stamped in enumerate(list_of_poses):
            pose_tmp = Pose()
            pose_tmp.position.x = pose_stamped.pose.position.x
            pose_tmp.position.y = pose_stamped.pose.position.y
            pose_tmp.position.z = pose_stamped.pose.position.z

            pose_tmp.orientation.x = pose_stamped.pose.orientation.x
            pose_tmp.orientation.y = pose_stamped.pose.orientation.y
            pose_tmp.orientation.z = pose_stamped.pose.orientation.z
            pose_tmp.orientation.w = pose_stamped.pose.orientation.w

            mat = pose_to_mat(pose_tmp)
            self.draw_functions.draw_rviz_box(mat, [.01, .01, .01],
                                              frame='/world',
                                              ns='place_' + str(index),
                                              id=1000 + index,
                                              duration=90,
                                              color=[0.5, 0.5, 0.0],
                                              opaque=1.0)

    def call_find_cluster_bounding_box(self, cluster):
        req = FindClusterBoundingBoxRequest()
        req.cluster = cluster
        service_name = "find_cluster_bounding_box"
        rospy.loginfo("waiting for find_cluster_bounding_box service")
        rospy.wait_for_service(service_name)
        rospy.loginfo("service found")
        serv = rospy.ServiceProxy(service_name, FindClusterBoundingBox)
        try:
            res = serv(req)
        except rospy.ServiceException, e:
            rospy.logerr("error when calling find_cluster_bounding_box: %s" %
                         e)
            return 0
        if not res.error_code:
            return (res.pose, res.box_dims)
        else:
            return (None, None)
Ejemplo n.º 2
0
class SrOpenSuitcase(object):
    """
    """

    # The xyz distance we want the palm link to be from the grasping point
    DISTANCE_TO_GRASP = [-0.1, -0.01, 0.04]
    # The distance used when computing the approach
    APPROACH_DISTANCE = 0.03

    def __init__(self,):
        """
        """
        self.markers_pub_ = rospy.Publisher("~markers", Marker)
        self.execution = Execution(use_database=False)

        # direct control of the elbow swing, WRJ1 and THJ5 for throwing the lid open
        self.publishers = {}
        self.publishers["ElbowJSwing"] = rospy.Publisher("/sa_es_position_controller/command", Float64, latch=True)
        self.publishers["WRJ1"] = rospy.Publisher(
            "/sh_wrj1_mixed_position_velocity_controller/command", Float64, latch=True
        )
        self.publishers["THJ5"] = rospy.Publisher(
            "/sh_wrj1_mixed_position_velocity_controller/command", Float64, latch=True
        )

        self.suitcase_src_ = rospy.Service("~open_suitcase", OpenSuitcase, self.open_lid)

    def open_lid(self, suitcase_req):
        suitcase = suitcase_req.suitcase
        self.display_suitcase_(suitcase)

        semi_circle = self.go_to_mechanism_and_grasp_(suitcase)

        if semi_circle == None:
            rospy.logerr("Failed to approach.")
            sys.exit(1)

        time.sleep(1.0)

        self.lift_lid_(suitcase, semi_circle)

        # throw the lid open
        self.throw_lid_open()

    def go_to_mechanism_and_grasp_(self, suitcase):
        # compute the full trajectory
        semi_circle = self.compute_semi_circle_traj_(suitcase)

        # compute the pregrasp and grasp
        grasp = Grasp()
        grasp_pose_ = PoseStamped()

        # the grasp is the first item of the semi circle
        grasp_pose_ = semi_circle[0]

        # copy the grasp_pose as a pre-grasp_pose
        pre_grasp_pose_ = copy.deepcopy(grasp_pose_)

        # add desired_approach_distance along the approach vector. above the object to plan pre-grasp pose
        # TODO: use the suitcase axis to approach from the perpendicular
        pre_grasp_pose_.pose.position.x = pre_grasp_pose_.pose.position.x - self.APPROACH_DISTANCE

        # TODO: find better postures
        grasp.pre_grasp_posture.name = [
            "FFJ0",
            "FFJ3",
            "FFJ4",
            "LFJ0",
            "LFJ3",
            "LFJ4",
            "LFJ5",
            "MFJ0",
            "MFJ3",
            "MFJ4",
            "RFJ0",
            "RFJ3",
            "RFJ4",
            "THJ1",
            "THJ2",
            "THJ3",
            "THJ4",
            "THJ5",
            "WRJ1",
            "WRJ2",
        ]
        grasp.pre_grasp_posture.position = [0.0] * 18
        grasp.pre_grasp_posture.position[grasp.pre_grasp_posture.name.index("THJ4")] = 58.0
        grasp.pre_grasp_posture.position[grasp.pre_grasp_posture.name.index("THJ5")] = -50.0

        grasp.grasp_posture.name = [
            "FFJ0",
            "FFJ3",
            "FFJ4",
            "LFJ0",
            "LFJ3",
            "LFJ4",
            "LFJ5",
            "MFJ0",
            "MFJ3",
            "MFJ4",
            "RFJ0",
            "RFJ3",
            "RFJ4",
            "THJ1",
            "THJ2",
            "THJ3",
            "THJ4",
            "THJ5",
            "WRJ1",
            "WRJ2",
        ]
        grasp.grasp_posture.position = [0.0] * 18
        grasp.grasp_posture.position[grasp.grasp_posture.name.index("THJ1")] = 57.0
        grasp.grasp_posture.position[grasp.grasp_posture.name.index("THJ2")] = 30.0
        grasp.grasp_posture.position[grasp.grasp_posture.name.index("THJ3")] = -15.0
        grasp.grasp_posture.position[grasp.grasp_posture.name.index("THJ4")] = 58.0
        grasp.grasp_posture.position[grasp.grasp_posture.name.index("THJ5")] = 40.0

        grasp.grasp_pose = grasp_pose_.pose

        # for distance from 0 (grasp_pose) to desired_approach distance (pre_grasp_pose) test IK/Collision and save result
        # decompose this in X steps depending on distance to do and max speed
        motion_plan_res = GetMotionPlanResponse()
        interpolated_motion_plan_res = self.execution.plan.get_interpolated_ik_motion_plan(
            pre_grasp_pose_, grasp_pose_, False
        )

        # check the result (depending on number of steps etc...)
        if interpolated_motion_plan_res.error_code.val == interpolated_motion_plan_res.error_code.SUCCESS:
            number_of_interpolated_steps = 0
            # check if one approach trajectory is feasible
            for interpolation_index, traj_error_code in enumerate(interpolated_motion_plan_res.trajectory_error_codes):
                if traj_error_code.val != 1:
                    rospy.logerr(
                        "One unfeasible approach-phase step found at "
                        + str(interpolation_index)
                        + "with val "
                        + str(traj_error_code.val)
                    )
                    break
                else:
                    number_of_interpolated_steps = interpolation_index

            # if trajectory is feasible then plan reach motion to pre-grasp pose
            if number_of_interpolated_steps + 1 == len(interpolated_motion_plan_res.trajectory.joint_trajectory.points):
                rospy.loginfo("Grasp number approach is possible, checking motion plan to pre-grasp")
                # print interpolated_motion_plan_res

                # check and plan motion to this pre_grasp_pose
                motion_plan_res = self.execution.plan.plan_arm_motion("right_arm", "jointspace", pre_grasp_pose_)

        # execution part
        if motion_plan_res.error_code.val == motion_plan_res.error_code.SUCCESS:
            # put hand in pre-grasp posture
            if self.execution.pre_grasp_exec(grasp) < 0:
                rospy.logerr("Failed to go in pregrasp.")
                sys.exit()

            # go there
            # filter the trajectory
            filtered_traj = self.execution.filter_traj_(motion_plan_res)

            self.execution.display_traj_(filtered_traj)

            # reach pregrasp pose
            if self.execution.send_traj_(filtered_traj) < 0:
                time.sleep(20)  # TODO use actionlib here

            # approach
            if self.execution.send_traj_(interpolated_motion_plan_res.trajectory.joint_trajectory) < 0:
                rospy.logerr("Failed to approach.")
                sys.exit()
            time.sleep(20)  # TODO use actionlib here

            # grasp
            if self.execution.grasp_exec(grasp) < 0:
                rospy.logerr("Failed to grasp.")
                sys.exit()
            time.sleep(20)  # TODO use actionlib here

        else:
            # Failed, don't return the computed traj
            return None

        # return the full traj
        return semi_circle

    def lift_lid_(self, suitcase, semi_circle):
        while len(semi_circle) > 0:
            self.execution.plan_and_execute_step_(semi_circle)
            time.sleep(0.5)

        return OpenSuitcaseResponse(OpenSuitcaseResponse.SUCCESS)

    def throw_lid_open(self):
        # get the current joint states
        res = self.execution.plan.get_joint_state_.call()
        current_joint_states = res.joint_state

        # flex the elbow, flip the wrist
        elbow_target = current_joint_states.position[current_joint_states.name.index("ElbowJSwing")] + 0.5
        wrist_target = current_joint_states.position[current_joint_states.name.index("WRJ1")] - 0.5
        thj5_target = -0.80

        self.publishers["ElbowJSwing"].publish(elbow_target)
        self.publishers["WRJ1"].publish(wrist_target)
        self.publishers["THJ5"].publish(thj5_target)

    def compute_semi_circle_traj_(self, suitcase, nb_steps=50):
        poses = []

        # compute a semi-circular trajectory, starting
        # from the suitcase mechanism, rotating
        # around the suitcase axis
        target = PoseStamped()
        target.header.frame_id = suitcase.header.frame_id
        target.pose = suitcase.opening_mechanism.pose_stamped.pose

        # axis_x and axis_z are the projection of the mechanism model
        # onto the suitcase axis (point around which we want to rotate)
        mechanism = [
            suitcase.opening_mechanism.pose_stamped.pose.position.x,
            suitcase.opening_mechanism.pose_stamped.pose.position.y,
            suitcase.opening_mechanism.pose_stamped.pose.position.z,
        ]
        axis = [
            suitcase.lid_axis_a.x + (suitcase.lid_axis_b.x - suitcase.lid_axis_a.x) / 2.0,
            suitcase.lid_axis_a.y + (suitcase.lid_axis_b.y - suitcase.lid_axis_a.y) / 2.0,
            suitcase.lid_axis_a.z + (suitcase.lid_axis_b.z - suitcase.lid_axis_a.z) / 2.0,
        ]

        # We're always starting with the palm straight along the x axis
        # TODO: use real suitcase axis instead?
        suitcase_axis = (1, 0, 0)
        for i in range(0, nb_steps + 1):
            # we're rotating from this angle around the suitcase axis to point towards the suitcase
            rotation_angle = float(i) * math.pi / 2.0 / float(nb_steps)

            ####
            # POSITION
            target.pose.position.x = axis[0] - (
                (axis[0] - mechanism[0] - self.DISTANCE_TO_GRASP[0]) * math.cos(rotation_angle)
            )
            target.pose.position.y = suitcase.opening_mechanism.pose_stamped.pose.position.y + self.DISTANCE_TO_GRASP[1]
            target.pose.position.z = (
                axis[2]
                + ((axis[0] - mechanism[0] - self.DISTANCE_TO_GRASP[0]) * math.sin(rotation_angle))
                + self.DISTANCE_TO_GRASP[2]
            )

            ####
            # ORIENTATION
            # limit the wrj1 axis
            rotation_angle = min(rotation_angle, math.radians(15.0))
            # add 90 degrees to point the axis toward the suitcase
            rotation_angle += math.pi / 2.0
            # orientation, palm z axis pointing towards the suitcase axes
            # z toward suitcase
            toward_z = transformations.quaternion_about_axis(math.pi / 2.0, (0, 0, 1))
            # then rotate as we go up to continue pointing at the axis
            step_rotation = transformations.quaternion_about_axis(rotation_angle, suitcase_axis)
            # combine those transform:
            orientation = transformations.quaternion_multiply(toward_z, step_rotation)

            target.pose.orientation.x = orientation[0]
            target.pose.orientation.y = orientation[1]
            target.pose.orientation.z = orientation[2]
            target.pose.orientation.w = orientation[3]
            poses.append(copy.deepcopy(target))

        return poses

    def display_suitcase_(self, suitcase):
        # display axes
        axes_marker = Marker()
        axes_marker.header.frame_id = suitcase.header.frame_id
        axes_marker.ns = "suitcase"
        axes_marker.id = 0
        axes_marker.type = Marker.LINE_STRIP
        axes_marker.action = Marker.ADD
        axes_marker.points.append(suitcase.lid_axis_a)
        axes_marker.points.append(suitcase.lid_axis_b)
        axes_marker.scale.x = 0.02

        axes_marker.color.a = 0.4
        axes_marker.color.r = 0.1
        axes_marker.color.g = 0.47
        axes_marker.color.b = 0.88

        self.markers_pub_.publish(axes_marker)

        # display mechanism
        mechanism_marker = Marker()
        mechanism_marker.header.frame_id = suitcase.header.frame_id
        mechanism_marker.ns = "suitcase"
        mechanism_marker.id = 1
        mechanism_marker.type = Marker.CUBE
        mechanism_marker.action = Marker.ADD

        mechanism_marker.pose = suitcase.opening_mechanism.pose_stamped.pose
        mechanism_marker.scale = suitcase.opening_mechanism.dimensions

        mechanism_marker.color.a = 0.4
        mechanism_marker.color.r = 0.1
        mechanism_marker.color.g = 0.47
        mechanism_marker.color.b = 0.88

        self.markers_pub_.publish(mechanism_marker)