BaxterArmClient.py 文件源码

def go_to_position(self, task, destination, height, offset_x, offset_y, offset_z):
        """
        Calculate goal position, send that to the robot and wait for response.

        Args:
            task (string): Pick or place action
            destination (int): Destination rod [0, 1, 2]
            height (float): Height of the goal position (based on number of disk on the rod)
            offset_x (float): Offset in robot's x axis
            offset_y (float): Offset in robot's y axis
            offset_z (float): Offset in robot's z axis
        """
        goal = Pose()
        trans = self.transformations()
        if task == 'pick':
            height = get_pick_height(height)
        else:
            height = get_place_height(height)
        goal = self.position(goal, trans, height)

        # Calculate offset from the markers
        if destination == 0: offset_y += self.left_rod_offset
        if destination == 1: offset_y += 0
        if destination == 2: offset_y -= self.right_rod_offset
        offset_x -= self.center_rod_offset

        offset_x -= 0.1   # Moving from rod to rod should be done 10 cm in front of them
        offset_x -= 0.03  # Back up a little to compensate for the width of the disks

        # Update goal with calculated offsets
        goal.position.x += offset_x
        goal.position.y += offset_y
        goal.position.z += offset_z
        goal_final = baxterGoal(id=1, pose=goal)

        # Send goal to Baxter arm server and wait for result
        self.left_client.send_goal_and_wait(goal_final)
        result = self.left_client.get_result()
        if result.status:
            return 1
        else:
            return Errors.RaiseGoToFailed(task, destination, height, offset_x, offset_y, offset_z)