예제 #1
0
  def make_grasps(self, pose_stamped, mega_angle=False):
      # setup defaults for the grasp
      g = Grasp()
      g.pre_grasp_posture = self.make_gripper_posture(GRIPPER_OPEN)
      g.grasp_posture = self.make_gripper_posture(GRIPPER_CLOSED)
      g.pre_grasp_approach = self.make_gripper_translation(0.05, 0.1)
      g.post_grasp_retreat = self.make_gripper_translation(0.05, 0.1, -1.0)
      g.grasp_pose = pose_stamped
 
      pitch_vals = [0, 0.2, -0.2, 0.4, -0.4]
      #pitch_vals = [0]
     
      yaw_vals = [-0.2, -0.1, 0, 0.1, 0.2]
      #yaw_vals = [0]
     
      if mega_angle:
          pitch_vals += [0.78, -0.78, 0.3, -0.3, 0.5, -0.5, 0.6, -0.6]
 
      # generate list of grasps
      grasps = []
      #for y in [-1.57, -0.78, 0, 0.78, 1.57]:
      for y in yaw_vals:
          for p in pitch_vals:
              q = quaternion_from_euler(0, 1.57-p, y)
              g.grasp_pose.pose.orientation.x = q[0]
              g.grasp_pose.pose.orientation.y = q[1]
              g.grasp_pose.pose.orientation.z = q[2]
              g.grasp_pose.pose.orientation.w = q[3]
              g.id = str(len(grasps))
              g.allowed_touch_objects = ["part"]
              g.max_contact_force = 0
              #g.grasp_quality = 1.0 - abs(p/2.0)
              grasps.append(copy.deepcopy(g))
      return grasps
def createGrasp(grasp_pose, allowed_touch_objects=[], pre_grasp_posture=None, grasp_posture=None, pre_grasp_approach=None, post_grasp_retreat=None, id_grasp="grasp_"):
    """Create a grasp for object in grasp_pose, allowing collisions with allowed_touch_objects list,
     with pre_grasp_posture and grasp_posture as positions of the hand. Also id name id_grasp."""
    grasp = Grasp()
    grasp.id = id_grasp
#     header = Header()
#     header.frame_id = "base_link"
#     header.stamp = rospy.Time.now()
#     grasp_pose_msg = PoseStamped(header, grasp_pose)
    grasp_pose_with_offset = add_offset_reem_hand(grasp_pose)
    grasp.grasp_pose = grasp_pose_with_offset
    
    if pre_grasp_posture == None:
        grasp.pre_grasp_posture = getPreGraspPosture
    else:
        grasp.pre_grasp_posture = pre_grasp_posture
    
    if grasp_posture == None:
        grasp.grasp_posture = getGraspPosture
    else:
        grasp.grasp_posture = grasp_posture
        
    grasp.allowed_touch_objects = allowed_touch_objects # ["table", "part"]
        
    if pre_grasp_approach != None:
        grasp.pre_grasp_approach = pre_grasp_approach
        
    if post_grasp_retreat != None:
        grasp.post_grasp_retreat = post_grasp_retreat


    grasp.max_contact_force = 0
    #grasp.grasp_quality = 0
    
    return grasp
예제 #3
0
def create_grasp(grasp_pose, allowed_touch_objects=[], pre_grasp_posture=None, grasp_posture=None,
                 pre_grasp_approach=None, post_grasp_retreat=None, id_grasp="grasp_"):
    grasp = Grasp()
    grasp.id = id_grasp
    grasp.grasp_pose = grasp_pose
    if pre_grasp_posture is None:
        grasp.pre_grasp_posture = get_grasp_posture(True)
    else:
        grasp.pre_grasp_posture = pre_grasp_posture

    if grasp_posture is None:
        grasp.grasp_posture = get_grasp_posture(False)
    else:
        grasp.grasp_posture = grasp_posture

    grasp.allowed_touch_objects = allowed_touch_objects

    if pre_grasp_approach is not None:
        grasp.pre_grasp_approach = pre_grasp_approach

    if post_grasp_retreat is not None:
        grasp.post_grasp_retreat = post_grasp_retreat

    grasp.max_contact_force = 0

    return grasp
    def make_grasps(self, initial_pose_stamped, allowed_touch_objects):
        # Initialize the grasp object
        g = Grasp()

        # Set the pre-grasp and grasp postures appropriately
        g.pre_grasp_posture = self.make_gripper_posture(GRIPPER_OPEN)
        g.grasp_posture = self.make_gripper_posture(GRIPPER_CLOSED)

        # Set the approach and retreat parameters as desired
        g.pre_grasp_approach = self.make_gripper_translation(0.01, 0.2, [0.0, 1.0, 0.0])
        g.post_grasp_retreat = self.make_gripper_translation(0.1, 0.2, [0.0, 0.0, 1.0])

        # Set the first grasp pose to the input pose
        g.grasp_pose = initial_pose_stamped

        # Pitch angles to try
        pitch_vals = [0, 0.1, -0.1, 0.2, -0.2, 0.3, -0.3]

        # Yaw angles to try
        yaw_vals = [0]

        # A list to hold the grasps
        grasps = []

        # Generate a grasp for each pitch and yaw angle
        for y in yaw_vals:
            for p in pitch_vals:
                # Create a quaternion from the Euler angles
                # q = quaternion_from_euler(0, p, y)

                # # Set the grasp pose orientation accordingly
                # g.grasp_pose.pose.orientation.x = q[0]
                # g.grasp_pose.pose.orientation.y = q[1]
                # g.grasp_pose.pose.orientation.z = q[2]
                # g.grasp_pose.pose.orientation.w = q[3]

                q = quaternion_from_euler(0, 0, -1.57079633)
                g.grasp_pose.pose.orientation.x = q[0]
                g.grasp_pose.pose.orientation.y = q[1]
                g.grasp_pose.pose.orientation.z = q[2]
                g.grasp_pose.pose.orientation.w = q[3]

                # Set and id for this grasp (simply needs to be unique)
                g.id = str(len(grasps))

                # Set the allowed touch objects to the input list
                g.allowed_touch_objects = allowed_touch_objects

                # Don't restrict contact force
                g.max_contact_force = 0

                # Degrade grasp quality for increasing pitch angles
                g.grasp_quality = 1.0 - abs(p)

                # Append the grasp to the list
                grasps.append(deepcopy(g))

        # Return the list
        return grasps
def createGrasp(grasp_pose):
    grasp = Grasp()
    grasp.id = "test"   
    grasp.grasp_pose = grasp_pose
    
#     grasp.pre_grasp_approach.direction.header.frame_id = "base_link"
#     grasp.pre_grasp_approach.direction.header.stamp = rospy.Time.now()
#     grasp.pre_grasp_approach.direction.vector.x = 1.0
#     grasp.pre_grasp_approach.direction.vector.y = 0.0
#     grasp.pre_grasp_approach.direction.vector.z = 0.0
#     grasp.pre_grasp_approach.desired_distance = 0.05
#     grasp.pre_grasp_approach.min_distance = 0.01
    
    grasp.pre_grasp_posture.header.frame_id = "base_link" # what link do i need here?
    grasp.pre_grasp_posture.header.stamp = rospy.Time.now() 
    grasp.pre_grasp_posture.joint_names = ["hand_right_thumb_joint", "hand_right_index_joint", "hand_right_middle_joint"]
    pos = JointTrajectoryPoint() # pre-grasp with thumb down and fingers open
    pos.positions.append(2.0)
    pos.positions.append(0.0)
    pos.positions.append(0.0)
    grasp.pre_grasp_posture.points.append(pos)
     
     
    grasp.grasp_posture.header.frame_id = "base_link"
    grasp.grasp_posture.header.stamp = rospy.Time.now() 
    grasp.grasp_posture.joint_names = ["hand_right_thumb_joint", "hand_right_index_joint", "hand_right_middle_joint"]
    pos = JointTrajectoryPoint() # grasp with all closed
    pos.positions.append(2.0)
    pos.positions.append(2.0)
    pos.positions.append(2.0)
    grasp.grasp_posture.points.append(pos)
    
#     grasp.post_grasp_retreat.direction.header.frame_id = "base_link"
#     grasp.post_grasp_retreat.direction.header.stamp = rospy.Time.now()
#     grasp.post_grasp_retreat.direction.vector.x = 0.0
#     grasp.post_grasp_retreat.direction.vector.y = 0.0
#     grasp.post_grasp_retreat.direction.vector.z = 1.0
#     grasp.post_grasp_retreat.desired_distance = 0.1
#     grasp.post_grasp_retreat.min_distance = 0.01
#     grasp.allowed_touch_objects = ["table", "part"]
    
    grasp.max_contact_force = 0
    
    return grasp
    def make_grasps(self, initial_pose, allow_touch_objects):
        # initialise a grasp object
        g = Grasp()

        g.pre_grasp_posture = self.make_grab_posture(self.open_joint_values)
        g.grasp_posture = self.make_grab_posture(self.closed_joint_values)

        g.pre_grasp_approach = self.make_grab_translation(0.01, 0.1, [0.0, 0.0, 1.0])
        g.post_grasp_retreat = self.make_grab_translation(0.1, 0.15, [0.0, 0.0, 1.0])

        g.grasp_pose = initial_pose

        # pitch_vals = [0, 0.1, -0.1, 0.2, -0.2, 0.4, -0.4]
        #
        # target_pose_arm_ref = self.tf_buffer.transform(initial_pose,'hand_iiwa_link_0')
        # x = target_pose_arm_ref.pose.position.x
        # y = target_pose_arm_ref.pose.position.y
        # yaw_vals = [math.atan2(y,x) + inc for inc in [0, 0.1, -0.1]]
        #
        grasps = []
        #
        # for yaw in yaw_vals:
        #     for pitch in pitch_vals:
        #         q = quaternion_from_euler(0,pitch,yaw)
        #
        #         g.grasp_pose.pose.orientation.x = q[0]
        #         g.grasp_pose.pose.orientation.y = q[1]
        #         g.grasp_pose.pose.orientation.z = q[2]
        #         g.grasp_pose.pose.orientation.w = q[3]

        g.id = str(len(grasps))

        g.allowed_touch_objects = allow_touch_objects
        g.max_contact_force = 0
        g.grasp_quality = 1.0 #- abs(pitch)

        grasps.append(g)

        return grasps
예제 #7
0
    def create_grasp(self, pose, grasp_id):
        """
        :type pose: Pose
            pose of the gripper for the grasp
        :type grasp_id: str
            name for the grasp
        :rtype: Grasp
        """
        g = Grasp()
        g.id = grasp_id

        pre_grasp_posture = JointTrajectory()
        pre_grasp_posture.header.frame_id = self._grasp_postures_frame_id
        pre_grasp_posture.joint_names = [
            name for name in self._gripper_joint_names.split()]
        jtpoint = JointTrajectoryPoint()
        jtpoint.positions = [
            float(pos) for pos in self._gripper_pre_grasp_positions.split()]
        jtpoint.time_from_start = rospy.Duration(self._time_pre_grasp_posture)
        pre_grasp_posture.points.append(jtpoint)

        grasp_posture = copy.deepcopy(pre_grasp_posture)
        grasp_posture.points[0].time_from_start = rospy.Duration(
            self._time_pre_grasp_posture + self._time_grasp_posture)
        jtpoint2 = JointTrajectoryPoint()
        jtpoint2.positions = [
            float(pos) for pos in self._gripper_grasp_positions.split()]
        jtpoint2.time_from_start = rospy.Duration(
            self._time_pre_grasp_posture +
            self._time_grasp_posture + self._time_grasp_posture_final)
        grasp_posture.points.append(jtpoint2)

        g.pre_grasp_posture = pre_grasp_posture
        g.grasp_posture = grasp_posture

        header = Header()
        header.frame_id = self._grasp_pose_frame_id  # base_footprint
        q = [pose.orientation.x, pose.orientation.y,
             pose.orientation.z, pose.orientation.w]
        # Fix orientation from gripper_link to parent_link (tool_link)
        fix_tool_to_gripper_rotation_q = quaternion_from_euler(
            math.radians(self._fix_tool_frame_to_grasping_frame_roll),
            math.radians(self._fix_tool_frame_to_grasping_frame_pitch),
            math.radians(self._fix_tool_frame_to_grasping_frame_yaw)
        )
        q = quaternion_multiply(q, fix_tool_to_gripper_rotation_q)
        fixed_pose = copy.deepcopy(pose)
        fixed_pose.orientation = Quaternion(*q)

        g.grasp_pose = PoseStamped(header, fixed_pose)
        g.grasp_quality = self._grasp_quality

        g.pre_grasp_approach = GripperTranslation()
        g.pre_grasp_approach.direction.vector.x = self._pre_grasp_direction_x  # NOQA
        g.pre_grasp_approach.direction.vector.y = self._pre_grasp_direction_y  # NOQA
        g.pre_grasp_approach.direction.vector.z = self._pre_grasp_direction_z  # NOQA
        g.pre_grasp_approach.direction.header.frame_id = self._grasp_postures_frame_id  # NOQA
        g.pre_grasp_approach.desired_distance = self._grasp_desired_distance  # NOQA
        g.pre_grasp_approach.min_distance = self._grasp_min_distance
        g.post_grasp_retreat = GripperTranslation()
        g.post_grasp_retreat.direction.vector.x = self._post_grasp_direction_x  # NOQA
        g.post_grasp_retreat.direction.vector.y = self._post_grasp_direction_y  # NOQA
        g.post_grasp_retreat.direction.vector.z = self._post_grasp_direction_z  # NOQA
        g.post_grasp_retreat.direction.header.frame_id = self._grasp_postures_frame_id  # NOQA
        g.post_grasp_retreat.desired_distance = self._grasp_desired_distance  # NOQA
        g.post_grasp_retreat.min_distance = self._grasp_min_distance

        g.max_contact_force = self._max_contact_force
        g.allowed_touch_objects = self._allowed_touch_objects

        return g
예제 #8
0
    def make_grasps(self,
                    initial_pose_stamped,
                    allowed_touch_objects,
                    grasp_opening=[0]):
        # Initialize the grasp object
        g = Grasp()

        # Set the pre-grasp and grasp postures appropriately;
        # grasp_opening should be a bit smaller than target width
        g.pre_grasp_posture = self.make_gripper_posture(self.gripper_opened)
        g.grasp_posture = self.make_gripper_posture(grasp_opening)

        # Set the approach and retreat parameters as desired
        g.pre_grasp_approach = self.make_gripper_translation(
            0.1, 0.15, [0.0, 0.0, -1.5])
        g.post_grasp_retreat = self.make_gripper_translation(
            0.1, 0.15, [0.0, 0.0, 1.5])

        # Set the first grasp pose to the input pose
        g.grasp_pose = initial_pose_stamped

        # Pitch angles to try
        pitch_vals = [
            0, 0.1, -0.1, 0.2, -0.2, 0.4, -0.4, 0.5, -0.5, 0.6, -0.6, 0.7,
            -0.7, 0.8, -0.8, 0.9, -0.9, 1.0, -1.0, 1.1, -1.1, 1.2, -1.2, 1.3,
            -1.3, 1.4, -1.4, 1.5, -1.5
        ]

        # Yaw angles to try; given the limited dofs of turtlebot_arm, we must calculate the heading
        # from arm base to the object to pick (first we must transform its pose to arm base frame)
        target_pose_arm_ref = self._tf2_buff.transform(initial_pose_stamped,
                                                       ARM_BASE_FRAME)
        x = target_pose_arm_ref.pose.position.x
        y = target_pose_arm_ref.pose.position.y
        yaw_vals = [atan2(y, x) + inc for inc in [0, 0.1, -0.1]]

        # A list to hold the grasps
        grasps = []

        # Generate a grasp for each pitch and yaw angle
        for yaw in yaw_vals:
            for pitch in pitch_vals:
                # Create a quaternion from the Euler angles
                q = quaternion_from_euler(0, pitch, yaw)

                # Set the grasp pose orientation accordingly
                g.grasp_pose.pose.orientation.x = q[0]
                g.grasp_pose.pose.orientation.y = q[1]
                g.grasp_pose.pose.orientation.z = q[2]
                g.grasp_pose.pose.orientation.w = q[3]

                # Set and id for this grasp (simply needs to be unique)
                g.id = str(len(grasps))

                # Set the allowed touch objects to the input list
                g.allowed_touch_objects = allowed_touch_objects

                # Don't restrict contact force
                g.max_contact_force = 0

                # Degrade grasp quality for increasing pitch angles
                g.grasp_quality = 1.0 - abs(pitch)

                # Append the grasp to the list
                grasps.append(deepcopy(g))

        # Return the list
        return grasps
예제 #9
0
    g.grasp_posture.joint_names = ["finger_joint"]

    pos = JointTrajectoryPoint()
    pos.positions.append(0.2)
    pos.effort.append(0.0)

    g.grasp_posture.points.append(pos)

    # set the post-grasp retreat
    g.post_grasp_retreat.direction.header.frame_id = "world"
    g.post_grasp_retreat.direction.vector.x = 0.0
    g.post_grasp_retreat.direction.vector.y = 0.0
    g.post_grasp_retreat.direction.vector.z = 1.0
    g.post_grasp_retreat.desired_distance = 0.25
    g.post_grasp_retreat.min_distance = 0.01

    g.allowed_touch_objects = ["table"]

    g.max_contact_force = 0

    # append the grasp to the list of grasps
    grasps.append(g)

    rospy.sleep(2)

    # pick the object
    robot.arm.pick("part", grasps)

    rospy.spin()
    roscpp_shutdown()
예제 #10
0
    pos.positions.append(0.2)
    pos.effort.append(0.0)
   
    g.grasp_posture.points.append(pos)

    # set the post-grasp retreat
    g.post_grasp_retreat.direction.header.frame_id = "base_footprint"
    g.post_grasp_retreat.direction.vector.x = 1
    g.post_grasp_retreat.direction.vector.y = -1
    g.post_grasp_retreat.direction.vector.z = 1
    g.post_grasp_retreat.desired_distance = 0.35
    g.post_grasp_retreat.min_distance = 0.01

    g.allowed_touch_objects = ["table"]

    g.max_contact_force = 0
    
    # append the grasp to the list of grasps
    grasps.append(g)

    
    
    # pick the object
    #robot.right_arm.pick("part", grasps)
    result = False
    n_attempts = 0
       
    # repeat until will succeed
    while result == False:
        result = robot.right_arm.pick("part", grasps)      
        n_attempts += 1
예제 #11
0
    def grasp_array(cls, msg, obj):
        # for me...
        for i in range(msg['_length_grasp']):
            gsp = Grasp()

            # direct descendants of grasp:
            gsp.id = msg["%s_id" % i]
            gsp.grasp_quality = msg["%s_grasp_quality" % i]
            gsp.max_contact_force = msg["%s_max_contact_force" % i]
            gsp.allowed_touch_objects = msg["%s_allowed_touch_objects" % i]

            # descendants of pre_grasp_posture:
            gsp.pre_grasp_posture.header = \
                decode.header(msg, gsp.pre_grasp_posture.header, "%s_pre" % i)
            gsp.pre_grasp_posture.joint_names = msg['%s_prejoint_names' % i]
            for j in range(msg['%s_length_points1' % i]):
                jtp1 = JointTrajectoryPoint()
                jtp1 = decode.grasp_points(msg, jtp1, "%s_%s_pre" % (i, j))
                gsp.pre_grasp_posture.points.append(jtp1)

            # descendants of grasp_posture:
            gsp.grasp_posture.header = \
                decode.header(msg, gsp.grasp_posture.header, "%s_posture" % i)
            gsp.grasp_posture.joint_names = msg["%s_joint_names" % i]
            for j in range(msg['%s_length_points2' % i]):
                jtp2 = JointTrajectoryPoint()
                jtp2 = decode.grasp_points(msg, jtp2, "%s_%s_posture" % (i, j))
                gsp.grasp_posture.points.append(jtp2)

            # descendants of grasp_pose:
            gsp.grasp_pose.header = \
                decode.header(msg, gsp.grasp_pose.header, "%s_pose" % i)
            gsp.grasp_pose.pose.position = \
                decode.position(msg, gsp.grasp_pose.pose.position,
                                "%s_pose" % i)
            gsp.grasp_pose.pose.orientation = \
                decode.orientation(msg, gsp.grasp_pose.pose.orientation,
                                   "%s_pose" % i)

            # descendants of pre_grasp_approach:
            gsp.pre_grasp_approach.direction.header = \
                decode.header(msg, gsp.pre_grasp_approach.direction.header,
                              "%s_app" % i)
            gsp.pre_grasp_approach.direction.vector = \
                decode.vector(msg, gsp.pre_grasp_approach.direction.vector,
                              "%s_app" % i)
            gsp.pre_grasp_approach.desired_distance = \
                msg['%s_appdesired_distance' % i]
            gsp.pre_grasp_approach.min_distance = msg['%s_appmin_distance' % i]

            # descendants of post_grasp_retreat:
            gsp.post_grasp_retreat.direction.header = \
                decode.header(msg, gsp.post_grasp_retreat.direction.header,
                              "%s_ret" % i)
            gsp.post_grasp_retreat.direction.vector = \
                decode.vector(msg, gsp.post_grasp_retreat.direction.vector,
                              "%s_ret" % i)
            gsp.post_grasp_retreat.desired_distance = \
                msg['%s_retdesired_distance' % i]
            gsp.post_grasp_retreat.min_distance = msg['%s_retmin_distance' % i]

            # descendants of post_place_retreat
            gsp.post_place_retreat.direction.header = \
                decode.header(msg, gsp.post_place_retreat.direction.header,
                              '%s_place' % i)
            gsp.post_place_retreat.direction.vector = \
                decode.vector(msg, gsp.post_place_retreat.direction.vector,
                              '%s_place' % i)
            gsp.post_place_retreat.desired_distance = \
                msg['%s_pldesired_distance' % i]
            gsp.post_place_retreat.min_distance = msg['%s_plmin_distance' % i]

            # append Grasp to GraspArray:
            obj.grasps.append(gsp)
        return(obj)
예제 #12
0
    def make_grasps(self, initial_pose_stamped, allowed_touch_objects):
        # Initialize the grasp object
        g = Grasp()

        # Set the pre-grasp and grasp postures appropriately
        g.pre_grasp_posture = self.make_gripper_posture(GRIPPER_OPEN)
        g.grasp_posture = self.make_gripper_posture(GRIPPER_GRASP)

        # Set the approach and retreat parameters as desired
        g.pre_grasp_approach = self.make_gripper_translation(
            0.1, 0.1, [0, 1, 0])
        g.post_grasp_retreat = self.make_gripper_translation(
            0.1, 0.15, [1, 0, 0])

        # Set the first grasp pose to the input pose
        g.grasp_pose = initial_pose_stamped

        ideal_roll = 0
        ideal_pitch = 0
        ideal_yaw = 0

        step_size = 0.1
        idx = 0.1
        idx_roll = ideal_roll + idx
        idx_pitch = ideal_pitch + idx
        idx_yaw = ideal_yaw + idx
        roll_vals = []
        pitch_vals = []
        yaw_vals = []
        while idx >= -0.1:
            roll_vals.append(idx_roll)
            pitch_vals.append(idx_pitch)
            yaw_vals.append(idx_yaw)
            idx -= step_size
            idx_roll -= step_size
            idx_pitch -= step_size
            idx_yaw -= step_size

        # A list to hold the grasps
        grasps = []

        print "Generating Poses"

        # Generate a grasp for each roll pitch and yaw angle
        for r in roll_vals:
            for y in yaw_vals:
                for p in pitch_vals:
                    # Create a quaternion from the Euler angles
                    q = quaternion_from_euler(r, p, y)

                    # Set the grasp pose orientation accordingly
                    g.grasp_pose.pose.orientation.x = q[0]
                    g.grasp_pose.pose.orientation.y = q[1]
                    g.grasp_pose.pose.orientation.z = q[2]
                    g.grasp_pose.pose.orientation.w = q[3]

                    # Set and id for this grasp (simply needs to be unique)
                    g.id = str(len(grasps))

                    # Set the allowed touch objects to the input list
                    g.allowed_touch_objects = allowed_touch_objects

                    # Don't restrict contact force
                    g.max_contact_force = 0

                    # Degrade grasp quality for increasing pitch angles
                    g.grasp_quality = 1.0 - abs(p)

                    # Append the grasp to the list
                    grasps.append(deepcopy(g))

        print "Generated " + g.id + " poses"
        # Return the list
        return grasps
예제 #13
0
    def create_grasp_2(self, pose, grasp_id):
        """
        :type pose: Pose
            pose of the gripper for the grasp
        :type grasp_id: str
            name for the grasp
        :rtype: Grasp
        """
        g = Grasp()
        g.id = grasp_id

        pre_grasp_posture = JointTrajectory()
        pre_grasp_posture.header.frame_id = self._grasp_postures_frame_id
        pre_grasp_posture.joint_names = [
            name for name in self._gripper_joint_names.split()
        ]
        jtpoint = JointTrajectoryPoint()
        jtpoint.positions = [
            float(pos) for pos in self._gripper_pre_grasp_positions.split()
        ]
        jtpoint.time_from_start = rospy.Duration(self._time_pre_grasp_posture)
        pre_grasp_posture.points.append(jtpoint)

        grasp_posture = copy.deepcopy(pre_grasp_posture)
        grasp_posture.points[0].time_from_start = rospy.Duration(
            self._time_pre_grasp_posture + self._time_grasp_posture)
        jtpoint2 = JointTrajectoryPoint()
        jtpoint2.positions = [
            float(pos) for pos in self._gripper_grasp_positions.split()
        ]
        jtpoint2.time_from_start = rospy.Duration(
            self._time_pre_grasp_posture + self._time_grasp_posture +
            self._time_grasp_posture_final)
        grasp_posture.points.append(jtpoint2)

        g.pre_grasp_posture = pre_grasp_posture
        g.grasp_posture = grasp_posture

        header = Header()
        header.frame_id = self._grasp_pose_frame_id

        fixed_pose = copy.deepcopy(pose)

        g.grasp_pose = PoseStamped(header, fixed_pose)
        g.grasp_quality = self._grasp_quality

        g.pre_grasp_approach = GripperTranslation()
        g.pre_grasp_approach.direction.vector.x = self._pre_grasp_direction_x  # NOQA
        g.pre_grasp_approach.direction.vector.y = self._pre_grasp_direction_y  # NOQA
        g.pre_grasp_approach.direction.vector.z = self._pre_grasp_direction_z  # NOQA
        g.pre_grasp_approach.direction.header.frame_id = self._grasp_postures_frame_id  # NOQA
        g.pre_grasp_approach.desired_distance = self._grasp_desired_distance  # NOQA
        g.pre_grasp_approach.min_distance = self._grasp_min_distance
        g.post_grasp_retreat = GripperTranslation()
        g.post_grasp_retreat.direction.vector.x = self._post_grasp_direction_x  # NOQA
        g.post_grasp_retreat.direction.vector.y = self._post_grasp_direction_y  # NOQA
        g.post_grasp_retreat.direction.vector.z = self._post_grasp_direction_z  # NOQA
        g.post_grasp_retreat.direction.header.frame_id = self._grasp_postures_frame_id  # NOQA
        g.post_grasp_retreat.desired_distance = 0.13  # NOQA
        g.post_grasp_retreat.min_distance = self._grasp_min_distance

        g.max_contact_force = self._max_contact_force
        g.allowed_touch_objects = self._allowed_touch_objects

        return g
예제 #14
0
    def create_grasp(self, pose, grasp_id):
        """
        :type pose: Pose
            pose of the gripper for the grasp
        :type grasp_id: str
            name for the grasp
        :rtype: Grasp
        """
        g = Grasp()
        g.id = grasp_id

        pre_grasp_posture = JointTrajectory()
        pre_grasp_posture.header.frame_id = self._grasp_postures_frame_id
        pre_grasp_posture.joint_names = [
            name for name in self._gripper_joint_names.split()
        ]
        jtpoint = JointTrajectoryPoint()
        jtpoint.positions = [
            float(pos) for pos in self._gripper_pre_grasp_positions.split()
        ]
        jtpoint.time_from_start = rospy.Duration(self._time_pre_grasp_posture)
        pre_grasp_posture.points.append(jtpoint)

        grasp_posture = copy.deepcopy(pre_grasp_posture)
        grasp_posture.points[0].time_from_start = rospy.Duration(
            self._time_pre_grasp_posture + self._time_grasp_posture)
        jtpoint2 = JointTrajectoryPoint()
        jtpoint2.positions = [
            float(pos) for pos in self._gripper_grasp_positions.split()
        ]
        jtpoint2.time_from_start = rospy.Duration(
            self._time_pre_grasp_posture + self._time_grasp_posture +
            self._time_grasp_posture_final)
        grasp_posture.points.append(jtpoint2)

        g.pre_grasp_posture = pre_grasp_posture
        g.grasp_posture = grasp_posture

        header = Header()
        header.frame_id = self._grasp_pose_frame_id
        q = [
            pose.orientation.x, pose.orientation.y, pose.orientation.z,
            pose.orientation.w
        ]
        # Fix orientation from gripper_link to parent_link (tool_link)
        fix_tool_to_gripper_rotation_q = quaternion_from_euler(
            math.radians(self._fix_tool_frame_to_grasping_frame_roll),
            math.radians(self._fix_tool_frame_to_grasping_frame_pitch),
            math.radians(self._fix_tool_frame_to_grasping_frame_yaw))
        q = quaternion_multiply(q, fix_tool_to_gripper_rotation_q)
        fixed_pose = copy.deepcopy(pose)
        fixed_pose.orientation = Quaternion(*q)

        g.grasp_pose = PoseStamped(header, fixed_pose)
        g.grasp_quality = self._grasp_quality

        g.pre_grasp_approach = GripperTranslation()
        g.pre_grasp_approach.direction.vector.x = self._pre_grasp_direction_x  # NOQA
        g.pre_grasp_approach.direction.vector.y = self._pre_grasp_direction_y  # NOQA
        g.pre_grasp_approach.direction.vector.z = self._pre_grasp_direction_z  # NOQA
        g.pre_grasp_approach.direction.header.frame_id = self._grasp_postures_frame_id  # NOQA
        g.pre_grasp_approach.desired_distance = self._grasp_desired_distance  # NOQA
        g.pre_grasp_approach.min_distance = self._grasp_min_distance
        g.post_grasp_retreat = GripperTranslation()
        g.post_grasp_retreat.direction.vector.x = self._post_grasp_direction_x  # NOQA
        g.post_grasp_retreat.direction.vector.y = self._post_grasp_direction_y  # NOQA
        g.post_grasp_retreat.direction.vector.z = self._post_grasp_direction_z  # NOQA
        g.post_grasp_retreat.direction.header.frame_id = self._grasp_postures_frame_id  # NOQA
        g.post_grasp_retreat.desired_distance = 0.13  # NOQA
        g.post_grasp_retreat.min_distance = self._grasp_min_distance

        g.max_contact_force = self._max_contact_force
        g.allowed_touch_objects = self._allowed_touch_objects

        return g