Bin Picking with Vision

Vision-guided robotic bin picking is an advanced automation technology that enables robots to autonomously locate, identify, and retrieve randomly oriented objects from unstructured containers or bins. By integrating sophisticated computer vision systems with robotic manipulation, these solutions address the challenge of handling cluttered, densely packed, and unpredictably positioned items in manufacturing, logistics, and warehousing environments. Cameras allow robots to perceive object geometries, calculate grasping points, and plan precise pick trajectories, significantly reducing manual labor and increasing throughput. In this tutorial, we introduce a vision-guided bin picking system using Jacobi motion planner with live visual data.

Video

Application Code

  1import math
  2from pathlib import Path
  3import numpy as np
  4import cv2
  5from PIL import Image
  6
  7from jacobi import Obstacle, Motion, Frame, Planner, Studio, PointCloud, CartesianWaypoint
  8from jacobi_vision.drivers import SimulatedCameraDriver
  9from jacobi_vision import ImageType, ColorImage
 10
 11
 12class VisionBinPicking:
 13    def __init__(self, project_path: Path):
 14        # Load the planner and get references to the robot and camera
 15        self.planner = Planner.load_from_project_file(project_path)
 16        self.robot = self.planner.environment.get_robot()
 17        self.camera = self.planner.environment.get_camera()
 18        self.camera_driver = SimulatedCameraDriver(self.camera, image_type=ImageType.RGBD)
 19        self.studio = Studio()
 20        self.studio.reset()
 21
 22        # Prepare obstacles for simulated picking
 23        self.object_for_arm = self.planner.environment.get_obstacle('deodorant_arm')
 24        self.object_for_carving = self.planner.environment.get_obstacle('deodorant_scaled')
 25        self.planner.environment.remove_obstacle(self.object_for_carving)
 26        self.point_cloud_obs = None
 27
 28        # Remove all "deodorant" obstacles from the environment - we will rely on vision for collision avoidance
 29        self.obstacles = self.planner.environment.get_obstacles()
 30        self.obstacles = [o for o in self.obstacles if 'deodorant' in o.name]
 31        for o in self.obstacles:
 32            self.planner.environment.remove_obstacle(o)
 33
 34        # Get references to the boxes we need to pack objects into
 35        self.boxes_to_pack = [self.planner.environment.get_obstacle(f'box_to_pack_{i}') for i in range(4)]
 36
 37        # Initialize dimensions and configurations
 38        self._initialize_dims()
 39
 40        # Move robot to home configuration at the start
 41        self.go_home()
 42
 43    def _initialize_dims(self):
 44        # Reference joint configurations for picking and placing
 45        self.reference_config_pick = [0.0, -1.571, 1.571, -1.571, -1.571, 0.0]
 46        self.reference_config_place = [-1.571, -1.571, 1.571, -1.571, -1.571, 0.0]
 47
 48        # Adjust rotations for placement orientation
 49        self.place_rot_adjustment = Frame(a=math.pi, b=0, c=math.pi / 2)
 50
 51        # Dimensions for the target box and objects
 52        self.box_to_pack_y = 0.3119882  # [m]
 53        self.object_width = 0.066675  # [m]
 54        self.object_height = 0.031496  # [m]
 55
 56        # Calculate spacing for placing multiple objects in a row
 57        self.packing_clearance = (self.box_to_pack_y - (4 * self.object_width)) / 5.0
 58
 59    def get_place_locations(self, box: Obstacle, slot: int):
 60        # Compute a target Frame for placing the object in the box at a given slot
 61        place_origin = box.origin * Frame(y=-self.box_to_pack_y / 2) * Frame(z=self.object_height)
 62        return place_origin * Frame(y=(slot + 1) * self.packing_clearance + (slot + 0.50) * self.object_width)
 63
 64    def take_snapshot(self):
 65        # Capture an RGB-D image from the camera
 66        image = self.camera_driver.get_image()
 67
 68        # Convert to a point cloud and filter out distant points
 69        points = image.to_point_cloud().tolist()
 70        points = [(p[0], p[1], p[2] - 0.002) for p in points if (p[0] < 0.3 and 0.8 < p[2] < 1.5)]
 71        points = points[::2]  # Downsample points for efficiency
 72
 73        # Create a point cloud obstacle in the environment
 74        point_cloud = PointCloud(points, resolution=0.01)
 75        self.point_cloud_obs = Obstacle(point_cloud, origin=self.camera.origin)
 76        self.planner.environment.add_obstacle(self.point_cloud_obs)
 77
 78        # Update the studio visualization of the point cloud
 79        self.studio.set_camera_point_cloud(np.array(points).flatten().tolist())
 80        return image
 81
 82    def go_home(self):
 83        # Plan and execute a motion to move the robot arm to a "home" configuration
 84        start = self.studio.get_joint_position()
 85        motion_to_go_home = Motion(name='gohome', robot=self.robot, start=start, goal=[math.pi / 2, -math.pi / 2, 0, -math.pi / 2, 0, 0])
 86        trajectory_to_go_home = self.planner.plan(motion_to_go_home)
 87        self.studio.run_trajectory(trajectory_to_go_home)
 88
 89    def pick_up_object(self, pose: Frame):
 90        # Clear any currently held item
 91        self.robot.item = None
 92
 93        # Plan a motion to pick the object from the given pose
 94        start = self.studio.get_joint_position(self.robot)
 95        motion_to_pick = Motion(name='pick', robot=self.robot, start=start, goal=pose)
 96        motion_to_pick.robot.item = None
 97
 98        # Find the closest object and carve it out from the point cloud to help with collision checking
 99        o = self._find_closest_object(pose)
100        self.object_for_carving.origin = Frame(z=0.002) * o.origin
101        self.planner.environment.carve_point_cloud(self.point_cloud_obs, self.object_for_carving)
102        self.planner.environment.update_point_cloud(self.point_cloud_obs)
103        self.studio.set_camera_point_cloud(np.array(self.point_cloud_obs.collision.points).flatten().tolist())
104
105        # Add the arm's object obstacle to help with collision-free planning
106        self.planner.environment.add_obstacle(self.object_for_arm)
107        trajectory_to_pick = self.planner.plan(motion_to_pick)
108        self.studio.run_trajectory(trajectory_to_pick)
109
110        # Hide the original object visually and attach it to the arm
111        o.for_visual = False
112        self.studio.update_obstacle(o)
113        self._attach_object_to_arm()
114
115    def _find_closest_object(self, pose: Frame):
116        # Find the obstacle closest to the given pose
117        return min(self.obstacles, key=lambda o: pose.translational_distance(o.origin), default=None)
118
119    def _attach_object_to_arm(self):
120        # Attach the picked object to the robot's arm for the simulation
121        object_obs = self.object_for_arm
122        object_obs.name = 'object_item'
123        object_obs.origin = Frame(x=0.0, y=-0.069, z=0.015)
124        object_obs.for_visual = True
125        self.studio.set_item(object_obs)
126
127    def place_object(self, box_to_pack: Obstacle, slot):
128        # Plan and execute a motion to place the picked object into a box slot
129        start = self.studio.get_joint_position(self.robot)
130        place_loc = self.get_place_locations(box_to_pack, slot)
131        goal = place_loc * Frame(z=0.01) * self.place_rot_adjustment
132        goal_cart = CartesianWaypoint(position=goal, reference_config=self.reference_config_place)
133
134        motion_to_place = Motion(name='place', robot=self.robot, start=start, goal=goal_cart)
135        trajectory_to_place = self.planner.plan(motion_to_place)
136        self.studio.run_trajectory(trajectory_to_place)
137
138        # Update the scene to show the placed object
139        o = self._find_closest_object(self.robot.calculate_tcp(start))
140        o.for_visual = True
141        o.origin = goal * Frame(y=-0.069)
142        self.studio.update_obstacle(o)
143
144        # Release the object from the arm
145        self.robot.item_obstacle = None
146        self.studio.set_item(None)
147
148    def detect_objects(self, image):
149        # Convert image to OpenCV format and detect objects by color (blue)
150        opencv_image = cv2.cvtColor(np.array(Image.fromarray(image.color)), cv2.COLOR_RGB2BGR)
151        hsv = cv2.cvtColor(opencv_image, cv2.COLOR_BGR2HSV)
152        lower_blue = np.array([100, 50, 50])
153        upper_blue = np.array([130, 255, 255])
154        mask = cv2.inRange(hsv, lower_blue, upper_blue)
155
156        # Find contours of detected objects
157        contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
158        rectangles = []
159        for contour in contours:
160            if cv2.contourArea(contour) > 100:
161                x, y, w, h = cv2.boundingRect(contour)
162                if 0.5 < float(w) / h < 2:
163                    center_x = x + w / 2
164                    center_y = y + h / 2
165                    rectangles.append((int(center_x), int(center_y)))
166
167        # Deproject pixel coordinates to 3D points relative to the camera
168        points = []
169        for i, (x, y) in enumerate(rectangles):
170            # Draw detection markers on the debug image
171            if i == 0:
172                cv2.circle(opencv_image, (x, y), 5, (0, 0, 255), -1)
173            cv2.rectangle(opencv_image, (x - 40, y - 70), (x + 40, y + 50), (0, 255, 0), 2)
174
175            # Get the depth and compute 3D coordinates in the scene
176            depth = image.depth[y][x]
177            p3d = image.deproject(np.array([[y, x, depth]]))[0]
178            p3d = self.camera.origin * Frame.from_translation(p3d[1], p3d[0], p3d[2]) * Frame(x=-0.085, y=0.08) * Frame(c=math.pi)
179            points.append(p3d)
180
181        # Save the detection result image
182        cv2.imwrite('detected_objects.png', opencv_image)
183        color_image = ColorImage.load_from_file('detected_objects.png')
184        # Set image to the camera in Studio
185        encoded_image = color_image.encode()
186        self.studio.set_camera_image_encoded(encoded_image, camera=self.camera)
187
188        return points
189
190
191if __name__ == '__main__':
192    project_name = 'bin-picking-with-vision.jacobi-project'
193    vision_packer = VisionBinPicking(project_name)
194
195    img = vision_packer.take_snapshot()
196    detected_objects = vision_packer.detect_objects(img)
197    for j, p in enumerate(detected_objects):
198        vision_packer.pick_up_object(p)
199        box_index = int(np.floor(j / 4))
200        box_slot = j % 4
201        vision_packer.place_object(vision_packer.boxes_to_pack[box_index], box_slot)
202
203    vision_packer.go_home()