Bimanual Depalletizing

Robotic depalletizing is crucial in the logistics industry, enabling efficient use-cases like distribution or order fulfillment. Fast motion planning can significantly reduce cycle times and boost productivity. Jacobi’s software can plan bimanual motions, enabling simultaneous manipulation of items, optimizing throughput and minimizing cycle times. This project features three operational modes: Bimanual Mode, where both arms plan and execute pick-and-place actions simultaneously; Single-Arm Dynamic Mode, where one arm’s motion is planned first and the other follows while considering the first arm’s trajectory, with both arms executing their motions at the same time; and Single-Arm Interlock Mode, where one arm’s motion is planned while the other remains static, and the trajectories are executed sequentially.

Video

Application Code

  1import time
  2
  3from pathlib import Path
  4import yaml
  5from jacobi import Trajectory, DynamicRobotTrajectory, Frame, Studio, Box, Obstacle, Planner, Motion, LinearSection
  6from jacobi import BimanualMotion, MultiRobotLinearSection, MultiRobotPoint
  7
  8
  9class BimanualDepalletizer:
 10    """
 11    This class supports three distinct planning modes, each dictating how the robot arms coordinate during pick-and-place operations:
 12
 13    1. Bimanual Mode ( self.mode = 'bimanual' ):
 14    - Plans for both arms simultaneously to synchronously perform pick and place actions.
 15    - Executes the planned trajectories for both arms simultaneously.
 16
 17    2. Single-Arm Dynamic Mode ( self.mode = 'dynamic' ):
 18    - Plans the motion for a single arm, then plans the motion for the other arm while considering the first arm's trajectory.
 19    - Executes the planned trajectories for both arms simultaneously.
 20
 21    3. Single-Arm Interlock Mode ( self.mode = 'interlock' ):
 22    - Plans the motion for a single arm, then plans the motion for the other arm considering the first arm's static position.
 23    - Executes the planned trajectories for both arms sequentially.
 24
 25    """
 26
 27    def __init__(self, mode='bimanual'):
 28        self.mode = mode
 29        self.planner = Planner.load_from_project_file('bimanual-depalletizing.jacobi-project')
 30        self.studio = Studio()
 31
 32        self.pallet_left = self.planner.environment.get_obstacle('pallet_left').origin
 33        self.pallet_right = self.planner.environment.get_obstacle('pallet_right').origin
 34        self.box_left = Obstacle('box-left', Box(0.4, 0.2, 0.2), color='E1B471')
 35        self.box_right = Obstacle('box-right', Box(0.4, 0.2, 0.2), color='E1B471')
 36        self.box_pattern = self.load_pattern_from_file('pattern_box.yml', self.box_left.collision)
 37
 38        self.dualarm = self.planner.environment.get_robot('dualarm')
 39
 40        self.home = self.planner.environment.get_waypoint('home').position
 41        self.place_pose = self.planner.environment.get_waypoint('place_pose').position
 42
 43        self.right_current_joint_position = self.home
 44        self.left_current_joint_position = self.home
 45        self.studio.set_joint_position(self.home, robot=self.dualarm.left)
 46        self.studio.set_joint_position(self.home, robot=self.dualarm.right)
 47        time.sleep(1)
 48
 49    @staticmethod
 50    def load_pattern_from_file(path: Path, box: Box) -> list[list[Frame]]:
 51        """Load a box pattern from a yaml file."""
 52
 53        with (Path(__file__).absolute().parent / path).open('r') as f:
 54            data = yaml.safe_load(f)
 55
 56        def parse(p: dict, axis: str) -> float:
 57            element = p.get(axis, 0.0)
 58            if isinstance(element, str):
 59                return eval(element, {'x': box.x, 'y': box.y, 'z': box.z, 'g': 0.01})
 60            return element
 61
 62        def to_frame(p: dict) -> Frame:
 63            return Frame(x=parse(p, 'x'), y=parse(p, 'y'), z=parse(p, 'z'), c=parse(p, 'c'))
 64
 65        return [[to_frame(box) for box in layer['boxes']] for layer in data['layers']]
 66
 67    def plan_cached(self, m: Motion) -> Trajectory:
 68        cache_directory = Path('cache/depal')
 69        cache_directory.mkdir(exist_ok=True, parents=True)
 70
 71        trajectory_path = cache_directory / f'{m.name}.json'
 72        if trajectory_path.exists():
 73            return Trajectory.from_json_file(trajectory_path)
 74
 75        trajectory = self.planner.plan(m)
 76        trajectory.to_json_file(trajectory_path)
 77        return trajectory
 78
 79    def set_item(self, box_to_set, robot):
 80        box_as_item = box_to_set.with_origin(Frame(z=-box_to_set.collision.z / 2))
 81        self.studio.set_item(box_as_item, robot)
 82        robot.item_obstacle = box_as_item
 83        self.studio.remove_obstacle(box_to_set)
 84        self.planner.environment.remove_obstacle(box_to_set)
 85
 86    def remove_item(self, robot):
 87        self.studio.set_item(None, robot)
 88        robot.item_obstacle = None
 89
 90    def single_arm_interlock_pick_cycle(self, box_left, box_right):
 91        # Left arm from place to pick, right arm from pick to place
 92        pick = box_left.origin * Frame(z=box_left.collision.z / 2)
 93        place = self.place_pose * box_right.origin.rotation
 94
 95        m1 = Motion(f'{box_left.name}-to-pick-interlock', self.dualarm.left, self.left_current_joint_position, pick)
 96        m1.linear_retraction = LinearSection(offset=Frame(z=0.05))
 97        m1.linear_approach = LinearSection(offset=Frame(z=0.05))
 98        trajectoryl = self.plan_cached(m1)
 99        self.studio.run_trajectory(trajectoryl, robot=self.dualarm.left)
100        self.left_current_joint_position = trajectoryl.positions[-1]
101        self.set_item(box_left, self.dualarm.left)
102
103        m2 = Motion(f'{box_right.name}-to-place-interlock', self.dualarm.right, self.right_current_joint_position, place)
104        m2.linear_retraction = LinearSection(offset=Frame(z=box_right.collision.z + 0.01))
105        m2.linear_approach = LinearSection(offset=Frame(z=0.05))
106        trajectoryr = self.plan_cached(m2)
107        self.studio.run_trajectory(trajectoryr, robot=self.dualarm.right)
108        self.right_current_joint_position = trajectoryr.positions[-1]
109        self.remove_item(self.dualarm.right)
110
111        # Left arm from pick to place, right arm from place to pick
112        pick = box_right.origin * Frame(z=box_right.collision.z / 2)
113        place = self.place_pose * box_left.origin.rotation
114
115        m1 = Motion(f'{box_right.name}-to-pick-interlock', self.dualarm.right, self.right_current_joint_position, pick)
116        m1.linear_retraction = LinearSection(offset=Frame(z=0.05))
117        m1.linear_approach = LinearSection(offset=Frame(z=0.05))
118        trajectoryr = self.plan_cached(m1)
119        self.studio.run_trajectory(trajectoryr, robot=self.dualarm.right)
120        self.right_current_joint_position = trajectoryr.positions[-1]
121        self.set_item(box_right, self.dualarm.right)
122
123        m2 = Motion(f'{box_left.name}-to-place-interlock', self.dualarm.left, self.left_current_joint_position, place)
124        m2.linear_retraction = LinearSection(offset=Frame(z=box_left.collision.z + 0.01))
125        m2.linear_approach = LinearSection(offset=Frame(z=0.05))
126        trajectoryl = self.plan_cached(m2)
127        self.studio.run_trajectory(trajectoryl, robot=self.dualarm.left)
128        self.left_current_joint_position = trajectoryl.positions[-1]
129        self.remove_item(self.dualarm.left)
130
131    def single_arm_dynamic_pick_cycle(self, box_left, box_right):
132        # Left arm from place to pick, right from pick to place
133        pick = box_left.origin * Frame(z=box_left.collision.z / 2)
134        place = self.place_pose * box_right.origin.rotation
135
136        m1 = Motion(f'{box_left.name}-to-pick-dynamic', self.dualarm.left, self.left_current_joint_position, pick)
137        m1.linear_retraction = LinearSection(offset=Frame(z=0.05))
138        m1.linear_approach = LinearSection(offset=Frame(z=0.05))
139        trajectoryl = self.plan_cached(m1)
140
141        self.planner.dynamic_robot_trajectories = [DynamicRobotTrajectory(trajectoryl, self.dualarm.left)]
142        m2 = Motion(f'{box_right.name}-to-place-dynamic', self.dualarm.right, self.right_current_joint_position, place)
143        m2.linear_retraction = LinearSection(offset=Frame(z=box_right.collision.z + 0.01))
144        m2.linear_approach = LinearSection(offset=Frame(z=0.05))
145        trajectoryr = self.plan_cached(m2)
146        self.planner.dynamic_robot_trajectories = []
147        self.studio.run_trajectories([(trajectoryl, self.dualarm.left), (trajectoryr, self.dualarm.right)])
148
149        self.left_current_joint_position = trajectoryl.positions[-1]
150        self.right_current_joint_position = trajectoryr.positions[-1]
151        self.set_item(box_left, self.dualarm.left)
152        self.remove_item(self.dualarm.right)
153
154        # Left arm from pick to place, right from place to pick
155        pick = box_right.origin * Frame(z=box_right.collision.z / 2)
156        place = self.place_pose * box_left.origin.rotation
157
158        m1 = Motion(f'{box_right.name}-to-pick-dynamic', self.dualarm.right, self.right_current_joint_position, pick)
159        m1.linear_retraction = LinearSection(offset=Frame(z=0.05))
160        m1.linear_approach = LinearSection(offset=Frame(z=0.05))
161        trajectoryr = self.plan_cached(m1)
162
163        self.planner.dynamic_robot_trajectories = [DynamicRobotTrajectory(trajectoryr, self.dualarm.right)]
164        m2 = Motion(f'{box_left.name}-to-place-dynamic', self.dualarm.left, self.left_current_joint_position, place)
165        m2.linear_retraction = LinearSection(offset=Frame(z=box_left.collision.z + 0.01))
166        m2.linear_approach = LinearSection(offset=Frame(z=0.05))
167        trajectoryl = self.plan_cached(m2)
168        self.planner.dynamic_robot_trajectories = []
169        self.studio.run_trajectories([(trajectoryl, self.dualarm.left), (trajectoryr, self.dualarm.right)])
170
171        self.left_current_joint_position = trajectoryl.positions[-1]
172        self.right_current_joint_position = trajectoryr.positions[-1]
173        self.set_item(box_right, self.dualarm.right)
174        self.remove_item(self.dualarm.left)
175
176    def bimanual_pick_cycle(self, box_left, box_right):
177        # Left from place to pick, right from pick to place
178        pick = box_left.origin * Frame(z=box_left.collision.z / 2)
179        place = self.place_pose * box_right.origin.rotation
180
181        start = MultiRobotPoint({
182            self.dualarm.left: self.left_current_joint_position,
183            self.dualarm.right: self.right_current_joint_position,
184        })
185        goal = MultiRobotPoint({
186            self.dualarm.left: pick,
187            self.dualarm.right: place,
188        })
189        m = BimanualMotion(f'left-{box_left.name}-right-place', self.dualarm, start, goal)
190        m.linear_retraction = MultiRobotLinearSection({
191            self.dualarm.left: LinearSection(offset=Frame(z=0.05)),
192            self.dualarm.right: LinearSection(offset=Frame(z=box_right.collision.z + 0.01)),
193        })
194        m.linear_approach = MultiRobotLinearSection({
195            self.dualarm.left: LinearSection(offset=Frame(z=0.05)),
196            self.dualarm.right: LinearSection(offset=Frame(z=0.05)),
197        })
198
199        trajectory = self.plan_cached(m)
200        self.studio.run_trajectory(trajectory, robot=self.dualarm)
201        self.left_current_joint_position = trajectory.positions[-1][0:6]
202        self.right_current_joint_position = trajectory.positions[-1][6:]
203        self.set_item(box_left, self.dualarm.left)
204        self.remove_item(self.dualarm.right)
205
206        # Left from pick to place, right from place to pick
207        pick = box_right.origin * Frame(z=box_right.collision.z / 2)
208        place = self.place_pose * box_left.origin.rotation
209
210        start = MultiRobotPoint({
211            self.dualarm.left: self.left_current_joint_position,
212            self.dualarm.right: self.right_current_joint_position,
213        })
214        goal = MultiRobotPoint({
215            self.dualarm.left: place,
216            self.dualarm.right: pick,
217        })
218        m = BimanualMotion(f'right-{box_right.name}-left-place', self.dualarm, start, goal)
219        m.linear_retraction = MultiRobotLinearSection({
220            self.dualarm.left: LinearSection(offset=Frame(z=box_left.collision.z + 0.01)),
221            self.dualarm.right: LinearSection(offset=Frame(z=0.05)),
222        })
223        m.linear_approach = MultiRobotLinearSection({
224            self.dualarm.left: LinearSection(offset=Frame(z=0.05)),
225            self.dualarm.right: LinearSection(offset=Frame(z=0.05)),
226        })
227
228        trajectory = self.plan_cached(m)
229        self.studio.run_trajectory(trajectory, robot=self.dualarm)
230        self.left_current_joint_position = trajectory.positions[-1][0:6]
231        self.right_current_joint_position = trajectory.positions[-1][6:]
232        self.set_item(box_right, self.dualarm.right)
233        self.remove_item(self.dualarm.left)
234
235    def load_pattern_onto_pallet(self, pattern, pallet: Frame, box: Obstacle):
236        obstacles = []
237
238        for i_layer, layer in enumerate(pattern):
239            for i_box, pose in enumerate(layer):
240                b = box.with_origin(pallet * pose)
241                b.name = f'{b.name}-{i_layer + 1}-{i_box + 1}'
242                self.studio.add_obstacle(b)
243                b_ = self.planner.environment.add_obstacle(b)
244                obstacles.append(b_)
245
246        return obstacles
247
248    def pick_cycle(self, box_left, box_right):
249        if self.mode == 'bimanual':
250            self.bimanual_pick_cycle(box_left, box_right)
251        elif self.mode == 'dynamic':
252            self.single_arm_dynamic_pick_cycle(box_left, box_right)
253        elif self.mode == 'interlock':
254            self.single_arm_interlock_pick_cycle(box_left, box_right)
255
256    def run(self):
257        # Load the box pattern onto the pallets
258        box_obstacles_left = self.load_pattern_onto_pallet(self.box_pattern, self.pallet_left, self.box_left)
259        box_obstacles_right = self.load_pattern_onto_pallet(self.box_pattern, self.pallet_right, self.box_right)
260
261        # Pick all boxes from the pallets
262        for box_l, box_r in zip(reversed(box_obstacles_left), reversed(box_obstacles_right)):
263            self.pick_cycle(box_l, box_r)
264
265        # Finish the last box and return to home for both arms
266        m_left = Motion('left-to-home', self.dualarm.left, self.left_current_joint_position, self.home)
267        t = self.plan_cached(m_left)
268        self.studio.run_trajectory(t, robot=self.dualarm.left)
269
270        m_right = Motion('right-place-last-box', self.dualarm.right, self.right_current_joint_position, self.place_pose)
271        t = self.plan_cached(m_right)
272        self.studio.run_trajectory(t, robot=self.dualarm.right)
273        self.right_current_joint_position = t.positions[-1]
274        self.remove_item(self.dualarm.right)
275
276        m_right = Motion('right-to-home', self.dualarm.right, self.right_current_joint_position, self.home)
277        t = self.plan_cached(m_right)
278        self.studio.run_trajectory(t, robot=self.dualarm.right)
279
280
281if __name__ == '__main__':
282    application = BimanualDepalletizer(mode='bimanual')
283    application.run()