MPlib is a lightweight python package for motion planning, which is decoupled from ROS and is easy to set up. With a few lines of python code, one can achieve most of the motion planning functionalities in robot manipulation.
+ +Pre-built pip packages support Ubuntu 18.04+ with Python 3.6+.
+ +pip install mplib
+See our tutorial for detailed usage and examples.
+1import sapien.core as sapien + 2from .demo_setup import DemoSetup + 3 + 4class PlanningDemo(DemoSetup): + 5 """ The shows the planner's ability to generate a collision free path with the straight path causes collisions """ + 6 def __init__(self): + 7 """ + 8 Same setup as demo.py except the boxes are of difference sizes and different use + 9 Red box is the target we want to grab + 10 Blue box is the obstacle we want to avoid + 11 green box is the landing pad on which we want to place the red box + 12 """ + 13 super().__init__() + 14 self.setup_scene() + 15 self.load_robot() + 16 self.setup_planner() + 17 + 18 # Set initial joint positions + 19 init_qpos = [0, 0.19, 0.0, -2.61, 0.0, 2.94, 0.78, 0, 0] + 20 self.robot.set_qpos(init_qpos) + 21 + 22 # table top + 23 builder = self.scene.create_actor_builder() + 24 builder.add_box_collision(half_size=[0.4, 0.4, 0.025]) + 25 builder.add_box_visual(half_size=[0.4, 0.4, 0.025]) + 26 self.table = builder.build_kinematic(name='table') + 27 self.table.set_pose(sapien.Pose([0.56, 0, - 0.025])) + 28 + 29 # red box is the target we want to grab + 30 builder = self.scene.create_actor_builder() + 31 builder.add_box_collision(half_size=[0.02, 0.02, 0.06]) + 32 builder.add_box_visual(half_size=[0.02, 0.02, 0.06], color=[1, 0, 0]) + 33 self.red_cube = builder.build(name='red_cube') + 34 self.red_cube.set_pose(sapien.Pose([0.7, 0, 0.06])) + 35 + 36 # green box is the landing pad on which we want to place the red box + 37 builder = self.scene.create_actor_builder() + 38 builder.add_box_collision(half_size=[0.04, 0.04, 0.005]) + 39 builder.add_box_visual(half_size=[0.04, 0.04, 0.005], color=[0, 1, 0]) + 40 self.green_cube = builder.build(name='green_cube') + 41 self.green_cube.set_pose(sapien.Pose([0.4, 0.3, 0.005])) + 42 + 43 # blue box is the obstacle we want to avoid + 44 builder = self.scene.create_actor_builder() + 45 builder.add_box_collision(half_size=[0.05, 0.2, 0.1]) + 46 builder.add_box_visual(half_size=[0.05, 0.2, 0.1], color=[0, 0, 1]) + 47 self.blue_cube = builder.build(name='blue_cube') + 48 self.blue_cube.set_pose(sapien.Pose([0.55, 0, 0.1])) + 49 + 50 def add_point_cloud(self): + 51 """ we tell the planner about the obstacle through a point cloud """ + 52 import trimesh + 53 box = trimesh.creation.box([0.1, 0.4, 0.2]) + 54 points, _ = trimesh.sample.sample_surface(box, 1000) + 55 points += [0.55, 0, 0.1] + 56 self.planner.update_point_cloud(points, radius=0.02) + 57 return + 58 + 59 def demo(self, with_screw=True, use_point_cloud=True, use_attach=True): + 60 """ + 61 We pick up the red box while avoiding the blue box and place it back down on top of the green box + 62 """ + 63 pickup_pose = [0.7, 0, 0.12, 0, 1, 0, 0] + 64 delivery_pose = [0.4, 0.3, 0.13, 0, 1, 0, 0] + 65 + 66 # tell the planner where the obstacle is + 67 if use_point_cloud: + 68 self.add_point_cloud() + 69 + 70 # move to the pickup pose + 71 pickup_pose[2] += 0.2 + 72 # no need to check collision against attached object since nothing picked up yet + 73 self.move_to_pose(pickup_pose, with_screw, use_point_cloud, use_attach=False) + 74 self.open_gripper() + 75 pickup_pose[2] -= 0.12 + 76 # no attach since nothing picked up yet + 77 self.move_to_pose(pickup_pose, with_screw, use_point_cloud, use_attach=False) + 78 self.close_gripper() + 79 + 80 if use_attach: + 81 self.planner.update_attached_box([0.04, 0.04, 0.12], [0, 0, 0.14, 1, 0, 0, 0]) + 82 + 83 # move to the delivery pose + 84 pickup_pose[2] += 0.12 + 85 self.move_to_pose(pickup_pose, with_screw, use_point_cloud, use_attach) + 86 delivery_pose[2] += 0.2 + 87 self.move_to_pose(delivery_pose, with_screw, use_point_cloud, use_attach) + 88 delivery_pose[2] -= 0.12 + 89 self.move_to_pose(delivery_pose, with_screw, use_point_cloud, use_attach) + 90 self.open_gripper() + 91 delivery_pose[2] += 0.12 + 92 self.move_to_pose(delivery_pose, with_screw, use_point_cloud, use_attach=False) + 93 + 94if __name__ == '__main__': + 95 """ + 96 As you change some of the parameters, you will see different behaviors + 97 In particular, when point cloud is not used, the robot will attemt to go through the blue box + 98 If attach is not used, the robot will avoid the blue box on its way to the pickup pose but will knock it over with the attached red cube on its way to the delivery pose + 99 """ +100 demo = PlanningDemo() +101 demo.demo(False, True, True) +
5class PlanningDemo(DemoSetup): + 6 """ The shows the planner's ability to generate a collision free path with the straight path causes collisions """ + 7 def __init__(self): + 8 """ + 9 Same setup as demo.py except the boxes are of difference sizes and different use +10 Red box is the target we want to grab +11 Blue box is the obstacle we want to avoid +12 green box is the landing pad on which we want to place the red box +13 """ +14 super().__init__() +15 self.setup_scene() +16 self.load_robot() +17 self.setup_planner() +18 +19 # Set initial joint positions +20 init_qpos = [0, 0.19, 0.0, -2.61, 0.0, 2.94, 0.78, 0, 0] +21 self.robot.set_qpos(init_qpos) +22 +23 # table top +24 builder = self.scene.create_actor_builder() +25 builder.add_box_collision(half_size=[0.4, 0.4, 0.025]) +26 builder.add_box_visual(half_size=[0.4, 0.4, 0.025]) +27 self.table = builder.build_kinematic(name='table') +28 self.table.set_pose(sapien.Pose([0.56, 0, - 0.025])) +29 +30 # red box is the target we want to grab +31 builder = self.scene.create_actor_builder() +32 builder.add_box_collision(half_size=[0.02, 0.02, 0.06]) +33 builder.add_box_visual(half_size=[0.02, 0.02, 0.06], color=[1, 0, 0]) +34 self.red_cube = builder.build(name='red_cube') +35 self.red_cube.set_pose(sapien.Pose([0.7, 0, 0.06])) +36 +37 # green box is the landing pad on which we want to place the red box +38 builder = self.scene.create_actor_builder() +39 builder.add_box_collision(half_size=[0.04, 0.04, 0.005]) +40 builder.add_box_visual(half_size=[0.04, 0.04, 0.005], color=[0, 1, 0]) +41 self.green_cube = builder.build(name='green_cube') +42 self.green_cube.set_pose(sapien.Pose([0.4, 0.3, 0.005])) +43 +44 # blue box is the obstacle we want to avoid +45 builder = self.scene.create_actor_builder() +46 builder.add_box_collision(half_size=[0.05, 0.2, 0.1]) +47 builder.add_box_visual(half_size=[0.05, 0.2, 0.1], color=[0, 0, 1]) +48 self.blue_cube = builder.build(name='blue_cube') +49 self.blue_cube.set_pose(sapien.Pose([0.55, 0, 0.1])) +50 +51 def add_point_cloud(self): +52 """ we tell the planner about the obstacle through a point cloud """ +53 import trimesh +54 box = trimesh.creation.box([0.1, 0.4, 0.2]) +55 points, _ = trimesh.sample.sample_surface(box, 1000) +56 points += [0.55, 0, 0.1] +57 self.planner.update_point_cloud(points, radius=0.02) +58 return +59 +60 def demo(self, with_screw=True, use_point_cloud=True, use_attach=True): +61 """ +62 We pick up the red box while avoiding the blue box and place it back down on top of the green box +63 """ +64 pickup_pose = [0.7, 0, 0.12, 0, 1, 0, 0] +65 delivery_pose = [0.4, 0.3, 0.13, 0, 1, 0, 0] +66 +67 # tell the planner where the obstacle is +68 if use_point_cloud: +69 self.add_point_cloud() +70 +71 # move to the pickup pose +72 pickup_pose[2] += 0.2 +73 # no need to check collision against attached object since nothing picked up yet +74 self.move_to_pose(pickup_pose, with_screw, use_point_cloud, use_attach=False) +75 self.open_gripper() +76 pickup_pose[2] -= 0.12 +77 # no attach since nothing picked up yet +78 self.move_to_pose(pickup_pose, with_screw, use_point_cloud, use_attach=False) +79 self.close_gripper() +80 +81 if use_attach: +82 self.planner.update_attached_box([0.04, 0.04, 0.12], [0, 0, 0.14, 1, 0, 0, 0]) +83 +84 # move to the delivery pose +85 pickup_pose[2] += 0.12 +86 self.move_to_pose(pickup_pose, with_screw, use_point_cloud, use_attach) +87 delivery_pose[2] += 0.2 +88 self.move_to_pose(delivery_pose, with_screw, use_point_cloud, use_attach) +89 delivery_pose[2] -= 0.12 +90 self.move_to_pose(delivery_pose, with_screw, use_point_cloud, use_attach) +91 self.open_gripper() +92 delivery_pose[2] += 0.12 +93 self.move_to_pose(delivery_pose, with_screw, use_point_cloud, use_attach=False) +
The shows the planner's ability to generate a collision free path with the straight path causes collisions
+7 def __init__(self): + 8 """ + 9 Same setup as demo.py except the boxes are of difference sizes and different use +10 Red box is the target we want to grab +11 Blue box is the obstacle we want to avoid +12 green box is the landing pad on which we want to place the red box +13 """ +14 super().__init__() +15 self.setup_scene() +16 self.load_robot() +17 self.setup_planner() +18 +19 # Set initial joint positions +20 init_qpos = [0, 0.19, 0.0, -2.61, 0.0, 2.94, 0.78, 0, 0] +21 self.robot.set_qpos(init_qpos) +22 +23 # table top +24 builder = self.scene.create_actor_builder() +25 builder.add_box_collision(half_size=[0.4, 0.4, 0.025]) +26 builder.add_box_visual(half_size=[0.4, 0.4, 0.025]) +27 self.table = builder.build_kinematic(name='table') +28 self.table.set_pose(sapien.Pose([0.56, 0, - 0.025])) +29 +30 # red box is the target we want to grab +31 builder = self.scene.create_actor_builder() +32 builder.add_box_collision(half_size=[0.02, 0.02, 0.06]) +33 builder.add_box_visual(half_size=[0.02, 0.02, 0.06], color=[1, 0, 0]) +34 self.red_cube = builder.build(name='red_cube') +35 self.red_cube.set_pose(sapien.Pose([0.7, 0, 0.06])) +36 +37 # green box is the landing pad on which we want to place the red box +38 builder = self.scene.create_actor_builder() +39 builder.add_box_collision(half_size=[0.04, 0.04, 0.005]) +40 builder.add_box_visual(half_size=[0.04, 0.04, 0.005], color=[0, 1, 0]) +41 self.green_cube = builder.build(name='green_cube') +42 self.green_cube.set_pose(sapien.Pose([0.4, 0.3, 0.005])) +43 +44 # blue box is the obstacle we want to avoid +45 builder = self.scene.create_actor_builder() +46 builder.add_box_collision(half_size=[0.05, 0.2, 0.1]) +47 builder.add_box_visual(half_size=[0.05, 0.2, 0.1], color=[0, 0, 1]) +48 self.blue_cube = builder.build(name='blue_cube') +49 self.blue_cube.set_pose(sapien.Pose([0.55, 0, 0.1])) +
Same setup as demo.py except the boxes are of difference sizes and different use +Red box is the target we want to grab +Blue box is the obstacle we want to avoid +green box is the landing pad on which we want to place the red box
+51 def add_point_cloud(self): +52 """ we tell the planner about the obstacle through a point cloud """ +53 import trimesh +54 box = trimesh.creation.box([0.1, 0.4, 0.2]) +55 points, _ = trimesh.sample.sample_surface(box, 1000) +56 points += [0.55, 0, 0.1] +57 self.planner.update_point_cloud(points, radius=0.02) +58 return +
we tell the planner about the obstacle through a point cloud
+60 def demo(self, with_screw=True, use_point_cloud=True, use_attach=True): +61 """ +62 We pick up the red box while avoiding the blue box and place it back down on top of the green box +63 """ +64 pickup_pose = [0.7, 0, 0.12, 0, 1, 0, 0] +65 delivery_pose = [0.4, 0.3, 0.13, 0, 1, 0, 0] +66 +67 # tell the planner where the obstacle is +68 if use_point_cloud: +69 self.add_point_cloud() +70 +71 # move to the pickup pose +72 pickup_pose[2] += 0.2 +73 # no need to check collision against attached object since nothing picked up yet +74 self.move_to_pose(pickup_pose, with_screw, use_point_cloud, use_attach=False) +75 self.open_gripper() +76 pickup_pose[2] -= 0.12 +77 # no attach since nothing picked up yet +78 self.move_to_pose(pickup_pose, with_screw, use_point_cloud, use_attach=False) +79 self.close_gripper() +80 +81 if use_attach: +82 self.planner.update_attached_box([0.04, 0.04, 0.12], [0, 0, 0.14, 1, 0, 0, 0]) +83 +84 # move to the delivery pose +85 pickup_pose[2] += 0.12 +86 self.move_to_pose(pickup_pose, with_screw, use_point_cloud, use_attach) +87 delivery_pose[2] += 0.2 +88 self.move_to_pose(delivery_pose, with_screw, use_point_cloud, use_attach) +89 delivery_pose[2] -= 0.12 +90 self.move_to_pose(delivery_pose, with_screw, use_point_cloud, use_attach) +91 self.open_gripper() +92 delivery_pose[2] += 0.12 +93 self.move_to_pose(delivery_pose, with_screw, use_point_cloud, use_attach=False) +
We pick up the red box while avoiding the blue box and place it back down on top of the green box
+1#!/usr/bin/env python3 + 2 + 3import numpy as np + 4import transforms3d + 5from .demo_setup import DemoSetup + 6 + 7class ConstrainedPlanningDemo(DemoSetup): + 8 """ + 9 This demo shows the planner's ability to plan with constraints. + 10 For this particular demo, we move to several poses while pointing the end effector roughly 15 degrees w.r.t. -z axis + 11 """ + 12 def __init__(self): + 13 """ set up the scene and load the robot """ + 14 super().__init__() + 15 self.setup_scene() + 16 self.load_robot() + 17 self.setup_planner() + 18 + 19 def add_point_cloud(self): + 20 """ add some random obstacles to make the planning more challenging """ + 21 import trimesh + 22 box = trimesh.creation.box([0.1, 0.4, 0.2]) + 23 points, _ = trimesh.sample.sample_surface(box, 1000) + 24 all_pts = np.concatenate([points+[-0.65, -0.1, 0.4], points+[0.55, 0, 0.1]], axis=0) + 25 self.planner.update_point_cloud(all_pts, radius=0.02) + 26 return + 27 + 28 def get_eef_z(self): + 29 """ helper function for constraint """ + 30 ee_idx = self.planner.link_name_2_idx[self.planner.move_group] + 31 ee_pose = self.planner.robot.get_pinocchio_model().get_link_pose(ee_idx) + 32 mat = transforms3d.quaternions.quat2mat(ee_pose[3:]) + 33 return mat[:,2] + 34 + 35 def make_f(self): + 36 """ + 37 create a constraint function that takes in a qpos and outputs a scalar + 38 A valid constraint function should evaluates to 0 when the constraint is satisfied + 39 See [ompl constrained planning](https://ompl.kavrakilab.org/constrainedPlanning.html) for more details + 40 """ + 41 def f(x, out): + 42 self.planner.robot.set_qpos(x) + 43 out[0] = self.get_eef_z().dot(np.array([0,0,-1]))-0.966 # maintain 15 degrees w.r.t. -z axis + 44 return f + 45 + 46 def make_j(self): + 47 """ + 48 create the jacobian of the constraint function w.r.t. qpos + 49 This is needed because the planner uses the jacobian to project a random sample to the constraint manifold + 50 """ + 51 def j(x, out): + 52 full_qpos = self.planner.pad_qpos(x) + 53 jac = self.planner.robot.get_pinocchio_model().compute_single_link_jacobian(full_qpos, len(self.planner.move_group_joint_indices)-1) + 54 rot_jac = jac[3:, self.planner.move_group_joint_indices] + 55 for i in range(len(self.planner.move_group_joint_indices)): + 56 out[i] = np.cross(rot_jac[:,i], self.get_eef_z()).dot(np.array([0,0,-1])) + 57 return j + 58 + 59 def demo(self): + 60 """ + 61 We first plan with constraints to three poses, then plan without constraints to the same poses + 62 While not always the case, sometimes without constraints, the end effector will tilt almost upside down + 63 """ + 64 # this starting pose has the end effector tilted roughly 15 degrees + 65 starting_qpos = [0, 0.19, 0.0, -2.61, 0.0, 2.88, 0.78, 0, 0] + 66 self.robot.set_qpos(starting_qpos) + 67 self.planner.robot.set_qpos(starting_qpos[:7]) + 68 # all these poses are constrain compatible (roughly 15 degrees w.r.t. -z axis) + 69 poses = [[-0.4, -0.3, 0.28, 0.0704682, -0.5356872, 0.8342834, 0.1097478], + 70 [0.6, 0.1, 0.44, 0.0704682, -0.5356872, -0.8342834, -0.1097478], + 71 [0, -0.3, 0.5, 0.1304237, -0.9914583, 0, 0]] + 72 + 73 # add some point cloud to make the planning more challenging so we can see the effect of no constraint + 74 self.add_point_cloud() + 75 + 76 # with constraint + 77 print("with constraint. all movements roughly maintain 15 degrees w.r.t. -z axis") + 78 for pose in poses: + 79 result = self.planner.plan_qpos_to_pose( + 80 pose, + 81 self.robot.get_qpos(), + 82 time_step=1/250, + 83 use_point_cloud=True, + 84 use_attach=False, + 85 planner_name="RRTConnect", + 86 constraint_function=self.make_f(), + 87 constraint_jacobian=self.make_j(), + 88 constraint_tolerance=0.05, + 89 ) + 90 if result['status'] != "Success": + 91 print(result['status']) + 92 return -1 + 93 self.follow_path(result) + 94 + 95 # without constraint + 96 print("without constraint. certain movements can sometimes tilt the end effector almost upside down") + 97 for pose in poses: + 98 result = self.planner.plan_qpos_to_pose( + 99 pose, +100 self.robot.get_qpos(), +101 time_step=1/250, +102 use_point_cloud=True, +103 use_attach=False, +104 planner_name="RRTConnect", +105 no_simplification=True +106 ) +107 if result['status'] != "Success": +108 print(result['status']) +109 return -1 +110 self.follow_path(result) +111 +112 +113if __name__ == '__main__': +114 demo = ConstrainedPlanningDemo() +115 demo.demo() +
8class ConstrainedPlanningDemo(DemoSetup): + 9 """ + 10 This demo shows the planner's ability to plan with constraints. + 11 For this particular demo, we move to several poses while pointing the end effector roughly 15 degrees w.r.t. -z axis + 12 """ + 13 def __init__(self): + 14 """ set up the scene and load the robot """ + 15 super().__init__() + 16 self.setup_scene() + 17 self.load_robot() + 18 self.setup_planner() + 19 + 20 def add_point_cloud(self): + 21 """ add some random obstacles to make the planning more challenging """ + 22 import trimesh + 23 box = trimesh.creation.box([0.1, 0.4, 0.2]) + 24 points, _ = trimesh.sample.sample_surface(box, 1000) + 25 all_pts = np.concatenate([points+[-0.65, -0.1, 0.4], points+[0.55, 0, 0.1]], axis=0) + 26 self.planner.update_point_cloud(all_pts, radius=0.02) + 27 return + 28 + 29 def get_eef_z(self): + 30 """ helper function for constraint """ + 31 ee_idx = self.planner.link_name_2_idx[self.planner.move_group] + 32 ee_pose = self.planner.robot.get_pinocchio_model().get_link_pose(ee_idx) + 33 mat = transforms3d.quaternions.quat2mat(ee_pose[3:]) + 34 return mat[:,2] + 35 + 36 def make_f(self): + 37 """ + 38 create a constraint function that takes in a qpos and outputs a scalar + 39 A valid constraint function should evaluates to 0 when the constraint is satisfied + 40 See [ompl constrained planning](https://ompl.kavrakilab.org/constrainedPlanning.html) for more details + 41 """ + 42 def f(x, out): + 43 self.planner.robot.set_qpos(x) + 44 out[0] = self.get_eef_z().dot(np.array([0,0,-1]))-0.966 # maintain 15 degrees w.r.t. -z axis + 45 return f + 46 + 47 def make_j(self): + 48 """ + 49 create the jacobian of the constraint function w.r.t. qpos + 50 This is needed because the planner uses the jacobian to project a random sample to the constraint manifold + 51 """ + 52 def j(x, out): + 53 full_qpos = self.planner.pad_qpos(x) + 54 jac = self.planner.robot.get_pinocchio_model().compute_single_link_jacobian(full_qpos, len(self.planner.move_group_joint_indices)-1) + 55 rot_jac = jac[3:, self.planner.move_group_joint_indices] + 56 for i in range(len(self.planner.move_group_joint_indices)): + 57 out[i] = np.cross(rot_jac[:,i], self.get_eef_z()).dot(np.array([0,0,-1])) + 58 return j + 59 + 60 def demo(self): + 61 """ + 62 We first plan with constraints to three poses, then plan without constraints to the same poses + 63 While not always the case, sometimes without constraints, the end effector will tilt almost upside down + 64 """ + 65 # this starting pose has the end effector tilted roughly 15 degrees + 66 starting_qpos = [0, 0.19, 0.0, -2.61, 0.0, 2.88, 0.78, 0, 0] + 67 self.robot.set_qpos(starting_qpos) + 68 self.planner.robot.set_qpos(starting_qpos[:7]) + 69 # all these poses are constrain compatible (roughly 15 degrees w.r.t. -z axis) + 70 poses = [[-0.4, -0.3, 0.28, 0.0704682, -0.5356872, 0.8342834, 0.1097478], + 71 [0.6, 0.1, 0.44, 0.0704682, -0.5356872, -0.8342834, -0.1097478], + 72 [0, -0.3, 0.5, 0.1304237, -0.9914583, 0, 0]] + 73 + 74 # add some point cloud to make the planning more challenging so we can see the effect of no constraint + 75 self.add_point_cloud() + 76 + 77 # with constraint + 78 print("with constraint. all movements roughly maintain 15 degrees w.r.t. -z axis") + 79 for pose in poses: + 80 result = self.planner.plan_qpos_to_pose( + 81 pose, + 82 self.robot.get_qpos(), + 83 time_step=1/250, + 84 use_point_cloud=True, + 85 use_attach=False, + 86 planner_name="RRTConnect", + 87 constraint_function=self.make_f(), + 88 constraint_jacobian=self.make_j(), + 89 constraint_tolerance=0.05, + 90 ) + 91 if result['status'] != "Success": + 92 print(result['status']) + 93 return -1 + 94 self.follow_path(result) + 95 + 96 # without constraint + 97 print("without constraint. certain movements can sometimes tilt the end effector almost upside down") + 98 for pose in poses: + 99 result = self.planner.plan_qpos_to_pose( +100 pose, +101 self.robot.get_qpos(), +102 time_step=1/250, +103 use_point_cloud=True, +104 use_attach=False, +105 planner_name="RRTConnect", +106 no_simplification=True +107 ) +108 if result['status'] != "Success": +109 print(result['status']) +110 return -1 +111 self.follow_path(result) +
This demo shows the planner's ability to plan with constraints. +For this particular demo, we move to several poses while pointing the end effector roughly 15 degrees w.r.t. -z axis
+13 def __init__(self): +14 """ set up the scene and load the robot """ +15 super().__init__() +16 self.setup_scene() +17 self.load_robot() +18 self.setup_planner() +
set up the scene and load the robot
+20 def add_point_cloud(self): +21 """ add some random obstacles to make the planning more challenging """ +22 import trimesh +23 box = trimesh.creation.box([0.1, 0.4, 0.2]) +24 points, _ = trimesh.sample.sample_surface(box, 1000) +25 all_pts = np.concatenate([points+[-0.65, -0.1, 0.4], points+[0.55, 0, 0.1]], axis=0) +26 self.planner.update_point_cloud(all_pts, radius=0.02) +27 return +
add some random obstacles to make the planning more challenging
+29 def get_eef_z(self): +30 """ helper function for constraint """ +31 ee_idx = self.planner.link_name_2_idx[self.planner.move_group] +32 ee_pose = self.planner.robot.get_pinocchio_model().get_link_pose(ee_idx) +33 mat = transforms3d.quaternions.quat2mat(ee_pose[3:]) +34 return mat[:,2] +
helper function for constraint
+36 def make_f(self): +37 """ +38 create a constraint function that takes in a qpos and outputs a scalar +39 A valid constraint function should evaluates to 0 when the constraint is satisfied +40 See [ompl constrained planning](https://ompl.kavrakilab.org/constrainedPlanning.html) for more details +41 """ +42 def f(x, out): +43 self.planner.robot.set_qpos(x) +44 out[0] = self.get_eef_z().dot(np.array([0,0,-1]))-0.966 # maintain 15 degrees w.r.t. -z axis +45 return f +
create a constraint function that takes in a qpos and outputs a scalar +A valid constraint function should evaluates to 0 when the constraint is satisfied +See ompl constrained planning for more details
+47 def make_j(self): +48 """ +49 create the jacobian of the constraint function w.r.t. qpos +50 This is needed because the planner uses the jacobian to project a random sample to the constraint manifold +51 """ +52 def j(x, out): +53 full_qpos = self.planner.pad_qpos(x) +54 jac = self.planner.robot.get_pinocchio_model().compute_single_link_jacobian(full_qpos, len(self.planner.move_group_joint_indices)-1) +55 rot_jac = jac[3:, self.planner.move_group_joint_indices] +56 for i in range(len(self.planner.move_group_joint_indices)): +57 out[i] = np.cross(rot_jac[:,i], self.get_eef_z()).dot(np.array([0,0,-1])) +58 return j +
create the jacobian of the constraint function w.r.t. qpos +This is needed because the planner uses the jacobian to project a random sample to the constraint manifold
+60 def demo(self): + 61 """ + 62 We first plan with constraints to three poses, then plan without constraints to the same poses + 63 While not always the case, sometimes without constraints, the end effector will tilt almost upside down + 64 """ + 65 # this starting pose has the end effector tilted roughly 15 degrees + 66 starting_qpos = [0, 0.19, 0.0, -2.61, 0.0, 2.88, 0.78, 0, 0] + 67 self.robot.set_qpos(starting_qpos) + 68 self.planner.robot.set_qpos(starting_qpos[:7]) + 69 # all these poses are constrain compatible (roughly 15 degrees w.r.t. -z axis) + 70 poses = [[-0.4, -0.3, 0.28, 0.0704682, -0.5356872, 0.8342834, 0.1097478], + 71 [0.6, 0.1, 0.44, 0.0704682, -0.5356872, -0.8342834, -0.1097478], + 72 [0, -0.3, 0.5, 0.1304237, -0.9914583, 0, 0]] + 73 + 74 # add some point cloud to make the planning more challenging so we can see the effect of no constraint + 75 self.add_point_cloud() + 76 + 77 # with constraint + 78 print("with constraint. all movements roughly maintain 15 degrees w.r.t. -z axis") + 79 for pose in poses: + 80 result = self.planner.plan_qpos_to_pose( + 81 pose, + 82 self.robot.get_qpos(), + 83 time_step=1/250, + 84 use_point_cloud=True, + 85 use_attach=False, + 86 planner_name="RRTConnect", + 87 constraint_function=self.make_f(), + 88 constraint_jacobian=self.make_j(), + 89 constraint_tolerance=0.05, + 90 ) + 91 if result['status'] != "Success": + 92 print(result['status']) + 93 return -1 + 94 self.follow_path(result) + 95 + 96 # without constraint + 97 print("without constraint. certain movements can sometimes tilt the end effector almost upside down") + 98 for pose in poses: + 99 result = self.planner.plan_qpos_to_pose( +100 pose, +101 self.robot.get_qpos(), +102 time_step=1/250, +103 use_point_cloud=True, +104 use_attach=False, +105 planner_name="RRTConnect", +106 no_simplification=True +107 ) +108 if result['status'] != "Success": +109 print(result['status']) +110 return -1 +111 self.follow_path(result) +
We first plan with constraints to three poses, then plan without constraints to the same poses +While not always the case, sometimes without constraints, the end effector will tilt almost upside down
+1import sapien.core as sapien + 2from .demo_setup import DemoSetup + 3 + 4class PlanningDemo(DemoSetup): + 5 """ + 6 This is the most basic demo of the motion planning library where the robot tries to shuffle three boxes around + 7 """ + 8 def __init__(self): + 9 """ +10 Setting up the scene, the planner, and adding some objects to the scene +11 Afterwards, put down a table and three boxes. For details on how to do this, see the sapien documentation +12 """ +13 super().__init__() +14 # load the world, the robot, and then setup the planner. See demo_setup.py for more details +15 self.setup_scene() +16 self.load_robot() +17 self.setup_planner() +18 +19 # Set initial joint positions +20 init_qpos = [0, 0.19, 0.0, -2.62, 0.0, 2.94, 0.79, 0, 0] +21 self.robot.set_qpos(init_qpos) +22 +23 # table top +24 builder = self.scene.create_actor_builder() +25 builder.add_box_collision(half_size=[0.4, 0.4, 0.025]) +26 builder.add_box_visual(half_size=[0.4, 0.4, 0.025]) +27 self.table = builder.build_kinematic(name='table') +28 self.table.set_pose(sapien.Pose([0.56, 0, - 0.025])) +29 +30 # boxes +31 builder = self.scene.create_actor_builder() +32 builder.add_box_collision(half_size=[0.02, 0.02, 0.06]) +33 builder.add_box_visual(half_size=[0.02, 0.02, 0.06], color=[1, 0, 0]) +34 self.red_cube = builder.build(name='red_cube') +35 self.red_cube.set_pose(sapien.Pose([0.4, 0.3, 0.06])) +36 +37 builder = self.scene.create_actor_builder() +38 builder.add_box_collision(half_size=[0.02, 0.02, 0.04]) +39 builder.add_box_visual(half_size=[0.02, 0.02, 0.04], color=[0, 1, 0]) +40 self.green_cube = builder.build(name='green_cube') +41 self.green_cube.set_pose(sapien.Pose([0.2, -0.3, 0.04])) +42 +43 builder = self.scene.create_actor_builder() +44 builder.add_box_collision(half_size=[0.02, 0.02, 0.07]) +45 builder.add_box_visual(half_size=[0.02, 0.02, 0.07], color=[0, 0, 1]) +46 self.blue_cube = builder.build(name='blue_cube') +47 self.blue_cube.set_pose(sapien.Pose([0.6, 0.1, 0.07])) +48 +49 def demo(self): +50 """ +51 Declare three poses for the robot to move to, each one corresponding to the position of a box +52 Pick up the box, and set it down 0.1m to the right of its original position +53 """ +54 poses = [[0.4, 0.3, 0.12, 0, 1, 0, 0], +55 [0.2, -0.3, 0.08, 0, 1, 0, 0], +56 [0.6, 0.1, 0.14, 0, 1, 0, 0]] +57 for i in range(3): +58 pose = poses[i] +59 pose[2] += 0.2 +60 self.move_to_pose(pose) +61 self.open_gripper() +62 pose[2] -= 0.12 +63 self.move_to_pose(pose) +64 self.close_gripper() +65 pose[2] += 0.12 +66 self.move_to_pose(pose) +67 pose[0] += 0.1 +68 self.move_to_pose(pose) +69 pose[2] -= 0.12 +70 self.move_to_pose(pose) +71 self.open_gripper() +72 pose[2] += 0.12 +73 self.move_to_pose(pose) +74 +75if __name__ == '__main__': +76 demo = PlanningDemo() +77 demo.demo() +
5class PlanningDemo(DemoSetup): + 6 """ + 7 This is the most basic demo of the motion planning library where the robot tries to shuffle three boxes around + 8 """ + 9 def __init__(self): +10 """ +11 Setting up the scene, the planner, and adding some objects to the scene +12 Afterwards, put down a table and three boxes. For details on how to do this, see the sapien documentation +13 """ +14 super().__init__() +15 # load the world, the robot, and then setup the planner. See demo_setup.py for more details +16 self.setup_scene() +17 self.load_robot() +18 self.setup_planner() +19 +20 # Set initial joint positions +21 init_qpos = [0, 0.19, 0.0, -2.62, 0.0, 2.94, 0.79, 0, 0] +22 self.robot.set_qpos(init_qpos) +23 +24 # table top +25 builder = self.scene.create_actor_builder() +26 builder.add_box_collision(half_size=[0.4, 0.4, 0.025]) +27 builder.add_box_visual(half_size=[0.4, 0.4, 0.025]) +28 self.table = builder.build_kinematic(name='table') +29 self.table.set_pose(sapien.Pose([0.56, 0, - 0.025])) +30 +31 # boxes +32 builder = self.scene.create_actor_builder() +33 builder.add_box_collision(half_size=[0.02, 0.02, 0.06]) +34 builder.add_box_visual(half_size=[0.02, 0.02, 0.06], color=[1, 0, 0]) +35 self.red_cube = builder.build(name='red_cube') +36 self.red_cube.set_pose(sapien.Pose([0.4, 0.3, 0.06])) +37 +38 builder = self.scene.create_actor_builder() +39 builder.add_box_collision(half_size=[0.02, 0.02, 0.04]) +40 builder.add_box_visual(half_size=[0.02, 0.02, 0.04], color=[0, 1, 0]) +41 self.green_cube = builder.build(name='green_cube') +42 self.green_cube.set_pose(sapien.Pose([0.2, -0.3, 0.04])) +43 +44 builder = self.scene.create_actor_builder() +45 builder.add_box_collision(half_size=[0.02, 0.02, 0.07]) +46 builder.add_box_visual(half_size=[0.02, 0.02, 0.07], color=[0, 0, 1]) +47 self.blue_cube = builder.build(name='blue_cube') +48 self.blue_cube.set_pose(sapien.Pose([0.6, 0.1, 0.07])) +49 +50 def demo(self): +51 """ +52 Declare three poses for the robot to move to, each one corresponding to the position of a box +53 Pick up the box, and set it down 0.1m to the right of its original position +54 """ +55 poses = [[0.4, 0.3, 0.12, 0, 1, 0, 0], +56 [0.2, -0.3, 0.08, 0, 1, 0, 0], +57 [0.6, 0.1, 0.14, 0, 1, 0, 0]] +58 for i in range(3): +59 pose = poses[i] +60 pose[2] += 0.2 +61 self.move_to_pose(pose) +62 self.open_gripper() +63 pose[2] -= 0.12 +64 self.move_to_pose(pose) +65 self.close_gripper() +66 pose[2] += 0.12 +67 self.move_to_pose(pose) +68 pose[0] += 0.1 +69 self.move_to_pose(pose) +70 pose[2] -= 0.12 +71 self.move_to_pose(pose) +72 self.open_gripper() +73 pose[2] += 0.12 +74 self.move_to_pose(pose) +
This is the most basic demo of the motion planning library where the robot tries to shuffle three boxes around
+9 def __init__(self): +10 """ +11 Setting up the scene, the planner, and adding some objects to the scene +12 Afterwards, put down a table and three boxes. For details on how to do this, see the sapien documentation +13 """ +14 super().__init__() +15 # load the world, the robot, and then setup the planner. See demo_setup.py for more details +16 self.setup_scene() +17 self.load_robot() +18 self.setup_planner() +19 +20 # Set initial joint positions +21 init_qpos = [0, 0.19, 0.0, -2.62, 0.0, 2.94, 0.79, 0, 0] +22 self.robot.set_qpos(init_qpos) +23 +24 # table top +25 builder = self.scene.create_actor_builder() +26 builder.add_box_collision(half_size=[0.4, 0.4, 0.025]) +27 builder.add_box_visual(half_size=[0.4, 0.4, 0.025]) +28 self.table = builder.build_kinematic(name='table') +29 self.table.set_pose(sapien.Pose([0.56, 0, - 0.025])) +30 +31 # boxes +32 builder = self.scene.create_actor_builder() +33 builder.add_box_collision(half_size=[0.02, 0.02, 0.06]) +34 builder.add_box_visual(half_size=[0.02, 0.02, 0.06], color=[1, 0, 0]) +35 self.red_cube = builder.build(name='red_cube') +36 self.red_cube.set_pose(sapien.Pose([0.4, 0.3, 0.06])) +37 +38 builder = self.scene.create_actor_builder() +39 builder.add_box_collision(half_size=[0.02, 0.02, 0.04]) +40 builder.add_box_visual(half_size=[0.02, 0.02, 0.04], color=[0, 1, 0]) +41 self.green_cube = builder.build(name='green_cube') +42 self.green_cube.set_pose(sapien.Pose([0.2, -0.3, 0.04])) +43 +44 builder = self.scene.create_actor_builder() +45 builder.add_box_collision(half_size=[0.02, 0.02, 0.07]) +46 builder.add_box_visual(half_size=[0.02, 0.02, 0.07], color=[0, 0, 1]) +47 self.blue_cube = builder.build(name='blue_cube') +48 self.blue_cube.set_pose(sapien.Pose([0.6, 0.1, 0.07])) +
Setting up the scene, the planner, and adding some objects to the scene +Afterwards, put down a table and three boxes. For details on how to do this, see the sapien documentation
+50 def demo(self): +51 """ +52 Declare three poses for the robot to move to, each one corresponding to the position of a box +53 Pick up the box, and set it down 0.1m to the right of its original position +54 """ +55 poses = [[0.4, 0.3, 0.12, 0, 1, 0, 0], +56 [0.2, -0.3, 0.08, 0, 1, 0, 0], +57 [0.6, 0.1, 0.14, 0, 1, 0, 0]] +58 for i in range(3): +59 pose = poses[i] +60 pose[2] += 0.2 +61 self.move_to_pose(pose) +62 self.open_gripper() +63 pose[2] -= 0.12 +64 self.move_to_pose(pose) +65 self.close_gripper() +66 pose[2] += 0.12 +67 self.move_to_pose(pose) +68 pose[0] += 0.1 +69 self.move_to_pose(pose) +70 pose[2] -= 0.12 +71 self.move_to_pose(pose) +72 self.open_gripper() +73 pose[2] += 0.12 +74 self.move_to_pose(pose) +
Declare three poses for the robot to move to, each one corresponding to the position of a box +Pick up the box, and set it down 0.1m to the right of its original position
+1#!/usr/bin/env python3 + 2 + 3import sapien.core as sapien + 4from sapien.utils.viewer import Viewer + 5import mplib + 6 + 7class DemoSetup(): + 8 """ + 9 This class is the super class to abstract away some of the setups for the demos + 10 You will need to install Sapien via `pip install sapien` for this to work if you want to use the viewer + 11 """ + 12 def __init__(self): + 13 """ Nothing to do """ + 14 pass + 15 + 16 def setup_scene(self, **kwargs): + 17 """ This is the Sapien simulator setup and has nothing to do with mplib """ + 18 # declare sapien sim + 19 self.engine = sapien.Engine() + 20 # declare sapien renderer + 21 self.renderer = sapien.SapienRenderer() + 22 # give renderer to sapien sim + 23 self.engine.set_renderer(self.renderer) + 24 + 25 # declare sapien scene + 26 scene_config = sapien.SceneConfig() + 27 self.scene = self.engine.create_scene(scene_config) + 28 # set simulation timestep + 29 self.scene.set_timestep(kwargs.get('timestep', 1 / 240)) + 30 # add ground to scene + 31 self.scene.add_ground(kwargs.get('ground_height', 0)) + 32 # set default physical material + 33 self.scene.default_physical_material = self.scene.create_physical_material( + 34 kwargs.get('static_friction', 1), + 35 kwargs.get('dynamic_friction', 1), + 36 kwargs.get('restitution', 0) + 37 ) + 38 # give some white ambient light of moderate intensity + 39 self.scene.set_ambient_light(kwargs.get('ambient_light', [0.5, 0.5, 0.5])) + 40 # default enable shadow unless specified otherwise + 41 shadow = kwargs.get('shadow', True) + 42 # default spotlight angle and intensity + 43 direction_lights = kwargs.get('direction_lights', [[[0, 1, -1], [0.5, 0.5, 0.5]]]) + 44 for direction_light in direction_lights: + 45 self.scene.add_directional_light(direction_light[0], direction_light[1], shadow=shadow) + 46 # default point lights position and intensity + 47 point_lights = kwargs.get('point_lights', [[[1,2,2], [1,1,1]], [[1,-2,2],[1,1,1]], [[-1,0,1],[1,1,1]]]) + 48 for point_light in point_lights: + 49 self.scene.add_point_light(point_light[0], point_light[1], shadow=shadow) + 50 + 51 # initialize viewer with camera position and orientation + 52 self.viewer = Viewer(self.renderer) + 53 self.viewer.set_scene(self.scene) + 54 self.viewer.set_camera_xyz( + 55 x=kwargs.get('camera_xyz_x', 1.2), + 56 y=kwargs.get('camera_xyz_y', 0.25), + 57 z=kwargs.get('camera_xyz_z', 0.4) + 58 ) + 59 self.viewer.set_camera_rpy( + 60 r=kwargs.get('camera_rpy_r', 0), + 61 p=kwargs.get('camera_rpy_p', -0.4), + 62 y=kwargs.get('camera_rpy_y', 2.7) + 63 ) + 64 + 65 def load_robot(self, **kwargs): + 66 """ + 67 This function loads a robot from a URDF file into the Sapien Scene created above. + 68 Note that does mean that setup_scene() must be called before this function. + 69 """ + 70 loader: sapien.URDFLoader = self.scene.create_urdf_loader() + 71 loader.fix_root_link = True + 72 self.robot: sapien.Articulation = loader.load(kwargs.get('urdf_path', "./data/panda/panda.urdf")) + 73 self.robot.set_root_pose( + 74 sapien.Pose(kwargs.get("robot_origin_xyz", [0,0,0]), kwargs.get("robot_origin_quat", [1,0,0,0])) + 75 ) + 76 self.active_joints = self.robot.get_active_joints() + 77 for joint in self.active_joints: + 78 joint.set_drive_property( + 79 stiffness=kwargs.get('joint_stiffness', 1000), + 80 damping=kwargs.get('joint_damping', 200) + 81 ) + 82 + 83 def setup_planner(self, **kwargs): + 84 """ + 85 Create an mplib planner using the default robot + 86 see planner.py for more details on the arguments + 87 """ + 88 self.planner = mplib.Planner( + 89 urdf=kwargs.get('urdf_path', "./data/panda/panda.urdf"), + 90 srdf=kwargs.get('srdf_path', "./data/panda/panda.srdf"), + 91 move_group=kwargs.get('move_group', 'panda_hand') + 92 ) + 93 + 94 def follow_path(self, result): + 95 """ Helper function to follow a path generated by the planner """ + 96 # number of waypoints in the path + 97 n_step = result['position'].shape[0] + 98 # this makes sure the robot stays neutrally boyant instead of sagging under gravity + 99 for i in range(n_step): +100 qf = self.robot.compute_passive_force( +101 external=False, +102 gravity=True, +103 coriolis_and_centrifugal=True) +104 self.robot.set_qf(qf) +105 # set the joint positions and velocities for move group joints only. The others are not the responsibility of the planner +106 for j in range(len(self.planner.move_group_joint_indices)): +107 self.active_joints[j].set_drive_target(result['position'][i][j]) +108 self.active_joints[j].set_drive_velocity_target(result['velocity'][i][j]) +109 # simulation step +110 self.scene.step() +111 # render every 4 simulation steps to make it faster +112 if i % 4 == 0: +113 self.scene.update_render() +114 self.viewer.render() +115 +116 def set_gripper(self, pos): +117 """ +118 Helper function to activate gripper joints +119 Args: +120 pos: position of the gripper joint in real number +121 """ +122 # The following two lines are particular to the panda robot +123 for joint in self.active_joints[-2:]: +124 joint.set_drive_target(pos) +125 # 100 steps is plenty to reach the target position +126 for i in range(100): +127 qf = self.robot.compute_passive_force( +128 external=False, +129 gravity=True, +130 coriolis_and_centrifugal=True) +131 self.robot.set_qf(qf) +132 self.scene.step() +133 if i % 4 == 0: +134 self.scene.update_render() +135 self.viewer.render() +136 +137 def open_gripper(self): +138 self.set_gripper(0.4) +139 +140 def close_gripper(self): +141 self.set_gripper(0) +142 +143 def move_to_pose_with_RRTConnect(self, pose, use_point_cloud=False, use_attach=False): +144 """ +145 Plan and follow a path to a pose using RRTConnect +146 +147 Args: +148 pose: [x, y, z, qx, qy, qz, qw] +149 use_point_cloud (optional): if to take the point cloud into consideration for collision checking. +150 use_attach (optional): if to take the attach into consideration for collision checking. +151 """ +152 # result is a dictionary with keys 'status', 'time', 'position', 'velocity', 'acceleration', 'duration' +153 result = self.planner.plan_qpos_to_pose( +154 pose, +155 self.robot.get_qpos(), +156 time_step=1/250, +157 use_point_cloud=use_point_cloud, +158 use_attach=use_attach, +159 planner_name="RRTConnect" +160 ) +161 if result['status'] != "Success": +162 print(result['status']) +163 return -1 +164 # do nothing if the planning fails; follow the path if the planning succeeds +165 self.follow_path(result) +166 return 0 +167 +168 def move_to_pose_with_screw(self, pose, use_point_cloud=False, use_attach=False): +169 """ +170 Interpolative planning with screw motion. +171 Will not avoid collision and will fail if the path contains collision. +172 """ +173 result = self.planner.plan_screw( +174 pose, self.robot.get_qpos(), time_step=1/250, use_point_cloud=use_point_cloud, use_attach=use_attach) +175 if result['status'] == "Success": +176 self.follow_path(result) +177 return 0 +178 else: +179 # fall back to RRTConnect if the screw motion fails (say contains collision) +180 return self.move_to_pose_with_RRTConnect(pose, use_point_cloud, use_attach) +181 +182 +183 def move_to_pose(self, pose, with_screw=True, use_point_cloud=False, use_attach=False): +184 """ API to multiplex between the two planning methods """ +185 if with_screw: +186 return self.move_to_pose_with_screw(pose, use_point_cloud, use_attach) +187 else: +188 return self.move_to_pose_with_RRTConnect(pose, use_point_cloud, use_attach) +
8class DemoSetup(): + 9 """ + 10 This class is the super class to abstract away some of the setups for the demos + 11 You will need to install Sapien via `pip install sapien` for this to work if you want to use the viewer + 12 """ + 13 def __init__(self): + 14 """ Nothing to do """ + 15 pass + 16 + 17 def setup_scene(self, **kwargs): + 18 """ This is the Sapien simulator setup and has nothing to do with mplib """ + 19 # declare sapien sim + 20 self.engine = sapien.Engine() + 21 # declare sapien renderer + 22 self.renderer = sapien.SapienRenderer() + 23 # give renderer to sapien sim + 24 self.engine.set_renderer(self.renderer) + 25 + 26 # declare sapien scene + 27 scene_config = sapien.SceneConfig() + 28 self.scene = self.engine.create_scene(scene_config) + 29 # set simulation timestep + 30 self.scene.set_timestep(kwargs.get('timestep', 1 / 240)) + 31 # add ground to scene + 32 self.scene.add_ground(kwargs.get('ground_height', 0)) + 33 # set default physical material + 34 self.scene.default_physical_material = self.scene.create_physical_material( + 35 kwargs.get('static_friction', 1), + 36 kwargs.get('dynamic_friction', 1), + 37 kwargs.get('restitution', 0) + 38 ) + 39 # give some white ambient light of moderate intensity + 40 self.scene.set_ambient_light(kwargs.get('ambient_light', [0.5, 0.5, 0.5])) + 41 # default enable shadow unless specified otherwise + 42 shadow = kwargs.get('shadow', True) + 43 # default spotlight angle and intensity + 44 direction_lights = kwargs.get('direction_lights', [[[0, 1, -1], [0.5, 0.5, 0.5]]]) + 45 for direction_light in direction_lights: + 46 self.scene.add_directional_light(direction_light[0], direction_light[1], shadow=shadow) + 47 # default point lights position and intensity + 48 point_lights = kwargs.get('point_lights', [[[1,2,2], [1,1,1]], [[1,-2,2],[1,1,1]], [[-1,0,1],[1,1,1]]]) + 49 for point_light in point_lights: + 50 self.scene.add_point_light(point_light[0], point_light[1], shadow=shadow) + 51 + 52 # initialize viewer with camera position and orientation + 53 self.viewer = Viewer(self.renderer) + 54 self.viewer.set_scene(self.scene) + 55 self.viewer.set_camera_xyz( + 56 x=kwargs.get('camera_xyz_x', 1.2), + 57 y=kwargs.get('camera_xyz_y', 0.25), + 58 z=kwargs.get('camera_xyz_z', 0.4) + 59 ) + 60 self.viewer.set_camera_rpy( + 61 r=kwargs.get('camera_rpy_r', 0), + 62 p=kwargs.get('camera_rpy_p', -0.4), + 63 y=kwargs.get('camera_rpy_y', 2.7) + 64 ) + 65 + 66 def load_robot(self, **kwargs): + 67 """ + 68 This function loads a robot from a URDF file into the Sapien Scene created above. + 69 Note that does mean that setup_scene() must be called before this function. + 70 """ + 71 loader: sapien.URDFLoader = self.scene.create_urdf_loader() + 72 loader.fix_root_link = True + 73 self.robot: sapien.Articulation = loader.load(kwargs.get('urdf_path', "./data/panda/panda.urdf")) + 74 self.robot.set_root_pose( + 75 sapien.Pose(kwargs.get("robot_origin_xyz", [0,0,0]), kwargs.get("robot_origin_quat", [1,0,0,0])) + 76 ) + 77 self.active_joints = self.robot.get_active_joints() + 78 for joint in self.active_joints: + 79 joint.set_drive_property( + 80 stiffness=kwargs.get('joint_stiffness', 1000), + 81 damping=kwargs.get('joint_damping', 200) + 82 ) + 83 + 84 def setup_planner(self, **kwargs): + 85 """ + 86 Create an mplib planner using the default robot + 87 see planner.py for more details on the arguments + 88 """ + 89 self.planner = mplib.Planner( + 90 urdf=kwargs.get('urdf_path', "./data/panda/panda.urdf"), + 91 srdf=kwargs.get('srdf_path', "./data/panda/panda.srdf"), + 92 move_group=kwargs.get('move_group', 'panda_hand') + 93 ) + 94 + 95 def follow_path(self, result): + 96 """ Helper function to follow a path generated by the planner """ + 97 # number of waypoints in the path + 98 n_step = result['position'].shape[0] + 99 # this makes sure the robot stays neutrally boyant instead of sagging under gravity +100 for i in range(n_step): +101 qf = self.robot.compute_passive_force( +102 external=False, +103 gravity=True, +104 coriolis_and_centrifugal=True) +105 self.robot.set_qf(qf) +106 # set the joint positions and velocities for move group joints only. The others are not the responsibility of the planner +107 for j in range(len(self.planner.move_group_joint_indices)): +108 self.active_joints[j].set_drive_target(result['position'][i][j]) +109 self.active_joints[j].set_drive_velocity_target(result['velocity'][i][j]) +110 # simulation step +111 self.scene.step() +112 # render every 4 simulation steps to make it faster +113 if i % 4 == 0: +114 self.scene.update_render() +115 self.viewer.render() +116 +117 def set_gripper(self, pos): +118 """ +119 Helper function to activate gripper joints +120 Args: +121 pos: position of the gripper joint in real number +122 """ +123 # The following two lines are particular to the panda robot +124 for joint in self.active_joints[-2:]: +125 joint.set_drive_target(pos) +126 # 100 steps is plenty to reach the target position +127 for i in range(100): +128 qf = self.robot.compute_passive_force( +129 external=False, +130 gravity=True, +131 coriolis_and_centrifugal=True) +132 self.robot.set_qf(qf) +133 self.scene.step() +134 if i % 4 == 0: +135 self.scene.update_render() +136 self.viewer.render() +137 +138 def open_gripper(self): +139 self.set_gripper(0.4) +140 +141 def close_gripper(self): +142 self.set_gripper(0) +143 +144 def move_to_pose_with_RRTConnect(self, pose, use_point_cloud=False, use_attach=False): +145 """ +146 Plan and follow a path to a pose using RRTConnect +147 +148 Args: +149 pose: [x, y, z, qx, qy, qz, qw] +150 use_point_cloud (optional): if to take the point cloud into consideration for collision checking. +151 use_attach (optional): if to take the attach into consideration for collision checking. +152 """ +153 # result is a dictionary with keys 'status', 'time', 'position', 'velocity', 'acceleration', 'duration' +154 result = self.planner.plan_qpos_to_pose( +155 pose, +156 self.robot.get_qpos(), +157 time_step=1/250, +158 use_point_cloud=use_point_cloud, +159 use_attach=use_attach, +160 planner_name="RRTConnect" +161 ) +162 if result['status'] != "Success": +163 print(result['status']) +164 return -1 +165 # do nothing if the planning fails; follow the path if the planning succeeds +166 self.follow_path(result) +167 return 0 +168 +169 def move_to_pose_with_screw(self, pose, use_point_cloud=False, use_attach=False): +170 """ +171 Interpolative planning with screw motion. +172 Will not avoid collision and will fail if the path contains collision. +173 """ +174 result = self.planner.plan_screw( +175 pose, self.robot.get_qpos(), time_step=1/250, use_point_cloud=use_point_cloud, use_attach=use_attach) +176 if result['status'] == "Success": +177 self.follow_path(result) +178 return 0 +179 else: +180 # fall back to RRTConnect if the screw motion fails (say contains collision) +181 return self.move_to_pose_with_RRTConnect(pose, use_point_cloud, use_attach) +182 +183 +184 def move_to_pose(self, pose, with_screw=True, use_point_cloud=False, use_attach=False): +185 """ API to multiplex between the two planning methods """ +186 if with_screw: +187 return self.move_to_pose_with_screw(pose, use_point_cloud, use_attach) +188 else: +189 return self.move_to_pose_with_RRTConnect(pose, use_point_cloud, use_attach) +
This class is the super class to abstract away some of the setups for the demos
+You will need to install Sapien via pip install sapien for this to work if you want to use the viewer
17 def setup_scene(self, **kwargs): +18 """ This is the Sapien simulator setup and has nothing to do with mplib """ +19 # declare sapien sim +20 self.engine = sapien.Engine() +21 # declare sapien renderer +22 self.renderer = sapien.SapienRenderer() +23 # give renderer to sapien sim +24 self.engine.set_renderer(self.renderer) +25 +26 # declare sapien scene +27 scene_config = sapien.SceneConfig() +28 self.scene = self.engine.create_scene(scene_config) +29 # set simulation timestep +30 self.scene.set_timestep(kwargs.get('timestep', 1 / 240)) +31 # add ground to scene +32 self.scene.add_ground(kwargs.get('ground_height', 0)) +33 # set default physical material +34 self.scene.default_physical_material = self.scene.create_physical_material( +35 kwargs.get('static_friction', 1), +36 kwargs.get('dynamic_friction', 1), +37 kwargs.get('restitution', 0) +38 ) +39 # give some white ambient light of moderate intensity +40 self.scene.set_ambient_light(kwargs.get('ambient_light', [0.5, 0.5, 0.5])) +41 # default enable shadow unless specified otherwise +42 shadow = kwargs.get('shadow', True) +43 # default spotlight angle and intensity +44 direction_lights = kwargs.get('direction_lights', [[[0, 1, -1], [0.5, 0.5, 0.5]]]) +45 for direction_light in direction_lights: +46 self.scene.add_directional_light(direction_light[0], direction_light[1], shadow=shadow) +47 # default point lights position and intensity +48 point_lights = kwargs.get('point_lights', [[[1,2,2], [1,1,1]], [[1,-2,2],[1,1,1]], [[-1,0,1],[1,1,1]]]) +49 for point_light in point_lights: +50 self.scene.add_point_light(point_light[0], point_light[1], shadow=shadow) +51 +52 # initialize viewer with camera position and orientation +53 self.viewer = Viewer(self.renderer) +54 self.viewer.set_scene(self.scene) +55 self.viewer.set_camera_xyz( +56 x=kwargs.get('camera_xyz_x', 1.2), +57 y=kwargs.get('camera_xyz_y', 0.25), +58 z=kwargs.get('camera_xyz_z', 0.4) +59 ) +60 self.viewer.set_camera_rpy( +61 r=kwargs.get('camera_rpy_r', 0), +62 p=kwargs.get('camera_rpy_p', -0.4), +63 y=kwargs.get('camera_rpy_y', 2.7) +64 ) +
This is the Sapien simulator setup and has nothing to do with mplib
+66 def load_robot(self, **kwargs): +67 """ +68 This function loads a robot from a URDF file into the Sapien Scene created above. +69 Note that does mean that setup_scene() must be called before this function. +70 """ +71 loader: sapien.URDFLoader = self.scene.create_urdf_loader() +72 loader.fix_root_link = True +73 self.robot: sapien.Articulation = loader.load(kwargs.get('urdf_path', "./data/panda/panda.urdf")) +74 self.robot.set_root_pose( +75 sapien.Pose(kwargs.get("robot_origin_xyz", [0,0,0]), kwargs.get("robot_origin_quat", [1,0,0,0])) +76 ) +77 self.active_joints = self.robot.get_active_joints() +78 for joint in self.active_joints: +79 joint.set_drive_property( +80 stiffness=kwargs.get('joint_stiffness', 1000), +81 damping=kwargs.get('joint_damping', 200) +82 ) +
This function loads a robot from a URDF file into the Sapien Scene created above. +Note that does mean that setup_scene() must be called before this function.
+84 def setup_planner(self, **kwargs): +85 """ +86 Create an mplib planner using the default robot +87 see planner.py for more details on the arguments +88 """ +89 self.planner = mplib.Planner( +90 urdf=kwargs.get('urdf_path', "./data/panda/panda.urdf"), +91 srdf=kwargs.get('srdf_path', "./data/panda/panda.srdf"), +92 move_group=kwargs.get('move_group', 'panda_hand') +93 ) +
Create an mplib planner using the default robot +see planner.py for more details on the arguments
+95 def follow_path(self, result): + 96 """ Helper function to follow a path generated by the planner """ + 97 # number of waypoints in the path + 98 n_step = result['position'].shape[0] + 99 # this makes sure the robot stays neutrally boyant instead of sagging under gravity +100 for i in range(n_step): +101 qf = self.robot.compute_passive_force( +102 external=False, +103 gravity=True, +104 coriolis_and_centrifugal=True) +105 self.robot.set_qf(qf) +106 # set the joint positions and velocities for move group joints only. The others are not the responsibility of the planner +107 for j in range(len(self.planner.move_group_joint_indices)): +108 self.active_joints[j].set_drive_target(result['position'][i][j]) +109 self.active_joints[j].set_drive_velocity_target(result['velocity'][i][j]) +110 # simulation step +111 self.scene.step() +112 # render every 4 simulation steps to make it faster +113 if i % 4 == 0: +114 self.scene.update_render() +115 self.viewer.render() +
Helper function to follow a path generated by the planner
+117 def set_gripper(self, pos): +118 """ +119 Helper function to activate gripper joints +120 Args: +121 pos: position of the gripper joint in real number +122 """ +123 # The following two lines are particular to the panda robot +124 for joint in self.active_joints[-2:]: +125 joint.set_drive_target(pos) +126 # 100 steps is plenty to reach the target position +127 for i in range(100): +128 qf = self.robot.compute_passive_force( +129 external=False, +130 gravity=True, +131 coriolis_and_centrifugal=True) +132 self.robot.set_qf(qf) +133 self.scene.step() +134 if i % 4 == 0: +135 self.scene.update_render() +136 self.viewer.render() +
Helper function to activate gripper joints +Args: + pos: position of the gripper joint in real number
+144 def move_to_pose_with_RRTConnect(self, pose, use_point_cloud=False, use_attach=False): +145 """ +146 Plan and follow a path to a pose using RRTConnect +147 +148 Args: +149 pose: [x, y, z, qx, qy, qz, qw] +150 use_point_cloud (optional): if to take the point cloud into consideration for collision checking. +151 use_attach (optional): if to take the attach into consideration for collision checking. +152 """ +153 # result is a dictionary with keys 'status', 'time', 'position', 'velocity', 'acceleration', 'duration' +154 result = self.planner.plan_qpos_to_pose( +155 pose, +156 self.robot.get_qpos(), +157 time_step=1/250, +158 use_point_cloud=use_point_cloud, +159 use_attach=use_attach, +160 planner_name="RRTConnect" +161 ) +162 if result['status'] != "Success": +163 print(result['status']) +164 return -1 +165 # do nothing if the planning fails; follow the path if the planning succeeds +166 self.follow_path(result) +167 return 0 +
Plan and follow a path to a pose using RRTConnect
+ +Args: + pose: [x, y, z, qx, qy, qz, qw] + use_point_cloud (optional): if to take the point cloud into consideration for collision checking. + use_attach (optional): if to take the attach into consideration for collision checking.
+169 def move_to_pose_with_screw(self, pose, use_point_cloud=False, use_attach=False): +170 """ +171 Interpolative planning with screw motion. +172 Will not avoid collision and will fail if the path contains collision. +173 """ +174 result = self.planner.plan_screw( +175 pose, self.robot.get_qpos(), time_step=1/250, use_point_cloud=use_point_cloud, use_attach=use_attach) +176 if result['status'] == "Success": +177 self.follow_path(result) +178 return 0 +179 else: +180 # fall back to RRTConnect if the screw motion fails (say contains collision) +181 return self.move_to_pose_with_RRTConnect(pose, use_point_cloud, use_attach) +
Interpolative planning with screw motion. +Will not avoid collision and will fail if the path contains collision.
+184 def move_to_pose(self, pose, with_screw=True, use_point_cloud=False, use_attach=False): +185 """ API to multiplex between the two planning methods """ +186 if with_screw: +187 return self.move_to_pose_with_screw(pose, use_point_cloud, use_attach) +188 else: +189 return self.move_to_pose_with_RRTConnect(pose, use_point_cloud, use_attach) +
API to multiplex between the two planning methods
+1#!/usr/bin/env python3 + 2 + 3import mplib + 4from .demo_setup import DemoSetup + 5 + 6class DetectCollisionDemo(DemoSetup): + 7 """ + 8 This demonstrates some of the collision detection functions in the planner. + 9 """ +10 def __init__(self): +11 """ Only the planner is needed this time. No simulation env required """ +12 super().__init__() +13 self.setup_planner() +14 +15 def print_collisions(self, collisions): +16 """ Helper function to abstract away the printing of collisions """ +17 if len(collisions) == 0: +18 print("No collision") +19 return +20 for collision in collisions: +21 print(f"{collision.link_name1} of entity {collision.object_name1} collides with " +22 f"{collision.link_name2} of entity {collision.object_name2}") +23 +24 def demo(self): +25 """ +26 We test several configurations: +27 1. Set robot to a self-collision-free qpos and check for self-collision returns no collision +28 2. Set robot to a self-collision qpos and check for self-collision returns a collision +29 3. Set robot to a env-collision-free qpos and check for env-collision returns no collision +30 4. Set robot to a env-collision qpos and check for env-collision returns a collision +31 5. Attempts to plan a path to a qpos is in collision with the world. This will cause the planner to timeout +32 6. Remove the floor and check for env-collision returns no collision +33 """ +34 floor = mplib.planner.fcl.Box([2,2,0.1]) # create a 2 x 2 x 0.1m box +35 # create a collision object for the floor, with a 10cm offset in the z direction +36 floor_fcl_collision_object = mplib.planner.fcl.CollisionObject(floor, [0,0,-0.1], [1,0,0,0]) +37 # update the planning world with the floor collision object +38 self.planner.set_normal_object("floor", floor_fcl_collision_object) +39 +40 print("\n----- self-collision-free qpos -----") +41 # if the joint qpos does not include the gripper joints, it will be set to the current gripper joint angle +42 self_collision_free_qpos = [0, 0.19, 0.0, -2.61, 0.0, 2.94, 0.78] +43 self.print_collisions(self.planner.check_for_self_collision(self.planner.robot, self_collision_free_qpos)) +44 +45 print("\n----- self-collision qpos -----") +46 self_collision_qpos = [0, 1.36, 0, -3, -3, 3, -1] +47 self.print_collisions(self.planner.check_for_self_collision(self.planner.robot, self_collision_qpos)) +48 +49 print("\n----- env-collision-free qpos -----") +50 env_collision_free_qpos = self_collision_free_qpos +51 self.print_collisions(self.planner.check_for_env_collision(self.planner.robot, env_collision_free_qpos)) +52 +53 print("\n----- env-collision qpos -----") +54 env_collision_qpos = [0, 1.5, 0, -1.5, 0, 0, 0] # this qpos causes several joints to dip below the floor +55 self.print_collisions(self.planner.check_for_env_collision(self.planner.robot, env_collision_qpos)) +56 +57 print("\n----- env-collision causing planner to timeout -----") +58 status, path = self.planner.planner.plan(start_state=self_collision_free_qpos, goal_states=[env_collision_qpos]) +59 print(status, path) +60 +61 print("\n----- no more env-collision after removing the floor -----") +62 self.planner.remove_normal_object("floor") +63 self.print_collisions(self.planner.check_for_env_collision(self.planner.robot, env_collision_qpos)) +64 +65if __name__ == '__main__': +66 """ Driver code """ +67 demo = DetectCollisionDemo() +68 demo.demo() +
7class DetectCollisionDemo(DemoSetup): + 8 """ + 9 This demonstrates some of the collision detection functions in the planner. +10 """ +11 def __init__(self): +12 """ Only the planner is needed this time. No simulation env required """ +13 super().__init__() +14 self.setup_planner() +15 +16 def print_collisions(self, collisions): +17 """ Helper function to abstract away the printing of collisions """ +18 if len(collisions) == 0: +19 print("No collision") +20 return +21 for collision in collisions: +22 print(f"{collision.link_name1} of entity {collision.object_name1} collides with " +23 f"{collision.link_name2} of entity {collision.object_name2}") +24 +25 def demo(self): +26 """ +27 We test several configurations: +28 1. Set robot to a self-collision-free qpos and check for self-collision returns no collision +29 2. Set robot to a self-collision qpos and check for self-collision returns a collision +30 3. Set robot to a env-collision-free qpos and check for env-collision returns no collision +31 4. Set robot to a env-collision qpos and check for env-collision returns a collision +32 5. Attempts to plan a path to a qpos is in collision with the world. This will cause the planner to timeout +33 6. Remove the floor and check for env-collision returns no collision +34 """ +35 floor = mplib.planner.fcl.Box([2,2,0.1]) # create a 2 x 2 x 0.1m box +36 # create a collision object for the floor, with a 10cm offset in the z direction +37 floor_fcl_collision_object = mplib.planner.fcl.CollisionObject(floor, [0,0,-0.1], [1,0,0,0]) +38 # update the planning world with the floor collision object +39 self.planner.set_normal_object("floor", floor_fcl_collision_object) +40 +41 print("\n----- self-collision-free qpos -----") +42 # if the joint qpos does not include the gripper joints, it will be set to the current gripper joint angle +43 self_collision_free_qpos = [0, 0.19, 0.0, -2.61, 0.0, 2.94, 0.78] +44 self.print_collisions(self.planner.check_for_self_collision(self.planner.robot, self_collision_free_qpos)) +45 +46 print("\n----- self-collision qpos -----") +47 self_collision_qpos = [0, 1.36, 0, -3, -3, 3, -1] +48 self.print_collisions(self.planner.check_for_self_collision(self.planner.robot, self_collision_qpos)) +49 +50 print("\n----- env-collision-free qpos -----") +51 env_collision_free_qpos = self_collision_free_qpos +52 self.print_collisions(self.planner.check_for_env_collision(self.planner.robot, env_collision_free_qpos)) +53 +54 print("\n----- env-collision qpos -----") +55 env_collision_qpos = [0, 1.5, 0, -1.5, 0, 0, 0] # this qpos causes several joints to dip below the floor +56 self.print_collisions(self.planner.check_for_env_collision(self.planner.robot, env_collision_qpos)) +57 +58 print("\n----- env-collision causing planner to timeout -----") +59 status, path = self.planner.planner.plan(start_state=self_collision_free_qpos, goal_states=[env_collision_qpos]) +60 print(status, path) +61 +62 print("\n----- no more env-collision after removing the floor -----") +63 self.planner.remove_normal_object("floor") +64 self.print_collisions(self.planner.check_for_env_collision(self.planner.robot, env_collision_qpos)) +
This demonstrates some of the collision detection functions in the planner.
+11 def __init__(self): +12 """ Only the planner is needed this time. No simulation env required """ +13 super().__init__() +14 self.setup_planner() +
Only the planner is needed this time. No simulation env required
+16 def print_collisions(self, collisions): +17 """ Helper function to abstract away the printing of collisions """ +18 if len(collisions) == 0: +19 print("No collision") +20 return +21 for collision in collisions: +22 print(f"{collision.link_name1} of entity {collision.object_name1} collides with " +23 f"{collision.link_name2} of entity {collision.object_name2}") +
Helper function to abstract away the printing of collisions
+25 def demo(self): +26 """ +27 We test several configurations: +28 1. Set robot to a self-collision-free qpos and check for self-collision returns no collision +29 2. Set robot to a self-collision qpos and check for self-collision returns a collision +30 3. Set robot to a env-collision-free qpos and check for env-collision returns no collision +31 4. Set robot to a env-collision qpos and check for env-collision returns a collision +32 5. Attempts to plan a path to a qpos is in collision with the world. This will cause the planner to timeout +33 6. Remove the floor and check for env-collision returns no collision +34 """ +35 floor = mplib.planner.fcl.Box([2,2,0.1]) # create a 2 x 2 x 0.1m box +36 # create a collision object for the floor, with a 10cm offset in the z direction +37 floor_fcl_collision_object = mplib.planner.fcl.CollisionObject(floor, [0,0,-0.1], [1,0,0,0]) +38 # update the planning world with the floor collision object +39 self.planner.set_normal_object("floor", floor_fcl_collision_object) +40 +41 print("\n----- self-collision-free qpos -----") +42 # if the joint qpos does not include the gripper joints, it will be set to the current gripper joint angle +43 self_collision_free_qpos = [0, 0.19, 0.0, -2.61, 0.0, 2.94, 0.78] +44 self.print_collisions(self.planner.check_for_self_collision(self.planner.robot, self_collision_free_qpos)) +45 +46 print("\n----- self-collision qpos -----") +47 self_collision_qpos = [0, 1.36, 0, -3, -3, 3, -1] +48 self.print_collisions(self.planner.check_for_self_collision(self.planner.robot, self_collision_qpos)) +49 +50 print("\n----- env-collision-free qpos -----") +51 env_collision_free_qpos = self_collision_free_qpos +52 self.print_collisions(self.planner.check_for_env_collision(self.planner.robot, env_collision_free_qpos)) +53 +54 print("\n----- env-collision qpos -----") +55 env_collision_qpos = [0, 1.5, 0, -1.5, 0, 0, 0] # this qpos causes several joints to dip below the floor +56 self.print_collisions(self.planner.check_for_env_collision(self.planner.robot, env_collision_qpos)) +57 +58 print("\n----- env-collision causing planner to timeout -----") +59 status, path = self.planner.planner.plan(start_state=self_collision_free_qpos, goal_states=[env_collision_qpos]) +60 print(status, path) +61 +62 print("\n----- no more env-collision after removing the floor -----") +63 self.planner.remove_normal_object("floor") +64 self.print_collisions(self.planner.check_for_env_collision(self.planner.robot, env_collision_qpos)) +
We test several configurations:
+ +1import sapien.core as sapien + 2from .demo_setup import DemoSetup + 3 + 4class PlanningDemo(DemoSetup): + 5 """ This is identical to demo.py except the whole scene is shifted to the bottom right by 1 meter respectively """ + 6 def __init__(self): + 7 """ + 8 Setting up the scene, the planner, and adding some objects to the scene + 9 Note that we place the robot at [1,1,0] in the simulation and also tell the planner that the robot is at [1,1,0] +10 Afterwards, put down a table and three boxes. For details on how to do this, see the sapien documentation +11 Compared to demo.py, all the props are shifted by 1 meter in the x and y direction +12 """ +13 super().__init__() +14 self.setup_scene() +15 self.load_robot(robot_origin_xyz=[1, 1, 0]) +16 self.setup_planner() +17 +18 # We also need to tell the planner where the base is since the sim and planner don't share info +19 self.planner.set_base_pose([1, 1, 0, 1, 0, 0, 0]) +20 +21 # Set initial joint positions +22 init_qpos = [0, 0.19, 0.0, -2.61, 0.0, 2.94, 0.78, 0, 0] +23 self.robot.set_qpos(init_qpos) +24 +25 # table top +26 builder = self.scene.create_actor_builder() +27 builder.add_box_collision(half_size=[0.4, 0.4, 0.025]) +28 builder.add_box_visual(half_size=[0.4, 0.4, 0.025]) +29 self.table = builder.build_kinematic(name='table') +30 self.table.set_pose(sapien.Pose([1.56, 1, -0.025])) +31 +32 # boxes +33 builder = self.scene.create_actor_builder() +34 builder.add_box_collision(half_size=[0.02, 0.02, 0.06]) +35 builder.add_box_visual(half_size=[0.02, 0.02, 0.06], color=[1, 0, 0]) +36 self.red_cube = builder.build(name='red_cube') +37 self.red_cube.set_pose(sapien.Pose([1.4, 1.3, 0.06])) +38 +39 builder = self.scene.create_actor_builder() +40 builder.add_box_collision(half_size=[0.02, 0.02, 0.04]) +41 builder.add_box_visual(half_size=[0.02, 0.02, 0.04], color=[0, 1, 0]) +42 self.green_cube = builder.build(name='green_cube') +43 self.green_cube.set_pose(sapien.Pose([1.2, 0.7, 0.04])) +44 +45 builder = self.scene.create_actor_builder() +46 builder.add_box_collision(half_size=[0.02, 0.02, 0.07]) +47 builder.add_box_visual(half_size=[0.02, 0.02, 0.07], color=[0, 0, 1]) +48 self.blue_cube = builder.build(name='blue_cube') +49 self.blue_cube.set_pose(sapien.Pose([1.6, 1.1, 0.07])) +50 +51 def demo(self): +52 """ +53 Same demo as demo.py. +54 Although we shifted everything, the poses have not changed because these are w.r.t. the robot base +55 Alternatively, we can also shift the poses by 1 meter in the x and y direction and tell the planner +56 the poses are specified with respect to the world +57 """ +58 poses = [[0.4, 0.3, 0.12, 0, 1, 0, 0], +59 [0.2, -0.3, 0.08, 0, 1, 0, 0], +60 [0.6, 0.1, 0.14, 0, 1, 0, 0]] +61 for i in range(3): +62 pose = poses[i] +63 pose[2] += 0.2 +64 self.move_to_pose(pose) +65 self.open_gripper() +66 pose[2] -= 0.12 +67 self.move_to_pose(pose) +68 self.close_gripper() +69 pose[2] += 0.12 +70 self.move_to_pose(pose) +71 pose[0] += 0.1 +72 self.move_to_pose(pose) +73 pose[2] -= 0.12 +74 self.move_to_pose(pose) +75 self.open_gripper() +76 pose[2] += 0.12 +77 self.move_to_pose(pose) +78 +79 # convert the poses to be w.r.t. the world +80 for pose in poses: +81 pose[0] += 1 +82 pose[1] += 1 +83 +84 # plan a path to the first pose +85 result = self.planner.plan_qpos_to_pose( +86 poses[0], +87 self.planner.robot.get_qpos(), +88 time_step=1/250, +89 wrt_world=True +90 ) +91 if result['status'] != "Success": +92 print(result['status']) +93 return -1 +94 self.follow_path(result) +95 +96if __name__ == '__main__': +97 """ Driver code """ +98 demo = PlanningDemo() +99 demo.demo() +
5class PlanningDemo(DemoSetup): + 6 """ This is identical to demo.py except the whole scene is shifted to the bottom right by 1 meter respectively """ + 7 def __init__(self): + 8 """ + 9 Setting up the scene, the planner, and adding some objects to the scene +10 Note that we place the robot at [1,1,0] in the simulation and also tell the planner that the robot is at [1,1,0] +11 Afterwards, put down a table and three boxes. For details on how to do this, see the sapien documentation +12 Compared to demo.py, all the props are shifted by 1 meter in the x and y direction +13 """ +14 super().__init__() +15 self.setup_scene() +16 self.load_robot(robot_origin_xyz=[1, 1, 0]) +17 self.setup_planner() +18 +19 # We also need to tell the planner where the base is since the sim and planner don't share info +20 self.planner.set_base_pose([1, 1, 0, 1, 0, 0, 0]) +21 +22 # Set initial joint positions +23 init_qpos = [0, 0.19, 0.0, -2.61, 0.0, 2.94, 0.78, 0, 0] +24 self.robot.set_qpos(init_qpos) +25 +26 # table top +27 builder = self.scene.create_actor_builder() +28 builder.add_box_collision(half_size=[0.4, 0.4, 0.025]) +29 builder.add_box_visual(half_size=[0.4, 0.4, 0.025]) +30 self.table = builder.build_kinematic(name='table') +31 self.table.set_pose(sapien.Pose([1.56, 1, -0.025])) +32 +33 # boxes +34 builder = self.scene.create_actor_builder() +35 builder.add_box_collision(half_size=[0.02, 0.02, 0.06]) +36 builder.add_box_visual(half_size=[0.02, 0.02, 0.06], color=[1, 0, 0]) +37 self.red_cube = builder.build(name='red_cube') +38 self.red_cube.set_pose(sapien.Pose([1.4, 1.3, 0.06])) +39 +40 builder = self.scene.create_actor_builder() +41 builder.add_box_collision(half_size=[0.02, 0.02, 0.04]) +42 builder.add_box_visual(half_size=[0.02, 0.02, 0.04], color=[0, 1, 0]) +43 self.green_cube = builder.build(name='green_cube') +44 self.green_cube.set_pose(sapien.Pose([1.2, 0.7, 0.04])) +45 +46 builder = self.scene.create_actor_builder() +47 builder.add_box_collision(half_size=[0.02, 0.02, 0.07]) +48 builder.add_box_visual(half_size=[0.02, 0.02, 0.07], color=[0, 0, 1]) +49 self.blue_cube = builder.build(name='blue_cube') +50 self.blue_cube.set_pose(sapien.Pose([1.6, 1.1, 0.07])) +51 +52 def demo(self): +53 """ +54 Same demo as demo.py. +55 Although we shifted everything, the poses have not changed because these are w.r.t. the robot base +56 Alternatively, we can also shift the poses by 1 meter in the x and y direction and tell the planner +57 the poses are specified with respect to the world +58 """ +59 poses = [[0.4, 0.3, 0.12, 0, 1, 0, 0], +60 [0.2, -0.3, 0.08, 0, 1, 0, 0], +61 [0.6, 0.1, 0.14, 0, 1, 0, 0]] +62 for i in range(3): +63 pose = poses[i] +64 pose[2] += 0.2 +65 self.move_to_pose(pose) +66 self.open_gripper() +67 pose[2] -= 0.12 +68 self.move_to_pose(pose) +69 self.close_gripper() +70 pose[2] += 0.12 +71 self.move_to_pose(pose) +72 pose[0] += 0.1 +73 self.move_to_pose(pose) +74 pose[2] -= 0.12 +75 self.move_to_pose(pose) +76 self.open_gripper() +77 pose[2] += 0.12 +78 self.move_to_pose(pose) +79 +80 # convert the poses to be w.r.t. the world +81 for pose in poses: +82 pose[0] += 1 +83 pose[1] += 1 +84 +85 # plan a path to the first pose +86 result = self.planner.plan_qpos_to_pose( +87 poses[0], +88 self.planner.robot.get_qpos(), +89 time_step=1/250, +90 wrt_world=True +91 ) +92 if result['status'] != "Success": +93 print(result['status']) +94 return -1 +95 self.follow_path(result) +
This is identical to demo.py except the whole scene is shifted to the bottom right by 1 meter respectively
+7 def __init__(self): + 8 """ + 9 Setting up the scene, the planner, and adding some objects to the scene +10 Note that we place the robot at [1,1,0] in the simulation and also tell the planner that the robot is at [1,1,0] +11 Afterwards, put down a table and three boxes. For details on how to do this, see the sapien documentation +12 Compared to demo.py, all the props are shifted by 1 meter in the x and y direction +13 """ +14 super().__init__() +15 self.setup_scene() +16 self.load_robot(robot_origin_xyz=[1, 1, 0]) +17 self.setup_planner() +18 +19 # We also need to tell the planner where the base is since the sim and planner don't share info +20 self.planner.set_base_pose([1, 1, 0, 1, 0, 0, 0]) +21 +22 # Set initial joint positions +23 init_qpos = [0, 0.19, 0.0, -2.61, 0.0, 2.94, 0.78, 0, 0] +24 self.robot.set_qpos(init_qpos) +25 +26 # table top +27 builder = self.scene.create_actor_builder() +28 builder.add_box_collision(half_size=[0.4, 0.4, 0.025]) +29 builder.add_box_visual(half_size=[0.4, 0.4, 0.025]) +30 self.table = builder.build_kinematic(name='table') +31 self.table.set_pose(sapien.Pose([1.56, 1, -0.025])) +32 +33 # boxes +34 builder = self.scene.create_actor_builder() +35 builder.add_box_collision(half_size=[0.02, 0.02, 0.06]) +36 builder.add_box_visual(half_size=[0.02, 0.02, 0.06], color=[1, 0, 0]) +37 self.red_cube = builder.build(name='red_cube') +38 self.red_cube.set_pose(sapien.Pose([1.4, 1.3, 0.06])) +39 +40 builder = self.scene.create_actor_builder() +41 builder.add_box_collision(half_size=[0.02, 0.02, 0.04]) +42 builder.add_box_visual(half_size=[0.02, 0.02, 0.04], color=[0, 1, 0]) +43 self.green_cube = builder.build(name='green_cube') +44 self.green_cube.set_pose(sapien.Pose([1.2, 0.7, 0.04])) +45 +46 builder = self.scene.create_actor_builder() +47 builder.add_box_collision(half_size=[0.02, 0.02, 0.07]) +48 builder.add_box_visual(half_size=[0.02, 0.02, 0.07], color=[0, 0, 1]) +49 self.blue_cube = builder.build(name='blue_cube') +50 self.blue_cube.set_pose(sapien.Pose([1.6, 1.1, 0.07])) +
Setting up the scene, the planner, and adding some objects to the scene +Note that we place the robot at [1,1,0] in the simulation and also tell the planner that the robot is at [1,1,0] +Afterwards, put down a table and three boxes. For details on how to do this, see the sapien documentation +Compared to demo.py, all the props are shifted by 1 meter in the x and y direction
+52 def demo(self): +53 """ +54 Same demo as demo.py. +55 Although we shifted everything, the poses have not changed because these are w.r.t. the robot base +56 Alternatively, we can also shift the poses by 1 meter in the x and y direction and tell the planner +57 the poses are specified with respect to the world +58 """ +59 poses = [[0.4, 0.3, 0.12, 0, 1, 0, 0], +60 [0.2, -0.3, 0.08, 0, 1, 0, 0], +61 [0.6, 0.1, 0.14, 0, 1, 0, 0]] +62 for i in range(3): +63 pose = poses[i] +64 pose[2] += 0.2 +65 self.move_to_pose(pose) +66 self.open_gripper() +67 pose[2] -= 0.12 +68 self.move_to_pose(pose) +69 self.close_gripper() +70 pose[2] += 0.12 +71 self.move_to_pose(pose) +72 pose[0] += 0.1 +73 self.move_to_pose(pose) +74 pose[2] -= 0.12 +75 self.move_to_pose(pose) +76 self.open_gripper() +77 pose[2] += 0.12 +78 self.move_to_pose(pose) +79 +80 # convert the poses to be w.r.t. the world +81 for pose in poses: +82 pose[0] += 1 +83 pose[1] += 1 +84 +85 # plan a path to the first pose +86 result = self.planner.plan_qpos_to_pose( +87 poses[0], +88 self.planner.robot.get_qpos(), +89 time_step=1/250, +90 wrt_world=True +91 ) +92 if result['status'] != "Success": +93 print(result['status']) +94 return -1 +95 self.follow_path(result) +
Same demo as demo.py. +Although we shifted everything, the poses have not changed because these are w.r.t. the robot base +Alternatively, we can also shift the poses by 1 meter in the x and y direction and tell the planner +the poses are specified with respect to the world
+1import sapien.core as sapien + 2import mplib + 3import numpy as np + 4from .demo_setup import DemoSetup + 5 + 6class PlanningDemo(DemoSetup): + 7 """ + 8 This demo is the same as collision_avoidance.py except we added a track for the robot to move along + 9 We reach the target in two stages: + 10 1. First, we move the base while fixing the arm joints + 11 2. Then, we move the arm while fixing the base joints + 12 This corresponds to a mobile robot which can move in the x and y direction with a manipulator on top + 13 """ + 14 def __init__(self): + 15 """ + 16 We have modified the urdf file to include a track for the robot to move along + 17 Otherwise, the setup is the same as collision_avoidance.py + 18 """ + 19 super().__init__() + 20 self.setup_scene() + 21 self.load_robot(urdf_path="./data/panda/panda_on_rail.urdf") + 22 link_names = ["track_x", "track_y", "panda_link0", "panda_link1", "panda_link2", "panda_link3", "panda_link4", "panda_link5", "panda_link6", "panda_link7", "panda_hand", "panda_leftfinger", "panda_rightfinger"] + 23 joint_names = ["move_x", "move_y", "panda_joint1", "panda_joint2", "panda_joint3", "panda_joint4", "panda_joint5", "panda_joint6", "panda_joint7", "panda_finger_joint1", "panda_finger_joint2"] + 24 self.setup_planner( + 25 urdf_path="./data/panda/panda_on_rail.urdf", + 26 srdf_path="./data/panda/panda_on_rail.srdf", + 27 link_names=link_names, + 28 joint_names=joint_names, + 29 joint_vel_limits=np.ones(9), + 30 joint_acc_limits=np.ones(9) + 31 ) + 32 + 33 # Set initial joint positions + 34 init_qpos = [0, 0, 0, 0.19, 0.0, -2.61, 0.0, 2.94, 0.78, 0, 0] + 35 self.robot.set_qpos(init_qpos) + 36 + 37 # table top + 38 builder = self.scene.create_actor_builder() + 39 builder.add_box_collision(half_size=[0.4, 0.4, 0.025]) + 40 builder.add_box_visual(half_size=[0.4, 0.4, 0.025]) + 41 self.table = builder.build_kinematic(name='table') + 42 self.table.set_pose(sapien.Pose([0.56, 0, - 0.025])) + 43 + 44 # boxes + 45 builder = self.scene.create_actor_builder() + 46 builder.add_box_collision(half_size=[0.02, 0.02, 0.06]) + 47 builder.add_box_visual(half_size=[0.02, 0.02, 0.06], color=[1, 0, 0]) + 48 self.red_cube = builder.build(name='red_cube') + 49 self.red_cube.set_pose(sapien.Pose([0.7, 0, 0.06])) + 50 + 51 builder = self.scene.create_actor_builder() + 52 builder.add_box_collision(half_size=[0.04, 0.04, 0.005]) + 53 builder.add_box_visual(half_size=[0.04, 0.04, 0.005], color=[0, 1, 0]) + 54 self.green_cube = builder.build(name='green_cube') + 55 self.green_cube.set_pose(sapien.Pose([0.4, 0.3, 0.005])) + 56 + 57 builder = self.scene.create_actor_builder() + 58 builder.add_box_collision(half_size=[0.05, 0.2, 0.1]) + 59 builder.add_box_visual(half_size=[0.05, 0.2, 0.1], color=[0, 0, 1]) + 60 self.blue_cube = builder.build(name='blue_cube') + 61 self.blue_cube.set_pose(sapien.Pose([0.55, 0, 0.1])) + 62 + 63 def add_point_cloud(self): + 64 """ see collision_avoidance.py for details """ + 65 import trimesh + 66 box = trimesh.creation.box([0.1, 0.4, 0.2]) + 67 points, _ = trimesh.sample.sample_surface(box, 1000) + 68 points += [0.55, 0, 0.1] + 69 self.planner.update_point_cloud(points) + 70 return + 71 + 72 def plan_without_base(self, pose, has_attach=False): + 73 """ a subroutine to plan a path without moving the base """ + 74 # now do a partial ik to the pose with the base fixed + 75 status, goal_qposes = self.planner.IK(pose, self.robot.get_qpos(), mask=[1,1,0,0,0,0,0,0,0]) + 76 if status != "Success": + 77 print("IK failed") + 78 exit(1) + 79 # now fix base and plan a path to the goal + 80 result = self.planner.plan_qpos_to_qpos(goal_qposes, self.robot.get_qpos(), + 81 use_point_cloud=True, use_attach=has_attach, time_step=1/250, fixed_joint_indices=range(2)) + 82 return result + 83 + 84 def move_in_two_stage(self, pose, has_attach=False): + 85 """ + 86 first, we do a full IK but only generate motions for the base + 87 then, do another partial IK for the arm and generate motions for the arm + 88 """ + 89 # do a full ik to the pose + 90 status, goal_qposes = self.planner.IK(pose, self.robot.get_qpos()) + 91 if status != "Success": + 92 print("IK failed") + 93 exit(1) + 94 # now fix arm joints and plan a path to the goal + 95 result = self.planner.plan_qpos_to_qpos(goal_qposes, self.robot.get_qpos(), + 96 use_point_cloud=True, use_attach=has_attach, time_step=1/250, fixed_joint_indices=range(2, 9)) + 97 # execute the planned path + 98 self.follow_path(result) + 99 result = self.plan_without_base(pose, has_attach) +100 # execute the planned path +101 self.follow_path(result) +102 +103 def demo(self): +104 """ +105 We reach the pick up and drop off poses in two stages, first by moving the base only and then the arm only +106 """ +107 pickup_pose = [0.7, 0, 0.12, 0, 1, 0, 0] +108 delivery_pose = [0.4, 0.3, 0.13, 0, 1, 0, 0] +109 +110 self.add_point_cloud() +111 # also add the target as a collision object so we don't hit it +112 fcl_red_cube = mplib.planner.fcl.Box([0.04, 0.04, 0.14]) +113 collision_object = mplib.planner.fcl.CollisionObject(fcl_red_cube, [0.7, 0, 0.07], [1, 0, 0, 0]) +114 self.planner.planning_world.set_normal_object("target", collision_object) +115 +116 # go above the target +117 pickup_pose[2] += 0.2 +118 self.move_in_two_stage(pickup_pose) +119 self.open_gripper() +120 # move down to pick +121 self.planner.planning_world.remove_normal_object("target") # remove the object so we don't report collision +122 pickup_pose[2] -= 0.12 +123 result = self.plan_without_base(pickup_pose) +124 self.follow_path(result) +125 self.close_gripper() +126 +127 # add the attached box to the planning world +128 self.planner.update_attached_box([0.04, 0.04, 0.12], [0, 0, 0.14, 1, 0, 0, 0]) +129 +130 pickup_pose[2] += 0.12 +131 result = self.plan_without_base(pickup_pose, has_attach=True) +132 self.follow_path(result) +133 delivery_pose[2] += 0.2 +134 # now go to the drop off in two stages +135 self.move_in_two_stage(delivery_pose, has_attach=True) +136 delivery_pose[2] -= 0.12 +137 result = self.plan_without_base(delivery_pose, has_attach=True) +138 self.follow_path(result) +139 self.open_gripper() +140 delivery_pose[2] += 0.12 +141 result = self.plan_without_base(delivery_pose) +142 self.follow_path(result) +143 +144if __name__ == '__main__': +145 demo = PlanningDemo() +146 demo.demo() +
7class PlanningDemo(DemoSetup): + 8 """ + 9 This demo is the same as collision_avoidance.py except we added a track for the robot to move along + 10 We reach the target in two stages: + 11 1. First, we move the base while fixing the arm joints + 12 2. Then, we move the arm while fixing the base joints + 13 This corresponds to a mobile robot which can move in the x and y direction with a manipulator on top + 14 """ + 15 def __init__(self): + 16 """ + 17 We have modified the urdf file to include a track for the robot to move along + 18 Otherwise, the setup is the same as collision_avoidance.py + 19 """ + 20 super().__init__() + 21 self.setup_scene() + 22 self.load_robot(urdf_path="./data/panda/panda_on_rail.urdf") + 23 link_names = ["track_x", "track_y", "panda_link0", "panda_link1", "panda_link2", "panda_link3", "panda_link4", "panda_link5", "panda_link6", "panda_link7", "panda_hand", "panda_leftfinger", "panda_rightfinger"] + 24 joint_names = ["move_x", "move_y", "panda_joint1", "panda_joint2", "panda_joint3", "panda_joint4", "panda_joint5", "panda_joint6", "panda_joint7", "panda_finger_joint1", "panda_finger_joint2"] + 25 self.setup_planner( + 26 urdf_path="./data/panda/panda_on_rail.urdf", + 27 srdf_path="./data/panda/panda_on_rail.srdf", + 28 link_names=link_names, + 29 joint_names=joint_names, + 30 joint_vel_limits=np.ones(9), + 31 joint_acc_limits=np.ones(9) + 32 ) + 33 + 34 # Set initial joint positions + 35 init_qpos = [0, 0, 0, 0.19, 0.0, -2.61, 0.0, 2.94, 0.78, 0, 0] + 36 self.robot.set_qpos(init_qpos) + 37 + 38 # table top + 39 builder = self.scene.create_actor_builder() + 40 builder.add_box_collision(half_size=[0.4, 0.4, 0.025]) + 41 builder.add_box_visual(half_size=[0.4, 0.4, 0.025]) + 42 self.table = builder.build_kinematic(name='table') + 43 self.table.set_pose(sapien.Pose([0.56, 0, - 0.025])) + 44 + 45 # boxes + 46 builder = self.scene.create_actor_builder() + 47 builder.add_box_collision(half_size=[0.02, 0.02, 0.06]) + 48 builder.add_box_visual(half_size=[0.02, 0.02, 0.06], color=[1, 0, 0]) + 49 self.red_cube = builder.build(name='red_cube') + 50 self.red_cube.set_pose(sapien.Pose([0.7, 0, 0.06])) + 51 + 52 builder = self.scene.create_actor_builder() + 53 builder.add_box_collision(half_size=[0.04, 0.04, 0.005]) + 54 builder.add_box_visual(half_size=[0.04, 0.04, 0.005], color=[0, 1, 0]) + 55 self.green_cube = builder.build(name='green_cube') + 56 self.green_cube.set_pose(sapien.Pose([0.4, 0.3, 0.005])) + 57 + 58 builder = self.scene.create_actor_builder() + 59 builder.add_box_collision(half_size=[0.05, 0.2, 0.1]) + 60 builder.add_box_visual(half_size=[0.05, 0.2, 0.1], color=[0, 0, 1]) + 61 self.blue_cube = builder.build(name='blue_cube') + 62 self.blue_cube.set_pose(sapien.Pose([0.55, 0, 0.1])) + 63 + 64 def add_point_cloud(self): + 65 """ see collision_avoidance.py for details """ + 66 import trimesh + 67 box = trimesh.creation.box([0.1, 0.4, 0.2]) + 68 points, _ = trimesh.sample.sample_surface(box, 1000) + 69 points += [0.55, 0, 0.1] + 70 self.planner.update_point_cloud(points) + 71 return + 72 + 73 def plan_without_base(self, pose, has_attach=False): + 74 """ a subroutine to plan a path without moving the base """ + 75 # now do a partial ik to the pose with the base fixed + 76 status, goal_qposes = self.planner.IK(pose, self.robot.get_qpos(), mask=[1,1,0,0,0,0,0,0,0]) + 77 if status != "Success": + 78 print("IK failed") + 79 exit(1) + 80 # now fix base and plan a path to the goal + 81 result = self.planner.plan_qpos_to_qpos(goal_qposes, self.robot.get_qpos(), + 82 use_point_cloud=True, use_attach=has_attach, time_step=1/250, fixed_joint_indices=range(2)) + 83 return result + 84 + 85 def move_in_two_stage(self, pose, has_attach=False): + 86 """ + 87 first, we do a full IK but only generate motions for the base + 88 then, do another partial IK for the arm and generate motions for the arm + 89 """ + 90 # do a full ik to the pose + 91 status, goal_qposes = self.planner.IK(pose, self.robot.get_qpos()) + 92 if status != "Success": + 93 print("IK failed") + 94 exit(1) + 95 # now fix arm joints and plan a path to the goal + 96 result = self.planner.plan_qpos_to_qpos(goal_qposes, self.robot.get_qpos(), + 97 use_point_cloud=True, use_attach=has_attach, time_step=1/250, fixed_joint_indices=range(2, 9)) + 98 # execute the planned path + 99 self.follow_path(result) +100 result = self.plan_without_base(pose, has_attach) +101 # execute the planned path +102 self.follow_path(result) +103 +104 def demo(self): +105 """ +106 We reach the pick up and drop off poses in two stages, first by moving the base only and then the arm only +107 """ +108 pickup_pose = [0.7, 0, 0.12, 0, 1, 0, 0] +109 delivery_pose = [0.4, 0.3, 0.13, 0, 1, 0, 0] +110 +111 self.add_point_cloud() +112 # also add the target as a collision object so we don't hit it +113 fcl_red_cube = mplib.planner.fcl.Box([0.04, 0.04, 0.14]) +114 collision_object = mplib.planner.fcl.CollisionObject(fcl_red_cube, [0.7, 0, 0.07], [1, 0, 0, 0]) +115 self.planner.planning_world.set_normal_object("target", collision_object) +116 +117 # go above the target +118 pickup_pose[2] += 0.2 +119 self.move_in_two_stage(pickup_pose) +120 self.open_gripper() +121 # move down to pick +122 self.planner.planning_world.remove_normal_object("target") # remove the object so we don't report collision +123 pickup_pose[2] -= 0.12 +124 result = self.plan_without_base(pickup_pose) +125 self.follow_path(result) +126 self.close_gripper() +127 +128 # add the attached box to the planning world +129 self.planner.update_attached_box([0.04, 0.04, 0.12], [0, 0, 0.14, 1, 0, 0, 0]) +130 +131 pickup_pose[2] += 0.12 +132 result = self.plan_without_base(pickup_pose, has_attach=True) +133 self.follow_path(result) +134 delivery_pose[2] += 0.2 +135 # now go to the drop off in two stages +136 self.move_in_two_stage(delivery_pose, has_attach=True) +137 delivery_pose[2] -= 0.12 +138 result = self.plan_without_base(delivery_pose, has_attach=True) +139 self.follow_path(result) +140 self.open_gripper() +141 delivery_pose[2] += 0.12 +142 result = self.plan_without_base(delivery_pose) +143 self.follow_path(result) +
This demo is the same as collision_avoidance.py except we added a track for the robot to move along +We reach the target in two stages:
+ +15 def __init__(self): +16 """ +17 We have modified the urdf file to include a track for the robot to move along +18 Otherwise, the setup is the same as collision_avoidance.py +19 """ +20 super().__init__() +21 self.setup_scene() +22 self.load_robot(urdf_path="./data/panda/panda_on_rail.urdf") +23 link_names = ["track_x", "track_y", "panda_link0", "panda_link1", "panda_link2", "panda_link3", "panda_link4", "panda_link5", "panda_link6", "panda_link7", "panda_hand", "panda_leftfinger", "panda_rightfinger"] +24 joint_names = ["move_x", "move_y", "panda_joint1", "panda_joint2", "panda_joint3", "panda_joint4", "panda_joint5", "panda_joint6", "panda_joint7", "panda_finger_joint1", "panda_finger_joint2"] +25 self.setup_planner( +26 urdf_path="./data/panda/panda_on_rail.urdf", +27 srdf_path="./data/panda/panda_on_rail.srdf", +28 link_names=link_names, +29 joint_names=joint_names, +30 joint_vel_limits=np.ones(9), +31 joint_acc_limits=np.ones(9) +32 ) +33 +34 # Set initial joint positions +35 init_qpos = [0, 0, 0, 0.19, 0.0, -2.61, 0.0, 2.94, 0.78, 0, 0] +36 self.robot.set_qpos(init_qpos) +37 +38 # table top +39 builder = self.scene.create_actor_builder() +40 builder.add_box_collision(half_size=[0.4, 0.4, 0.025]) +41 builder.add_box_visual(half_size=[0.4, 0.4, 0.025]) +42 self.table = builder.build_kinematic(name='table') +43 self.table.set_pose(sapien.Pose([0.56, 0, - 0.025])) +44 +45 # boxes +46 builder = self.scene.create_actor_builder() +47 builder.add_box_collision(half_size=[0.02, 0.02, 0.06]) +48 builder.add_box_visual(half_size=[0.02, 0.02, 0.06], color=[1, 0, 0]) +49 self.red_cube = builder.build(name='red_cube') +50 self.red_cube.set_pose(sapien.Pose([0.7, 0, 0.06])) +51 +52 builder = self.scene.create_actor_builder() +53 builder.add_box_collision(half_size=[0.04, 0.04, 0.005]) +54 builder.add_box_visual(half_size=[0.04, 0.04, 0.005], color=[0, 1, 0]) +55 self.green_cube = builder.build(name='green_cube') +56 self.green_cube.set_pose(sapien.Pose([0.4, 0.3, 0.005])) +57 +58 builder = self.scene.create_actor_builder() +59 builder.add_box_collision(half_size=[0.05, 0.2, 0.1]) +60 builder.add_box_visual(half_size=[0.05, 0.2, 0.1], color=[0, 0, 1]) +61 self.blue_cube = builder.build(name='blue_cube') +62 self.blue_cube.set_pose(sapien.Pose([0.55, 0, 0.1])) +
We have modified the urdf file to include a track for the robot to move along +Otherwise, the setup is the same as collision_avoidance.py
+64 def add_point_cloud(self): +65 """ see collision_avoidance.py for details """ +66 import trimesh +67 box = trimesh.creation.box([0.1, 0.4, 0.2]) +68 points, _ = trimesh.sample.sample_surface(box, 1000) +69 points += [0.55, 0, 0.1] +70 self.planner.update_point_cloud(points) +71 return +
see collision_avoidance.py for details
+73 def plan_without_base(self, pose, has_attach=False): +74 """ a subroutine to plan a path without moving the base """ +75 # now do a partial ik to the pose with the base fixed +76 status, goal_qposes = self.planner.IK(pose, self.robot.get_qpos(), mask=[1,1,0,0,0,0,0,0,0]) +77 if status != "Success": +78 print("IK failed") +79 exit(1) +80 # now fix base and plan a path to the goal +81 result = self.planner.plan_qpos_to_qpos(goal_qposes, self.robot.get_qpos(), +82 use_point_cloud=True, use_attach=has_attach, time_step=1/250, fixed_joint_indices=range(2)) +83 return result +
a subroutine to plan a path without moving the base
+85 def move_in_two_stage(self, pose, has_attach=False): + 86 """ + 87 first, we do a full IK but only generate motions for the base + 88 then, do another partial IK for the arm and generate motions for the arm + 89 """ + 90 # do a full ik to the pose + 91 status, goal_qposes = self.planner.IK(pose, self.robot.get_qpos()) + 92 if status != "Success": + 93 print("IK failed") + 94 exit(1) + 95 # now fix arm joints and plan a path to the goal + 96 result = self.planner.plan_qpos_to_qpos(goal_qposes, self.robot.get_qpos(), + 97 use_point_cloud=True, use_attach=has_attach, time_step=1/250, fixed_joint_indices=range(2, 9)) + 98 # execute the planned path + 99 self.follow_path(result) +100 result = self.plan_without_base(pose, has_attach) +101 # execute the planned path +102 self.follow_path(result) +
first, we do a full IK but only generate motions for the base +then, do another partial IK for the arm and generate motions for the arm
+104 def demo(self): +105 """ +106 We reach the pick up and drop off poses in two stages, first by moving the base only and then the arm only +107 """ +108 pickup_pose = [0.7, 0, 0.12, 0, 1, 0, 0] +109 delivery_pose = [0.4, 0.3, 0.13, 0, 1, 0, 0] +110 +111 self.add_point_cloud() +112 # also add the target as a collision object so we don't hit it +113 fcl_red_cube = mplib.planner.fcl.Box([0.04, 0.04, 0.14]) +114 collision_object = mplib.planner.fcl.CollisionObject(fcl_red_cube, [0.7, 0, 0.07], [1, 0, 0, 0]) +115 self.planner.planning_world.set_normal_object("target", collision_object) +116 +117 # go above the target +118 pickup_pose[2] += 0.2 +119 self.move_in_two_stage(pickup_pose) +120 self.open_gripper() +121 # move down to pick +122 self.planner.planning_world.remove_normal_object("target") # remove the object so we don't report collision +123 pickup_pose[2] -= 0.12 +124 result = self.plan_without_base(pickup_pose) +125 self.follow_path(result) +126 self.close_gripper() +127 +128 # add the attached box to the planning world +129 self.planner.update_attached_box([0.04, 0.04, 0.12], [0, 0, 0.14, 1, 0, 0, 0]) +130 +131 pickup_pose[2] += 0.12 +132 result = self.plan_without_base(pickup_pose, has_attach=True) +133 self.follow_path(result) +134 delivery_pose[2] += 0.2 +135 # now go to the drop off in two stages +136 self.move_in_two_stage(delivery_pose, has_attach=True) +137 delivery_pose[2] -= 0.12 +138 result = self.plan_without_base(delivery_pose, has_attach=True) +139 self.follow_path(result) +140 self.open_gripper() +141 delivery_pose[2] += 0.12 +142 result = self.plan_without_base(delivery_pose) +143 self.follow_path(result) +
We reach the pick up and drop off poses in two stages, first by moving the base only and then the arm only
+1from typing import Sequence, Tuple, Union + 2 + 3import os + 4import numpy as np + 5from transforms3d.quaternions import quat2mat, mat2quat + 6import toppra as ta + 7import toppra.constraint as constraint + 8import toppra.algorithm as algo + 9 + 10from mplib.pymp import * + 11 + 12 + 13class Planner: + 14 """Motion planner.""" + 15 + 16 # TODO(jigu): default joint vel and acc limits + 17 # TODO(jigu): how does user link names and joint names are exactly used? + 18 + 19 def __init__( + 20 self, + 21 urdf: str, + 22 move_group: str, + 23 srdf: str = "", + 24 package_keyword_replacement: str = "", + 25 user_link_names: Sequence[str] = [], + 26 user_joint_names: Sequence[str] = [], + 27 joint_vel_limits: Union[Sequence[float], np.ndarray] = [], + 28 joint_acc_limits: Union[Sequence[float], np.ndarray] = [], + 29 **kwargs + 30 ): + 31 r"""Motion planner for robots. + 32 + 33 Args: + 34 urdf: Unified Robot Description Format file. + 35 user_link_names: names of links, the order. if empty, all links will be used. + 36 user_joint_names: names of the joints to plan. if empty, all active joints will be used. + 37 move_group: target link to move, usually the end-effector. + 38 joint_vel_limits: maximum joint velocities for time parameterization, + 39 which should have the same length as + 40 joint_acc_limits: maximum joint accelerations for time parameterization, + 41 which should have the same length as + 42 srdf: Semantic Robot Description Format file. + 43 References: + 44 http://docs.ros.org/en/kinetic/api/moveit_tutorials/html/doc/urdf_srdf/urdf_srdf_tutorial.html + 45 + 46 """ + 47 self.urdf = urdf + 48 if srdf == "" and os.path.exists(urdf.replace(".urdf", ".srdf")): + 49 self.srdf = urdf.replace(".urdf", ".srdf") + 50 print(f"No SRDF file provided but found {self.srdf}") + 51 + 52 # replace package:// keyword if exists + 53 urdf = self.replace_package_keyword(package_keyword_replacement) + 54 + 55 self.robot = articulation.ArticulatedModel( + 56 urdf, + 57 srdf, + 58 [0, 0, -9.81], + 59 user_joint_names, + 60 user_link_names, + 61 verbose=False, + 62 convex=True, + 63 ) + 64 self.pinocchio_model = self.robot.get_pinocchio_model() + 65 + 66 self.planning_world = planning_world.PlanningWorld( + 67 [self.robot], + 68 ["robot"], + 69 kwargs.get("normal_objects", []), + 70 kwargs.get("normal_object_names", []) + 71 ) + 72 + 73 if srdf == "": + 74 self.generate_collision_pair() + 75 self.robot.update_SRDF(self.srdf) + 76 + 77 self.pinocchio_model = self.robot.get_pinocchio_model() + 78 self.user_link_names = self.pinocchio_model.get_link_names() + 79 self.user_joint_names = self.pinocchio_model.get_joint_names() + 80 + 81 self.joint_name_2_idx = {} + 82 for i, joint in enumerate(self.user_joint_names): + 83 self.joint_name_2_idx[joint] = i + 84 self.link_name_2_idx = {} + 85 for i, link in enumerate(self.user_link_names): + 86 self.link_name_2_idx[link] = i + 87 + 88 assert move_group in self.user_link_names,\ + 89 f"end-effector not found as one of the links in {self.user_link_names}" + 90 self.move_group = move_group + 91 self.robot.set_move_group(self.move_group) + 92 self.move_group_joint_indices = self.robot.get_move_group_joint_indices() + 93 + 94 self.joint_types = self.pinocchio_model.get_joint_types() + 95 self.joint_limits = np.concatenate(self.pinocchio_model.get_joint_limits()) + 96 self.joint_vel_limits = joint_vel_limits if len(joint_vel_limits) else np.ones(len(self.move_group_joint_indices)) + 97 self.joint_acc_limits = joint_acc_limits if len(joint_acc_limits) else np.ones(len(self.move_group_joint_indices)) + 98 self.move_group_link_id = self.link_name_2_idx[self.move_group] + 99 assert len(self.joint_vel_limits) == len(self.joint_acc_limits),\ +100 f"length of joint_vel_limits ({len(self.joint_vel_limits)}) =/= "\ +101 f"length of joint_acc_limits ({len(self.joint_acc_limits)})" +102 assert len(self.joint_vel_limits) == len(self.move_group_joint_indices),\ +103 f"length of joint_vel_limits ({len(self.joint_vel_limits)}) =/= "\ +104 f"length of move_group ({len(self.move_group_joint_indices)})" +105 assert len(self.joint_vel_limits) <= len(self.joint_limits),\ +106 f"length of joint_vel_limits ({len(self.joint_vel_limits)}) > "\ +107 f"number of total joints ({len(self.joint_limits)})" +108 +109 self.planning_world = planning_world.PlanningWorld([self.robot], ["robot"], [], []) +110 self.planner = ompl.OMPLPlanner(world=self.planning_world) +111 +112 +113 def replace_package_keyword(self, package_keyword_replacement): +114 """ +115 some ROS URDF files use package:// keyword to refer the package dir +116 replace it with the given string (default is empty) +117 +118 Args: +119 package_keyword_replacement: the string to replace 'package://' keyword +120 """ +121 rtn_urdf = self.urdf +122 with open(self.urdf, "r") as in_f: +123 content = in_f.read() +124 if "package://" in content: +125 rtn_urdf = self.urdf.replace(".urdf", "_package_keyword_replaced.urdf") +126 content = content.replace("package://", package_keyword_replacement) +127 if not os.path.exists(rtn_urdf): +128 with open(rtn_urdf, "w") as out_f: +129 out_f.write(content) +130 return rtn_urdf +131 +132 def generate_collision_pair(self, sample_time = 1000000, echo_freq = 100000): +133 """ +134 we read the srdf file to get the link pairs that should not collide. +135 if not provided, we need to randomly sample configurations to find the link pairs that will always collide. +136 """ +137 print("Since no SRDF file is provided. We will first detect link pairs that will always collide. This may take several minutes.") +138 n_link = len(self.user_link_names) +139 cnt = np.zeros((n_link, n_link), dtype=np.int32) +140 for i in range(sample_time): +141 qpos = self.pinocchio_model.get_random_configuration() +142 self.robot.set_qpos(qpos, True) +143 collisions = self.planning_world.collide_full() +144 for collision in collisions: +145 u = self.link_name_2_idx[collision.link_name1] +146 v = self.link_name_2_idx[collision.link_name2] +147 cnt[u][v] += 1 +148 if i % echo_freq == 0: +149 print("Finish %.1f%%!" % (i * 100 / sample_time)) +150 +151 import xml.etree.ElementTree as ET +152 from xml.dom import minidom +153 +154 root = ET.Element('robot') +155 robot_name = self.urdf.split('/')[-1].split('.')[0] +156 root.set('name', robot_name) +157 self.srdf = self.urdf.replace(".urdf", ".srdf") +158 +159 for i in range(n_link): +160 for j in range(n_link): +161 if cnt[i][j] == sample_time: +162 link1 = self.user_link_names[i] +163 link2 = self.user_link_names[j] +164 print("Ignore collision pair: (%s, %s), reason: always collide" % (link1, link2)) +165 collision = ET.SubElement(root, 'disable_collisions') +166 collision.set('link1', link1) +167 collision.set('link2', link2) +168 collision.set('reason', 'Default') +169 srdffile = open(self.srdf, "w") +170 srdffile.write(minidom.parseString(ET.tostring(root)).toprettyxml(indent=" ")) +171 srdffile.close() +172 print("Saving the SRDF file to %s" % self.srdf) +173 +174 def distance_6D(self, p1, q1, p2, q2): +175 """ +176 compute the distance between two poses +177 +178 Args: +179 p1: position of pose 1 +180 q1: quaternion of pose 1 +181 p2: position of pose 2 +182 q2: quaternion of pose 2 +183 """ +184 return np.linalg.norm(p1 - p2) + min( +185 np.linalg.norm(q1 - q2), np.linalg.norm(q1 + q2) +186 ) +187 +188 def check_joint_limit(self, q): +189 """ +190 check if the joint configuration is within the joint limits +191 +192 Args: +193 q: joint configuration +194 +195 Returns: +196 True if the joint configuration is within the joint limits +197 """ +198 n = len(q) +199 flag = True +200 for i in range(n): +201 if self.joint_types[i].startswith("JointModelR"): +202 if np.abs(q[i] - self.joint_limits[i][0]) < 1e-3: +203 continue +204 q[i] -= ( +205 2 +206 * np.pi +207 * np.floor((q[i] - self.joint_limits[i][0]) / (2 * np.pi)) +208 ) +209 if q[i] > self.joint_limits[i][1] + 1e-3: +210 flag = False +211 else: +212 if ( +213 q[i] < self.joint_limits[i][0] - 1e-3 +214 or q[i] > self.joint_limits[i][1] + 1e-3 +215 ): +216 flag = False +217 return flag +218 +219 def pad_qpos(self, qpos, articulation=None): +220 """ +221 if the user does not provide the full qpos but only the move_group joints, +222 pad the qpos with the rest of the joints +223 """ +224 if len(qpos) == len(self.move_group_joint_indices): +225 tmp = articulation.get_qpos() if articulation is not None else self.robot.get_qpos() +226 tmp[:len(qpos)] = qpos +227 qpos = tmp +228 +229 assert len(qpos) == len(self.joint_limits),\ +230 f"length of qpos ({len(qpos)}) =/= "\ +231 f"number of total joints ({len(self.joint_limits)})" +232 +233 return qpos +234 +235 def check_for_collision(self, collision_function, articulation: articulation.ArticulatedModel=None, qpos: np.ndarray=None) -> list: +236 """ helper function to check for collision """ +237 # handle no user input +238 if articulation is None: +239 articulation = self.robot +240 if qpos is None: +241 qpos = articulation.get_qpos() +242 qpos = self.pad_qpos(qpos, articulation) +243 +244 # first save the current qpos +245 old_qpos = articulation.get_qpos() +246 # set robot to new qpos +247 articulation.set_qpos(qpos, True) +248 # find the index of the articulation inside the array +249 idx = self.planning_world.get_articulations().index(articulation) +250 # check for self-collision +251 collisions = collision_function(idx) +252 # reset qpos +253 articulation.set_qpos(old_qpos, True) +254 return collisions +255 +256 def check_for_self_collision(self, articulation: articulation.ArticulatedModel=None, qpos: np.ndarray=None) -> list: +257 """Check if the robot is in self-collision. +258 +259 Args: +260 articulation: robot model. if none will be self.robot +261 qpos: robot configuration. if none will be the current pose +262 +263 Returns: +264 A list of collisions. +265 """ +266 return self.check_for_collision(self.planning_world.self_collide, articulation, qpos) +267 +268 def check_for_env_collision(self, articulation: articulation.ArticulatedModel=None, qpos: np.ndarray=None, with_point_cloud=False, use_attach=False) -> list: +269 """Check if the robot is in collision with the environment +270 +271 Args: +272 articulation: robot model. if none will be self.robot +273 qpos: robot configuration. if none will be the current pose +274 with_point_cloud: whether to check collision against point cloud +275 use_attach: whether to include the object attached to the end effector in collision checking +276 Returns: +277 A list of collisions. +278 """ +279 # store previous results +280 prev_use_point_cloud = self.planning_world.use_point_cloud +281 prev_use_attach = self.planning_world.use_attach +282 self.planning_world.set_use_point_cloud(with_point_cloud) +283 self.planning_world.set_use_attach(use_attach) +284 +285 results = self.check_for_collision(self.planning_world.collide_with_others, articulation, qpos) +286 +287 # restore +288 self.planning_world.set_use_point_cloud(prev_use_point_cloud) +289 self.planning_world.set_use_attach(prev_use_attach) +290 return results +291 +292 def IK(self, goal_pose, start_qpos, mask = [], n_init_qpos=20, threshold=1e-3): +293 """ +294 Inverse kinematics +295 +296 Args: +297 goal_pose: [x,y,z,qw,qx,qy,qz] pose of the goal +298 start_qpos: initial configuration +299 mask: if the value at a given index is True, the joint is *not* used in the IK +300 n_init_qpos: number of random initial configurations +301 threshold: threshold for the distance between the goal pose and the result pose +302 """ +303 +304 index = self.link_name_2_idx[self.move_group] +305 min_dis = 1e9 +306 idx = self.move_group_joint_indices +307 qpos0 = np.copy(start_qpos) +308 results = [] +309 self.robot.set_qpos(start_qpos, True) +310 for i in range(n_init_qpos): +311 ik_results = self.pinocchio_model.compute_IK_CLIK( +312 index, goal_pose, start_qpos, mask +313 ) +314 flag = self.check_joint_limit(ik_results[0]) # will clip qpos +315 +316 # check collision +317 self.planning_world.set_qpos_all(ik_results[0][idx]) +318 if (len(self.planning_world.collide_full()) != 0): +319 flag = False +320 +321 if flag: +322 self.pinocchio_model.compute_forward_kinematics(ik_results[0]) +323 new_pose = self.pinocchio_model.get_link_pose(index) +324 tmp_dis = self.distance_6D( +325 goal_pose[:3], goal_pose[3:], new_pose[:3], new_pose[3:] +326 ) +327 if tmp_dis < min_dis: +328 min_dis = tmp_dis +329 if tmp_dis < threshold: +330 result = ik_results[0] +331 unique = True +332 for j in range(len(results)): +333 if np.linalg.norm(results[j][idx] - result[idx]) < 0.1: +334 unique = False +335 if unique: +336 results.append(result) +337 start_qpos = self.pinocchio_model.get_random_configuration() +338 mask_len = len(mask) +339 if mask_len > 0: +340 for j in range(mask_len): +341 if mask[j]: +342 start_qpos[j] = qpos0[j] +343 if len(results) != 0: +344 status = "Success" +345 elif min_dis != 1e9: +346 status = ( +347 "IK Failed! Distance %lf is greater than threshold %lf." +348 % (min_dis, threshold) +349 ) +350 else: +351 status = "IK Failed! Cannot find valid solution." +352 return status, results +353 +354 def TOPP(self, path, step=0.1, verbose=False): +355 """ +356 Time-Optimal Path Parameterization +357 +358 Args: +359 path: numpy array of shape (n, dof) +360 step: step size for the discretization +361 verbose: if True, will print the log of TOPPRA +362 """ +363 +364 N_samples = path.shape[0] +365 dof = path.shape[1] +366 assert dof == len(self.joint_vel_limits) +367 assert dof == len(self.joint_acc_limits) +368 ss = np.linspace(0, 1, N_samples) +369 path = ta.SplineInterpolator(ss, path) +370 pc_vel = constraint.JointVelocityConstraint(self.joint_vel_limits) +371 pc_acc = constraint.JointAccelerationConstraint(self.joint_acc_limits) +372 instance = algo.TOPPRA( +373 [pc_vel, pc_acc], path, parametrizer="ParametrizeConstAccel" +374 ) +375 jnt_traj = instance.compute_trajectory() +376 ts_sample = np.linspace( +377 0, jnt_traj.duration, int(jnt_traj.duration / step) +378 ) +379 qs_sample = jnt_traj(ts_sample) +380 qds_sample = jnt_traj(ts_sample, 1) +381 qdds_sample = jnt_traj(ts_sample, 2) +382 return ts_sample, qs_sample, qds_sample, qdds_sample, jnt_traj.duration +383 +384 def update_point_cloud(self, pc, radius=1e-3): +385 """ +386 Args: +387 pc: numpy array of shape (n, 3) +388 radius: radius of each point. This gives a buffer around each point that planner will avoid +389 """ +390 self.planning_world.update_point_cloud(pc, radius) +391 +392 def update_attached_tool(self, fcl_collision_geometry, pose, link_id=-1): +393 """ helper function to update the attached tool """ +394 if link_id == -1: +395 link_id = self.move_group_link_id +396 self.planning_world.update_attached_tool(fcl_collision_geometry, link_id, pose) +397 +398 def update_attached_sphere(self, radius, pose, link_id=-1): +399 """ +400 attach a sphere to some link +401 +402 Args: +403 radius: radius of the sphere +404 pose: [x,y,z,qw,qx,qy,qz] pose of the sphere +405 link_id: if not provided, the end effector will be the target. +406 """ +407 if link_id == -1: +408 link_id = self.move_group_link_id +409 self.planning_world.update_attached_sphere(radius, link_id, pose) +410 +411 def update_attached_box(self, size, pose, link_id=-1): +412 """ +413 attach a box to some link +414 +415 Args: +416 size: [x,y,z] size of the box +417 pose: [x,y,z,qw,qx,qy,qz] pose of the box +418 link_id: if not provided, the end effector will be the target. +419 """ +420 if link_id == -1: +421 link_id = self.move_group_link_id +422 self.planning_world.update_attached_box(size, link_id, pose) +423 +424 def update_attached_mesh(self, mesh_path, pose, link_id=-1): +425 """ +426 attach a mesh to some link +427 +428 Args: +429 mesh_path: path to the mesh +430 pose: [x,y,z,qw,qx,qy,qz] pose of the mesh +431 link_id: if not provided, the end effector will be the target. +432 """ +433 if link_id == -1: +434 link_id = self.move_group_link_id +435 self.planning_world.update_attached_mesh(mesh_path, link_id, pose) +436 +437 def set_base_pose(self, pose): +438 """ +439 tell the planner where the base of the robot is w.r.t the world frame +440 +441 Args: +442 pose: [x,y,z,qw,qx,qy,qz] pose of the base +443 """ +444 self.robot.set_base_pose(pose) +445 +446 def set_normal_object(self, name, collision_object): +447 """ adds or updates a non-articulated collision object in the scene """ +448 self.planning_world.set_normal_object(name, collision_object) +449 +450 def remove_normal_object(self, name): +451 """ returns true if the object was removed, false if it was not found """ +452 return self.planning_world.remove_normal_object(name) +453 +454 def plan_qpos_to_qpos( +455 self, +456 goal_qposes: list, +457 current_qpos, +458 time_step=0.1, +459 rrt_range=0.1, +460 planning_time=1, +461 fix_joint_limits=True, +462 use_point_cloud=False, +463 use_attach=False, +464 verbose=False, +465 planner_name="RRTConnect", +466 no_simplification=False, +467 constraint_function=None, +468 constraint_jacobian=None, +469 constraint_tolerance=1e-3, +470 fixed_joint_indices=[] +471 ): +472 """ +473 plan a path from a specified joint position to a goal pose +474 +475 Args: +476 goal_pose: 7D pose of the end-effector [x,y,z,qw,qx,qy,qz] +477 current_qpos: current joint configuration (either full or move_group joints) +478 mask: mask for IK. When set, the IK will leave certain joints out of planning +479 time_step: time step for TOPP +480 rrt_range: step size for RRT +481 planning_time: time limit for RRT +482 fix_joint_limits: if True, will clip the joint configuration to be within the joint limits +483 use_point_cloud: if True, will use the point cloud to avoid collision +484 use_attach: if True, will consider the attached tool collision when planning +485 verbose: if True, will print the log of OMPL and TOPPRA +486 planner_name: planner name pick from {"RRTConnect", "RRT*"} +487 fixed_joint_indices: list of indices of joints that are fixed during planning +488 constraint_function: evals to 0 when constraint is satisfied +489 constraint_jacobian: jacobian of constraint_function +490 constraint_tolerance: tolerance for constraint_function +491 no_simplification: if true, will not simplify the path. constraint planning does not support simplification +492 """ +493 self.planning_world.set_use_point_cloud(use_point_cloud) +494 self.planning_world.set_use_attach(use_attach) +495 n = current_qpos.shape[0] +496 if fix_joint_limits: +497 for i in range(n): +498 if current_qpos[i] < self.joint_limits[i][0]: +499 current_qpos[i] = self.joint_limits[i][0] + 1e-3 +500 if current_qpos[i] > self.joint_limits[i][1]: +501 current_qpos[i] = self.joint_limits[i][1] - 1e-3 +502 +503 current_qpos = self.pad_qpos(current_qpos) +504 +505 self.robot.set_qpos(current_qpos, True) +506 collisions = self.planning_world.collide_full() +507 if len(collisions) != 0: +508 print("Invalid start state!") +509 for collision in collisions: +510 print("%s and %s collide!" % (collision.link_name1, collision.link_name2)) +511 +512 idx = self.move_group_joint_indices +513 +514 goal_qpos_ = [] +515 for i in range(len(goal_qposes)): +516 goal_qpos_.append(goal_qposes[i][idx]) +517 +518 fixed_joints = set() +519 for joint_idx in fixed_joint_indices: +520 fixed_joints.add(ompl.FixedJoint(0, joint_idx, current_qpos[joint_idx])) +521 +522 assert len(current_qpos[idx]) == len(goal_qpos_[0]) +523 status, path = self.planner.plan( +524 current_qpos[idx], +525 goal_qpos_, +526 range=rrt_range, +527 time=planning_time, +528 fixed_joints=fixed_joints, +529 verbose=verbose, +530 planner_name=planner_name, +531 no_simplification=no_simplification, +532 constraint_function=constraint_function, +533 constraint_jacobian=constraint_jacobian, +534 constraint_tolerance=constraint_tolerance +535 ) +536 +537 if status == "Exact solution": +538 if verbose: ta.setup_logging("INFO") +539 else: ta.setup_logging("WARNING") +540 times, pos, vel, acc, duration = self.TOPP(path, time_step) +541 return { +542 "status": "Success", +543 "time": times, +544 "position": pos, +545 "velocity": vel, +546 "acceleration": acc, +547 "duration": duration, +548 } +549 else: +550 return {"status": "RRT Failed. %s" % status} +551 +552 def plan_qpos_to_pose( +553 self, +554 goal_pose, +555 current_qpos, +556 mask = [], +557 time_step=0.1, +558 rrt_range=0.1, +559 planning_time=1, +560 fix_joint_limits=True, +561 use_point_cloud=False, +562 use_attach=False, +563 verbose=False, +564 wrt_world=False, +565 planner_name="RRTConnect", +566 no_simplification=False, +567 constraint_function=None, +568 constraint_jacobian=None, +569 constraint_tolerance=1e-3 +570 ): +571 """ +572 plan from a start configuration to a goal pose of the end-effector +573 +574 Args: +575 goal_pose: [x,y,z,qw,qx,qy,qz] pose of the goal +576 current_qpos: current joint configuration (either full or move_group joints) +577 mask: if the value at a given index is True, the joint is *not* used in the IK +578 time_step: time step for TOPPRA (time parameterization of path) +579 rrt_range: step size for RRT +580 planning_time: time limit for RRT +581 fix_joint_limits: if True, will clip the joint configuration to be within the joint limits +582 use_point_cloud: if True, will use the point cloud to avoid collision +583 use_attach: if True, will consider the attached tool collision when planning +584 verbose: if True, will print the log of OMPL and TOPPRA +585 wrt_world: if true, interpret the target pose with respect to the world frame instead of the base frame +586 """ +587 n = current_qpos.shape[0] +588 if fix_joint_limits: +589 for i in range(n): +590 if current_qpos[i] < self.joint_limits[i][0]: +591 current_qpos[i] = self.joint_limits[i][0] + 1e-3 +592 if current_qpos[i] > self.joint_limits[i][1]: +593 current_qpos[i] = self.joint_limits[i][1] - 1e-3 +594 +595 if wrt_world: +596 base_pose = self.robot.get_base_pose() +597 base_tf = np.eye(4) +598 base_tf[0:3, 3] = base_pose[:3] +599 base_tf[0:3, 0:3] = quat2mat(base_pose[3:]) +600 goal_tf = np.eye(4) +601 goal_tf[0:3, 3] = goal_pose[:3] +602 goal_tf[0:3, 0:3] = quat2mat(goal_pose[3:]) +603 goal_tf = np.linalg.inv(base_tf).dot(goal_tf) +604 goal_pose[:3] = goal_tf[0:3, 3] +605 goal_pose[3:] = mat2quat(goal_tf[0:3, 0:3]) +606 +607 idx = self.move_group_joint_indices # we need to take only the move_group joints when planning +608 +609 ik_status, goal_qpos = self.IK(goal_pose, current_qpos, mask) +610 if ik_status != "Success": +611 return {"status": ik_status} +612 +613 if verbose: +614 print("IK results:") +615 for i in range(len(goal_qpos)): +616 print(goal_qpos[i]) +617 +618 goal_qpos_ = [] +619 for i in range(len(goal_qpos)): +620 goal_qpos_.append(goal_qpos[i][idx]) +621 self.robot.set_qpos(current_qpos, True) +622 +623 ik_status, goal_qpos = self.IK(goal_pose, current_qpos, mask) +624 if ik_status != "Success": +625 return {"status": ik_status} +626 +627 if verbose: +628 print("IK results:") +629 for i in range(len(goal_qpos)): +630 print(goal_qpos[i]) +631 +632 return self.plan_qpos_to_qpos( +633 goal_qpos, +634 current_qpos, +635 time_step, +636 rrt_range, +637 planning_time, +638 fix_joint_limits, +639 use_point_cloud, +640 use_attach, +641 verbose, +642 planner_name, +643 no_simplification, +644 constraint_function, +645 constraint_jacobian, +646 constraint_tolerance +647 ) +648 +649 def plan_screw( +650 self, +651 target_pose, +652 qpos, +653 qpos_step=0.1, +654 time_step=0.1, +655 use_point_cloud=False, +656 use_attach=False, +657 verbose=False, +658 ): +659 """ +660 plan from a start configuration to a goal pose of the end-effector using screw motion +661 +662 Args: +663 target_pose: [x,y,z,qw,qx,qy,qz] pose of the goal +664 qpos: current joint configuration (either full or move_group joints) +665 qpos_step: size of the random step for RRT +666 time_step: time step for the discretization +667 use_point_cloud: if True, will use the point cloud to avoid collision +668 use_attach: if True, will use the attached tool to avoid collision +669 verbose: if True, will print the log of TOPPRA +670 """ +671 self.planning_world.set_use_point_cloud(use_point_cloud) +672 self.planning_world.set_use_attach(use_attach) +673 qpos = self.pad_qpos(qpos.copy()) +674 self.robot.set_qpos(qpos, True) +675 +676 def pose7D2mat(pose): +677 mat = np.eye(4) +678 mat[0:3, 3] = pose[:3] +679 mat[0:3, 0:3] = quat2mat(pose[3:]) +680 return mat +681 +682 def skew(vec): +683 return np.array( +684 [ +685 [0, -vec[2], vec[1]], +686 [vec[2], 0, -vec[0]], +687 [-vec[1], vec[0], 0], +688 ] +689 ) +690 +691 def pose2exp_coordinate(pose: np.ndarray) -> Tuple[np.ndarray, float]: +692 def rot2so3(rotation: np.ndarray): +693 assert rotation.shape == (3, 3) +694 if np.isclose(rotation.trace(), 3): +695 return np.zeros(3), 1 +696 if np.isclose(rotation.trace(), -1): +697 return np.zeros(3), -1e6 +698 theta = np.arccos((rotation.trace() - 1) / 2) +699 omega = ( +700 1 +701 / 2 +702 / np.sin(theta) +703 * np.array( +704 [ +705 rotation[2, 1] - rotation[1, 2], +706 rotation[0, 2] - rotation[2, 0], +707 rotation[1, 0] - rotation[0, 1], +708 ] +709 ).T +710 ) +711 return omega, theta +712 +713 omega, theta = rot2so3(pose[:3, :3]) +714 if theta < -1e5: +715 return omega, theta +716 ss = skew(omega) +717 inv_left_jacobian = ( +718 np.eye(3) / theta +719 - 0.5 * ss +720 + (1.0 / theta - 0.5 / np.tan(theta / 2)) * ss @ ss +721 ) +722 v = inv_left_jacobian @ pose[:3, 3] +723 return np.concatenate([v, omega]), theta +724 +725 self.pinocchio_model.compute_forward_kinematics(qpos) +726 ee_index = self.link_name_2_idx[self.move_group] +727 current_p = pose7D2mat(self.pinocchio_model.get_link_pose(ee_index)) +728 target_p = pose7D2mat(target_pose) +729 relative_transform = target_p @ np.linalg.inv(current_p) +730 +731 omega, theta = pose2exp_coordinate(relative_transform) +732 +733 if theta < -1e4: +734 return {"status": "screw plan failed."} +735 omega = omega.reshape((-1, 1)) * theta +736 +737 index = self.move_group_joint_indices +738 path = [np.copy(qpos[index])] +739 +740 while True: +741 self.pinocchio_model.compute_full_jacobian(qpos) +742 J = self.pinocchio_model.get_link_jacobian(ee_index, local=False) +743 delta_q = np.linalg.pinv(J) @ omega +744 delta_q *= qpos_step / (np.linalg.norm(delta_q)) +745 delta_twist = J @ delta_q +746 +747 flag = False +748 if np.linalg.norm(delta_twist) > np.linalg.norm(omega): +749 ratio = np.linalg.norm(omega) / np.linalg.norm(delta_twist) +750 delta_q = delta_q * ratio +751 delta_twist = delta_twist * ratio +752 flag = True +753 +754 qpos += delta_q.reshape(-1) +755 omega -= delta_twist +756 +757 def check_joint_limit(q): +758 n = len(q) +759 for i in range(n): +760 if ( +761 q[i] < self.joint_limits[i][0] - 1e-3 +762 or q[i] > self.joint_limits[i][1] + 1e-3 +763 ): +764 return False +765 return True +766 +767 within_joint_limit = check_joint_limit(qpos) +768 self.planning_world.set_qpos_all(qpos[index]) +769 collide = self.planning_world.collide() +770 +771 if ( +772 np.linalg.norm(delta_twist) < 1e-4 +773 or collide +774 or within_joint_limit == False +775 ): +776 return {"status": "screw plan failed"} +777 +778 path.append(np.copy(qpos[index])) +779 +780 if flag: +781 if verbose: +782 ta.setup_logging("INFO") +783 else: +784 ta.setup_logging("WARNING") +785 times, pos, vel, acc, duration = self.TOPP( +786 np.vstack(path), time_step +787 ) +788 return { +789 "status": "Success", +790 "time": times, +791 "position": pos, +792 "velocity": vel, +793 "acceleration": acc, +794 "duration": duration, +795 } +
14class Planner: + 15 """Motion planner.""" + 16 + 17 # TODO(jigu): default joint vel and acc limits + 18 # TODO(jigu): how does user link names and joint names are exactly used? + 19 + 20 def __init__( + 21 self, + 22 urdf: str, + 23 move_group: str, + 24 srdf: str = "", + 25 package_keyword_replacement: str = "", + 26 user_link_names: Sequence[str] = [], + 27 user_joint_names: Sequence[str] = [], + 28 joint_vel_limits: Union[Sequence[float], np.ndarray] = [], + 29 joint_acc_limits: Union[Sequence[float], np.ndarray] = [], + 30 **kwargs + 31 ): + 32 r"""Motion planner for robots. + 33 + 34 Args: + 35 urdf: Unified Robot Description Format file. + 36 user_link_names: names of links, the order. if empty, all links will be used. + 37 user_joint_names: names of the joints to plan. if empty, all active joints will be used. + 38 move_group: target link to move, usually the end-effector. + 39 joint_vel_limits: maximum joint velocities for time parameterization, + 40 which should have the same length as + 41 joint_acc_limits: maximum joint accelerations for time parameterization, + 42 which should have the same length as + 43 srdf: Semantic Robot Description Format file. + 44 References: + 45 http://docs.ros.org/en/kinetic/api/moveit_tutorials/html/doc/urdf_srdf/urdf_srdf_tutorial.html + 46 + 47 """ + 48 self.urdf = urdf + 49 if srdf == "" and os.path.exists(urdf.replace(".urdf", ".srdf")): + 50 self.srdf = urdf.replace(".urdf", ".srdf") + 51 print(f"No SRDF file provided but found {self.srdf}") + 52 + 53 # replace package:// keyword if exists + 54 urdf = self.replace_package_keyword(package_keyword_replacement) + 55 + 56 self.robot = articulation.ArticulatedModel( + 57 urdf, + 58 srdf, + 59 [0, 0, -9.81], + 60 user_joint_names, + 61 user_link_names, + 62 verbose=False, + 63 convex=True, + 64 ) + 65 self.pinocchio_model = self.robot.get_pinocchio_model() + 66 + 67 self.planning_world = planning_world.PlanningWorld( + 68 [self.robot], + 69 ["robot"], + 70 kwargs.get("normal_objects", []), + 71 kwargs.get("normal_object_names", []) + 72 ) + 73 + 74 if srdf == "": + 75 self.generate_collision_pair() + 76 self.robot.update_SRDF(self.srdf) + 77 + 78 self.pinocchio_model = self.robot.get_pinocchio_model() + 79 self.user_link_names = self.pinocchio_model.get_link_names() + 80 self.user_joint_names = self.pinocchio_model.get_joint_names() + 81 + 82 self.joint_name_2_idx = {} + 83 for i, joint in enumerate(self.user_joint_names): + 84 self.joint_name_2_idx[joint] = i + 85 self.link_name_2_idx = {} + 86 for i, link in enumerate(self.user_link_names): + 87 self.link_name_2_idx[link] = i + 88 + 89 assert move_group in self.user_link_names,\ + 90 f"end-effector not found as one of the links in {self.user_link_names}" + 91 self.move_group = move_group + 92 self.robot.set_move_group(self.move_group) + 93 self.move_group_joint_indices = self.robot.get_move_group_joint_indices() + 94 + 95 self.joint_types = self.pinocchio_model.get_joint_types() + 96 self.joint_limits = np.concatenate(self.pinocchio_model.get_joint_limits()) + 97 self.joint_vel_limits = joint_vel_limits if len(joint_vel_limits) else np.ones(len(self.move_group_joint_indices)) + 98 self.joint_acc_limits = joint_acc_limits if len(joint_acc_limits) else np.ones(len(self.move_group_joint_indices)) + 99 self.move_group_link_id = self.link_name_2_idx[self.move_group] +100 assert len(self.joint_vel_limits) == len(self.joint_acc_limits),\ +101 f"length of joint_vel_limits ({len(self.joint_vel_limits)}) =/= "\ +102 f"length of joint_acc_limits ({len(self.joint_acc_limits)})" +103 assert len(self.joint_vel_limits) == len(self.move_group_joint_indices),\ +104 f"length of joint_vel_limits ({len(self.joint_vel_limits)}) =/= "\ +105 f"length of move_group ({len(self.move_group_joint_indices)})" +106 assert len(self.joint_vel_limits) <= len(self.joint_limits),\ +107 f"length of joint_vel_limits ({len(self.joint_vel_limits)}) > "\ +108 f"number of total joints ({len(self.joint_limits)})" +109 +110 self.planning_world = planning_world.PlanningWorld([self.robot], ["robot"], [], []) +111 self.planner = ompl.OMPLPlanner(world=self.planning_world) +112 +113 +114 def replace_package_keyword(self, package_keyword_replacement): +115 """ +116 some ROS URDF files use package:// keyword to refer the package dir +117 replace it with the given string (default is empty) +118 +119 Args: +120 package_keyword_replacement: the string to replace 'package://' keyword +121 """ +122 rtn_urdf = self.urdf +123 with open(self.urdf, "r") as in_f: +124 content = in_f.read() +125 if "package://" in content: +126 rtn_urdf = self.urdf.replace(".urdf", "_package_keyword_replaced.urdf") +127 content = content.replace("package://", package_keyword_replacement) +128 if not os.path.exists(rtn_urdf): +129 with open(rtn_urdf, "w") as out_f: +130 out_f.write(content) +131 return rtn_urdf +132 +133 def generate_collision_pair(self, sample_time = 1000000, echo_freq = 100000): +134 """ +135 we read the srdf file to get the link pairs that should not collide. +136 if not provided, we need to randomly sample configurations to find the link pairs that will always collide. +137 """ +138 print("Since no SRDF file is provided. We will first detect link pairs that will always collide. This may take several minutes.") +139 n_link = len(self.user_link_names) +140 cnt = np.zeros((n_link, n_link), dtype=np.int32) +141 for i in range(sample_time): +142 qpos = self.pinocchio_model.get_random_configuration() +143 self.robot.set_qpos(qpos, True) +144 collisions = self.planning_world.collide_full() +145 for collision in collisions: +146 u = self.link_name_2_idx[collision.link_name1] +147 v = self.link_name_2_idx[collision.link_name2] +148 cnt[u][v] += 1 +149 if i % echo_freq == 0: +150 print("Finish %.1f%%!" % (i * 100 / sample_time)) +151 +152 import xml.etree.ElementTree as ET +153 from xml.dom import minidom +154 +155 root = ET.Element('robot') +156 robot_name = self.urdf.split('/')[-1].split('.')[0] +157 root.set('name', robot_name) +158 self.srdf = self.urdf.replace(".urdf", ".srdf") +159 +160 for i in range(n_link): +161 for j in range(n_link): +162 if cnt[i][j] == sample_time: +163 link1 = self.user_link_names[i] +164 link2 = self.user_link_names[j] +165 print("Ignore collision pair: (%s, %s), reason: always collide" % (link1, link2)) +166 collision = ET.SubElement(root, 'disable_collisions') +167 collision.set('link1', link1) +168 collision.set('link2', link2) +169 collision.set('reason', 'Default') +170 srdffile = open(self.srdf, "w") +171 srdffile.write(minidom.parseString(ET.tostring(root)).toprettyxml(indent=" ")) +172 srdffile.close() +173 print("Saving the SRDF file to %s" % self.srdf) +174 +175 def distance_6D(self, p1, q1, p2, q2): +176 """ +177 compute the distance between two poses +178 +179 Args: +180 p1: position of pose 1 +181 q1: quaternion of pose 1 +182 p2: position of pose 2 +183 q2: quaternion of pose 2 +184 """ +185 return np.linalg.norm(p1 - p2) + min( +186 np.linalg.norm(q1 - q2), np.linalg.norm(q1 + q2) +187 ) +188 +189 def check_joint_limit(self, q): +190 """ +191 check if the joint configuration is within the joint limits +192 +193 Args: +194 q: joint configuration +195 +196 Returns: +197 True if the joint configuration is within the joint limits +198 """ +199 n = len(q) +200 flag = True +201 for i in range(n): +202 if self.joint_types[i].startswith("JointModelR"): +203 if np.abs(q[i] - self.joint_limits[i][0]) < 1e-3: +204 continue +205 q[i] -= ( +206 2 +207 * np.pi +208 * np.floor((q[i] - self.joint_limits[i][0]) / (2 * np.pi)) +209 ) +210 if q[i] > self.joint_limits[i][1] + 1e-3: +211 flag = False +212 else: +213 if ( +214 q[i] < self.joint_limits[i][0] - 1e-3 +215 or q[i] > self.joint_limits[i][1] + 1e-3 +216 ): +217 flag = False +218 return flag +219 +220 def pad_qpos(self, qpos, articulation=None): +221 """ +222 if the user does not provide the full qpos but only the move_group joints, +223 pad the qpos with the rest of the joints +224 """ +225 if len(qpos) == len(self.move_group_joint_indices): +226 tmp = articulation.get_qpos() if articulation is not None else self.robot.get_qpos() +227 tmp[:len(qpos)] = qpos +228 qpos = tmp +229 +230 assert len(qpos) == len(self.joint_limits),\ +231 f"length of qpos ({len(qpos)}) =/= "\ +232 f"number of total joints ({len(self.joint_limits)})" +233 +234 return qpos +235 +236 def check_for_collision(self, collision_function, articulation: articulation.ArticulatedModel=None, qpos: np.ndarray=None) -> list: +237 """ helper function to check for collision """ +238 # handle no user input +239 if articulation is None: +240 articulation = self.robot +241 if qpos is None: +242 qpos = articulation.get_qpos() +243 qpos = self.pad_qpos(qpos, articulation) +244 +245 # first save the current qpos +246 old_qpos = articulation.get_qpos() +247 # set robot to new qpos +248 articulation.set_qpos(qpos, True) +249 # find the index of the articulation inside the array +250 idx = self.planning_world.get_articulations().index(articulation) +251 # check for self-collision +252 collisions = collision_function(idx) +253 # reset qpos +254 articulation.set_qpos(old_qpos, True) +255 return collisions +256 +257 def check_for_self_collision(self, articulation: articulation.ArticulatedModel=None, qpos: np.ndarray=None) -> list: +258 """Check if the robot is in self-collision. +259 +260 Args: +261 articulation: robot model. if none will be self.robot +262 qpos: robot configuration. if none will be the current pose +263 +264 Returns: +265 A list of collisions. +266 """ +267 return self.check_for_collision(self.planning_world.self_collide, articulation, qpos) +268 +269 def check_for_env_collision(self, articulation: articulation.ArticulatedModel=None, qpos: np.ndarray=None, with_point_cloud=False, use_attach=False) -> list: +270 """Check if the robot is in collision with the environment +271 +272 Args: +273 articulation: robot model. if none will be self.robot +274 qpos: robot configuration. if none will be the current pose +275 with_point_cloud: whether to check collision against point cloud +276 use_attach: whether to include the object attached to the end effector in collision checking +277 Returns: +278 A list of collisions. +279 """ +280 # store previous results +281 prev_use_point_cloud = self.planning_world.use_point_cloud +282 prev_use_attach = self.planning_world.use_attach +283 self.planning_world.set_use_point_cloud(with_point_cloud) +284 self.planning_world.set_use_attach(use_attach) +285 +286 results = self.check_for_collision(self.planning_world.collide_with_others, articulation, qpos) +287 +288 # restore +289 self.planning_world.set_use_point_cloud(prev_use_point_cloud) +290 self.planning_world.set_use_attach(prev_use_attach) +291 return results +292 +293 def IK(self, goal_pose, start_qpos, mask = [], n_init_qpos=20, threshold=1e-3): +294 """ +295 Inverse kinematics +296 +297 Args: +298 goal_pose: [x,y,z,qw,qx,qy,qz] pose of the goal +299 start_qpos: initial configuration +300 mask: if the value at a given index is True, the joint is *not* used in the IK +301 n_init_qpos: number of random initial configurations +302 threshold: threshold for the distance between the goal pose and the result pose +303 """ +304 +305 index = self.link_name_2_idx[self.move_group] +306 min_dis = 1e9 +307 idx = self.move_group_joint_indices +308 qpos0 = np.copy(start_qpos) +309 results = [] +310 self.robot.set_qpos(start_qpos, True) +311 for i in range(n_init_qpos): +312 ik_results = self.pinocchio_model.compute_IK_CLIK( +313 index, goal_pose, start_qpos, mask +314 ) +315 flag = self.check_joint_limit(ik_results[0]) # will clip qpos +316 +317 # check collision +318 self.planning_world.set_qpos_all(ik_results[0][idx]) +319 if (len(self.planning_world.collide_full()) != 0): +320 flag = False +321 +322 if flag: +323 self.pinocchio_model.compute_forward_kinematics(ik_results[0]) +324 new_pose = self.pinocchio_model.get_link_pose(index) +325 tmp_dis = self.distance_6D( +326 goal_pose[:3], goal_pose[3:], new_pose[:3], new_pose[3:] +327 ) +328 if tmp_dis < min_dis: +329 min_dis = tmp_dis +330 if tmp_dis < threshold: +331 result = ik_results[0] +332 unique = True +333 for j in range(len(results)): +334 if np.linalg.norm(results[j][idx] - result[idx]) < 0.1: +335 unique = False +336 if unique: +337 results.append(result) +338 start_qpos = self.pinocchio_model.get_random_configuration() +339 mask_len = len(mask) +340 if mask_len > 0: +341 for j in range(mask_len): +342 if mask[j]: +343 start_qpos[j] = qpos0[j] +344 if len(results) != 0: +345 status = "Success" +346 elif min_dis != 1e9: +347 status = ( +348 "IK Failed! Distance %lf is greater than threshold %lf." +349 % (min_dis, threshold) +350 ) +351 else: +352 status = "IK Failed! Cannot find valid solution." +353 return status, results +354 +355 def TOPP(self, path, step=0.1, verbose=False): +356 """ +357 Time-Optimal Path Parameterization +358 +359 Args: +360 path: numpy array of shape (n, dof) +361 step: step size for the discretization +362 verbose: if True, will print the log of TOPPRA +363 """ +364 +365 N_samples = path.shape[0] +366 dof = path.shape[1] +367 assert dof == len(self.joint_vel_limits) +368 assert dof == len(self.joint_acc_limits) +369 ss = np.linspace(0, 1, N_samples) +370 path = ta.SplineInterpolator(ss, path) +371 pc_vel = constraint.JointVelocityConstraint(self.joint_vel_limits) +372 pc_acc = constraint.JointAccelerationConstraint(self.joint_acc_limits) +373 instance = algo.TOPPRA( +374 [pc_vel, pc_acc], path, parametrizer="ParametrizeConstAccel" +375 ) +376 jnt_traj = instance.compute_trajectory() +377 ts_sample = np.linspace( +378 0, jnt_traj.duration, int(jnt_traj.duration / step) +379 ) +380 qs_sample = jnt_traj(ts_sample) +381 qds_sample = jnt_traj(ts_sample, 1) +382 qdds_sample = jnt_traj(ts_sample, 2) +383 return ts_sample, qs_sample, qds_sample, qdds_sample, jnt_traj.duration +384 +385 def update_point_cloud(self, pc, radius=1e-3): +386 """ +387 Args: +388 pc: numpy array of shape (n, 3) +389 radius: radius of each point. This gives a buffer around each point that planner will avoid +390 """ +391 self.planning_world.update_point_cloud(pc, radius) +392 +393 def update_attached_tool(self, fcl_collision_geometry, pose, link_id=-1): +394 """ helper function to update the attached tool """ +395 if link_id == -1: +396 link_id = self.move_group_link_id +397 self.planning_world.update_attached_tool(fcl_collision_geometry, link_id, pose) +398 +399 def update_attached_sphere(self, radius, pose, link_id=-1): +400 """ +401 attach a sphere to some link +402 +403 Args: +404 radius: radius of the sphere +405 pose: [x,y,z,qw,qx,qy,qz] pose of the sphere +406 link_id: if not provided, the end effector will be the target. +407 """ +408 if link_id == -1: +409 link_id = self.move_group_link_id +410 self.planning_world.update_attached_sphere(radius, link_id, pose) +411 +412 def update_attached_box(self, size, pose, link_id=-1): +413 """ +414 attach a box to some link +415 +416 Args: +417 size: [x,y,z] size of the box +418 pose: [x,y,z,qw,qx,qy,qz] pose of the box +419 link_id: if not provided, the end effector will be the target. +420 """ +421 if link_id == -1: +422 link_id = self.move_group_link_id +423 self.planning_world.update_attached_box(size, link_id, pose) +424 +425 def update_attached_mesh(self, mesh_path, pose, link_id=-1): +426 """ +427 attach a mesh to some link +428 +429 Args: +430 mesh_path: path to the mesh +431 pose: [x,y,z,qw,qx,qy,qz] pose of the mesh +432 link_id: if not provided, the end effector will be the target. +433 """ +434 if link_id == -1: +435 link_id = self.move_group_link_id +436 self.planning_world.update_attached_mesh(mesh_path, link_id, pose) +437 +438 def set_base_pose(self, pose): +439 """ +440 tell the planner where the base of the robot is w.r.t the world frame +441 +442 Args: +443 pose: [x,y,z,qw,qx,qy,qz] pose of the base +444 """ +445 self.robot.set_base_pose(pose) +446 +447 def set_normal_object(self, name, collision_object): +448 """ adds or updates a non-articulated collision object in the scene """ +449 self.planning_world.set_normal_object(name, collision_object) +450 +451 def remove_normal_object(self, name): +452 """ returns true if the object was removed, false if it was not found """ +453 return self.planning_world.remove_normal_object(name) +454 +455 def plan_qpos_to_qpos( +456 self, +457 goal_qposes: list, +458 current_qpos, +459 time_step=0.1, +460 rrt_range=0.1, +461 planning_time=1, +462 fix_joint_limits=True, +463 use_point_cloud=False, +464 use_attach=False, +465 verbose=False, +466 planner_name="RRTConnect", +467 no_simplification=False, +468 constraint_function=None, +469 constraint_jacobian=None, +470 constraint_tolerance=1e-3, +471 fixed_joint_indices=[] +472 ): +473 """ +474 plan a path from a specified joint position to a goal pose +475 +476 Args: +477 goal_pose: 7D pose of the end-effector [x,y,z,qw,qx,qy,qz] +478 current_qpos: current joint configuration (either full or move_group joints) +479 mask: mask for IK. When set, the IK will leave certain joints out of planning +480 time_step: time step for TOPP +481 rrt_range: step size for RRT +482 planning_time: time limit for RRT +483 fix_joint_limits: if True, will clip the joint configuration to be within the joint limits +484 use_point_cloud: if True, will use the point cloud to avoid collision +485 use_attach: if True, will consider the attached tool collision when planning +486 verbose: if True, will print the log of OMPL and TOPPRA +487 planner_name: planner name pick from {"RRTConnect", "RRT*"} +488 fixed_joint_indices: list of indices of joints that are fixed during planning +489 constraint_function: evals to 0 when constraint is satisfied +490 constraint_jacobian: jacobian of constraint_function +491 constraint_tolerance: tolerance for constraint_function +492 no_simplification: if true, will not simplify the path. constraint planning does not support simplification +493 """ +494 self.planning_world.set_use_point_cloud(use_point_cloud) +495 self.planning_world.set_use_attach(use_attach) +496 n = current_qpos.shape[0] +497 if fix_joint_limits: +498 for i in range(n): +499 if current_qpos[i] < self.joint_limits[i][0]: +500 current_qpos[i] = self.joint_limits[i][0] + 1e-3 +501 if current_qpos[i] > self.joint_limits[i][1]: +502 current_qpos[i] = self.joint_limits[i][1] - 1e-3 +503 +504 current_qpos = self.pad_qpos(current_qpos) +505 +506 self.robot.set_qpos(current_qpos, True) +507 collisions = self.planning_world.collide_full() +508 if len(collisions) != 0: +509 print("Invalid start state!") +510 for collision in collisions: +511 print("%s and %s collide!" % (collision.link_name1, collision.link_name2)) +512 +513 idx = self.move_group_joint_indices +514 +515 goal_qpos_ = [] +516 for i in range(len(goal_qposes)): +517 goal_qpos_.append(goal_qposes[i][idx]) +518 +519 fixed_joints = set() +520 for joint_idx in fixed_joint_indices: +521 fixed_joints.add(ompl.FixedJoint(0, joint_idx, current_qpos[joint_idx])) +522 +523 assert len(current_qpos[idx]) == len(goal_qpos_[0]) +524 status, path = self.planner.plan( +525 current_qpos[idx], +526 goal_qpos_, +527 range=rrt_range, +528 time=planning_time, +529 fixed_joints=fixed_joints, +530 verbose=verbose, +531 planner_name=planner_name, +532 no_simplification=no_simplification, +533 constraint_function=constraint_function, +534 constraint_jacobian=constraint_jacobian, +535 constraint_tolerance=constraint_tolerance +536 ) +537 +538 if status == "Exact solution": +539 if verbose: ta.setup_logging("INFO") +540 else: ta.setup_logging("WARNING") +541 times, pos, vel, acc, duration = self.TOPP(path, time_step) +542 return { +543 "status": "Success", +544 "time": times, +545 "position": pos, +546 "velocity": vel, +547 "acceleration": acc, +548 "duration": duration, +549 } +550 else: +551 return {"status": "RRT Failed. %s" % status} +552 +553 def plan_qpos_to_pose( +554 self, +555 goal_pose, +556 current_qpos, +557 mask = [], +558 time_step=0.1, +559 rrt_range=0.1, +560 planning_time=1, +561 fix_joint_limits=True, +562 use_point_cloud=False, +563 use_attach=False, +564 verbose=False, +565 wrt_world=False, +566 planner_name="RRTConnect", +567 no_simplification=False, +568 constraint_function=None, +569 constraint_jacobian=None, +570 constraint_tolerance=1e-3 +571 ): +572 """ +573 plan from a start configuration to a goal pose of the end-effector +574 +575 Args: +576 goal_pose: [x,y,z,qw,qx,qy,qz] pose of the goal +577 current_qpos: current joint configuration (either full or move_group joints) +578 mask: if the value at a given index is True, the joint is *not* used in the IK +579 time_step: time step for TOPPRA (time parameterization of path) +580 rrt_range: step size for RRT +581 planning_time: time limit for RRT +582 fix_joint_limits: if True, will clip the joint configuration to be within the joint limits +583 use_point_cloud: if True, will use the point cloud to avoid collision +584 use_attach: if True, will consider the attached tool collision when planning +585 verbose: if True, will print the log of OMPL and TOPPRA +586 wrt_world: if true, interpret the target pose with respect to the world frame instead of the base frame +587 """ +588 n = current_qpos.shape[0] +589 if fix_joint_limits: +590 for i in range(n): +591 if current_qpos[i] < self.joint_limits[i][0]: +592 current_qpos[i] = self.joint_limits[i][0] + 1e-3 +593 if current_qpos[i] > self.joint_limits[i][1]: +594 current_qpos[i] = self.joint_limits[i][1] - 1e-3 +595 +596 if wrt_world: +597 base_pose = self.robot.get_base_pose() +598 base_tf = np.eye(4) +599 base_tf[0:3, 3] = base_pose[:3] +600 base_tf[0:3, 0:3] = quat2mat(base_pose[3:]) +601 goal_tf = np.eye(4) +602 goal_tf[0:3, 3] = goal_pose[:3] +603 goal_tf[0:3, 0:3] = quat2mat(goal_pose[3:]) +604 goal_tf = np.linalg.inv(base_tf).dot(goal_tf) +605 goal_pose[:3] = goal_tf[0:3, 3] +606 goal_pose[3:] = mat2quat(goal_tf[0:3, 0:3]) +607 +608 idx = self.move_group_joint_indices # we need to take only the move_group joints when planning +609 +610 ik_status, goal_qpos = self.IK(goal_pose, current_qpos, mask) +611 if ik_status != "Success": +612 return {"status": ik_status} +613 +614 if verbose: +615 print("IK results:") +616 for i in range(len(goal_qpos)): +617 print(goal_qpos[i]) +618 +619 goal_qpos_ = [] +620 for i in range(len(goal_qpos)): +621 goal_qpos_.append(goal_qpos[i][idx]) +622 self.robot.set_qpos(current_qpos, True) +623 +624 ik_status, goal_qpos = self.IK(goal_pose, current_qpos, mask) +625 if ik_status != "Success": +626 return {"status": ik_status} +627 +628 if verbose: +629 print("IK results:") +630 for i in range(len(goal_qpos)): +631 print(goal_qpos[i]) +632 +633 return self.plan_qpos_to_qpos( +634 goal_qpos, +635 current_qpos, +636 time_step, +637 rrt_range, +638 planning_time, +639 fix_joint_limits, +640 use_point_cloud, +641 use_attach, +642 verbose, +643 planner_name, +644 no_simplification, +645 constraint_function, +646 constraint_jacobian, +647 constraint_tolerance +648 ) +649 +650 def plan_screw( +651 self, +652 target_pose, +653 qpos, +654 qpos_step=0.1, +655 time_step=0.1, +656 use_point_cloud=False, +657 use_attach=False, +658 verbose=False, +659 ): +660 """ +661 plan from a start configuration to a goal pose of the end-effector using screw motion +662 +663 Args: +664 target_pose: [x,y,z,qw,qx,qy,qz] pose of the goal +665 qpos: current joint configuration (either full or move_group joints) +666 qpos_step: size of the random step for RRT +667 time_step: time step for the discretization +668 use_point_cloud: if True, will use the point cloud to avoid collision +669 use_attach: if True, will use the attached tool to avoid collision +670 verbose: if True, will print the log of TOPPRA +671 """ +672 self.planning_world.set_use_point_cloud(use_point_cloud) +673 self.planning_world.set_use_attach(use_attach) +674 qpos = self.pad_qpos(qpos.copy()) +675 self.robot.set_qpos(qpos, True) +676 +677 def pose7D2mat(pose): +678 mat = np.eye(4) +679 mat[0:3, 3] = pose[:3] +680 mat[0:3, 0:3] = quat2mat(pose[3:]) +681 return mat +682 +683 def skew(vec): +684 return np.array( +685 [ +686 [0, -vec[2], vec[1]], +687 [vec[2], 0, -vec[0]], +688 [-vec[1], vec[0], 0], +689 ] +690 ) +691 +692 def pose2exp_coordinate(pose: np.ndarray) -> Tuple[np.ndarray, float]: +693 def rot2so3(rotation: np.ndarray): +694 assert rotation.shape == (3, 3) +695 if np.isclose(rotation.trace(), 3): +696 return np.zeros(3), 1 +697 if np.isclose(rotation.trace(), -1): +698 return np.zeros(3), -1e6 +699 theta = np.arccos((rotation.trace() - 1) / 2) +700 omega = ( +701 1 +702 / 2 +703 / np.sin(theta) +704 * np.array( +705 [ +706 rotation[2, 1] - rotation[1, 2], +707 rotation[0, 2] - rotation[2, 0], +708 rotation[1, 0] - rotation[0, 1], +709 ] +710 ).T +711 ) +712 return omega, theta +713 +714 omega, theta = rot2so3(pose[:3, :3]) +715 if theta < -1e5: +716 return omega, theta +717 ss = skew(omega) +718 inv_left_jacobian = ( +719 np.eye(3) / theta +720 - 0.5 * ss +721 + (1.0 / theta - 0.5 / np.tan(theta / 2)) * ss @ ss +722 ) +723 v = inv_left_jacobian @ pose[:3, 3] +724 return np.concatenate([v, omega]), theta +725 +726 self.pinocchio_model.compute_forward_kinematics(qpos) +727 ee_index = self.link_name_2_idx[self.move_group] +728 current_p = pose7D2mat(self.pinocchio_model.get_link_pose(ee_index)) +729 target_p = pose7D2mat(target_pose) +730 relative_transform = target_p @ np.linalg.inv(current_p) +731 +732 omega, theta = pose2exp_coordinate(relative_transform) +733 +734 if theta < -1e4: +735 return {"status": "screw plan failed."} +736 omega = omega.reshape((-1, 1)) * theta +737 +738 index = self.move_group_joint_indices +739 path = [np.copy(qpos[index])] +740 +741 while True: +742 self.pinocchio_model.compute_full_jacobian(qpos) +743 J = self.pinocchio_model.get_link_jacobian(ee_index, local=False) +744 delta_q = np.linalg.pinv(J) @ omega +745 delta_q *= qpos_step / (np.linalg.norm(delta_q)) +746 delta_twist = J @ delta_q +747 +748 flag = False +749 if np.linalg.norm(delta_twist) > np.linalg.norm(omega): +750 ratio = np.linalg.norm(omega) / np.linalg.norm(delta_twist) +751 delta_q = delta_q * ratio +752 delta_twist = delta_twist * ratio +753 flag = True +754 +755 qpos += delta_q.reshape(-1) +756 omega -= delta_twist +757 +758 def check_joint_limit(q): +759 n = len(q) +760 for i in range(n): +761 if ( +762 q[i] < self.joint_limits[i][0] - 1e-3 +763 or q[i] > self.joint_limits[i][1] + 1e-3 +764 ): +765 return False +766 return True +767 +768 within_joint_limit = check_joint_limit(qpos) +769 self.planning_world.set_qpos_all(qpos[index]) +770 collide = self.planning_world.collide() +771 +772 if ( +773 np.linalg.norm(delta_twist) < 1e-4 +774 or collide +775 or within_joint_limit == False +776 ): +777 return {"status": "screw plan failed"} +778 +779 path.append(np.copy(qpos[index])) +780 +781 if flag: +782 if verbose: +783 ta.setup_logging("INFO") +784 else: +785 ta.setup_logging("WARNING") +786 times, pos, vel, acc, duration = self.TOPP( +787 np.vstack(path), time_step +788 ) +789 return { +790 "status": "Success", +791 "time": times, +792 "position": pos, +793 "velocity": vel, +794 "acceleration": acc, +795 "duration": duration, +796 } +
Motion planner.
+20 def __init__( + 21 self, + 22 urdf: str, + 23 move_group: str, + 24 srdf: str = "", + 25 package_keyword_replacement: str = "", + 26 user_link_names: Sequence[str] = [], + 27 user_joint_names: Sequence[str] = [], + 28 joint_vel_limits: Union[Sequence[float], np.ndarray] = [], + 29 joint_acc_limits: Union[Sequence[float], np.ndarray] = [], + 30 **kwargs + 31 ): + 32 r"""Motion planner for robots. + 33 + 34 Args: + 35 urdf: Unified Robot Description Format file. + 36 user_link_names: names of links, the order. if empty, all links will be used. + 37 user_joint_names: names of the joints to plan. if empty, all active joints will be used. + 38 move_group: target link to move, usually the end-effector. + 39 joint_vel_limits: maximum joint velocities for time parameterization, + 40 which should have the same length as + 41 joint_acc_limits: maximum joint accelerations for time parameterization, + 42 which should have the same length as + 43 srdf: Semantic Robot Description Format file. + 44 References: + 45 http://docs.ros.org/en/kinetic/api/moveit_tutorials/html/doc/urdf_srdf/urdf_srdf_tutorial.html + 46 + 47 """ + 48 self.urdf = urdf + 49 if srdf == "" and os.path.exists(urdf.replace(".urdf", ".srdf")): + 50 self.srdf = urdf.replace(".urdf", ".srdf") + 51 print(f"No SRDF file provided but found {self.srdf}") + 52 + 53 # replace package:// keyword if exists + 54 urdf = self.replace_package_keyword(package_keyword_replacement) + 55 + 56 self.robot = articulation.ArticulatedModel( + 57 urdf, + 58 srdf, + 59 [0, 0, -9.81], + 60 user_joint_names, + 61 user_link_names, + 62 verbose=False, + 63 convex=True, + 64 ) + 65 self.pinocchio_model = self.robot.get_pinocchio_model() + 66 + 67 self.planning_world = planning_world.PlanningWorld( + 68 [self.robot], + 69 ["robot"], + 70 kwargs.get("normal_objects", []), + 71 kwargs.get("normal_object_names", []) + 72 ) + 73 + 74 if srdf == "": + 75 self.generate_collision_pair() + 76 self.robot.update_SRDF(self.srdf) + 77 + 78 self.pinocchio_model = self.robot.get_pinocchio_model() + 79 self.user_link_names = self.pinocchio_model.get_link_names() + 80 self.user_joint_names = self.pinocchio_model.get_joint_names() + 81 + 82 self.joint_name_2_idx = {} + 83 for i, joint in enumerate(self.user_joint_names): + 84 self.joint_name_2_idx[joint] = i + 85 self.link_name_2_idx = {} + 86 for i, link in enumerate(self.user_link_names): + 87 self.link_name_2_idx[link] = i + 88 + 89 assert move_group in self.user_link_names,\ + 90 f"end-effector not found as one of the links in {self.user_link_names}" + 91 self.move_group = move_group + 92 self.robot.set_move_group(self.move_group) + 93 self.move_group_joint_indices = self.robot.get_move_group_joint_indices() + 94 + 95 self.joint_types = self.pinocchio_model.get_joint_types() + 96 self.joint_limits = np.concatenate(self.pinocchio_model.get_joint_limits()) + 97 self.joint_vel_limits = joint_vel_limits if len(joint_vel_limits) else np.ones(len(self.move_group_joint_indices)) + 98 self.joint_acc_limits = joint_acc_limits if len(joint_acc_limits) else np.ones(len(self.move_group_joint_indices)) + 99 self.move_group_link_id = self.link_name_2_idx[self.move_group] +100 assert len(self.joint_vel_limits) == len(self.joint_acc_limits),\ +101 f"length of joint_vel_limits ({len(self.joint_vel_limits)}) =/= "\ +102 f"length of joint_acc_limits ({len(self.joint_acc_limits)})" +103 assert len(self.joint_vel_limits) == len(self.move_group_joint_indices),\ +104 f"length of joint_vel_limits ({len(self.joint_vel_limits)}) =/= "\ +105 f"length of move_group ({len(self.move_group_joint_indices)})" +106 assert len(self.joint_vel_limits) <= len(self.joint_limits),\ +107 f"length of joint_vel_limits ({len(self.joint_vel_limits)}) > "\ +108 f"number of total joints ({len(self.joint_limits)})" +109 +110 self.planning_world = planning_world.PlanningWorld([self.robot], ["robot"], [], []) +111 self.planner = ompl.OMPLPlanner(world=self.planning_world) +
Motion planner for robots.
+ +Args: + urdf: Unified Robot Description Format file. + user_link_names: names of links, the order. if empty, all links will be used. + user_joint_names: names of the joints to plan. if empty, all active joints will be used. + move_group: target link to move, usually the end-effector. + joint_vel_limits: maximum joint velocities for time parameterization, + which should have the same length as + joint_acc_limits: maximum joint accelerations for time parameterization, + which should have the same length as + srdf: Semantic Robot Description Format file. +References: + http://docs.ros.org/en/kinetic/api/moveit_tutorials/html/doc/urdf_srdf/urdf_srdf_tutorial.html
+114 def replace_package_keyword(self, package_keyword_replacement): +115 """ +116 some ROS URDF files use package:// keyword to refer the package dir +117 replace it with the given string (default is empty) +118 +119 Args: +120 package_keyword_replacement: the string to replace 'package://' keyword +121 """ +122 rtn_urdf = self.urdf +123 with open(self.urdf, "r") as in_f: +124 content = in_f.read() +125 if "package://" in content: +126 rtn_urdf = self.urdf.replace(".urdf", "_package_keyword_replaced.urdf") +127 content = content.replace("package://", package_keyword_replacement) +128 if not os.path.exists(rtn_urdf): +129 with open(rtn_urdf, "w") as out_f: +130 out_f.write(content) +131 return rtn_urdf +
some ROS URDF files use package:// keyword to refer the package dir +replace it with the given string (default is empty)
+ +Args: + package_keyword_replacement: the string to replace 'package://' keyword
+133 def generate_collision_pair(self, sample_time = 1000000, echo_freq = 100000): +134 """ +135 we read the srdf file to get the link pairs that should not collide. +136 if not provided, we need to randomly sample configurations to find the link pairs that will always collide. +137 """ +138 print("Since no SRDF file is provided. We will first detect link pairs that will always collide. This may take several minutes.") +139 n_link = len(self.user_link_names) +140 cnt = np.zeros((n_link, n_link), dtype=np.int32) +141 for i in range(sample_time): +142 qpos = self.pinocchio_model.get_random_configuration() +143 self.robot.set_qpos(qpos, True) +144 collisions = self.planning_world.collide_full() +145 for collision in collisions: +146 u = self.link_name_2_idx[collision.link_name1] +147 v = self.link_name_2_idx[collision.link_name2] +148 cnt[u][v] += 1 +149 if i % echo_freq == 0: +150 print("Finish %.1f%%!" % (i * 100 / sample_time)) +151 +152 import xml.etree.ElementTree as ET +153 from xml.dom import minidom +154 +155 root = ET.Element('robot') +156 robot_name = self.urdf.split('/')[-1].split('.')[0] +157 root.set('name', robot_name) +158 self.srdf = self.urdf.replace(".urdf", ".srdf") +159 +160 for i in range(n_link): +161 for j in range(n_link): +162 if cnt[i][j] == sample_time: +163 link1 = self.user_link_names[i] +164 link2 = self.user_link_names[j] +165 print("Ignore collision pair: (%s, %s), reason: always collide" % (link1, link2)) +166 collision = ET.SubElement(root, 'disable_collisions') +167 collision.set('link1', link1) +168 collision.set('link2', link2) +169 collision.set('reason', 'Default') +170 srdffile = open(self.srdf, "w") +171 srdffile.write(minidom.parseString(ET.tostring(root)).toprettyxml(indent=" ")) +172 srdffile.close() +173 print("Saving the SRDF file to %s" % self.srdf) +
we read the srdf file to get the link pairs that should not collide. +if not provided, we need to randomly sample configurations to find the link pairs that will always collide.
+175 def distance_6D(self, p1, q1, p2, q2): +176 """ +177 compute the distance between two poses +178 +179 Args: +180 p1: position of pose 1 +181 q1: quaternion of pose 1 +182 p2: position of pose 2 +183 q2: quaternion of pose 2 +184 """ +185 return np.linalg.norm(p1 - p2) + min( +186 np.linalg.norm(q1 - q2), np.linalg.norm(q1 + q2) +187 ) +
compute the distance between two poses
+ +Args: + p1: position of pose 1 + q1: quaternion of pose 1 + p2: position of pose 2 + q2: quaternion of pose 2
+189 def check_joint_limit(self, q): +190 """ +191 check if the joint configuration is within the joint limits +192 +193 Args: +194 q: joint configuration +195 +196 Returns: +197 True if the joint configuration is within the joint limits +198 """ +199 n = len(q) +200 flag = True +201 for i in range(n): +202 if self.joint_types[i].startswith("JointModelR"): +203 if np.abs(q[i] - self.joint_limits[i][0]) < 1e-3: +204 continue +205 q[i] -= ( +206 2 +207 * np.pi +208 * np.floor((q[i] - self.joint_limits[i][0]) / (2 * np.pi)) +209 ) +210 if q[i] > self.joint_limits[i][1] + 1e-3: +211 flag = False +212 else: +213 if ( +214 q[i] < self.joint_limits[i][0] - 1e-3 +215 or q[i] > self.joint_limits[i][1] + 1e-3 +216 ): +217 flag = False +218 return flag +
check if the joint configuration is within the joint limits
+ +Args: + q: joint configuration
+ +Returns: + True if the joint configuration is within the joint limits
+220 def pad_qpos(self, qpos, articulation=None): +221 """ +222 if the user does not provide the full qpos but only the move_group joints, +223 pad the qpos with the rest of the joints +224 """ +225 if len(qpos) == len(self.move_group_joint_indices): +226 tmp = articulation.get_qpos() if articulation is not None else self.robot.get_qpos() +227 tmp[:len(qpos)] = qpos +228 qpos = tmp +229 +230 assert len(qpos) == len(self.joint_limits),\ +231 f"length of qpos ({len(qpos)}) =/= "\ +232 f"number of total joints ({len(self.joint_limits)})" +233 +234 return qpos +
if the user does not provide the full qpos but only the move_group joints, +pad the qpos with the rest of the joints
+236 def check_for_collision(self, collision_function, articulation: articulation.ArticulatedModel=None, qpos: np.ndarray=None) -> list: +237 """ helper function to check for collision """ +238 # handle no user input +239 if articulation is None: +240 articulation = self.robot +241 if qpos is None: +242 qpos = articulation.get_qpos() +243 qpos = self.pad_qpos(qpos, articulation) +244 +245 # first save the current qpos +246 old_qpos = articulation.get_qpos() +247 # set robot to new qpos +248 articulation.set_qpos(qpos, True) +249 # find the index of the articulation inside the array +250 idx = self.planning_world.get_articulations().index(articulation) +251 # check for self-collision +252 collisions = collision_function(idx) +253 # reset qpos +254 articulation.set_qpos(old_qpos, True) +255 return collisions +
helper function to check for collision
+257 def check_for_self_collision(self, articulation: articulation.ArticulatedModel=None, qpos: np.ndarray=None) -> list: +258 """Check if the robot is in self-collision. +259 +260 Args: +261 articulation: robot model. if none will be self.robot +262 qpos: robot configuration. if none will be the current pose +263 +264 Returns: +265 A list of collisions. +266 """ +267 return self.check_for_collision(self.planning_world.self_collide, articulation, qpos) +
Check if the robot is in self-collision.
+ +Args: + articulation: robot model. if none will be self.robot + qpos: robot configuration. if none will be the current pose
+ +Returns: + A list of collisions.
+269 def check_for_env_collision(self, articulation: articulation.ArticulatedModel=None, qpos: np.ndarray=None, with_point_cloud=False, use_attach=False) -> list: +270 """Check if the robot is in collision with the environment +271 +272 Args: +273 articulation: robot model. if none will be self.robot +274 qpos: robot configuration. if none will be the current pose +275 with_point_cloud: whether to check collision against point cloud +276 use_attach: whether to include the object attached to the end effector in collision checking +277 Returns: +278 A list of collisions. +279 """ +280 # store previous results +281 prev_use_point_cloud = self.planning_world.use_point_cloud +282 prev_use_attach = self.planning_world.use_attach +283 self.planning_world.set_use_point_cloud(with_point_cloud) +284 self.planning_world.set_use_attach(use_attach) +285 +286 results = self.check_for_collision(self.planning_world.collide_with_others, articulation, qpos) +287 +288 # restore +289 self.planning_world.set_use_point_cloud(prev_use_point_cloud) +290 self.planning_world.set_use_attach(prev_use_attach) +291 return results +
Check if the robot is in collision with the environment
+ +Args: + articulation: robot model. if none will be self.robot + qpos: robot configuration. if none will be the current pose + with_point_cloud: whether to check collision against point cloud + use_attach: whether to include the object attached to the end effector in collision checking +Returns: + A list of collisions.
+293 def IK(self, goal_pose, start_qpos, mask = [], n_init_qpos=20, threshold=1e-3): +294 """ +295 Inverse kinematics +296 +297 Args: +298 goal_pose: [x,y,z,qw,qx,qy,qz] pose of the goal +299 start_qpos: initial configuration +300 mask: if the value at a given index is True, the joint is *not* used in the IK +301 n_init_qpos: number of random initial configurations +302 threshold: threshold for the distance between the goal pose and the result pose +303 """ +304 +305 index = self.link_name_2_idx[self.move_group] +306 min_dis = 1e9 +307 idx = self.move_group_joint_indices +308 qpos0 = np.copy(start_qpos) +309 results = [] +310 self.robot.set_qpos(start_qpos, True) +311 for i in range(n_init_qpos): +312 ik_results = self.pinocchio_model.compute_IK_CLIK( +313 index, goal_pose, start_qpos, mask +314 ) +315 flag = self.check_joint_limit(ik_results[0]) # will clip qpos +316 +317 # check collision +318 self.planning_world.set_qpos_all(ik_results[0][idx]) +319 if (len(self.planning_world.collide_full()) != 0): +320 flag = False +321 +322 if flag: +323 self.pinocchio_model.compute_forward_kinematics(ik_results[0]) +324 new_pose = self.pinocchio_model.get_link_pose(index) +325 tmp_dis = self.distance_6D( +326 goal_pose[:3], goal_pose[3:], new_pose[:3], new_pose[3:] +327 ) +328 if tmp_dis < min_dis: +329 min_dis = tmp_dis +330 if tmp_dis < threshold: +331 result = ik_results[0] +332 unique = True +333 for j in range(len(results)): +334 if np.linalg.norm(results[j][idx] - result[idx]) < 0.1: +335 unique = False +336 if unique: +337 results.append(result) +338 start_qpos = self.pinocchio_model.get_random_configuration() +339 mask_len = len(mask) +340 if mask_len > 0: +341 for j in range(mask_len): +342 if mask[j]: +343 start_qpos[j] = qpos0[j] +344 if len(results) != 0: +345 status = "Success" +346 elif min_dis != 1e9: +347 status = ( +348 "IK Failed! Distance %lf is greater than threshold %lf." +349 % (min_dis, threshold) +350 ) +351 else: +352 status = "IK Failed! Cannot find valid solution." +353 return status, results +
Inverse kinematics
+ +Args: + goal_pose: [x,y,z,qw,qx,qy,qz] pose of the goal + start_qpos: initial configuration + mask: if the value at a given index is True, the joint is not used in the IK + n_init_qpos: number of random initial configurations + threshold: threshold for the distance between the goal pose and the result pose
+355 def TOPP(self, path, step=0.1, verbose=False): +356 """ +357 Time-Optimal Path Parameterization +358 +359 Args: +360 path: numpy array of shape (n, dof) +361 step: step size for the discretization +362 verbose: if True, will print the log of TOPPRA +363 """ +364 +365 N_samples = path.shape[0] +366 dof = path.shape[1] +367 assert dof == len(self.joint_vel_limits) +368 assert dof == len(self.joint_acc_limits) +369 ss = np.linspace(0, 1, N_samples) +370 path = ta.SplineInterpolator(ss, path) +371 pc_vel = constraint.JointVelocityConstraint(self.joint_vel_limits) +372 pc_acc = constraint.JointAccelerationConstraint(self.joint_acc_limits) +373 instance = algo.TOPPRA( +374 [pc_vel, pc_acc], path, parametrizer="ParametrizeConstAccel" +375 ) +376 jnt_traj = instance.compute_trajectory() +377 ts_sample = np.linspace( +378 0, jnt_traj.duration, int(jnt_traj.duration / step) +379 ) +380 qs_sample = jnt_traj(ts_sample) +381 qds_sample = jnt_traj(ts_sample, 1) +382 qdds_sample = jnt_traj(ts_sample, 2) +383 return ts_sample, qs_sample, qds_sample, qdds_sample, jnt_traj.duration +
Time-Optimal Path Parameterization
+ +Args: + path: numpy array of shape (n, dof) + step: step size for the discretization + verbose: if True, will print the log of TOPPRA
+385 def update_point_cloud(self, pc, radius=1e-3): +386 """ +387 Args: +388 pc: numpy array of shape (n, 3) +389 radius: radius of each point. This gives a buffer around each point that planner will avoid +390 """ +391 self.planning_world.update_point_cloud(pc, radius) +
Args: + pc: numpy array of shape (n, 3) + radius: radius of each point. This gives a buffer around each point that planner will avoid
+393 def update_attached_tool(self, fcl_collision_geometry, pose, link_id=-1): +394 """ helper function to update the attached tool """ +395 if link_id == -1: +396 link_id = self.move_group_link_id +397 self.planning_world.update_attached_tool(fcl_collision_geometry, link_id, pose) +
helper function to update the attached tool
+399 def update_attached_sphere(self, radius, pose, link_id=-1): +400 """ +401 attach a sphere to some link +402 +403 Args: +404 radius: radius of the sphere +405 pose: [x,y,z,qw,qx,qy,qz] pose of the sphere +406 link_id: if not provided, the end effector will be the target. +407 """ +408 if link_id == -1: +409 link_id = self.move_group_link_id +410 self.planning_world.update_attached_sphere(radius, link_id, pose) +
attach a sphere to some link
+ +Args: + radius: radius of the sphere + pose: [x,y,z,qw,qx,qy,qz] pose of the sphere + link_id: if not provided, the end effector will be the target.
+412 def update_attached_box(self, size, pose, link_id=-1): +413 """ +414 attach a box to some link +415 +416 Args: +417 size: [x,y,z] size of the box +418 pose: [x,y,z,qw,qx,qy,qz] pose of the box +419 link_id: if not provided, the end effector will be the target. +420 """ +421 if link_id == -1: +422 link_id = self.move_group_link_id +423 self.planning_world.update_attached_box(size, link_id, pose) +
attach a box to some link
+ +Args: + size: [x,y,z] size of the box + pose: [x,y,z,qw,qx,qy,qz] pose of the box + link_id: if not provided, the end effector will be the target.
+425 def update_attached_mesh(self, mesh_path, pose, link_id=-1): +426 """ +427 attach a mesh to some link +428 +429 Args: +430 mesh_path: path to the mesh +431 pose: [x,y,z,qw,qx,qy,qz] pose of the mesh +432 link_id: if not provided, the end effector will be the target. +433 """ +434 if link_id == -1: +435 link_id = self.move_group_link_id +436 self.planning_world.update_attached_mesh(mesh_path, link_id, pose) +
attach a mesh to some link
+ +Args: + mesh_path: path to the mesh + pose: [x,y,z,qw,qx,qy,qz] pose of the mesh + link_id: if not provided, the end effector will be the target.
+438 def set_base_pose(self, pose): +439 """ +440 tell the planner where the base of the robot is w.r.t the world frame +441 +442 Args: +443 pose: [x,y,z,qw,qx,qy,qz] pose of the base +444 """ +445 self.robot.set_base_pose(pose) +
tell the planner where the base of the robot is w.r.t the world frame
+ +Args: + pose: [x,y,z,qw,qx,qy,qz] pose of the base
+447 def set_normal_object(self, name, collision_object): +448 """ adds or updates a non-articulated collision object in the scene """ +449 self.planning_world.set_normal_object(name, collision_object) +
adds or updates a non-articulated collision object in the scene
+451 def remove_normal_object(self, name): +452 """ returns true if the object was removed, false if it was not found """ +453 return self.planning_world.remove_normal_object(name) +
returns true if the object was removed, false if it was not found
+455 def plan_qpos_to_qpos( +456 self, +457 goal_qposes: list, +458 current_qpos, +459 time_step=0.1, +460 rrt_range=0.1, +461 planning_time=1, +462 fix_joint_limits=True, +463 use_point_cloud=False, +464 use_attach=False, +465 verbose=False, +466 planner_name="RRTConnect", +467 no_simplification=False, +468 constraint_function=None, +469 constraint_jacobian=None, +470 constraint_tolerance=1e-3, +471 fixed_joint_indices=[] +472 ): +473 """ +474 plan a path from a specified joint position to a goal pose +475 +476 Args: +477 goal_pose: 7D pose of the end-effector [x,y,z,qw,qx,qy,qz] +478 current_qpos: current joint configuration (either full or move_group joints) +479 mask: mask for IK. When set, the IK will leave certain joints out of planning +480 time_step: time step for TOPP +481 rrt_range: step size for RRT +482 planning_time: time limit for RRT +483 fix_joint_limits: if True, will clip the joint configuration to be within the joint limits +484 use_point_cloud: if True, will use the point cloud to avoid collision +485 use_attach: if True, will consider the attached tool collision when planning +486 verbose: if True, will print the log of OMPL and TOPPRA +487 planner_name: planner name pick from {"RRTConnect", "RRT*"} +488 fixed_joint_indices: list of indices of joints that are fixed during planning +489 constraint_function: evals to 0 when constraint is satisfied +490 constraint_jacobian: jacobian of constraint_function +491 constraint_tolerance: tolerance for constraint_function +492 no_simplification: if true, will not simplify the path. constraint planning does not support simplification +493 """ +494 self.planning_world.set_use_point_cloud(use_point_cloud) +495 self.planning_world.set_use_attach(use_attach) +496 n = current_qpos.shape[0] +497 if fix_joint_limits: +498 for i in range(n): +499 if current_qpos[i] < self.joint_limits[i][0]: +500 current_qpos[i] = self.joint_limits[i][0] + 1e-3 +501 if current_qpos[i] > self.joint_limits[i][1]: +502 current_qpos[i] = self.joint_limits[i][1] - 1e-3 +503 +504 current_qpos = self.pad_qpos(current_qpos) +505 +506 self.robot.set_qpos(current_qpos, True) +507 collisions = self.planning_world.collide_full() +508 if len(collisions) != 0: +509 print("Invalid start state!") +510 for collision in collisions: +511 print("%s and %s collide!" % (collision.link_name1, collision.link_name2)) +512 +513 idx = self.move_group_joint_indices +514 +515 goal_qpos_ = [] +516 for i in range(len(goal_qposes)): +517 goal_qpos_.append(goal_qposes[i][idx]) +518 +519 fixed_joints = set() +520 for joint_idx in fixed_joint_indices: +521 fixed_joints.add(ompl.FixedJoint(0, joint_idx, current_qpos[joint_idx])) +522 +523 assert len(current_qpos[idx]) == len(goal_qpos_[0]) +524 status, path = self.planner.plan( +525 current_qpos[idx], +526 goal_qpos_, +527 range=rrt_range, +528 time=planning_time, +529 fixed_joints=fixed_joints, +530 verbose=verbose, +531 planner_name=planner_name, +532 no_simplification=no_simplification, +533 constraint_function=constraint_function, +534 constraint_jacobian=constraint_jacobian, +535 constraint_tolerance=constraint_tolerance +536 ) +537 +538 if status == "Exact solution": +539 if verbose: ta.setup_logging("INFO") +540 else: ta.setup_logging("WARNING") +541 times, pos, vel, acc, duration = self.TOPP(path, time_step) +542 return { +543 "status": "Success", +544 "time": times, +545 "position": pos, +546 "velocity": vel, +547 "acceleration": acc, +548 "duration": duration, +549 } +550 else: +551 return {"status": "RRT Failed. %s" % status} +
plan a path from a specified joint position to a goal pose
+ +Args: + goal_pose: 7D pose of the end-effector [x,y,z,qw,qx,qy,qz] + current_qpos: current joint configuration (either full or move_group joints) + mask: mask for IK. When set, the IK will leave certain joints out of planning + time_step: time step for TOPP + rrt_range: step size for RRT + planning_time: time limit for RRT + fix_joint_limits: if True, will clip the joint configuration to be within the joint limits + use_point_cloud: if True, will use the point cloud to avoid collision + use_attach: if True, will consider the attached tool collision when planning + verbose: if True, will print the log of OMPL and TOPPRA + planner_name: planner name pick from {"RRTConnect", "RRT*"} + fixed_joint_indices: list of indices of joints that are fixed during planning + constraint_function: evals to 0 when constraint is satisfied + constraint_jacobian: jacobian of constraint_function + constraint_tolerance: tolerance for constraint_function + no_simplification: if true, will not simplify the path. constraint planning does not support simplification
+553 def plan_qpos_to_pose( +554 self, +555 goal_pose, +556 current_qpos, +557 mask = [], +558 time_step=0.1, +559 rrt_range=0.1, +560 planning_time=1, +561 fix_joint_limits=True, +562 use_point_cloud=False, +563 use_attach=False, +564 verbose=False, +565 wrt_world=False, +566 planner_name="RRTConnect", +567 no_simplification=False, +568 constraint_function=None, +569 constraint_jacobian=None, +570 constraint_tolerance=1e-3 +571 ): +572 """ +573 plan from a start configuration to a goal pose of the end-effector +574 +575 Args: +576 goal_pose: [x,y,z,qw,qx,qy,qz] pose of the goal +577 current_qpos: current joint configuration (either full or move_group joints) +578 mask: if the value at a given index is True, the joint is *not* used in the IK +579 time_step: time step for TOPPRA (time parameterization of path) +580 rrt_range: step size for RRT +581 planning_time: time limit for RRT +582 fix_joint_limits: if True, will clip the joint configuration to be within the joint limits +583 use_point_cloud: if True, will use the point cloud to avoid collision +584 use_attach: if True, will consider the attached tool collision when planning +585 verbose: if True, will print the log of OMPL and TOPPRA +586 wrt_world: if true, interpret the target pose with respect to the world frame instead of the base frame +587 """ +588 n = current_qpos.shape[0] +589 if fix_joint_limits: +590 for i in range(n): +591 if current_qpos[i] < self.joint_limits[i][0]: +592 current_qpos[i] = self.joint_limits[i][0] + 1e-3 +593 if current_qpos[i] > self.joint_limits[i][1]: +594 current_qpos[i] = self.joint_limits[i][1] - 1e-3 +595 +596 if wrt_world: +597 base_pose = self.robot.get_base_pose() +598 base_tf = np.eye(4) +599 base_tf[0:3, 3] = base_pose[:3] +600 base_tf[0:3, 0:3] = quat2mat(base_pose[3:]) +601 goal_tf = np.eye(4) +602 goal_tf[0:3, 3] = goal_pose[:3] +603 goal_tf[0:3, 0:3] = quat2mat(goal_pose[3:]) +604 goal_tf = np.linalg.inv(base_tf).dot(goal_tf) +605 goal_pose[:3] = goal_tf[0:3, 3] +606 goal_pose[3:] = mat2quat(goal_tf[0:3, 0:3]) +607 +608 idx = self.move_group_joint_indices # we need to take only the move_group joints when planning +609 +610 ik_status, goal_qpos = self.IK(goal_pose, current_qpos, mask) +611 if ik_status != "Success": +612 return {"status": ik_status} +613 +614 if verbose: +615 print("IK results:") +616 for i in range(len(goal_qpos)): +617 print(goal_qpos[i]) +618 +619 goal_qpos_ = [] +620 for i in range(len(goal_qpos)): +621 goal_qpos_.append(goal_qpos[i][idx]) +622 self.robot.set_qpos(current_qpos, True) +623 +624 ik_status, goal_qpos = self.IK(goal_pose, current_qpos, mask) +625 if ik_status != "Success": +626 return {"status": ik_status} +627 +628 if verbose: +629 print("IK results:") +630 for i in range(len(goal_qpos)): +631 print(goal_qpos[i]) +632 +633 return self.plan_qpos_to_qpos( +634 goal_qpos, +635 current_qpos, +636 time_step, +637 rrt_range, +638 planning_time, +639 fix_joint_limits, +640 use_point_cloud, +641 use_attach, +642 verbose, +643 planner_name, +644 no_simplification, +645 constraint_function, +646 constraint_jacobian, +647 constraint_tolerance +648 ) +
plan from a start configuration to a goal pose of the end-effector
+ +Args: + goal_pose: [x,y,z,qw,qx,qy,qz] pose of the goal + current_qpos: current joint configuration (either full or move_group joints) + mask: if the value at a given index is True, the joint is not used in the IK + time_step: time step for TOPPRA (time parameterization of path) + rrt_range: step size for RRT + planning_time: time limit for RRT + fix_joint_limits: if True, will clip the joint configuration to be within the joint limits + use_point_cloud: if True, will use the point cloud to avoid collision + use_attach: if True, will consider the attached tool collision when planning + verbose: if True, will print the log of OMPL and TOPPRA + wrt_world: if true, interpret the target pose with respect to the world frame instead of the base frame
+650 def plan_screw( +651 self, +652 target_pose, +653 qpos, +654 qpos_step=0.1, +655 time_step=0.1, +656 use_point_cloud=False, +657 use_attach=False, +658 verbose=False, +659 ): +660 """ +661 plan from a start configuration to a goal pose of the end-effector using screw motion +662 +663 Args: +664 target_pose: [x,y,z,qw,qx,qy,qz] pose of the goal +665 qpos: current joint configuration (either full or move_group joints) +666 qpos_step: size of the random step for RRT +667 time_step: time step for the discretization +668 use_point_cloud: if True, will use the point cloud to avoid collision +669 use_attach: if True, will use the attached tool to avoid collision +670 verbose: if True, will print the log of TOPPRA +671 """ +672 self.planning_world.set_use_point_cloud(use_point_cloud) +673 self.planning_world.set_use_attach(use_attach) +674 qpos = self.pad_qpos(qpos.copy()) +675 self.robot.set_qpos(qpos, True) +676 +677 def pose7D2mat(pose): +678 mat = np.eye(4) +679 mat[0:3, 3] = pose[:3] +680 mat[0:3, 0:3] = quat2mat(pose[3:]) +681 return mat +682 +683 def skew(vec): +684 return np.array( +685 [ +686 [0, -vec[2], vec[1]], +687 [vec[2], 0, -vec[0]], +688 [-vec[1], vec[0], 0], +689 ] +690 ) +691 +692 def pose2exp_coordinate(pose: np.ndarray) -> Tuple[np.ndarray, float]: +693 def rot2so3(rotation: np.ndarray): +694 assert rotation.shape == (3, 3) +695 if np.isclose(rotation.trace(), 3): +696 return np.zeros(3), 1 +697 if np.isclose(rotation.trace(), -1): +698 return np.zeros(3), -1e6 +699 theta = np.arccos((rotation.trace() - 1) / 2) +700 omega = ( +701 1 +702 / 2 +703 / np.sin(theta) +704 * np.array( +705 [ +706 rotation[2, 1] - rotation[1, 2], +707 rotation[0, 2] - rotation[2, 0], +708 rotation[1, 0] - rotation[0, 1], +709 ] +710 ).T +711 ) +712 return omega, theta +713 +714 omega, theta = rot2so3(pose[:3, :3]) +715 if theta < -1e5: +716 return omega, theta +717 ss = skew(omega) +718 inv_left_jacobian = ( +719 np.eye(3) / theta +720 - 0.5 * ss +721 + (1.0 / theta - 0.5 / np.tan(theta / 2)) * ss @ ss +722 ) +723 v = inv_left_jacobian @ pose[:3, 3] +724 return np.concatenate([v, omega]), theta +725 +726 self.pinocchio_model.compute_forward_kinematics(qpos) +727 ee_index = self.link_name_2_idx[self.move_group] +728 current_p = pose7D2mat(self.pinocchio_model.get_link_pose(ee_index)) +729 target_p = pose7D2mat(target_pose) +730 relative_transform = target_p @ np.linalg.inv(current_p) +731 +732 omega, theta = pose2exp_coordinate(relative_transform) +733 +734 if theta < -1e4: +735 return {"status": "screw plan failed."} +736 omega = omega.reshape((-1, 1)) * theta +737 +738 index = self.move_group_joint_indices +739 path = [np.copy(qpos[index])] +740 +741 while True: +742 self.pinocchio_model.compute_full_jacobian(qpos) +743 J = self.pinocchio_model.get_link_jacobian(ee_index, local=False) +744 delta_q = np.linalg.pinv(J) @ omega +745 delta_q *= qpos_step / (np.linalg.norm(delta_q)) +746 delta_twist = J @ delta_q +747 +748 flag = False +749 if np.linalg.norm(delta_twist) > np.linalg.norm(omega): +750 ratio = np.linalg.norm(omega) / np.linalg.norm(delta_twist) +751 delta_q = delta_q * ratio +752 delta_twist = delta_twist * ratio +753 flag = True +754 +755 qpos += delta_q.reshape(-1) +756 omega -= delta_twist +757 +758 def check_joint_limit(q): +759 n = len(q) +760 for i in range(n): +761 if ( +762 q[i] < self.joint_limits[i][0] - 1e-3 +763 or q[i] > self.joint_limits[i][1] + 1e-3 +764 ): +765 return False +766 return True +767 +768 within_joint_limit = check_joint_limit(qpos) +769 self.planning_world.set_qpos_all(qpos[index]) +770 collide = self.planning_world.collide() +771 +772 if ( +773 np.linalg.norm(delta_twist) < 1e-4 +774 or collide +775 or within_joint_limit == False +776 ): +777 return {"status": "screw plan failed"} +778 +779 path.append(np.copy(qpos[index])) +780 +781 if flag: +782 if verbose: +783 ta.setup_logging("INFO") +784 else: +785 ta.setup_logging("WARNING") +786 times, pos, vel, acc, duration = self.TOPP( +787 np.vstack(path), time_step +788 ) +789 return { +790 "status": "Success", +791 "time": times, +792 "position": pos, +793 "velocity": vel, +794 "acceleration": acc, +795 "duration": duration, +796 } +
plan from a start configuration to a goal pose of the end-effector using screw motion
+ +Args: + target_pose: [x,y,z,qw,qx,qy,qz] pose of the goal + qpos: current joint configuration (either full or move_group joints) + qpos_step: size of the random step for RRT + time_step: time step for the discretization + use_point_cloud: if True, will use the point cloud to avoid collision + use_attach: if True, will use the attached tool to avoid collision + verbose: if True, will print the log of TOPPRA
+Motion planning python binding. To see its documentation, please see the stub files in your IDE.
+MPlib is a lightweight python package for motion planning, which is decoupled from ROS and is easy to set up. With a few lines of python code, one can achieve most of the motion planning functionalities in robot manipulation.
\n\nPre-built pip packages support Ubuntu 18.04+ with Python 3.6+.
\n\npip install mplib\nSee our tutorial for detailed usage and examples.
\n"}, {"fullname": "mplib.examples", "modulename": "mplib.examples", "kind": "module", "doc": "\n"}, {"fullname": "mplib.examples.collision_avoidance", "modulename": "mplib.examples.collision_avoidance", "kind": "module", "doc": "\n"}, {"fullname": "mplib.examples.collision_avoidance.PlanningDemo", "modulename": "mplib.examples.collision_avoidance", "qualname": "PlanningDemo", "kind": "class", "doc": "The shows the planner's ability to generate a collision free path with the straight path causes collisions
\n", "bases": "mplib.examples.demo_setup.DemoSetup"}, {"fullname": "mplib.examples.collision_avoidance.PlanningDemo.__init__", "modulename": "mplib.examples.collision_avoidance", "qualname": "PlanningDemo.__init__", "kind": "function", "doc": "Same setup as demo.py except the boxes are of difference sizes and different use\nRed box is the target we want to grab\nBlue box is the obstacle we want to avoid\ngreen box is the landing pad on which we want to place the red box
\n", "signature": "()"}, {"fullname": "mplib.examples.collision_avoidance.PlanningDemo.table", "modulename": "mplib.examples.collision_avoidance", "qualname": "PlanningDemo.table", "kind": "variable", "doc": "\n"}, {"fullname": "mplib.examples.collision_avoidance.PlanningDemo.red_cube", "modulename": "mplib.examples.collision_avoidance", "qualname": "PlanningDemo.red_cube", "kind": "variable", "doc": "\n"}, {"fullname": "mplib.examples.collision_avoidance.PlanningDemo.green_cube", "modulename": "mplib.examples.collision_avoidance", "qualname": "PlanningDemo.green_cube", "kind": "variable", "doc": "\n"}, {"fullname": "mplib.examples.collision_avoidance.PlanningDemo.blue_cube", "modulename": "mplib.examples.collision_avoidance", "qualname": "PlanningDemo.blue_cube", "kind": "variable", "doc": "\n"}, {"fullname": "mplib.examples.collision_avoidance.PlanningDemo.add_point_cloud", "modulename": "mplib.examples.collision_avoidance", "qualname": "PlanningDemo.add_point_cloud", "kind": "function", "doc": "we tell the planner about the obstacle through a point cloud
\n", "signature": "(self):", "funcdef": "def"}, {"fullname": "mplib.examples.collision_avoidance.PlanningDemo.demo", "modulename": "mplib.examples.collision_avoidance", "qualname": "PlanningDemo.demo", "kind": "function", "doc": "We pick up the red box while avoiding the blue box and place it back down on top of the green box
\n", "signature": "(self, with_screw=True, use_point_cloud=True, use_attach=True):", "funcdef": "def"}, {"fullname": "mplib.examples.constrained_planning", "modulename": "mplib.examples.constrained_planning", "kind": "module", "doc": "\n"}, {"fullname": "mplib.examples.constrained_planning.ConstrainedPlanningDemo", "modulename": "mplib.examples.constrained_planning", "qualname": "ConstrainedPlanningDemo", "kind": "class", "doc": "This demo shows the planner's ability to plan with constraints.\nFor this particular demo, we move to several poses while pointing the end effector roughly 15 degrees w.r.t. -z axis
\n", "bases": "mplib.examples.demo_setup.DemoSetup"}, {"fullname": "mplib.examples.constrained_planning.ConstrainedPlanningDemo.__init__", "modulename": "mplib.examples.constrained_planning", "qualname": "ConstrainedPlanningDemo.__init__", "kind": "function", "doc": "set up the scene and load the robot
\n", "signature": "()"}, {"fullname": "mplib.examples.constrained_planning.ConstrainedPlanningDemo.add_point_cloud", "modulename": "mplib.examples.constrained_planning", "qualname": "ConstrainedPlanningDemo.add_point_cloud", "kind": "function", "doc": "add some random obstacles to make the planning more challenging
\n", "signature": "(self):", "funcdef": "def"}, {"fullname": "mplib.examples.constrained_planning.ConstrainedPlanningDemo.get_eef_z", "modulename": "mplib.examples.constrained_planning", "qualname": "ConstrainedPlanningDemo.get_eef_z", "kind": "function", "doc": "helper function for constraint
\n", "signature": "(self):", "funcdef": "def"}, {"fullname": "mplib.examples.constrained_planning.ConstrainedPlanningDemo.make_f", "modulename": "mplib.examples.constrained_planning", "qualname": "ConstrainedPlanningDemo.make_f", "kind": "function", "doc": "create a constraint function that takes in a qpos and outputs a scalar\nA valid constraint function should evaluates to 0 when the constraint is satisfied\nSee ompl constrained planning for more details
\n", "signature": "(self):", "funcdef": "def"}, {"fullname": "mplib.examples.constrained_planning.ConstrainedPlanningDemo.make_j", "modulename": "mplib.examples.constrained_planning", "qualname": "ConstrainedPlanningDemo.make_j", "kind": "function", "doc": "create the jacobian of the constraint function w.r.t. qpos\nThis is needed because the planner uses the jacobian to project a random sample to the constraint manifold
\n", "signature": "(self):", "funcdef": "def"}, {"fullname": "mplib.examples.constrained_planning.ConstrainedPlanningDemo.demo", "modulename": "mplib.examples.constrained_planning", "qualname": "ConstrainedPlanningDemo.demo", "kind": "function", "doc": "We first plan with constraints to three poses, then plan without constraints to the same poses\nWhile not always the case, sometimes without constraints, the end effector will tilt almost upside down
\n", "signature": "(self):", "funcdef": "def"}, {"fullname": "mplib.examples.demo", "modulename": "mplib.examples.demo", "kind": "module", "doc": "\n"}, {"fullname": "mplib.examples.demo.PlanningDemo", "modulename": "mplib.examples.demo", "qualname": "PlanningDemo", "kind": "class", "doc": "This is the most basic demo of the motion planning library where the robot tries to shuffle three boxes around
\n", "bases": "mplib.examples.demo_setup.DemoSetup"}, {"fullname": "mplib.examples.demo.PlanningDemo.__init__", "modulename": "mplib.examples.demo", "qualname": "PlanningDemo.__init__", "kind": "function", "doc": "Setting up the scene, the planner, and adding some objects to the scene\nAfterwards, put down a table and three boxes. For details on how to do this, see the sapien documentation
\n", "signature": "()"}, {"fullname": "mplib.examples.demo.PlanningDemo.table", "modulename": "mplib.examples.demo", "qualname": "PlanningDemo.table", "kind": "variable", "doc": "\n"}, {"fullname": "mplib.examples.demo.PlanningDemo.red_cube", "modulename": "mplib.examples.demo", "qualname": "PlanningDemo.red_cube", "kind": "variable", "doc": "\n"}, {"fullname": "mplib.examples.demo.PlanningDemo.green_cube", "modulename": "mplib.examples.demo", "qualname": "PlanningDemo.green_cube", "kind": "variable", "doc": "\n"}, {"fullname": "mplib.examples.demo.PlanningDemo.blue_cube", "modulename": "mplib.examples.demo", "qualname": "PlanningDemo.blue_cube", "kind": "variable", "doc": "\n"}, {"fullname": "mplib.examples.demo.PlanningDemo.demo", "modulename": "mplib.examples.demo", "qualname": "PlanningDemo.demo", "kind": "function", "doc": "Declare three poses for the robot to move to, each one corresponding to the position of a box\nPick up the box, and set it down 0.1m to the right of its original position
\n", "signature": "(self):", "funcdef": "def"}, {"fullname": "mplib.examples.demo_setup", "modulename": "mplib.examples.demo_setup", "kind": "module", "doc": "\n"}, {"fullname": "mplib.examples.demo_setup.DemoSetup", "modulename": "mplib.examples.demo_setup", "qualname": "DemoSetup", "kind": "class", "doc": "This class is the super class to abstract away some of the setups for the demos\nYou will need to install Sapien via pip install sapien for this to work if you want to use the viewer
Nothing to do
\n", "signature": "()"}, {"fullname": "mplib.examples.demo_setup.DemoSetup.setup_scene", "modulename": "mplib.examples.demo_setup", "qualname": "DemoSetup.setup_scene", "kind": "function", "doc": "This is the Sapien simulator setup and has nothing to do with mplib
\n", "signature": "(self, **kwargs):", "funcdef": "def"}, {"fullname": "mplib.examples.demo_setup.DemoSetup.load_robot", "modulename": "mplib.examples.demo_setup", "qualname": "DemoSetup.load_robot", "kind": "function", "doc": "This function loads a robot from a URDF file into the Sapien Scene created above.\nNote that does mean that setup_scene() must be called before this function.
\n", "signature": "(self, **kwargs):", "funcdef": "def"}, {"fullname": "mplib.examples.demo_setup.DemoSetup.setup_planner", "modulename": "mplib.examples.demo_setup", "qualname": "DemoSetup.setup_planner", "kind": "function", "doc": "Create an mplib planner using the default robot\nsee planner.py for more details on the arguments
\n", "signature": "(self, **kwargs):", "funcdef": "def"}, {"fullname": "mplib.examples.demo_setup.DemoSetup.follow_path", "modulename": "mplib.examples.demo_setup", "qualname": "DemoSetup.follow_path", "kind": "function", "doc": "Helper function to follow a path generated by the planner
\n", "signature": "(self, result):", "funcdef": "def"}, {"fullname": "mplib.examples.demo_setup.DemoSetup.set_gripper", "modulename": "mplib.examples.demo_setup", "qualname": "DemoSetup.set_gripper", "kind": "function", "doc": "Helper function to activate gripper joints\nArgs:\n pos: position of the gripper joint in real number
\n", "signature": "(self, pos):", "funcdef": "def"}, {"fullname": "mplib.examples.demo_setup.DemoSetup.open_gripper", "modulename": "mplib.examples.demo_setup", "qualname": "DemoSetup.open_gripper", "kind": "function", "doc": "\n", "signature": "(self):", "funcdef": "def"}, {"fullname": "mplib.examples.demo_setup.DemoSetup.close_gripper", "modulename": "mplib.examples.demo_setup", "qualname": "DemoSetup.close_gripper", "kind": "function", "doc": "\n", "signature": "(self):", "funcdef": "def"}, {"fullname": "mplib.examples.demo_setup.DemoSetup.move_to_pose_with_RRTConnect", "modulename": "mplib.examples.demo_setup", "qualname": "DemoSetup.move_to_pose_with_RRTConnect", "kind": "function", "doc": "Plan and follow a path to a pose using RRTConnect
\n\nArgs:\n pose: [x, y, z, qx, qy, qz, qw]\n use_point_cloud (optional): if to take the point cloud into consideration for collision checking.\n use_attach (optional): if to take the attach into consideration for collision checking.
\n", "signature": "(self, pose, use_point_cloud=False, use_attach=False):", "funcdef": "def"}, {"fullname": "mplib.examples.demo_setup.DemoSetup.move_to_pose_with_screw", "modulename": "mplib.examples.demo_setup", "qualname": "DemoSetup.move_to_pose_with_screw", "kind": "function", "doc": "Interpolative planning with screw motion.\nWill not avoid collision and will fail if the path contains collision.
\n", "signature": "(self, pose, use_point_cloud=False, use_attach=False):", "funcdef": "def"}, {"fullname": "mplib.examples.demo_setup.DemoSetup.move_to_pose", "modulename": "mplib.examples.demo_setup", "qualname": "DemoSetup.move_to_pose", "kind": "function", "doc": "API to multiplex between the two planning methods
\n", "signature": "(self, pose, with_screw=True, use_point_cloud=False, use_attach=False):", "funcdef": "def"}, {"fullname": "mplib.examples.detect_collision", "modulename": "mplib.examples.detect_collision", "kind": "module", "doc": "\n"}, {"fullname": "mplib.examples.detect_collision.DetectCollisionDemo", "modulename": "mplib.examples.detect_collision", "qualname": "DetectCollisionDemo", "kind": "class", "doc": "This demonstrates some of the collision detection functions in the planner.
\n", "bases": "mplib.examples.demo_setup.DemoSetup"}, {"fullname": "mplib.examples.detect_collision.DetectCollisionDemo.__init__", "modulename": "mplib.examples.detect_collision", "qualname": "DetectCollisionDemo.__init__", "kind": "function", "doc": "Only the planner is needed this time. No simulation env required
\n", "signature": "()"}, {"fullname": "mplib.examples.detect_collision.DetectCollisionDemo.print_collisions", "modulename": "mplib.examples.detect_collision", "qualname": "DetectCollisionDemo.print_collisions", "kind": "function", "doc": "Helper function to abstract away the printing of collisions
\n", "signature": "(self, collisions):", "funcdef": "def"}, {"fullname": "mplib.examples.detect_collision.DetectCollisionDemo.demo", "modulename": "mplib.examples.detect_collision", "qualname": "DetectCollisionDemo.demo", "kind": "function", "doc": "We test several configurations:
\n\nThis is identical to demo.py except the whole scene is shifted to the bottom right by 1 meter respectively
\n", "bases": "mplib.examples.demo_setup.DemoSetup"}, {"fullname": "mplib.examples.moving_robot.PlanningDemo.__init__", "modulename": "mplib.examples.moving_robot", "qualname": "PlanningDemo.__init__", "kind": "function", "doc": "Setting up the scene, the planner, and adding some objects to the scene\nNote that we place the robot at [1,1,0] in the simulation and also tell the planner that the robot is at [1,1,0]\nAfterwards, put down a table and three boxes. For details on how to do this, see the sapien documentation\nCompared to demo.py, all the props are shifted by 1 meter in the x and y direction
\n", "signature": "()"}, {"fullname": "mplib.examples.moving_robot.PlanningDemo.table", "modulename": "mplib.examples.moving_robot", "qualname": "PlanningDemo.table", "kind": "variable", "doc": "\n"}, {"fullname": "mplib.examples.moving_robot.PlanningDemo.red_cube", "modulename": "mplib.examples.moving_robot", "qualname": "PlanningDemo.red_cube", "kind": "variable", "doc": "\n"}, {"fullname": "mplib.examples.moving_robot.PlanningDemo.green_cube", "modulename": "mplib.examples.moving_robot", "qualname": "PlanningDemo.green_cube", "kind": "variable", "doc": "\n"}, {"fullname": "mplib.examples.moving_robot.PlanningDemo.blue_cube", "modulename": "mplib.examples.moving_robot", "qualname": "PlanningDemo.blue_cube", "kind": "variable", "doc": "\n"}, {"fullname": "mplib.examples.moving_robot.PlanningDemo.demo", "modulename": "mplib.examples.moving_robot", "qualname": "PlanningDemo.demo", "kind": "function", "doc": "Same demo as demo.py.\nAlthough we shifted everything, the poses have not changed because these are w.r.t. the robot base\nAlternatively, we can also shift the poses by 1 meter in the x and y direction and tell the planner\nthe poses are specified with respect to the world
\n", "signature": "(self):", "funcdef": "def"}, {"fullname": "mplib.examples.two_stage_motion", "modulename": "mplib.examples.two_stage_motion", "kind": "module", "doc": "\n"}, {"fullname": "mplib.examples.two_stage_motion.PlanningDemo", "modulename": "mplib.examples.two_stage_motion", "qualname": "PlanningDemo", "kind": "class", "doc": "This demo is the same as collision_avoidance.py except we added a track for the robot to move along\nWe reach the target in two stages:
\n\nWe have modified the urdf file to include a track for the robot to move along\nOtherwise, the setup is the same as collision_avoidance.py
\n", "signature": "()"}, {"fullname": "mplib.examples.two_stage_motion.PlanningDemo.table", "modulename": "mplib.examples.two_stage_motion", "qualname": "PlanningDemo.table", "kind": "variable", "doc": "\n"}, {"fullname": "mplib.examples.two_stage_motion.PlanningDemo.red_cube", "modulename": "mplib.examples.two_stage_motion", "qualname": "PlanningDemo.red_cube", "kind": "variable", "doc": "\n"}, {"fullname": "mplib.examples.two_stage_motion.PlanningDemo.green_cube", "modulename": "mplib.examples.two_stage_motion", "qualname": "PlanningDemo.green_cube", "kind": "variable", "doc": "\n"}, {"fullname": "mplib.examples.two_stage_motion.PlanningDemo.blue_cube", "modulename": "mplib.examples.two_stage_motion", "qualname": "PlanningDemo.blue_cube", "kind": "variable", "doc": "\n"}, {"fullname": "mplib.examples.two_stage_motion.PlanningDemo.add_point_cloud", "modulename": "mplib.examples.two_stage_motion", "qualname": "PlanningDemo.add_point_cloud", "kind": "function", "doc": "see collision_avoidance.py for details
\n", "signature": "(self):", "funcdef": "def"}, {"fullname": "mplib.examples.two_stage_motion.PlanningDemo.plan_without_base", "modulename": "mplib.examples.two_stage_motion", "qualname": "PlanningDemo.plan_without_base", "kind": "function", "doc": "a subroutine to plan a path without moving the base
\n", "signature": "(self, pose, has_attach=False):", "funcdef": "def"}, {"fullname": "mplib.examples.two_stage_motion.PlanningDemo.move_in_two_stage", "modulename": "mplib.examples.two_stage_motion", "qualname": "PlanningDemo.move_in_two_stage", "kind": "function", "doc": "first, we do a full IK but only generate motions for the base\nthen, do another partial IK for the arm and generate motions for the arm
\n", "signature": "(self, pose, has_attach=False):", "funcdef": "def"}, {"fullname": "mplib.examples.two_stage_motion.PlanningDemo.demo", "modulename": "mplib.examples.two_stage_motion", "qualname": "PlanningDemo.demo", "kind": "function", "doc": "We reach the pick up and drop off poses in two stages, first by moving the base only and then the arm only
\n", "signature": "(self):", "funcdef": "def"}, {"fullname": "mplib.planner", "modulename": "mplib.planner", "kind": "module", "doc": "\n"}, {"fullname": "mplib.planner.Planner", "modulename": "mplib.planner", "qualname": "Planner", "kind": "class", "doc": "Motion planner.
\n"}, {"fullname": "mplib.planner.Planner.__init__", "modulename": "mplib.planner", "qualname": "Planner.__init__", "kind": "function", "doc": "Motion planner for robots.
\n\nArgs:\n urdf: Unified Robot Description Format file.\n user_link_names: names of links, the order. if empty, all links will be used.\n user_joint_names: names of the joints to plan. if empty, all active joints will be used.\n move_group: target link to move, usually the end-effector.\n joint_vel_limits: maximum joint velocities for time parameterization,\n which should have the same length as\n joint_acc_limits: maximum joint accelerations for time parameterization,\n which should have the same length as\n srdf: Semantic Robot Description Format file.\nReferences:\n http://docs.ros.org/en/kinetic/api/moveit_tutorials/html/doc/urdf_srdf/urdf_srdf_tutorial.html
\n", "signature": "(\turdf: str,\tmove_group: str,\tsrdf: str = '',\tpackage_keyword_replacement: str = '',\tuser_link_names: Sequence[str] = [],\tuser_joint_names: Sequence[str] = [],\tjoint_vel_limits: Union[Sequence[float], numpy.ndarray] = [],\tjoint_acc_limits: Union[Sequence[float], numpy.ndarray] = [],\t**kwargs)"}, {"fullname": "mplib.planner.Planner.urdf", "modulename": "mplib.planner", "qualname": "Planner.urdf", "kind": "variable", "doc": "\n"}, {"fullname": "mplib.planner.Planner.robot", "modulename": "mplib.planner", "qualname": "Planner.robot", "kind": "variable", "doc": "\n"}, {"fullname": "mplib.planner.Planner.pinocchio_model", "modulename": "mplib.planner", "qualname": "Planner.pinocchio_model", "kind": "variable", "doc": "\n"}, {"fullname": "mplib.planner.Planner.planning_world", "modulename": "mplib.planner", "qualname": "Planner.planning_world", "kind": "variable", "doc": "\n"}, {"fullname": "mplib.planner.Planner.user_link_names", "modulename": "mplib.planner", "qualname": "Planner.user_link_names", "kind": "variable", "doc": "\n"}, {"fullname": "mplib.planner.Planner.user_joint_names", "modulename": "mplib.planner", "qualname": "Planner.user_joint_names", "kind": "variable", "doc": "\n"}, {"fullname": "mplib.planner.Planner.joint_name_2_idx", "modulename": "mplib.planner", "qualname": "Planner.joint_name_2_idx", "kind": "variable", "doc": "\n"}, {"fullname": "mplib.planner.Planner.link_name_2_idx", "modulename": "mplib.planner", "qualname": "Planner.link_name_2_idx", "kind": "variable", "doc": "\n"}, {"fullname": "mplib.planner.Planner.move_group", "modulename": "mplib.planner", "qualname": "Planner.move_group", "kind": "variable", "doc": "\n"}, {"fullname": "mplib.planner.Planner.move_group_joint_indices", "modulename": "mplib.planner", "qualname": "Planner.move_group_joint_indices", "kind": "variable", "doc": "\n"}, {"fullname": "mplib.planner.Planner.joint_types", "modulename": "mplib.planner", "qualname": "Planner.joint_types", "kind": "variable", "doc": "\n"}, {"fullname": "mplib.planner.Planner.joint_limits", "modulename": "mplib.planner", "qualname": "Planner.joint_limits", "kind": "variable", "doc": "\n"}, {"fullname": "mplib.planner.Planner.joint_vel_limits", "modulename": "mplib.planner", "qualname": "Planner.joint_vel_limits", "kind": "variable", "doc": "\n"}, {"fullname": "mplib.planner.Planner.joint_acc_limits", "modulename": "mplib.planner", "qualname": "Planner.joint_acc_limits", "kind": "variable", "doc": "\n"}, {"fullname": "mplib.planner.Planner.move_group_link_id", "modulename": "mplib.planner", "qualname": "Planner.move_group_link_id", "kind": "variable", "doc": "\n"}, {"fullname": "mplib.planner.Planner.planner", "modulename": "mplib.planner", "qualname": "Planner.planner", "kind": "variable", "doc": "\n"}, {"fullname": "mplib.planner.Planner.replace_package_keyword", "modulename": "mplib.planner", "qualname": "Planner.replace_package_keyword", "kind": "function", "doc": "some ROS URDF files use package:// keyword to refer the package dir\nreplace it with the given string (default is empty)
\n\nArgs:\n package_keyword_replacement: the string to replace 'package://' keyword
\n", "signature": "(self, package_keyword_replacement):", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.generate_collision_pair", "modulename": "mplib.planner", "qualname": "Planner.generate_collision_pair", "kind": "function", "doc": "we read the srdf file to get the link pairs that should not collide.\nif not provided, we need to randomly sample configurations to find the link pairs that will always collide.
\n", "signature": "(self, sample_time=1000000, echo_freq=100000):", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.distance_6D", "modulename": "mplib.planner", "qualname": "Planner.distance_6D", "kind": "function", "doc": "compute the distance between two poses
\n\nArgs:\n p1: position of pose 1\n q1: quaternion of pose 1\n p2: position of pose 2\n q2: quaternion of pose 2
\n", "signature": "(self, p1, q1, p2, q2):", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.check_joint_limit", "modulename": "mplib.planner", "qualname": "Planner.check_joint_limit", "kind": "function", "doc": "check if the joint configuration is within the joint limits
\n\nArgs:\n q: joint configuration
\n\nReturns:\n True if the joint configuration is within the joint limits
\n", "signature": "(self, q):", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.pad_qpos", "modulename": "mplib.planner", "qualname": "Planner.pad_qpos", "kind": "function", "doc": "if the user does not provide the full qpos but only the move_group joints,\npad the qpos with the rest of the joints
\n", "signature": "(self, qpos, articulation=None):", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.check_for_collision", "modulename": "mplib.planner", "qualname": "Planner.check_for_collision", "kind": "function", "doc": "helper function to check for collision
\n", "signature": "(\tself,\tcollision_function,\tarticulation: mplib.pymp.articulation.ArticulatedModel = None,\tqpos: numpy.ndarray = None) -> list:", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.check_for_self_collision", "modulename": "mplib.planner", "qualname": "Planner.check_for_self_collision", "kind": "function", "doc": "Check if the robot is in self-collision.
\n\nArgs:\n articulation: robot model. if none will be self.robot\n qpos: robot configuration. if none will be the current pose
\n\nReturns:\n A list of collisions.
\n", "signature": "(\tself,\tarticulation: mplib.pymp.articulation.ArticulatedModel = None,\tqpos: numpy.ndarray = None) -> list:", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.check_for_env_collision", "modulename": "mplib.planner", "qualname": "Planner.check_for_env_collision", "kind": "function", "doc": "Check if the robot is in collision with the environment
\n\nArgs:\n articulation: robot model. if none will be self.robot\n qpos: robot configuration. if none will be the current pose\n with_point_cloud: whether to check collision against point cloud\n use_attach: whether to include the object attached to the end effector in collision checking\nReturns:\n A list of collisions.
\n", "signature": "(\tself,\tarticulation: mplib.pymp.articulation.ArticulatedModel = None,\tqpos: numpy.ndarray = None,\twith_point_cloud=False,\tuse_attach=False) -> list:", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.IK", "modulename": "mplib.planner", "qualname": "Planner.IK", "kind": "function", "doc": "Inverse kinematics
\n\nArgs:\n goal_pose: [x,y,z,qw,qx,qy,qz] pose of the goal\n start_qpos: initial configuration\n mask: if the value at a given index is True, the joint is not used in the IK\n n_init_qpos: number of random initial configurations\n threshold: threshold for the distance between the goal pose and the result pose
\n", "signature": "(\tself,\tgoal_pose,\tstart_qpos,\tmask=[],\tn_init_qpos=20,\tthreshold=0.001):", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.TOPP", "modulename": "mplib.planner", "qualname": "Planner.TOPP", "kind": "function", "doc": "Time-Optimal Path Parameterization
\n\nArgs:\n path: numpy array of shape (n, dof)\n step: step size for the discretization\n verbose: if True, will print the log of TOPPRA
\n", "signature": "(self, path, step=0.1, verbose=False):", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.update_point_cloud", "modulename": "mplib.planner", "qualname": "Planner.update_point_cloud", "kind": "function", "doc": "Args:\n pc: numpy array of shape (n, 3)\n radius: radius of each point. This gives a buffer around each point that planner will avoid
\n", "signature": "(self, pc, radius=0.001):", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.update_attached_tool", "modulename": "mplib.planner", "qualname": "Planner.update_attached_tool", "kind": "function", "doc": "helper function to update the attached tool
\n", "signature": "(self, fcl_collision_geometry, pose, link_id=-1):", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.update_attached_sphere", "modulename": "mplib.planner", "qualname": "Planner.update_attached_sphere", "kind": "function", "doc": "attach a sphere to some link
\n\nArgs:\n radius: radius of the sphere\n pose: [x,y,z,qw,qx,qy,qz] pose of the sphere\n link_id: if not provided, the end effector will be the target.
\n", "signature": "(self, radius, pose, link_id=-1):", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.update_attached_box", "modulename": "mplib.planner", "qualname": "Planner.update_attached_box", "kind": "function", "doc": "attach a box to some link
\n\nArgs:\n size: [x,y,z] size of the box\n pose: [x,y,z,qw,qx,qy,qz] pose of the box\n link_id: if not provided, the end effector will be the target.
\n", "signature": "(self, size, pose, link_id=-1):", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.update_attached_mesh", "modulename": "mplib.planner", "qualname": "Planner.update_attached_mesh", "kind": "function", "doc": "attach a mesh to some link
\n\nArgs:\n mesh_path: path to the mesh\n pose: [x,y,z,qw,qx,qy,qz] pose of the mesh\n link_id: if not provided, the end effector will be the target.
\n", "signature": "(self, mesh_path, pose, link_id=-1):", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.set_base_pose", "modulename": "mplib.planner", "qualname": "Planner.set_base_pose", "kind": "function", "doc": "tell the planner where the base of the robot is w.r.t the world frame
\n\nArgs:\n pose: [x,y,z,qw,qx,qy,qz] pose of the base
\n", "signature": "(self, pose):", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.set_normal_object", "modulename": "mplib.planner", "qualname": "Planner.set_normal_object", "kind": "function", "doc": "adds or updates a non-articulated collision object in the scene
\n", "signature": "(self, name, collision_object):", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.remove_normal_object", "modulename": "mplib.planner", "qualname": "Planner.remove_normal_object", "kind": "function", "doc": "returns true if the object was removed, false if it was not found
\n", "signature": "(self, name):", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.plan_qpos_to_qpos", "modulename": "mplib.planner", "qualname": "Planner.plan_qpos_to_qpos", "kind": "function", "doc": "plan a path from a specified joint position to a goal pose
\n\nArgs:\n goal_pose: 7D pose of the end-effector [x,y,z,qw,qx,qy,qz]\n current_qpos: current joint configuration (either full or move_group joints)\n mask: mask for IK. When set, the IK will leave certain joints out of planning\n time_step: time step for TOPP\n rrt_range: step size for RRT\n planning_time: time limit for RRT\n fix_joint_limits: if True, will clip the joint configuration to be within the joint limits\n use_point_cloud: if True, will use the point cloud to avoid collision\n use_attach: if True, will consider the attached tool collision when planning\n verbose: if True, will print the log of OMPL and TOPPRA\n planner_name: planner name pick from {\"RRTConnect\", \"RRT*\"}\n fixed_joint_indices: list of indices of joints that are fixed during planning\n constraint_function: evals to 0 when constraint is satisfied\n constraint_jacobian: jacobian of constraint_function\n constraint_tolerance: tolerance for constraint_function\n no_simplification: if true, will not simplify the path. constraint planning does not support simplification
\n", "signature": "(\tself,\tgoal_qposes: list,\tcurrent_qpos,\ttime_step=0.1,\trrt_range=0.1,\tplanning_time=1,\tfix_joint_limits=True,\tuse_point_cloud=False,\tuse_attach=False,\tverbose=False,\tplanner_name='RRTConnect',\tno_simplification=False,\tconstraint_function=None,\tconstraint_jacobian=None,\tconstraint_tolerance=0.001,\tfixed_joint_indices=[]):", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.plan_qpos_to_pose", "modulename": "mplib.planner", "qualname": "Planner.plan_qpos_to_pose", "kind": "function", "doc": "plan from a start configuration to a goal pose of the end-effector
\n\nArgs:\n goal_pose: [x,y,z,qw,qx,qy,qz] pose of the goal\n current_qpos: current joint configuration (either full or move_group joints)\n mask: if the value at a given index is True, the joint is not used in the IK\n time_step: time step for TOPPRA (time parameterization of path)\n rrt_range: step size for RRT\n planning_time: time limit for RRT\n fix_joint_limits: if True, will clip the joint configuration to be within the joint limits\n use_point_cloud: if True, will use the point cloud to avoid collision\n use_attach: if True, will consider the attached tool collision when planning\n verbose: if True, will print the log of OMPL and TOPPRA\n wrt_world: if true, interpret the target pose with respect to the world frame instead of the base frame
\n", "signature": "(\tself,\tgoal_pose,\tcurrent_qpos,\tmask=[],\ttime_step=0.1,\trrt_range=0.1,\tplanning_time=1,\tfix_joint_limits=True,\tuse_point_cloud=False,\tuse_attach=False,\tverbose=False,\twrt_world=False,\tplanner_name='RRTConnect',\tno_simplification=False,\tconstraint_function=None,\tconstraint_jacobian=None,\tconstraint_tolerance=0.001):", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.plan_screw", "modulename": "mplib.planner", "qualname": "Planner.plan_screw", "kind": "function", "doc": "plan from a start configuration to a goal pose of the end-effector using screw motion
\n\nArgs:\n target_pose: [x,y,z,qw,qx,qy,qz] pose of the goal\n qpos: current joint configuration (either full or move_group joints)\n qpos_step: size of the random step for RRT\n time_step: time step for the discretization\n use_point_cloud: if True, will use the point cloud to avoid collision\n use_attach: if True, will use the attached tool to avoid collision\n verbose: if True, will print the log of TOPPRA
\n", "signature": "(\tself,\ttarget_pose,\tqpos,\tqpos_step=0.1,\ttime_step=0.1,\tuse_point_cloud=False,\tuse_attach=False,\tverbose=False):", "funcdef": "def"}, {"fullname": "mplib.pymp", "modulename": "mplib.pymp", "kind": "module", "doc": "Motion planning python binding. To see its documentation, please see the stub files in your IDE.
\n"}]; + + // mirrored in build-search-index.js (part 1) + // Also split on html tags. this is a cheap heuristic, but good enough. + elasticlunr.tokenizer.setSeperator(/[\s\-.;&_'"=,()]+|<[^>]*>/); + + let searchIndex; + if (docs._isPrebuiltIndex) { + console.info("using precompiled search index"); + searchIndex = elasticlunr.Index.load(docs); + } else { + console.time("building search index"); + // mirrored in build-search-index.js (part 2) + searchIndex = elasticlunr(function () { + this.pipeline.remove(elasticlunr.stemmer); + this.pipeline.remove(elasticlunr.stopWordFilter); + this.addField("qualname"); + this.addField("fullname"); + this.addField("annotation"); + this.addField("default_value"); + this.addField("signature"); + this.addField("bases"); + this.addField("doc"); + this.setRef("fullname"); + }); + for (let doc of docs) { + searchIndex.addDoc(doc); + } + console.timeEnd("building search index"); + } + + return (term) => searchIndex.search(term, { + fields: { + qualname: {boost: 4}, + fullname: {boost: 2}, + annotation: {boost: 2}, + default_value: {boost: 2}, + signature: {boost: 2}, + bases: {boost: 2}, + doc: {boost: 1}, + }, + expand: true + }); +})(); \ No newline at end of file diff --git a/mplib/README.md b/mplib/README.md new file mode 120000 index 0000000..32d46ee --- /dev/null +++ b/mplib/README.md @@ -0,0 +1 @@ +../README.md \ No newline at end of file diff --git a/mplib/__init__.py b/mplib/__init__.py index 1c3f8f4..477ef55 100644 --- a/mplib/__init__.py +++ b/mplib/__init__.py @@ -1,2 +1,5 @@ -from .planner import Planner +""" +.. include:: ./README.md +""" +from mplib.planner import Planner diff --git a/examples/__init__.py b/mplib/examples/__init__.py similarity index 100% rename from examples/__init__.py rename to mplib/examples/__init__.py diff --git a/examples/collision_avoidance.py b/mplib/examples/collision_avoidance.py similarity index 99% rename from examples/collision_avoidance.py rename to mplib/examples/collision_avoidance.py index e851464..f49382d 100644 --- a/examples/collision_avoidance.py +++ b/mplib/examples/collision_avoidance.py @@ -1,5 +1,5 @@ import sapien.core as sapien -from demo_setup import DemoSetup +from .demo_setup import DemoSetup class PlanningDemo(DemoSetup): """ The shows the planner's ability to generate a collision free path with the straight path causes collisions """ diff --git a/examples/constrained_planning.py b/mplib/examples/constrained_planning.py similarity index 99% rename from examples/constrained_planning.py rename to mplib/examples/constrained_planning.py index bada5a6..33e967c 100644 --- a/examples/constrained_planning.py +++ b/mplib/examples/constrained_planning.py @@ -2,7 +2,7 @@ import numpy as np import transforms3d -from demo_setup import DemoSetup +from .demo_setup import DemoSetup class ConstrainedPlanningDemo(DemoSetup): """ diff --git a/examples/demo.py b/mplib/examples/demo.py similarity index 98% rename from examples/demo.py rename to mplib/examples/demo.py index 6ebbe2d..6f47c8c 100644 --- a/examples/demo.py +++ b/mplib/examples/demo.py @@ -1,5 +1,5 @@ import sapien.core as sapien -from demo_setup import DemoSetup +from .demo_setup import DemoSetup class PlanningDemo(DemoSetup): """ diff --git a/examples/demo_setup.py b/mplib/examples/demo_setup.py similarity index 100% rename from examples/demo_setup.py rename to mplib/examples/demo_setup.py diff --git a/examples/detect_collision.py b/mplib/examples/detect_collision.py similarity index 98% rename from examples/detect_collision.py rename to mplib/examples/detect_collision.py index d1481f7..efcd75a 100644 --- a/examples/detect_collision.py +++ b/mplib/examples/detect_collision.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 import mplib -from demo_setup import DemoSetup +from .demo_setup import DemoSetup class DetectCollisionDemo(DemoSetup): """ diff --git a/examples/moving_robot.py b/mplib/examples/moving_robot.py similarity index 99% rename from examples/moving_robot.py rename to mplib/examples/moving_robot.py index dbd8036..cf354ff 100644 --- a/examples/moving_robot.py +++ b/mplib/examples/moving_robot.py @@ -1,5 +1,5 @@ import sapien.core as sapien -from demo_setup import DemoSetup +from .demo_setup import DemoSetup class PlanningDemo(DemoSetup): """ This is identical to demo.py except the whole scene is shifted to the bottom right by 1 meter respectively """ diff --git a/examples/two_stage_motion.py b/mplib/examples/two_stage_motion.py similarity index 99% rename from examples/two_stage_motion.py rename to mplib/examples/two_stage_motion.py index 5f279fe..ff70de4 100644 --- a/examples/two_stage_motion.py +++ b/mplib/examples/two_stage_motion.py @@ -1,7 +1,7 @@ import sapien.core as sapien import mplib import numpy as np -from demo_setup import DemoSetup +from .demo_setup import DemoSetup class PlanningDemo(DemoSetup): """ diff --git a/setup.py b/setup.py index e153277..bdcb67b 100644 --- a/setup.py +++ b/setup.py @@ -89,4 +89,6 @@ def build_extension(self, ext): ext_modules=[CMakeExtension("mplib.pymp")], cmdclass={"build_ext": CMakeBuild}, zip_safe=False, + package_data={"mplib": ["README.md"]}, + include_package_data=True, )