Bimanual Pick and Place

Robotic systems with bimanual pick and place capabilities use two robotic arms to simultaneously pick up and manipulate objects, enabling more complex and versatile handling tasks. The adoption of bimanual robots has been increasing in industries such as manufacturing and assembly due to their ability to perform operations that require coordination between two hands, closely mimicking human dexterity and improving efficiency. However, programming and synchronizing two robotic arms introduces significant complexity, leading to higher costs in engineering, calibration, and maintenance. In this tutorial, we introduce a short program created using Jacobi, designed to be easily configurable for bimanual pick and place operations.

Video

Application Code

  1import time
  2from jacobi import Planner, Studio, MultiRobotPoint, MultiRobotLinearSection, BimanualMotion, LinearSection, Frame, CartesianWaypoint
  3import numpy as np
  4
  5
  6class BimanualPickAndPlace:
  7    def __init__(self, project_file='bimanual-pick-and-place.jacobi-project'):
  8        # Initialize studio and planner
  9        self.studio = Studio()
 10        self.planner = Planner.load_from_project_file(project_file)
 11        self.yumi = self.planner.environment.get_robots()[0]
 12        self.yumi.set_speed(0.4)
 13        self.to_pick, self.to_place = self.get_obstacles_to_pick_and_place()
 14        self.start = self.calculate_grasp_points(self.yumi, self.to_pick[0])
 15        self.home = MultiRobotPoint({self.yumi.left: CartesianWaypoint(Frame.from_euler(
 16            0.5, 0.1, 0.45, np.pi, 0.0, 0.0)), self.yumi.right: CartesianWaypoint(Frame.from_euler(0.5, -0.15, 0.45, np.pi, 0.0, 0.0))})
 17
 18    def calculate_grasp_points(self, robot, box):
 19        left_wp = CartesianWaypoint(Frame.from_euler(
 20            box.origin.translation[0], box.origin.translation[1] + 0.125, box.origin.translation[2], np.pi, 0.0, 0.0))
 21        right_wp = CartesianWaypoint(Frame.from_euler(
 22            box.origin.translation[0], box.origin.translation[1] - 0.125, box.origin.translation[2], np.pi, 0.0, 0.0))
 23        return MultiRobotPoint({robot.left: left_wp, robot.right: right_wp})
 24
 25    def get_obstacles_to_pick_and_place(self):
 26        b1 = self.planner.environment.get_obstacle('b1')
 27        b2 = self.planner.environment.get_obstacle('b2')
 28        b3 = self.planner.environment.get_obstacle('b3')
 29        to_pick = [b1, b2, b3]
 30        a1 = self.planner.environment.get_obstacle('a1')
 31        a2 = self.planner.environment.get_obstacle('a2')
 32        a3 = self.planner.environment.get_obstacle('a3')
 33        to_place = [a1, a2, a3]
 34        self.planner.environment.remove_obstacle(a1)
 35        self.planner.environment.remove_obstacle(a2)
 36        self.planner.environment.remove_obstacle(a3)
 37        return to_pick, to_place
 38
 39    def go_to_pick_point(self, box):
 40        # Disable collision for the box to be picked
 41        box.for_collision = False
 42        self.planner.environment.remove_obstacle(box)
 43
 44        # Calculate a trajectory from the home position to the pick point
 45        self.start = self.calculate_grasp_points(self.yumi, box)
 46        motion = BimanualMotion('', self.yumi, self.home, self.start)
 47        motion.linear_approach = MultiRobotLinearSection({self.yumi.left: LinearSection(offset=Frame.from_translation(
 48            0, 0, -0.04)), self.yumi.right: LinearSection(offset=Frame.from_translation(0, 0, -0.04))})
 49        trajectory = self.planner.plan(motion)
 50        self.studio.run_trajectory(trajectory, robot=self.yumi)
 51        self.start = MultiRobotPoint(
 52            {self.yumi.left: trajectory.positions[-1][0:7], self.yumi.right: trajectory.positions[-1][7:14]})
 53
 54    def pick_and_place_item(self, i):
 55        b = self.to_pick[i - 1]
 56
 57        self.go_to_pick_point(b)
 58        time.sleep(0.1)
 59
 60        # Calculate a trajectory from the pick point to the home position
 61        motion = BimanualMotion('', self.yumi, self.start, self.home)
 62        motion.linear_retraction = MultiRobotLinearSection({self.yumi.left: LinearSection(offset=Frame.from_translation(
 63            0, 0, -0.05)), self.yumi.right: LinearSection(offset=Frame.from_translation(0, 0, -0.05))})
 64        motion.is_coordinated = True
 65        trajectory = self.planner.plan(motion)
 66
 67        # Calculate a trajectory from the home position to the place point
 68        goal = self.calculate_grasp_points(self.yumi, self.to_place[i - 1])
 69        self.planner.environment.remove_obstacle(self.to_place[i - 1])
 70        self.home = MultiRobotPoint(
 71            {self.yumi.left: trajectory.positions[-1][0:7], self.yumi.right: trajectory.positions[-1][7:14]})
 72        motion = BimanualMotion('', self.yumi, self.home, goal)
 73        motion.is_coordinated = True
 74        motion.linear_approach = MultiRobotLinearSection({self.yumi.left: LinearSection(offset=Frame.from_translation(
 75            0, 0, -0.03)), self.yumi.right: LinearSection(offset=Frame.from_translation(0, 0, -0.03))})
 76        trajectory += self.planner.plan(motion)
 77
 78        # Before running the trajectory, remove the picked box from the environment
 79        # and add it as an item to the robot end-effector
 80        b.for_collision = False
 81        b.for_visual = False
 82        self.studio.remove_obstacle(b)
 83        item = b
 84        item.name = 'item'
 85        item.for_visual = True
 86        item.origin = Frame.from_translation(0.0, 0.125, 0.0)
 87        self.studio.set_item(item, self.yumi.left)
 88
 89        # Run the trajectory from pick point to place point
 90        self.studio.run_trajectory(trajectory, robot=self.yumi)
 91
 92        # Remove the item from the robot end-effector and place it as an obstacle to the environment
 93        item.for_visual = False
 94        self.studio.update_obstacle(item)
 95        self.to_place[i - 1].for_visual = True
 96        self.studio.update_obstacle(self.to_place[i - 1])
 97
 98        # Calculate a trajectory from the place point to the home position
 99        next_start = MultiRobotPoint({self.yumi.left: trajectory.positions[-1][0:7],
100                                      self.yumi.right: trajectory.positions[-1][7:14]})
101        motion = BimanualMotion('', self.yumi, next_start, self.home)
102        motion.linear_retraction = MultiRobotLinearSection({self.yumi.left: LinearSection(offset=Frame.from_translation(
103            0, 0, -0.04)), self.yumi.right: LinearSection(offset=Frame.from_translation(0, 0, -0.04))})
104        traj = self.planner.plan(motion)
105
106        # Run the trajectory from place point to the home position
107        self.studio.run_trajectory(traj, robot=self.yumi)
108
109    def run_pick_and_place(self):
110        for i in range(1, 4):
111            self.pick_and_place_item(i)
112
113
114if __name__ == '__main__':
115    bimanual_task = BimanualPickAndPlace()
116    bimanual_task.run_pick_and_place()