More WIP migrating robot to robot_cell

docs/examples/02_description_models/files/02_robot_from_library.py

@@ -1,8 +1,8 @@
 from compas_robots.resources import GithubPackageMeshLoader
 from compas_fab.robots import RobotCellLibrary
 
-# Load robot from RobotLibrary
-# RobotLibrary also contains .ur5(), .ur10e(), abb_irb120_3_58(), abb_irb4600_40_255(), .rfl(), .panda()
+# Load robot from RobotCellLibrary
+# RobotCellLibrary also contains .ur5(), .ur10e(), abb_irb120_3_58(), abb_irb4600_40_255(), .rfl(), .panda()
 robot_cell, robot_cell_state = RobotCellLibrary.ur5()
 robot_model = robot_cell.robot_model
 robot_semantics = robot_cell.robot_semantics

docs/examples/02_description_models/files/05_robot_model_chains.py

@@ -3,7 +3,7 @@
 from compas_fab.robots import RobotCellLibrary
 from compas_robots.model import Joint
 
-# RobotLibrary also contains .ur5(), .ur10e(), abb_irb120_3_58(), abb_irb4600_40_255(), .rfl(), .panda()
+# RobotCellLibrary also contains .ur5(), .ur10e(), abb_irb120_3_58(), abb_irb4600_40_255(), .rfl(), .panda()
 robot_cell, robot_cell_state = RobotCellLibrary.panda(load_geometry=False)
 
 model = robot_cell.robot_model

docs/examples/02_description_models/files/05_robot_viewer.py

@@ -10,8 +10,8 @@
 viewer.renderer.rendermode = "lighted"
 viewer.ui.sidedock.show = True
 
-# Load robot from RobotLibrary
-# RobotLibrary also contains .ur5(), .ur10e(), abb_irb120_3_58(), abb_irb4600_40_255(), .rfl(), .panda()
+# Load robot from RobotCellLibrary
+# RobotCellLibrary also contains .ur5(), .ur10e(), abb_irb120_3_58(), abb_irb4600_40_255(), .rfl(), .panda()
 robot_cell, robot_cell_state = RobotCellLibrary.ur5()
 model = robot_cell.robot_model
 

docs/examples/03_backends_ros/files/01_ros_test.py

@@ -30,7 +30,7 @@
     touch_links = ["flange", "tool0", "wrist_3_link"]
     rigid_body_name = "cone"
     ground_frame = Frame([1, 1, 0], [1, 0, 0], [0, 1, 0])
-    header_frame_id = robot.root_name
+    header_frame_id = robot_cell.root_name
     attachment_frame = Frame.worldXY()
 
     # --------------------------------------------------------------
@@ -48,7 +48,7 @@
 
     def _async_add_co(callback, errback):
         collision_object = CollisionObject(
-            header=Header(frame_id=robot.model.root.name),
+            header=Header(frame_id=robot_cell.root_name),
             id=rigid_body_name,
             meshes=[ROSMesh.from_mesh(cone_mesh)],
             mesh_poses=[Pose()],
@@ -122,7 +122,7 @@ def _async_detach(callback, errback):
 
         # Sets the position of stationary rigid bodies
         co = CollisionObject(
-            header=Header(frame_id=robot.model.root.name),
+            header=Header(frame_id=robot_cell.root_name),
             id=rigid_body_name,
             pose=Pose.from_frame(ground_frame),
             operation=CollisionObject.MOVE,

docs/examples/03_backends_ros/files/03_inverse_kinematics.py

@@ -8,15 +8,13 @@
 
 with RosClient() as client:
     robot_cell = client.load_robot_cell()
-    assert robot.name == "ur5_robot"
+    assert robot_cell.robot_model.name == "ur5_robot"
     planner = MoveItPlanner(client)
 
     frame_WCF = Frame([0.3, 0.1, 0.5], [1, 0, 0], [0, 1, 0])
     target = FrameTarget(frame_WCF, TargetMode.ROBOT)
 
-    start_configuration = robot.zero_configuration()
-    start_state = RobotCellState.from_robot_configuration(robot, start_configuration)
-
+    start_state = RobotCellState.from_robot_cell(robot_cell)
     configuration = planner.inverse_kinematics(target, start_state)
 
     print("Found configuration", configuration)

docs/examples/03_backends_ros/files/03_iter_inverse_kinematics.py

@@ -8,14 +8,13 @@
 
 with RosClient() as client:
     robot_cell = client.load_robot_cell()
-    assert robot.name == "ur5_robot"
+    assert robot_cell.robot_model.name == "ur5_robot"
     planner = MoveItPlanner(client)
 
     frame_WCF = Frame([0.3, 0.1, 0.5], [1, 0, 0], [0, 1, 0])
     target = FrameTarget(frame_WCF, TargetMode.ROBOT)
 
-    start_configuration = robot.zero_configuration()
-    start_state = RobotCellState.from_robot_configuration(robot, start_configuration)
+    start_state = RobotCellState.from_robot_cell(robot_cell)
 
     result_count = 0
     for config in planner.iter_inverse_kinematics(target, start_state, options=dict(max_results=10)):

docs/examples/05_backends_pybullet/files/02_forward_kinematics.py

@@ -23,7 +23,6 @@
     planner.set_robot_cell(robot_cell)
 
     configuration = Configuration.from_revolute_values([-2.238, -1.153, -2.174, 0.185, 0.667, 0.0])
-    # The `RobotCellState.from_robot_configuration` method can be used when the robot is the only element in the cell
     robot_cell_state.robot_configuration = configuration
     # In this demo, the default planning group is used for the forward kinematics
     frame_WCF = planner.forward_kinematics(robot_cell_state, TargetMode.ROBOT)

docs/examples/05_backends_pybullet/files/02_iter_inverse_kinematics.py

@@ -18,12 +18,9 @@
     frame_WCF = Frame([0.3, 0.1, 0.5], [1, 0, 0], [0, 1, 0])
     target = FrameTarget(frame_WCF, TargetMode.ROBOT)
 
-    start_configuration = robot_cell.zero_configuration()
-    start_state = RobotCellState.from_robot_configuration(robot, start_configuration)
-
     options = {"max_results": 20}
     result_count = 0
-    for config in planner.iter_inverse_kinematics(target, start_state, options=options):
+    for config in planner.iter_inverse_kinematics(target, robot_cell_state, options=options):
         print("Found configuration", config)
         result_count += 1
     print("Found %d configurations" % result_count)

docs/examples/06_backends_kinematics/files/01_forward_kinematics.py

@@ -20,7 +20,6 @@
 
 # Configuration for FK calculation
 configuration = Configuration.from_revolute_values([0.0, 4.8, 1.5, 1.1, 1.9, 3.1])
-# The `RobotCellState.from_robot_configuration` method can be used when the robot is the only element in the cell
 robot_cell_state.robot_configuration = configuration
 
 # AnalyticalKinematicsPlanner.forward_kinematics(), do not support `planning_group` parameter, it must be left as None.

docs/examples/06_backends_kinematics/files/03_iter_ik_pybullet.py

@@ -1,18 +1,22 @@
 from compas.geometry import Frame
-from compas_fab.backends import AnalyticalInverseKinematics
+from compas_fab.backends import AnalyticalPyBulletPlanner
+from compas_fab.backends import UR5Kinematics
 from compas_fab.backends import PyBulletClient
+from compas_fab.robots import RobotCellLibrary
+from compas_fab.robots import FrameTarget
+from compas_fab.robots import TargetMode
 
 frame_WCF = Frame((0.381, 0.093, 0.382), (0.371, -0.292, -0.882), (0.113, 0.956, -0.269))
 
 
 with PyBulletClient(connection_type="direct") as client:
-    robot = client.load_ur5(load_geometry=True)
+    robot_cell, robot_cell_state = RobotCellLibrary.ur5()
 
-    ik = AnalyticalInverseKinematics(client)
-    # set a new IK function
-    client.inverse_kinematics = ik.inverse_kinematics
+    kinematics = UR5Kinematics()
+    planner = AnalyticalPyBulletPlanner(client, kinematics)
 
-    options = {"solver": "ur5", "check_collision": True, "keep_order": True}
+    options = {"check_collision": True, "keep_order": True}
 
-    for config in robot.iter_inverse_kinematics(frame_WCF, options=options):
+    target = FrameTarget(frame_WCF, TargetMode.ROBOT)
+    for config in planner.iter_inverse_kinematics(target, robot_cell_state, options=options):
         print(config)

docs/examples/06_backends_kinematics/files/04_cartesian_path_analytic_pybullet.py

@@ -2,7 +2,14 @@
 from compas.geometry import Frame
 
 from compas_fab.backends import AnalyticalPyBulletClient
+from compas_fab.backends import AnalyticalPyBulletPlanner
+from compas_fab.backends import UR5Kinematics
+from compas_fab.robots import RobotCellLibrary
+
 from compas_fab.robots import FrameWaypoints
+from compas_fab.robots import FrameTarget
+from compas_fab.robots import TargetMode
+
 
 frames_WCF = [
     Frame((0.407, 0.073, 0.320), (0.922, 0.000, 0.388), (0.113, 0.956, -0.269)),
@@ -13,14 +20,16 @@
 ]
 
 with AnalyticalPyBulletClient(connection_type="direct") as client:
-    # TODO: Change to planner.iter_inverse_kinematics
+    robot_cell, robot_cell_state = RobotCellLibrary.ur5()
+
+    kinematics = UR5Kinematics()
+    planner = AnalyticalPyBulletPlanner(client, kinematics)
 
-    robot = client.load_ur5(load_geometry=True)
+    start_configuration = planner.inverse_kinematics(FrameTarget(frames_WCF[0], TargetMode.ROBOT), robot_cell_state)
+    robot_cell_state.robot_configuration = start_configuration
 
-    options = {"solver": "ur5", "check_collision": True}
-    start_configuration = list(robot.iter_inverse_kinematics(frames_WCF[0], options=options))[-1]
-    waypoints = FrameWaypoints(frames_WCF)
-    trajectory = robot.plan_cartesian_motion(waypoints, start_configuration=start_configuration, options=options)
+    waypoints = FrameWaypoints(frames_WCF, TargetMode.ROBOT)
+    trajectory = planner.plan_cartesian_motion(waypoints, robot_cell_state)
     assert trajectory.fraction == 1.0
 
     j = [c.joint_values for c in trajectory.points]

docs/examples/07_reachability_map/files/01_example_1D.py

@@ -1,14 +1,18 @@
-import os
 import math
-from compas.geometry import Point
-from compas.geometry import Plane
+import os
+
 from compas.geometry import Frame
+from compas.geometry import Plane
+from compas.geometry import Point
 from compas.geometry import Sphere
 
-from compas_fab.backends import AnalyticalInverseKinematics
-from compas_fab.backends import PyBulletClient
-from compas_fab.robots import ReachabilityMap
 from compas_fab import DATA
+from compas_fab.backends import AnalyticalPyBulletClient
+from compas_fab.backends import AnalyticalPyBulletPlanner
+from compas_fab.backends import UR5Kinematics
+from compas_fab.robots import ReachabilityMap
+from compas_fab.robots import RobotCellLibrary
+from compas_fab.robots import TargetMode
 
 # 1. Define frames on a sphere
 sphere = Sphere((0.4, 0, 0), 0.15)
@@ -32,14 +36,15 @@ def points_on_sphere_generator(sphere):
 
 # 3. Create reachability map 1D
 
-with PyBulletClient(connection_type='direct') as client:
+with AnalyticalPyBulletClient(connection_type="direct") as client:
     # load robot and define settings
-    robot = client.load_ur5(load_geometry=True)
-    ik = AnalyticalInverseKinematics(client)
-    client.inverse_kinematics = ik.inverse_kinematics
+    robot_cell, robot_cell_state = RobotCellLibrary.ur5()
+    planner = AnalyticalPyBulletPlanner(client, UR5Kinematics())
+    planner.set_robot_cell(robot_cell, robot_cell_state)
+
     options = {"solver": "ur5", "check_collision": True, "keep_order": True}
     # calculate reachability map
     map = ReachabilityMap()
-    map.calculate(points_on_sphere_generator(sphere), robot, options)
+    map.calculate(points_on_sphere_generator(sphere), planner, robot_cell_state, TargetMode.ROBOT, options)
     # save to json
     map.to_json(os.path.join(DATA, "reachability", "map1D.json"))

docs/examples/07_reachability_map/files/02_example_2D_deviation_vectors.py

@@ -1,16 +1,19 @@
-import os
 import math
-from compas.geometry import Point
-from compas.geometry import Plane
+import os
+
 from compas.geometry import Frame
+from compas.geometry import Plane
+from compas.geometry import Point
 from compas.geometry import Sphere
 
-from compas_fab.backends import AnalyticalInverseKinematics
-from compas_fab.backends import PyBulletClient
+from compas_fab import DATA
+from compas_fab.backends import AnalyticalPyBulletClient
+from compas_fab.backends import AnalyticalPyBulletPlanner
+from compas_fab.backends import UR5Kinematics
 from compas_fab.robots import ReachabilityMap
+from compas_fab.robots import RobotCellLibrary
+from compas_fab.robots import TargetMode
 from compas_fab.robots.reachability_map import DeviationVectorsGenerator
-from compas_fab import DATA
-
 
 # 1. Define generators
 
@@ -38,24 +41,26 @@ def deviation_vector_generator(frame):
         yaxis = frame.zaxis.cross(xaxis)
         yield Frame(frame.point, xaxis, yaxis)
 
+
 # 2. Create 2D generator
 
 
 def generator():
     for frame in points_on_sphere_generator(sphere):
         yield deviation_vector_generator(frame)
 
-# 3. Create reachability map 2D
 
+# 3. Create reachability map 2D
 
-with PyBulletClient(connection_type='direct') as client:
+with AnalyticalPyBulletClient(connection_type="direct") as client:
     # load robot and define settings
-    robot = client.load_ur5(load_geometry=True)
-    ik = AnalyticalInverseKinematics(client)
-    client.inverse_kinematics = ik.inverse_kinematics
+    robot_cell, robot_cell_state = RobotCellLibrary.ur5()
+    planner = AnalyticalPyBulletPlanner(client, UR5Kinematics())
+    planner.set_robot_cell(robot_cell, robot_cell_state)
+
     options = {"solver": "ur5", "check_collision": True, "keep_order": True}
     # calculate reachability map
     map = ReachabilityMap()
-    map.calculate(generator(), robot, options)
+    map.calculate(generator(), planner, robot_cell_state, TargetMode.ROBOT, options)
     # save to json
     map.to_json(os.path.join(DATA, "reachability", "map2D_deviation.json"))

docs/examples/07_reachability_map/files/03_example_2D_sphere_points.py

@@ -1,15 +1,19 @@
-import os
 import math
-from compas.geometry import Point
-from compas.geometry import Vector
-from compas.geometry import Plane
+import os
+
 from compas.geometry import Frame
+from compas.geometry import Plane
+from compas.geometry import Point
 from compas.geometry import Sphere
+from compas.geometry import Vector
 
-from compas_fab.backends import AnalyticalInverseKinematics
-from compas_fab.backends import PyBulletClient
-from compas_fab.robots import ReachabilityMap
 from compas_fab import DATA
+from compas_fab.backends import AnalyticalPyBulletClient
+from compas_fab.backends import AnalyticalPyBulletPlanner
+from compas_fab.backends import UR5Kinematics
+from compas_fab.robots import ReachabilityMap
+from compas_fab.robots import RobotCellLibrary
+from compas_fab.robots import TargetMode
 
 # 1. Define generators
 
@@ -37,24 +41,26 @@ def sphere_generator():
             center = sphere.point + Vector(x, 0, z) * 0.05
             yield Sphere(center, sphere.radius)
 
+
 # 2. Create 2D generator
 
 
 def generator():
     for sphere in sphere_generator():
         yield points_on_sphere_generator(sphere)
 
-# 3. Create reachability map 2D
 
+# 3. Create reachability map 2D
 
-with PyBulletClient(connection_type='direct') as client:
+with AnalyticalPyBulletClient(connection_type="direct") as client:
     # load robot and define settings
-    robot = client.load_ur5(load_geometry=True)
-    ik = AnalyticalInverseKinematics(client)
-    client.inverse_kinematics = ik.inverse_kinematics
+    robot_cell, robot_cell_state = RobotCellLibrary.ur5()
+    planner = AnalyticalPyBulletPlanner(client, UR5Kinematics())
+    planner.set_robot_cell(robot_cell, robot_cell_state)
+
     options = {"solver": "ur5", "check_collision": True, "keep_order": True}
     # calculate reachability map
     map = ReachabilityMap()
-    map.calculate(generator(), robot, options)
+    map.calculate(generator(), planner, robot_cell_state, TargetMode.ROBOT, options)
     # save to json
     map.to_json(os.path.join(DATA, "reachability", "map2D_spheres.json"))