예제 #1
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 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 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 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 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
예제 #6
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
예제 #7
0
def compute_grasp(joints_open_position,
                  open_hand_time,
                  close_hand_time,
                  pre_grasp_aproach_direction,
                  pre_grasp_desired_dis,
                  pre_gras_min_dis,
                  post_grasp_aproach_direction,
                  post_grasp_desired_dis,
                  post_gras_min_dis,
                  world_file,
                  package_name,
                  package_folder,
                  world_object_pose,
                  aditional_rotation=None,
                  aditional_translation=None,
                  max_contact_force=-1,
                  allowed_touch_objects=[],
                  reference_frame="world"):
    """
    Compute the grasp of an object
    inputs:
    --joints_open_position: dictionary of the joints and angle in open position (pre-grasp)
    --open_hand_time: time desired to open hand
    --close_hand_time: time to reach grasp position with the fingers.
    --pre_grasp_aproach_direction:  (x,y,z) direction end effector will aproach grasp_poses in world frame
    --pre_grasp_desired_dis: distance will travel when aproaching 
    --pre_gras_min_dis: the min distance that must be considered feasible before the grasp is even attempted
    --post_grasp_aproach_direction: (x,y,z) direction end effector will retreat after closing the hand (from grasp_pose). in world frame
    --post_grasp_desired_dis: distance that will retreat
    --post_gras_min_dis: min distance to consider is enough far away?
    --max_contact_force: the maximum contact force to use while grasping (<=0 to disable)
    --allowed_touch_objects: objects that can be touched/pushed/moved in the course of grasping. list of strings.
    --world_file: world file of the hand holding the object. For example 'mpl_checker_v3.xml'
    --package_name: name of the package where the world file is located
    --package_folder: location of the folders "moldels" and "worlds" from graspit, inside the package
    --world_object_pose: object Pose in world frame (gazebo/moveit)
    --aditional rotation and translation: can be include in case of unconsidered transformations between the object in gazebo and graspit (not needed of them if frame if the same when object in graspit and gazebo when no rotation and translation is the same)
    --reference_frame: directions and object position reference frame
    """
    filename = "worlds/" + world_file
    #read world file
    robot_joints, T_robot_graspit, T_object_graspit = utils.read_world_file(
        filename, package_name, package_folder)

    #create moveit Grasp
    grasp = Grasp()

    #grasp id (optional)

    #define pre-grasp joints posture. basically open hand.
    grasp.pre_grasp_posture = utils.get_posture(joints_open_position,
                                                open_hand_time)

    #define hand finger posture during grasping
    grasp.grasp_posture = utils.get_posture(robot_joints, close_hand_time)

    #define grasp pose: position of the end effector during grasp
    grasp.grasp_pose.header.frame_id = reference_frame  #pose in this reference frame
    grasp.grasp_pose.pose = utils.get_robot_pose(T_robot_graspit,
                                                 T_object_graspit,
                                                 world_object_pose,
                                                 aditional_rotation,
                                                 aditional_translation)

    #grasp quality (no needed/used. can be obtained from graspit)

    #pre_grasp_approach.The approach direction the robot will move when aproaching the object
    grasp.pre_grasp_approach = utils.get_gripper_translation(
        pre_grasp_aproach_direction, pre_grasp_desired_dis, pre_gras_min_dis,
        reference_frame)

    #post_grasp_retreat. The retreat direction to take after a grasp has been completed (object is attached)
    grasp.post_grasp_retreat = utils.get_gripper_translation(
        post_grasp_aproach_direction, post_grasp_desired_dis,
        post_gras_min_dis, reference_frame)

    #max contact force
    grasp.max_contact_force = max_contact_force

    #allowd_tocuh_force
    grasp.allowed_touch_objects = allowed_touch_objects

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

        #g.grasp_pose = initial_pose_stamped
        #q0 = g.grasp_pose.pose.orientation.w
        #q1 = g.grasp_pose.pose.orientation.x
        #q2 = g.grasp_pose.pose.orientation.y
        #q3 = g.grasp_pose.pose.orientation.z
        #eulerR = atan2(2*(q0*q1+q2*q3),1-2*(q1*q1+q2*q2))
        #eulerP = asin(2*(q0*q2-q1*q3))
        #eulerY = atan2(2*(q0*q3+q1*q2),1-2*(q2*q2+q3*q3))
        #print(eulerR)
        #print(eulerY)
        #print(eulerP)
        # 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.1, [1.0, 0.0, 0.0])
        g.post_grasp_retreat = self.make_gripper_translation(
            0.1, 0.15, [0.0, 0, 1.7])

        # 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, 0.5, 0.4, 0.6]

        # Yaw angles to try
        yaw_vals = [0]

        # Roll angles to try
        roll_vals = [-3.14, 0]

        # A list to hold the grasps
        grasps = []

        # Generate a grasp for each 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))

        # Return the list
        return grasps
예제 #9
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
예제 #10
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.01, 0.1, [1.0, 0.0, 0.0])
        g.post_grasp_retreat = self.make_gripper_translation(
            0.1, 0.15, [0.0, -1.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.4, -0.4]

        # 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.tf_listener.transformPose(
            ARM_BASE_FRAME, initial_pose_stamped)
        x = target_pose_arm_ref.pose.position.x
        y = target_pose_arm_ref.pose.position.y

        self.pick_yaw = atan2(
            y,
            x)  # check in make_places method why we store the calculated yaw
        yaw_vals = [self.pick_yaw]

        # 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
예제 #11
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
예제 #12
0
def compute_checkers_grasp(checker_row, checker_col, open_hand_time,
                           close_hand_time, pre_grasp_aproach_direction,
                           pre_grasp_desired_dis, pre_gras_min_dis,
                           post_grasp_aproach_direction,
                           post_grasp_desired_dis, post_gras_min_dis,
                           max_contact_force, allowed_touch_objects,
                           robot_base_frame, grasp_file, pregrasp_file):
    def cell_info_client(from_row, from_col):
        """
        function to get checker information from gazebo
        """
        rospy.wait_for_service('checkers/cell_info/')
        try:
            cell_info = rospy.ServiceProxy('checkers/cell_info/', CellInfo)
            resp = cell_info(from_row, from_col)
            return resp
        except rospy.ServiceException as e:
            rospy.loginfo("Service call checker info failed: %s" % e)

    #define some parameters
    # grasp_file="mpl_checker_v4.xml" #grasping position
    # pregrasp_file="mpl_checker_pregrasp.xml"
    package_folder = 'resources'
    package_name = 'mpl_graspit'

    #create moveit Grasp
    grasp = Grasp()

    #find object information
    cell_info = cell_info_client(checker_row, checker_col)
    if not cell_info.available:
        #object pose in world frame
        objects_pose_w = cell_info.pose_checker
        # print("object pose")
        # print(str(objects_pose_w))

        #compute angle between shoulder and object (set a starting angle to start looking for for the grip of the checker. Later check. there is an angle that alwyas work?)
        angle = int(
            compute_angle_object_frame("mpl_right_arm__humerus",
                                       objects_pose_w))
        print("start looking IK from angle " + str(angle))

        # if robot_base_frame !="world":
        #     objects_pose_r=utils.transform_pose_between_frames(objects_pose_w, "world", robot_base_frame)#object pose in robot frame
        # else:
        #     objects_pose_r=objects_pose_w

        #read world file grasp
        filename = "worlds/" + grasp_file
        robot_joints_grasp, T_robot_graspit, T_object_graspit = utils.read_world_file(
            filename, package_name, package_folder)

        #read world file pregrasp
        filename_pre = "worlds/" + pregrasp_file
        robot_joints_pregrasp, _, _ = utils.read_world_file(
            filename_pre, package_name, package_folder)

        #check robot pose
        aditional_translation = None
        #find hand rotation that have inverse kinematic solution
        step = 1

        # for deg in range(angle,90,step):#check all circle. Rotation in Z axis
        for deg in range(angle, 360, step):
            rospy.loginfo("IK solver trying pose: %i deg rotation" % deg)
            aditional_rotation = np.array([
                math.cos(math.radians(deg / 2)), 0, 0,
                math.sin(math.radians(deg / 2))
            ])
            robot_pose_w = utils.get_robot_pose(T_robot_graspit,
                                                T_object_graspit,
                                                objects_pose_w,
                                                aditional_rotation,
                                                aditional_translation)

            if robot_base_frame != "world":
                robot_pose_r = utils.transform_pose_between_frames(
                    robot_pose_w, "world",
                    robot_base_frame)  #object pose in robot frame
            else:
                robot_pose_r = robot_pose_w
            # robot_pose_r=utils.get_robot_pose(T_robot_graspit ,T_object_graspit, objects_pose_r,aditional_rotation, aditional_translation)
            #tranform pose to robot frame

            #check is pose is reachabe using inverse kinematics (service must be available)
            is_valid = (IK_client(robot_pose_r)).is_solution
            if is_valid:
                rospy.loginfo("Pose can be achived: %i" % deg)

                #----complete grasp information---------------
                #define pre-grasp joints posture. basically open hand.
                grasp.pre_grasp_posture = utils.get_posture(
                    robot_joints_pregrasp, open_hand_time)

                #define hand finger posture during grasping
                grasp.grasp_posture = utils.get_posture(
                    robot_joints_grasp, close_hand_time)

                #define grasp pose: position of the end effector during grasp
                grasp.grasp_pose.header.frame_id = robot_base_frame  #pose in this reference frame
                grasp.grasp_pose.pose = robot_pose_r

                #pre_grasp_approach.The approach direction the robot will move when aproaching the object
                grasp.pre_grasp_approach = utils.get_gripper_translation(
                    pre_grasp_aproach_direction, pre_grasp_desired_dis,
                    pre_gras_min_dis, robot_base_frame)

                #post_grasp_retreat. The retreat direction to take after a grasp has been completed (object is attached)
                grasp.post_grasp_retreat = utils.get_gripper_translation(
                    post_grasp_aproach_direction, post_grasp_desired_dis,
                    post_gras_min_dis, robot_base_frame)

                #max contact force
                grasp.max_contact_force = max_contact_force

                #allowd_tocuh_force
                grasp.allowed_touch_objects = allowed_touch_objects

                return grasp

        # rospy.loginfo("Pose CAN NOT  be achived")
        # return None

        # grasp_result=grasps.compute_grasp(
        #     joints_open_position,open_hand_time,
        #     close_hand_time,
        #     pre_grasp_aproach_direction, pre_grasp_desired_dis, pre_gras_min_dis,
        #     post_grasp_aproach_direction, post_grasp_desired_dis, post_gras_min_dis,
        #     grasp_file,
        #     package_name,
        #     package_folder,
        #     world_object_pose,
        #     aditional_rotation,
        #     aditional_translation,
        #     max_contact_force,
        #     allowed_touch_objects,
        #     reference_frame)
        # return grasp_result

    else:
        rospy.loginfo("Cell " + cell_info.cell_name +
                      " doesn't have a checker piece")
예제 #13
0
    def __init__(self, robot_name="panda_arm", frame="panda_link0"):
        try:
            moveit_commander.roscpp_initialize(sys.argv)
            rospy.init_node(name="pick_place_test")
            self.scene = PlanningSceneInterface()
            self.scene_pub = rospy.Publisher('planning_scene',
                                             PlanningScene,
                                             queue_size=10)
            # region Robot initial
            self.robot = MoveGroupCommander(robot_name)
            self.robot.set_goal_joint_tolerance(0.00001)
            self.robot.set_goal_position_tolerance(0.00001)
            self.robot.set_goal_orientation_tolerance(0.01)
            self.robot.set_goal_tolerance(0.00001)
            self.robot.allow_replanning(True)
            self.robot.set_pose_reference_frame(frame)
            self.robot.set_planning_time(3)
            # endregion
            self.gripper = MoveGroupCommander("hand")
            self.gripper.set_joint_value_target(GRIPPER_CLOSED)
            self.gripper.go()
            self.gripper.set_joint_value_target(GRIPPER_OPEN)
            self.gripper.go()
            self.gripper.set_joint_value_target(GRIPPER_CLOSED)
            self.gripper.go()

            # Robot go home
            self.robot.set_named_target("home")
            self.robot.go()
            # clear all object in world
            self.clear_all_object()

            table_pose = un.Pose(0, 0, -10, 0, 0, 0)
            table_color = un.Color(255, 255, 0, 100)
            self.add_object_box("table", table_pose, table_color, frame,
                                (2000, 2000, 10))

            bearing_pose = un.Pose(250, 250, 500, -90, 45, -90)
            bearing_color = un.Color(255, 0, 255, 255)
            bearing_file_name = "../stl/bearing.stl"
            self.add_object_mesh("bearing", bearing_pose, bearing_color, frame,
                                 bearing_file_name)
            obpose = self.scene.get_object_poses(["bearing"])
            # self.robot.set_support_surface_name("table")
            g = Grasp()
            # Create gripper position open or close
            g.pre_grasp_posture = self.open_gripper()
            g.grasp_posture = self.close_gripper()
            g.pre_grasp_approach = self.make_gripper_translation(
                0.01, 0.1, [0, 1.0, 0])
            g.post_grasp_retreat = self.make_gripper_translation(
                0.01, 0.9, [0, 1.0, 0])
            p = PoseStamped()
            p.header.frame_id = "panda_link0"
            p.pose.orientation = obpose["bearing"].orientation
            p.pose.position = obpose["bearing"].position
            g.grasp_pose = p
            g.allowed_touch_objects = ["bearing"]
            a = []
            a.append(g)
            result = self.robot.pick(object_name="bearing", grasp=a)
            print(result)

        except Exception as ex:
            print(ex)
            moveit_commander.roscpp_shutdown()

        moveit_commander.roscpp_shutdown()
예제 #14
0
    # rospy.sleep(20)
    # (aim_x, aim_y, aim_z, aim_yaw) = onine_arm.get_valid_pose(item_translation[0], item_translation[1], item_translation[2], - 0.080)

    grasp_pose = PoseStamped()
    grasp_pose.header.frame_id = "left_gripper_link"
    grasp_pose.header.stamp = rospy.Time.now()
    grasp_pose.pose.position.x = aim_x
    grasp_pose.pose.position.y = aim_y
    grasp_pose.pose.position.z = aim_z
    grasp_pose.pose.orientation = Quaternion(
        *quaternion_from_euler(0.0, 0, aim_yaw))

    g = Grasp()
    g.pre_grasp_posture = onine_arm.make_gripper_posture(0.09)
    g.grasp_posture = onine_arm.make_gripper_posture(0.01)
    g.pre_grasp_approach = onine_arm.make_gripper_translation(0.08, 0.10)
    g.post_grasp_retreat = onine_arm.make_gripper_translation(0.08, 0.10, -1.0)
    g.grasp_pose = grasp_pose

    #2 degrees resolution
    pitch_vals = [
        -0.10472, -0.0698132, -0.0349066, 0, 0.0349066, 0.0698132, 0.10472
    ]
    height_vals = [
        -0.005, -0.004, -0.003, -0.002, -0.001, 0, 0.001, 0.002, 0.003, 0.004,
        0.005
    ]
    # generate list of grasps
    grasps = []
    for h in height_vals:
    g = Grasp()

    g.grasp_pose = tar_pose

    g.pre_grasp_approach.direction.vector.z = 1
    g.pre_grasp_approach.direction.header.frame_id = 'endeff'
    g.pre_grasp_approach.min_distance = 0.04
    g.pre_grasp_approach.desired_distance = 0.10

    g.post_grasp_retreat.direction.vector.z = -1
    g.post_grasp_retreat.direction.header.frame_id = 'endeff'
    g.post_grasp_retreat.min_distance = 0.04
    g.post_grasp_retreat.desired_distance = 0.10

    g.grasp_posture = make_gripper_posture(0)

    g.allowed_touch_objects = ["part"]

    grasps = []
    grasps.append(copy.deepcopy(g))

    p = PlaceLocation()

    p.post_place_posture = make_gripper_posture(-1.1158)

    p.place_pose.header.frame_id = 'world'
    p.place_pose.pose.position.x = -0.13341
    p.place_pose.pose.position.y = 0.12294
    p.place_pose.pose.position.z = 0.099833
    p.place_pose.pose.orientation.x = 0
예제 #16
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