From c652a395965f824ad0f9cb32de57358c382d61ed Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Sun, 20 Oct 2024 00:06:16 +0800 Subject: [PATCH] WIP Migration to RobotCell --- CHANGELOG.md | 5 +- .../files/02_robot_from_library.py | 10 +- .../files/02_robot_from_ros.py | 4 +- .../files/02_robot_from_ros_with_cache.py | 4 +- .../files/03_robot_rhino_from_ros.py | 4 +- .../files/05_robot_model_chains.py | 14 +- .../files/05_robot_viewer.py | 8 +- .../01_robot_cell_with_kinematic_tools.py | 6 +- .../files/01_ros_robot_cell_with_tools.py | 11 +- .../files/01_ros_set_robot_cell.py | 6 +- .../03_backends_ros/files/01_ros_test.py | 2 +- .../03_backends_ros/files/02_robot_model.py | 6 +- .../files/03_forward_kinematics.py | 4 +- .../files/03_forward_kinematics_urdf.py | 6 +- .../files/03_inverse_kinematics.py | 2 +- .../files/03_iter_inverse_kinematics.py | 2 +- .../files/04_plan_cartesian_motion.py | 5 +- ..._cartesian_motion_with_attached_objects.py | 6 +- .../03_backends_ros/files/04_plan_motion.py | 8 +- .../files/gh_robot_visualisation.py | 8 +- .../files/01_robot_cell_with_tools.py | 10 +- .../files/02_forward_kinematics with_tools.py | 2 +- .../files/02_forward_kinematics.py | 9 +- .../files/02_inverse_kinematics.py | 9 +- ...2_inverse_kinematics_multiple_solutions.py | 9 +- .../02_inverse_kinematics_semi_constrained.py | 7 +- .../files/02_iter_inverse_kinematics.py | 8 +- .../files/01_forward_kinematics.py | 7 +- .../files/02_inverse_kinematics with_tools.py | 10 +- .../files/02_inverse_kinematics.py | 5 +- .../files/03_analytical_pybullet_planner.py | 8 +- scripts/extract_robot_package_from_ros.py | 6 +- .../analytical_forward_kinematics.py | 3 +- .../analytical_plan_cartesian_motion.py | 23 +- .../analytical_pybullet_inverse_kinematics.py | 3 +- .../pybullet_check_collision.py | 11 +- .../pybullet_forward_kinematics.py | 16 +- .../pybullet_inverse_kinematics.py | 37 +- .../pybullet_plan_cartesian_motion.py | 24 +- .../pybullet_set_robot_cell.py | 11 +- src/compas_fab/backends/pybullet/client.py | 26 +- .../move_it_forward_kinematics.py | 12 +- .../move_it_inverse_kinematics.py | 52 +- .../move_it_plan_cartesian_motion.py | 13 +- src/compas_fab/backends/ros/client.py | 24 +- .../components/Cf_VisualizeRobot/code.py | 2 +- .../components/Cf_VisualizeTrajectory/code.py | 7 +- src/compas_fab/robots/__init__.py | 5 - src/compas_fab/robots/conftest.py | 4 +- src/compas_fab/robots/constraints.py | 18 +- src/compas_fab/robots/robot.py | 1753 +---------------- src/compas_fab/robots/robot_cell.py | 42 +- src/compas_fab/robots/robot_library.py | 70 +- src/compas_fab/robots/semantics.py | 6 +- src/compas_fab/robots/trajectory.py | 14 +- .../test_analytical_kinematics_planner.py | 7 +- .../test_analytical_pybullet_planner.py | 4 +- .../backends/pybullet/test_pybullet_client.py | 122 +- .../pybullet/test_pybullet_planner_fk_ik.py | 99 +- ..._pybullet_planner_plan_cartesian_motion.py | 2 +- tests/robots/obsolete_test_robot.py | 9 +- tests/robots/test_robot_library.py | 4 +- tests/robots/test_semantics.py | 8 +- 63 files changed, 478 insertions(+), 2164 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 97463f3e6..2e5b609c8 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -40,7 +40,10 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 * Changed GH Component `ConstraintsFromTargetConfiguration` to `ConfigurationTarget`. ### Removed - +* Removed `attached_collision_meshes` attribute from `JointTrajectory` class. +* Removed `Robot.merge_group_with_full_configuration` as it can be covered by `Configuration.merged`. +* Removed function `Robot.get_group_names_from_link_name` as it is too oddly specific. +* Removed `Robot.get_position_by_joint_name` as Configuration class can now be directly accessed by joint name. * Removed `Tool` class from `compas_fab.robots` module. * Removed Backend Feature `AddCollisionMesh`, `AppendCollisionMesh`, `AddAttachedCollisionMesh`, `RemoveCollisionMesh` and `RemoveAttachedCollisionMesh`. * Removed `inverse_kinematics`, `forward_kinematics`, `plan_cartesian_motion`, and `plan_motion` methods from ClientInterface, access them using the planner instead. diff --git a/docs/examples/02_description_models/files/02_robot_from_library.py b/docs/examples/02_description_models/files/02_robot_from_library.py index 7ea9c9c63..aed5e501d 100644 --- a/docs/examples/02_description_models/files/02_robot_from_library.py +++ b/docs/examples/02_description_models/files/02_robot_from_library.py @@ -1,10 +1,10 @@ from compas_robots.resources import GithubPackageMeshLoader -from compas_fab.robots import RobotLibrary +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() -robot = RobotLibrary.ur5() -model = robot.model +robot_cell, robot_cell_state = RobotCellLibrary.ur5() +robot_model = robot_cell.robot_model +robot_semantics = robot_cell.robot_semantics -# Also load geometry -print(model) +print(robot_model) diff --git a/docs/examples/02_description_models/files/02_robot_from_ros.py b/docs/examples/02_description_models/files/02_robot_from_ros.py index 16710a370..ea23b69cc 100644 --- a/docs/examples/02_description_models/files/02_robot_from_ros.py +++ b/docs/examples/02_description_models/files/02_robot_from_ros.py @@ -1,6 +1,6 @@ from compas_fab.backends import RosClient with RosClient() as ros: - robot = ros.load_robot(load_geometry=True, precision=12) + robot_cell = ros.load_robot_cell(load_geometry=True, precision=12) - print(robot.model) + print(robot_cell.robot_model) diff --git a/docs/examples/02_description_models/files/02_robot_from_ros_with_cache.py b/docs/examples/02_description_models/files/02_robot_from_ros_with_cache.py index fe9c7d6ed..b20dd6e0e 100644 --- a/docs/examples/02_description_models/files/02_robot_from_ros_with_cache.py +++ b/docs/examples/02_description_models/files/02_robot_from_ros_with_cache.py @@ -5,6 +5,6 @@ with RosClient() as ros: # Load complete model from ROS and set a local cache location local_directory = os.path.join(os.path.expanduser("~"), "robot_description", "robot_name") - robot = ros.load_robot(load_geometry=True, local_cache_directory=local_directory, precision=12) + robot_cell = ros.load_robot_cell(load_geometry=True, local_cache_directory=local_directory, precision=12) - print(robot.model) + robot_cell.print_info() diff --git a/docs/examples/02_description_models/files/03_robot_rhino_from_ros.py b/docs/examples/02_description_models/files/03_robot_rhino_from_ros.py index 67d54881b..af186e0ae 100644 --- a/docs/examples/02_description_models/files/03_robot_rhino_from_ros.py +++ b/docs/examples/02_description_models/files/03_robot_rhino_from_ros.py @@ -3,9 +3,9 @@ with RosClient() as ros: # Load complete model from ROS - robot = ros.load_robot(load_geometry=True, precision=12) + robot_cell = ros.load_robot_cell(load_geometry=True, precision=12) # Visualize robot scene = Scene() - scene_object = scene.add(robot.model) + scene_object = scene.add(robot_cell) scene.draw() diff --git a/docs/examples/02_description_models/files/05_robot_model_chains.py b/docs/examples/02_description_models/files/05_robot_model_chains.py index f268d009d..349d8fda6 100644 --- a/docs/examples/02_description_models/files/05_robot_model_chains.py +++ b/docs/examples/02_description_models/files/05_robot_model_chains.py @@ -1,12 +1,12 @@ # This example file demonstrates how to print the chain of links and joints in a robot model. -from compas_fab.robots import RobotLibrary +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() -robot = RobotLibrary.panda(load_geometry=False) +robot_cell, robot_cell_state = RobotCellLibrary.panda(load_geometry=False) -model = robot.model +model = robot_cell.robot_model print("Robot Model Chain:") # ---------------------------------------------- @@ -38,12 +38,12 @@ def print_joint(joint, level=1): def print_planning_group_chain(group): print("Planning Group: {}".format(group)) - base_link = robot.get_base_link_name(group) + base_link = robot_cell.get_base_link_name(group) print("Base Link: {}".format(base_link)) - tip_link = robot.get_end_effector_link_name(group) + tip_link = robot_cell.get_end_effector_link_name(group) print("Tip Link: {}".format(tip_link)) print("--------------------") - group_object = robot.semantics.groups[group] + group_object = robot_cell.robot_semantics.groups[group] joints_in_group = group_object["joints"] links_in_group = group_object["links"] @@ -84,5 +84,5 @@ def print_joint(joint, level=1): print("") -for group in robot.semantics.groups: +for group in robot_cell.robot_semantics.groups: print_planning_group_chain(group) diff --git a/docs/examples/02_description_models/files/05_robot_viewer.py b/docs/examples/02_description_models/files/05_robot_viewer.py index fc521ba92..3462a17bb 100644 --- a/docs/examples/02_description_models/files/05_robot_viewer.py +++ b/docs/examples/02_description_models/files/05_robot_viewer.py @@ -1,5 +1,5 @@ # This example does not work with IronPython -from compas_fab.robots import RobotLibrary +from compas_fab.robots import RobotCellLibrary from compas_robots.viewer.scene.robotmodelobject import RobotModelObject from compas_robots.model import Joint from compas_viewer.components import Slider @@ -12,8 +12,8 @@ # Load robot from RobotLibrary # RobotLibrary also contains .ur5(), .ur10e(), abb_irb120_3_58(), abb_irb4600_40_255(), .rfl(), .panda() -robot = RobotLibrary.ur5() -model = robot.model +robot_cell, robot_cell_state = RobotCellLibrary.ur5() +model = robot_cell.robot_model start_configuration = model.zero_configuration() robot_object: RobotModelObject = viewer.scene.add(model, show_lines=False, configuration=start_configuration) # type: ignore @@ -30,7 +30,7 @@ def rotate(slider: Slider, value: float): # Create one slider for each joint -for joint in robot.get_configurable_joints(): +for joint in robot_cell.get_configurable_joints(): starting_val = start_configuration[joint.name] print(joint.name, Joint.SUPPORTED_TYPES[joint.type], joint.limit.lower, joint.limit.upper, starting_val) # Units are in radians or meters diff --git a/docs/examples/03_backends_ros/files/01_robot_cell_with_kinematic_tools.py b/docs/examples/03_backends_ros/files/01_robot_cell_with_kinematic_tools.py index 4c172c229..0aef8bd5a 100644 --- a/docs/examples/03_backends_ros/files/01_robot_cell_with_kinematic_tools.py +++ b/docs/examples/03_backends_ros/files/01_robot_cell_with_kinematic_tools.py @@ -7,15 +7,13 @@ from compas_fab.robots import ToolLibrary with RosClient() as client: - robot = client.load_robot() + robot_cell = client.load_robot_cell() planner = MoveItPlanner(client) # ========= # Example 1 # ========= - # Create a robot cell using the robot from the client - robot_cell = RobotCell(robot) # Add a kinematic gripper tool to the robot cell gripper = ToolLibrary.kinematic_gripper() robot_cell.tool_models[gripper.name] = gripper @@ -23,7 +21,7 @@ robot_cell_state = RobotCellState.from_robot_cell(robot_cell) robot_cell_state.set_tool_attached_to_group( gripper.name, - robot.main_group_name, + robot_cell.main_group_name, attachment_frame=Frame([0.0, 0.0, 0.0], [0.0, 0.0, 1.0], [1.0, 0.0, 0.0]), touch_links=["wrist_3_link"], # This is the link that the tool is attached to ) diff --git a/docs/examples/03_backends_ros/files/01_ros_robot_cell_with_tools.py b/docs/examples/03_backends_ros/files/01_ros_robot_cell_with_tools.py index f1913ecd6..9050509be 100644 --- a/docs/examples/03_backends_ros/files/01_ros_robot_cell_with_tools.py +++ b/docs/examples/03_backends_ros/files/01_ros_robot_cell_with_tools.py @@ -11,7 +11,7 @@ from compas_fab.robots import RigidBody with RosClient() as client: - robot = client.load_robot() + robot_cell = client.load_robot_cell() planner = MoveItPlanner(client) # ========= @@ -19,7 +19,6 @@ # ========= # Create a robot cell with the robot from the client - robot_cell = RobotCell(robot) # Add a floor as rigid body to the robot cell, this will be static. floor_mesh = Mesh.from_stl(compas_fab.get("planning_scene/floor.stl")) robot_cell.rigid_body_models["floor"] = RigidBody.from_mesh(floor_mesh) @@ -34,7 +33,7 @@ # In order to see the tool attached it is necessary to update the robot_cell_state robot_cell_state = RobotCellState.from_robot_cell(robot_cell) # Modify the tool state to attach the cone to the robot - robot_cell_state.tool_states["cone"].attached_to_group = robot.main_group_name + robot_cell_state.tool_states["cone"].attached_to_group = robot_cell.main_group_name robot_cell_state.tool_states["cone"].attachment_frame = Frame([0.0, 0.0, 0.0], [0.0, 0.0, 1.0], [1.0, 0.0, 0.0]) # Specify the link of the robot that the tool is allowed to collide with robot_cell_state.tool_states["cone"].touch_links = ["wrist_3_link"] @@ -54,7 +53,7 @@ # Demonstrate that it is possible to have multiple tools in the robot cell # Create a robot cell with the robot from the client - robot_cell = RobotCell(robot) + robot_cell = client.load_robot_cell() # Add a two tools (gripper and cone) to the robot cell gripper = ToolLibrary.static_gripper_small() robot_cell.tool_models[gripper.name] = gripper @@ -65,14 +64,14 @@ # It will also ensure that only one tool is attached to the specified group by removing others robot_cell_state.set_tool_attached_to_group( gripper.name, - robot.main_group_name, + robot_cell.main_group_name, attachment_frame=Frame([0.0, 0.0, 0.0], [0.0, 0.0, 1.0], [1.0, 0.0, 0.0]), touch_links=["wrist_3_link"], ) # Specify the location of the detached cone tool robot_cell_state.tool_states[cone_tool.name].frame = Frame([1.0, 1.0, 0.0], [0.0, 0.0, 1.0], [1.0, 0.0, 0.0]) # Move the robot to a different configuration - robot_cell_state.robot_configuration = robot.zero_configuration() + robot_cell_state.robot_configuration = robot_cell.zero_configuration() robot_cell_state.robot_configuration._joint_values[1] = -0.5 robot_cell_state.robot_configuration._joint_values[2] = 0.5 result = planner.set_robot_cell(robot_cell, robot_cell_state) diff --git a/docs/examples/03_backends_ros/files/01_ros_set_robot_cell.py b/docs/examples/03_backends_ros/files/01_ros_set_robot_cell.py index 971326339..3e013c569 100644 --- a/docs/examples/03_backends_ros/files/01_ros_set_robot_cell.py +++ b/docs/examples/03_backends_ros/files/01_ros_set_robot_cell.py @@ -7,15 +7,13 @@ from compas_fab.robots import RigidBody with RosClient() as client: - robot = client.load_robot() + robot_cell = client.load_robot_cell() planner = MoveItPlanner(client) # ========= # Example 1 # ========= - # Create a robot cell with a floor geometry - robot_cell = RobotCell(robot) floor_mesh = Mesh.from_stl(compas_fab.get("planning_scene/floor.stl")) robot_cell.rigid_body_models["floor"] = RigidBody.from_mesh(floor_mesh) @@ -30,7 +28,7 @@ # ========= # Create another robot cell with a cone geometry - robot_cell = RobotCell(robot) + robot_cell = client.load_robot_cell() cone = Mesh.from_stl(compas_fab.get("planning_scene/cone.stl")).scaled(5) robot_cell.rigid_body_models["cone"] = RigidBody.from_mesh(cone) diff --git a/docs/examples/03_backends_ros/files/01_ros_test.py b/docs/examples/03_backends_ros/files/01_ros_test.py index 30ab6dd13..0c6e9d5db 100644 --- a/docs/examples/03_backends_ros/files/01_ros_test.py +++ b/docs/examples/03_backends_ros/files/01_ros_test.py @@ -9,7 +9,7 @@ from compas_fab.robots import RobotCellState with RosClient() as client: - robot = client.load_robot() + robot_cell = client.load_robot_cell() planner = MoveItPlanner(client) # ========= diff --git a/docs/examples/03_backends_ros/files/02_robot_model.py b/docs/examples/03_backends_ros/files/02_robot_model.py index c299a92e8..ab271e95b 100644 --- a/docs/examples/03_backends_ros/files/02_robot_model.py +++ b/docs/examples/03_backends_ros/files/02_robot_model.py @@ -1,7 +1,7 @@ from compas_fab.backends import RosClient with RosClient() as client: - robot = client.load_robot() - robot.info() + robot_cell = client.load_robot_cell() + robot_cell.print_info() - assert robot.name == 'ur5_robot' + assert robot_cell.robot_model.name == "ur5_robot" diff --git a/docs/examples/03_backends_ros/files/03_forward_kinematics.py b/docs/examples/03_backends_ros/files/03_forward_kinematics.py index 145b07139..4fbe35a01 100644 --- a/docs/examples/03_backends_ros/files/03_forward_kinematics.py +++ b/docs/examples/03_backends_ros/files/03_forward_kinematics.py @@ -2,8 +2,8 @@ from compas_fab.backends import RosClient with RosClient() as client: - robot = client.load_robot() - print(robot.name) + robot_cell = client.load_robot_cell() + print(robot_cell.robot_model.name) assert robot.name == "ur5_robot" configuration = Configuration.from_revolute_values([-2.238, -1.153, -2.174, 0.185, 0.667, 0.0]) diff --git a/docs/examples/03_backends_ros/files/03_forward_kinematics_urdf.py b/docs/examples/03_backends_ros/files/03_forward_kinematics_urdf.py index 02d4eb645..b9bdf42d6 100644 --- a/docs/examples/03_backends_ros/files/03_forward_kinematics_urdf.py +++ b/docs/examples/03_backends_ros/files/03_forward_kinematics_urdf.py @@ -1,10 +1,10 @@ from compas_robots import Configuration -from compas_fab.robots import RobotLibrary +from compas_fab.robots import RobotCellLibrary -robot = RobotLibrary.ur5() +robot_cell, robot_cell_state = RobotCellLibrary.ur5() configuration = Configuration.from_revolute_values([-2.238, -1.153, -2.174, 0.185, 0.667, 0.0]) -frame_WCF = robot.forward_kinematics(configuration) +frame_WCF = robot_cell.robot_model.forward_kinematics(configuration) print("Frame in the world coordinate system") print(str(frame_WCF.point)) diff --git a/docs/examples/03_backends_ros/files/03_inverse_kinematics.py b/docs/examples/03_backends_ros/files/03_inverse_kinematics.py index 509e6e1bd..a8e26af4f 100644 --- a/docs/examples/03_backends_ros/files/03_inverse_kinematics.py +++ b/docs/examples/03_backends_ros/files/03_inverse_kinematics.py @@ -7,7 +7,7 @@ from compas_fab.robots import TargetMode with RosClient() as client: - robot = client.load_robot() + robot_cell = client.load_robot_cell() assert robot.name == "ur5_robot" planner = MoveItPlanner(client) diff --git a/docs/examples/03_backends_ros/files/03_iter_inverse_kinematics.py b/docs/examples/03_backends_ros/files/03_iter_inverse_kinematics.py index a671ed78c..cf05af155 100644 --- a/docs/examples/03_backends_ros/files/03_iter_inverse_kinematics.py +++ b/docs/examples/03_backends_ros/files/03_iter_inverse_kinematics.py @@ -7,7 +7,7 @@ from compas_fab.robots import TargetMode with RosClient() as client: - robot = client.load_robot() + robot_cell = client.load_robot_cell() assert robot.name == "ur5_robot" planner = MoveItPlanner(client) diff --git a/docs/examples/03_backends_ros/files/04_plan_cartesian_motion.py b/docs/examples/03_backends_ros/files/04_plan_cartesian_motion.py index 66021f71e..a2eacf1b7 100644 --- a/docs/examples/03_backends_ros/files/04_plan_cartesian_motion.py +++ b/docs/examples/03_backends_ros/files/04_plan_cartesian_motion.py @@ -8,12 +8,11 @@ from compas_fab.robots import RobotCellState with RosClient() as client: - robot = client.load_robot() planner = MoveItPlanner(client) - robot_cell = RobotCell(robot) + robot_cell = client.load_robot_cell() robot_cell_state = RobotCellState.from_robot_cell(robot_cell) - assert robot.name == "ur5_robot" + assert robot_cell.robot_model.name == "ur5_robot" frames = [] frames.append(Frame([0.3, 0.1, 0.5], [1, 0, 0], [0, 1, 0])) diff --git a/docs/examples/03_backends_ros/files/04_plan_cartesian_motion_with_attached_objects.py b/docs/examples/03_backends_ros/files/04_plan_cartesian_motion_with_attached_objects.py index 4c5c764db..e38efe6ef 100644 --- a/docs/examples/03_backends_ros/files/04_plan_cartesian_motion_with_attached_objects.py +++ b/docs/examples/03_backends_ros/files/04_plan_cartesian_motion_with_attached_objects.py @@ -21,15 +21,13 @@ target_frames.append(Frame([0.0, -0.4, 0.45], [1, 0, 0], [0, 1, 0])) with RosClient() as client: - robot = client.load_robot() + robot_cell = client.load_robot_cell() planner = MoveItPlanner(client) # ===================================== # Step 1: Creation of Robot Cell # ===================================== - # Create a robot cell using the robot from the client - robot_cell = RobotCell(robot) # Add a rigid body for the floor floor_mesh = Mesh.from_stl(compas_fab.get("planning_scene/floor.stl")) robot_cell.rigid_body_models["floor"] = RigidBody.from_mesh(floor_mesh) @@ -55,7 +53,7 @@ robot_cell_state = RobotCellState.from_robot_cell(robot_cell) robot_cell_state.set_tool_attached_to_group( gripper.name, - robot.main_group_name, + robot_cell.main_group_name, attachment_frame=Frame([0.0, 0.0, 0.0], [0.0, 0.0, 1.0], [1.0, 0.0, 0.0]), touch_links=["wrist_3_link"], # This is the link that the tool is attached to ) diff --git a/docs/examples/03_backends_ros/files/04_plan_motion.py b/docs/examples/03_backends_ros/files/04_plan_motion.py index 23305248c..ec48057f7 100644 --- a/docs/examples/03_backends_ros/files/04_plan_motion.py +++ b/docs/examples/03_backends_ros/files/04_plan_motion.py @@ -8,17 +8,17 @@ from compas_fab.backends import MoveItPlanner with RosClient() as client: - robot = client.load_robot() + robot_cell = client.load_robot_cell() planner = MoveItPlanner(client) - assert robot.name == "ur5_robot" + assert robot_cell.robot_model.name == "ur5_robot" frame = Frame([0.4, 0.3, 0.4], [0, 1, 0], [0, 0, 1]) tolerance_position = 0.001 tolerance_orientation = math.radians(1) - start_configuration = robot.zero_configuration() + start_configuration = robot_cell.zero_configuration() start_configuration.joint_values = (-3.530, 3.830, -0.580, -3.330, 4.760, 0.000) - group = robot.main_group_name + group = robot_cell.main_group_name # create target from frame target = FrameTarget( diff --git a/docs/examples/03_backends_ros/files/gh_robot_visualisation.py b/docs/examples/03_backends_ros/files/gh_robot_visualisation.py index e8ddee5e5..8d4d4c227 100644 --- a/docs/examples/03_backends_ros/files/gh_robot_visualisation.py +++ b/docs/examples/03_backends_ros/files/gh_robot_visualisation.py @@ -11,8 +11,6 @@ Whether or not to show the robot's collision meshes. show_frames: bool Whether or not to show the robot's joint frames. - show_base_frame: bool - Whether or not to show the robot's base frame. show_end_effector_frame: bool Whether or not to show the robot's end-effector frame. Output: @@ -22,6 +20,7 @@ base_frame_WCF: The robot's base frame. ee_frame_WCF: The robot's end-effector frame. """ + from __future__ import print_function if robot and full_configuration: @@ -40,9 +39,6 @@ axes = robot.transformed_axes(full_configuration, group) frames = robot.transformed_frames(full_configuration, group) - if show_base_frame: - base_frame_WCF = robot.get_base_frame(group, full_configuration) - if show_end_effector_frame: - ee_frame_WCF = robot.get_end_effector_frame(group, full_configuration) + ee_frame_WCF = robot.model.forward_kinematics(full_configuration) print("End-effector", ee_frame_WCF) diff --git a/docs/examples/05_backends_pybullet/files/01_robot_cell_with_tools.py b/docs/examples/05_backends_pybullet/files/01_robot_cell_with_tools.py index 7e252a298..68cc2b88c 100644 --- a/docs/examples/05_backends_pybullet/files/01_robot_cell_with_tools.py +++ b/docs/examples/05_backends_pybullet/files/01_robot_cell_with_tools.py @@ -6,7 +6,7 @@ from compas_fab.robots import RobotCell from compas_fab.robots import RobotCellState from compas_fab.robots import RigidBody -from compas_fab.robots import RobotLibrary +from compas_fab.robots import RobotCellLibrary from compas.geometry import Frame from compas.geometry import Box from compas_robots import ToolModel @@ -15,8 +15,7 @@ # --------------------------------------------------------------------- # Create a robot cell and add objects to it # --------------------------------------------------------------------- - robot = RobotLibrary.abb_irb4600_40_255() - robot_cell = RobotCell(robot) + robot_cell, robot_cell_state = RobotCellLibrary.abb_irb4600_40_255() # Add Static Collision Geometry floor_mesh = Mesh.from_stl(compas_fab.get("planning_scene/floor.stl")) @@ -34,15 +33,14 @@ # ------------------------------------------------------------------------ # Create a RobotCellState to represent the current state of the robot cell # ------------------------------------------------------------------------ - robot_cell_state = RobotCellState.from_robot_cell(robot_cell) # Change the robot's configuration for demonstration purposes - configuration = robot.zero_configuration() + configuration = robot_cell.zero_configuration() configuration.joint_values[1] = 0.5 # Change the second joint angle to 0.5 [rad] robot_cell_state.robot_configuration = configuration # Attach the tool to the robot's main group - robot_cell_state.set_tool_attached_to_group("cone", robot.main_group_name) + robot_cell_state.set_tool_attached_to_group("cone", robot_cell.main_group_name) # Attach the workpiece to the tool workpiece_grasp_frame = Frame([0, 0, 0.1], [1, 0, 0], [0, 1, 0]) diff --git a/docs/examples/05_backends_pybullet/files/02_forward_kinematics with_tools.py b/docs/examples/05_backends_pybullet/files/02_forward_kinematics with_tools.py index 9008929d3..00c95742a 100644 --- a/docs/examples/05_backends_pybullet/files/02_forward_kinematics with_tools.py +++ b/docs/examples/05_backends_pybullet/files/02_forward_kinematics with_tools.py @@ -7,7 +7,7 @@ from compas_fab.robots import TargetMode from compas_fab.robots import RobotCellState from compas_fab.robots import RigidBody -from compas_fab.robots import RobotLibrary +from compas_fab.robots import RobotCellLibrary from compas.geometry import Frame from compas.geometry import Box from compas_robots import Configuration diff --git a/docs/examples/05_backends_pybullet/files/02_forward_kinematics.py b/docs/examples/05_backends_pybullet/files/02_forward_kinematics.py index 37459712d..845aaf549 100644 --- a/docs/examples/05_backends_pybullet/files/02_forward_kinematics.py +++ b/docs/examples/05_backends_pybullet/files/02_forward_kinematics.py @@ -4,14 +4,14 @@ from compas_fab.backends import PyBulletPlanner from compas_fab.robots import RobotCell from compas_fab.robots import RobotCellState -from compas_fab.robots import RobotLibrary +from compas_fab.robots import RobotCellLibrary from compas_fab.robots import TargetMode # Starting the PyBulletClient with the "direct" mode means that the GUI is not shown with PyBulletClient("direct") as client: # The robot in this example is loaded from a URDF file - robot = RobotLibrary.ur5() + robot_cell, robot_cell_state = RobotCellLibrary.ur5() # The planner object is needed to call the forward kinematics function planner = PyBulletPlanner(client) @@ -20,12 +20,11 @@ # FK without tools # ---------------- # This is a simple robot cell with only the robot - robot_cell = RobotCell(robot) 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 = RobotCellState.from_robot_configuration(robot, configuration) + 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) @@ -36,6 +35,6 @@ # --------------------------------- # FK for all the links in the robot # --------------------------------- - for link_name in robot.get_link_names(): + for link_name in robot_cell.get_link_names(): frame_WCF = planner.forward_kinematics_to_link(robot_cell_state, link_name) print("Frame of link '{}' : {}".format((link_name, frame_WCF))) diff --git a/docs/examples/05_backends_pybullet/files/02_inverse_kinematics.py b/docs/examples/05_backends_pybullet/files/02_inverse_kinematics.py index 10b23648f..dbe2eb167 100644 --- a/docs/examples/05_backends_pybullet/files/02_inverse_kinematics.py +++ b/docs/examples/05_backends_pybullet/files/02_inverse_kinematics.py @@ -5,21 +5,16 @@ from compas_fab.robots import FrameTarget from compas_fab.robots import RobotCell from compas_fab.robots import RobotCellState -from compas_fab.robots import RobotLibrary +from compas_fab.robots import RobotCellLibrary from compas_fab.robots import TargetMode with PyBulletClient() as client: # Create a robot cell with a UR5 robot - robot = RobotLibrary.ur5() - robot_cell = RobotCell(robot) + robot_cell, robot_cell_state = RobotCellLibrary.ur5() planner = PyBulletPlanner(client) planner.set_robot_cell(robot_cell) - # Create a RobotCellState from the robot's zero configuration - start_configuration = robot.zero_configuration() - robot_cell_state = RobotCellState.from_robot_configuration(robot, start_configuration) - # The FrameTarget represents the robot's planner coordinate frame (PCF) # For the UR5 robot, the PCF is equal to the frame of the 'tool0' link frame_WCF = Frame([0.3, 0.1, 0.5], [1, 0, 0], [0, 1, 0]) diff --git a/docs/examples/05_backends_pybullet/files/02_inverse_kinematics_multiple_solutions.py b/docs/examples/05_backends_pybullet/files/02_inverse_kinematics_multiple_solutions.py index c2b91696e..47ba5f0cc 100644 --- a/docs/examples/05_backends_pybullet/files/02_inverse_kinematics_multiple_solutions.py +++ b/docs/examples/05_backends_pybullet/files/02_inverse_kinematics_multiple_solutions.py @@ -5,23 +5,18 @@ from compas_fab.robots import FrameTarget from compas_fab.robots import RobotCell from compas_fab.robots import RobotCellState -from compas_fab.robots import RobotLibrary +from compas_fab.robots import RobotCellLibrary from compas_fab.robots import TargetMode with PyBulletClient() as client: # This example uses the panda robot, which has 7 joints. # When given a FrameTarget, the robot will have multiple inverse kinematics solutions. - robot = RobotLibrary.panda() + robot_cell, robot_cell_state = RobotCellLibrary.panda() # Set RobotCell using the planner function - robot_cell = RobotCell(robot) planner = PyBulletPlanner(client) planner.set_robot_cell(robot_cell) - # Create initial state - start_configuration = robot.zero_configuration() - robot_cell_state = RobotCellState.from_robot_configuration(robot, start_configuration) - # Create target frame_WCF = Frame([0.5, 0.1, 0.5], [1, 0, 0], [0, 1, 0]) target = FrameTarget(frame_WCF, TargetMode.ROBOT) diff --git a/docs/examples/05_backends_pybullet/files/02_inverse_kinematics_semi_constrained.py b/docs/examples/05_backends_pybullet/files/02_inverse_kinematics_semi_constrained.py index 1db702c9a..88e5934e5 100644 --- a/docs/examples/05_backends_pybullet/files/02_inverse_kinematics_semi_constrained.py +++ b/docs/examples/05_backends_pybullet/files/02_inverse_kinematics_semi_constrained.py @@ -13,21 +13,18 @@ from compas_fab.robots import RobotCellLibrary from compas_fab.robots import RigidBodyLibrary from compas_fab.robots import RobotCellState -from compas_fab.robots import RobotLibrary +from compas_fab.robots import RobotCellLibrary from compas_fab.robots import TargetMode # NOTE: The semi-constrained IK mode cannot be used with tools with PyBulletClient() as client: # Create a robot cell with a UR5 robot - robot = RobotLibrary.ur5() - robot_cell = RobotCell(robot) + robot_cell, robot_cell_state = RobotCellLibrary.ur5() # Add a target marker to visualize the target robot_cell.rigid_body_models["target_marker"] = RigidBodyLibrary.target_marker(size=0.15) - robot_cell_state = RobotCellState.from_robot_cell(robot_cell) - planner = PyBulletPlanner(client) planner.set_robot_cell(robot_cell) diff --git a/docs/examples/05_backends_pybullet/files/02_iter_inverse_kinematics.py b/docs/examples/05_backends_pybullet/files/02_iter_inverse_kinematics.py index 9d53402d5..4678ee12d 100644 --- a/docs/examples/05_backends_pybullet/files/02_iter_inverse_kinematics.py +++ b/docs/examples/05_backends_pybullet/files/02_iter_inverse_kinematics.py @@ -6,19 +6,19 @@ from compas_fab.robots import FrameTarget from compas_fab.robots import RobotCell from compas_fab.robots import RobotCellState -from compas_fab.robots import RobotLibrary +from compas_fab.robots import RobotCellLibrary from compas_fab.robots import TargetMode with PyBulletClient(connection_type="direct") as client: - robot = RobotLibrary.ur5() + robot_cell, robot_cell_state = RobotCellLibrary.ur5() planner = PyBulletPlanner(client) - planner.set_robot_cell(RobotCell(robot)) + planner.set_robot_cell(robot_cell) 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_configuration = robot_cell.zero_configuration() start_state = RobotCellState.from_robot_configuration(robot, start_configuration) options = {"max_results": 20} diff --git a/docs/examples/06_backends_kinematics/files/01_forward_kinematics.py b/docs/examples/06_backends_kinematics/files/01_forward_kinematics.py index 8207deceb..1b9cb452f 100644 --- a/docs/examples/06_backends_kinematics/files/01_forward_kinematics.py +++ b/docs/examples/06_backends_kinematics/files/01_forward_kinematics.py @@ -1,6 +1,6 @@ from compas.geometry import Frame from compas_robots import Configuration -from compas_fab.robots import RobotLibrary +from compas_fab.robots import RobotCellLibrary from compas_fab.backends import AnalyticalInverseKinematics from compas_fab.backends import AnalyticalKinematicsPlanner from compas_fab.backends.kinematics.solvers import UR5Kinematics @@ -10,19 +10,18 @@ from compas_fab.robots import RobotCellState # Not loading the robot's geometry because AnalyticalKinematicsPlanner does not use it for collision checking -robot = RobotLibrary.ur5(load_geometry=False) +robot_cell, robot_cell_state = RobotCellLibrary.ur5(load_geometry=False) # The kinematics_solver must match the robot's kinematics planner = AnalyticalKinematicsPlanner(UR5Kinematics()) # This is a simple robot cell with only the robot -robot_cell = RobotCell(robot) planner.set_robot_cell(robot_cell) # 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 = RobotCellState.from_robot_configuration(robot, configuration) +robot_cell_state.robot_configuration = configuration # AnalyticalKinematicsPlanner.forward_kinematics(), do not support `planning_group` parameter, it must be left as None. # The `link` option is also not supported and cannot be used. diff --git a/docs/examples/06_backends_kinematics/files/02_inverse_kinematics with_tools.py b/docs/examples/06_backends_kinematics/files/02_inverse_kinematics with_tools.py index b789ce182..c5000c60e 100644 --- a/docs/examples/06_backends_kinematics/files/02_inverse_kinematics with_tools.py +++ b/docs/examples/06_backends_kinematics/files/02_inverse_kinematics with_tools.py @@ -3,7 +3,7 @@ from compas.geometry import Box import compas_fab -from compas_fab.robots import RobotLibrary +from compas_fab.robots import RobotCellLibrary from compas_fab.robots import FrameTarget from compas_fab.robots import RigidBody from compas_fab.robots import RigidBodyLibrary @@ -18,7 +18,7 @@ from compas_fab.robots import RobotCellState # Not loading the robot's geometry because AnalyticalKinematicsPlanner does not use it for collision checking -robot = RobotLibrary.ur5(load_geometry=False) +robot_cell, robot_cell_state = RobotCellLibrary.ur5(load_geometry=False) # The kinematics_solver must match the robot's kinematics planner = AnalyticalKinematicsPlanner(UR5Kinematics()) @@ -26,7 +26,6 @@ # --------------------------------------------------------------------- # Create a robot cell and add objects to it # --------------------------------------------------------------------- -robot_cell = RobotCell(robot) # Add Static Collision Geometry floor_mesh = Mesh.from_stl(compas_fab.get("planning_scene/floor.stl")) @@ -40,9 +39,6 @@ # The robot cell is passed to the planner planner.set_robot_cell(robot_cell) -# Create robot cell state, the default state from (.from_robot_cell) does not attach tools to the robot -robot_cell_state = RobotCellState.from_robot_cell(robot_cell) - # ----------------- # Define the target # ----------------- @@ -65,7 +61,7 @@ # Second demonstrate the IK with tools # ------------------------------------ # Modify the cell state to attach the tool to the robot -robot_cell_state.set_tool_attached_to_group("cone", robot.main_group_name) +robot_cell_state.set_tool_attached_to_group("cone", robot_cell.main_group_name) # Create a target with TargetMode.TOOL target = FrameTarget(target_frame, TargetMode.TOOL) config = planner.inverse_kinematics(target, robot_cell_state) diff --git a/docs/examples/06_backends_kinematics/files/02_inverse_kinematics.py b/docs/examples/06_backends_kinematics/files/02_inverse_kinematics.py index cf4a75326..c782a2f4c 100644 --- a/docs/examples/06_backends_kinematics/files/02_inverse_kinematics.py +++ b/docs/examples/06_backends_kinematics/files/02_inverse_kinematics.py @@ -1,5 +1,5 @@ from compas.geometry import Frame -from compas_fab.robots import RobotLibrary +from compas_fab.robots import RobotCellLibrary from compas_fab.robots import FrameTarget from compas_fab.robots import TargetMode @@ -11,13 +11,12 @@ from compas_fab.robots import RobotCellState # Not loading the robot's geometry because AnalyticalKinematicsPlanner does not use it for collision checking -robot = RobotLibrary.ur5(load_geometry=False) +robot_cell, robot_cell_state = RobotCellLibrary.ur5(load_geometry=False) # The kinematics_solver must match the robot's kinematics planner = AnalyticalKinematicsPlanner(UR5Kinematics()) # This is a simple robot cell with only the robot -robot_cell = RobotCell(robot) planner.set_robot_cell(robot_cell) # IK Target diff --git a/docs/examples/06_backends_kinematics/files/03_analytical_pybullet_planner.py b/docs/examples/06_backends_kinematics/files/03_analytical_pybullet_planner.py index 2d1bd320b..1b7d87481 100644 --- a/docs/examples/06_backends_kinematics/files/03_analytical_pybullet_planner.py +++ b/docs/examples/06_backends_kinematics/files/03_analytical_pybullet_planner.py @@ -10,20 +10,19 @@ from compas_robots import ToolModel from compas_robots import Configuration from compas_fab.robots import RobotCellState -from compas_fab.robots import RobotLibrary +from compas_fab.robots import RobotCellLibrary from compas_fab.robots import RobotCell from compas_fab.robots import FrameTarget from compas_fab.robots import TargetMode with AnalyticalPyBulletClient(connection_type="gui") as client: - robot = RobotLibrary.ur5(load_geometry=True) + robot_cell, robot_cell_state = RobotCellLibrary.ur5(load_geometry=True) planner = AnalyticalPyBulletPlanner(client, UR5Kinematics()) # --------------------------------------------------------------------- # Create a robot cell and add objects to it # --------------------------------------------------------------------- - robot_cell = RobotCell(robot) # Add Static Collision Geometry floor_mesh = Mesh.from_stl(compas_fab.get("planning_scene/floor.stl")) @@ -41,10 +40,9 @@ # ------------------------------------------------------------------------ # Create a RobotCellState to represent the current state of the robot cell # ------------------------------------------------------------------------ - robot_cell_state = RobotCellState.from_robot_cell(robot_cell) # Attach the tool to the robot's main group - robot_cell_state.set_tool_attached_to_group("cone", robot.main_group_name) + robot_cell_state.set_tool_attached_to_group("cone", robot_cell.main_group_name) # Attach the workpiece to the tool workpiece_grasp_frame = Frame([0, 0, 0.1], [1, 0, 0], [0, 1, 0]) diff --git a/scripts/extract_robot_package_from_ros.py b/scripts/extract_robot_package_from_ros.py index a98978820..0cd1d4d7e 100644 --- a/scripts/extract_robot_package_from_ros.py +++ b/scripts/extract_robot_package_from_ros.py @@ -33,9 +33,9 @@ with RosClient() as client: # Standard way to load a robot from a MoveIt! instance. - robot = client.load_robot() - robot.info() - print(robot.name) + robot_cell = client.load_robot_cell() + robot_cell.print_info() + print(robot_cell.robot_model.name) # The RosFileServerLoader is used with a modified local_cache_directory argument local_cache_directory = os.path.join(HERE, "robot_packages") diff --git a/src/compas_fab/backends/kinematics/backend_features/analytical_forward_kinematics.py b/src/compas_fab/backends/kinematics/backend_features/analytical_forward_kinematics.py index e7969a785..7b8b4f8bb 100644 --- a/src/compas_fab/backends/kinematics/backend_features/analytical_forward_kinematics.py +++ b/src/compas_fab/backends/kinematics/backend_features/analytical_forward_kinematics.py @@ -30,9 +30,8 @@ def forward_kinematics(self, robot_cell_state, target_mode, scale=None, group=No """Calculate the forward kinematics for a given joint configuration.""" planner = self # type: AnalyticalKinematicsPlanner robot_cell = self.client.robot_cell # type: RobotCell - robot = self.client.robot_cell.robot # type: Robot - if group is not None and group != robot.main_group_name: + if group is not None and group != robot_cell.main_group_name: # NOTE: Analytical IK must operate on all the joints as defined in the KinematicsSolver # There is a chance that the planning group specified by the URDF does not match with these joints # At the moment there is no automatic check for this except to assume that the main group diff --git a/src/compas_fab/backends/kinematics/backend_features/analytical_plan_cartesian_motion.py b/src/compas_fab/backends/kinematics/backend_features/analytical_plan_cartesian_motion.py index a04453870..f6f8db978 100644 --- a/src/compas_fab/backends/kinematics/backend_features/analytical_plan_cartesian_motion.py +++ b/src/compas_fab/backends/kinematics/backend_features/analytical_plan_cartesian_motion.py @@ -10,6 +10,14 @@ from compas_fab.robots import FrameWaypoints from compas_fab.robots import PointAxisWaypoints +from compas import IPY + +if not IPY: + from typing import TYPE_CHECKING + + if TYPE_CHECKING: + from compas_fab.backends import AnalyticalPyBulletPlanner # noqa: F401 + class AnalyticalPlanCartesianMotion(PlanCartesianMotion): """ """ @@ -55,6 +63,7 @@ def plan_cartesian_motion(self, robot, waypoints, start_configuration=None, grou def _plan_cartesian_motion_with_frame_waypoints( self, robot, waypoints, start_configuration=None, group=None, options=None ): + # type: (Robot, FrameWaypoints, Configuration, str, dict) -> JointTrajectory """Calculates a cartesian motion path with frame waypoints. Planner behavior: @@ -62,14 +71,18 @@ def _plan_cartesian_motion_with_frame_waypoints( - The path is checked to ensure that the joint values are continuous and that revolution values are the smallest possible. - There is no interpolation in between frames (i.e. 'max_step' parameter is not supported), only the input frames are used. """ + planner = self # type: AnalyticalPyBulletPlanner + robot_cell = planner.client.robot_cell + waypoints = waypoints.normalized_to_meters() + # TODO: This function needs some serious rewrites as the continuity check is a bit flawed. + # TODO: Convert the target frames to the robot's base frame using new method in planner class - frames_WCF = waypoints.target_frames + target_frames = waypoints.target_frames # convert the frame WCF to RCF - base_frame = robot.get_base_frame(group=group, full_configuration=start_configuration) - frames_RCF = [base_frame.to_local_coordinates(frame_WCF) for frame_WCF in frames_WCF] + pcf_frames = robot_cell.target_frames_to_pcf(start_configuration, target_frames, waypoints.target_mode, group) # 'keep_order' is set to True, so that iter_inverse_kinematics will return the configurations in the same order across all frames options = options or {} @@ -78,7 +91,7 @@ def _plan_cartesian_motion_with_frame_waypoints( # iterate over all input frames and calculate the inverse kinematics, no interpolation in between frames configurations_along_path = [] # TODO: Change to planner.iter_inverse_kinematics - for frame in frames_RCF: + for frame in pcf_frames: configurations = list(robot.iter_inverse_kinematics(frame, options=options)) configurations_along_path.append(configurations) @@ -105,7 +118,7 @@ def _plan_cartesian_motion_with_frame_waypoints( path = paths[idx] path = self.smooth_configurations(path) trajectory = JointTrajectory() - trajectory.fraction = len(path) / len(frames_RCF) + trajectory.fraction = len(path) / len(pcf_frames) # Technically trajectory.fraction should always be 1.0 because otherwise, the path would be rejected earlier trajectory.joint_names = path[0].joint_names trajectory.points = [JointTrajectoryPoint(config.joint_values, config.joint_types) for config in path] diff --git a/src/compas_fab/backends/kinematics/backend_features/analytical_pybullet_inverse_kinematics.py b/src/compas_fab/backends/kinematics/backend_features/analytical_pybullet_inverse_kinematics.py index 24a28393e..ad227b096 100644 --- a/src/compas_fab/backends/kinematics/backend_features/analytical_pybullet_inverse_kinematics.py +++ b/src/compas_fab/backends/kinematics/backend_features/analytical_pybullet_inverse_kinematics.py @@ -49,8 +49,9 @@ def iter_inverse_kinematics(self, target, start_state=None, group=None, options= """ + client = self.client # type: PyBulletClient - group = group or self.client.robot.main_group_name + group = group or client.robot_cell.main_group_name if isinstance(target, FrameTarget): return self.iter_inverse_kinematics_frame_target( diff --git a/src/compas_fab/backends/pybullet/backend_features/pybullet_check_collision.py b/src/compas_fab/backends/pybullet/backend_features/pybullet_check_collision.py index 99236e285..2c28872b8 100644 --- a/src/compas_fab/backends/pybullet/backend_features/pybullet_check_collision.py +++ b/src/compas_fab/backends/pybullet/backend_features/pybullet_check_collision.py @@ -145,8 +145,8 @@ def verbose_print(msg): collision_messages.append(cc_pair_info + " - COLLISION") collision_pairs.append( ( - client.robot.model.get_link_by_name(link_1_name), - client.robot.model.get_link_by_name(link_2_name), + client.robot_cell.robot_model.get_link_by_name(link_1_name), + client.robot_cell.robot_model.get_link_by_name(link_2_name), ) ) if not full_report: # Fail on first error if full_report is not requested @@ -177,7 +177,10 @@ def verbose_print(msg): verbose_print(cc_pair_info + " - COLLISION") collision_messages.append(cc_pair_info + " - COLLISION") collision_pairs.append( - (client.robot.model.get_link_by_name(link_name), client.robot_cell.tool_models[tool_name]) + ( + client.robot_cell.robot_model.get_link_by_name(link_name), + client.robot_cell.tool_models[tool_name], + ) ) if not full_report: # Fail on first error if full_report is not requested raise CollisionCheckError("\n".join(collision_messages), collision_pairs) @@ -208,7 +211,7 @@ def verbose_print(msg): collision_messages.append(cc_pair_info + " (body_id '{}') - COLLISION".format(body_id)) collision_pairs.append( ( - client.robot.model.get_link_by_name(link_name), + client.robot_cell.robot_model.get_link_by_name(link_name), client.robot_cell.rigid_body_models[body_name], ) ) diff --git a/src/compas_fab/backends/pybullet/backend_features/pybullet_forward_kinematics.py b/src/compas_fab/backends/pybullet/backend_features/pybullet_forward_kinematics.py index a41866ff8..97f104521 100644 --- a/src/compas_fab/backends/pybullet/backend_features/pybullet_forward_kinematics.py +++ b/src/compas_fab/backends/pybullet/backend_features/pybullet_forward_kinematics.py @@ -17,7 +17,7 @@ from compas_fab.backends import PyBulletClient # noqa: F401 from compas_fab.backends import PyBulletPlanner # noqa: F401 - from compas_fab.robots import Robot # noqa: F401 + from compas_fab.robots import RobotCell # noqa: F401 from compas_fab.robots import RobotCellState # noqa: F401 from compas_fab.robots import TargetMode # noqa: F401 @@ -75,21 +75,21 @@ def forward_kinematics(self, robot_cell_state, target_mode, group=None, scale=No # Housekeeping for intellisense planner = self # type: PyBulletPlanner client = planner.client # type: PyBulletClient - robot = client.robot # type: Robot - group = group or robot.main_group_name + robot_cell = client.robot_cell # type: RobotCell + group = group or robot_cell.main_group_name # Check if the target mode is valid for the robot cell state planner.ensure_robot_cell_state_supports_target_mode(robot_cell_state, target_mode, group) # Check if the planning group is supported by the planner - if group not in robot.group_names: + if group not in robot_cell.group_names: raise PlanningGroupNotExistsError("Planning group '{}' is not supported by PyBullet planner.".format(group)) # Setting the entire robot cell state, including the robot configuration planner.set_robot_cell_state(robot_cell_state) # Retrieve the PCF of the group - link_name = robot.get_end_effector_link_name(group) + link_name = robot_cell.get_end_effector_link_name(group) link_id = client.robot_link_puids[link_name] pcf_frame = client._get_link_frame(link_id, client.robot_puid) @@ -136,11 +136,11 @@ def forward_kinematics_to_link(self, robot_cell_state, link_name=None, group=Non # Housekeeping for intellisense planner = self # type: PyBulletPlanner client = planner.client # type: PyBulletClient - robot = client.robot # type: Robot - group = group or robot.main_group_name + robot_cell = client.robot_cell # type: RobotCell + group = group or robot_cell.main_group_name # Check if the planning group is supported by the planner - if group not in robot.group_names: + if group not in robot_cell.group_names: raise PlanningGroupNotExistsError("Planning group '{}' is not supported by PyBullet planner.".format(group)) # Setting the entire robot cell state, including the robot configuration diff --git a/src/compas_fab/backends/pybullet/backend_features/pybullet_inverse_kinematics.py b/src/compas_fab/backends/pybullet/backend_features/pybullet_inverse_kinematics.py index 4af335615..5fed20f5d 100644 --- a/src/compas_fab/backends/pybullet/backend_features/pybullet_inverse_kinematics.py +++ b/src/compas_fab/backends/pybullet/backend_features/pybullet_inverse_kinematics.py @@ -16,7 +16,7 @@ from compas_fab.robots import RobotCellState # noqa: F401 from compas_fab.robots import Target # noqa: F401 - from compas_fab.robots import Robot # noqa: F401 + from compas_fab.robots import RobotCell # noqa: F401 from compas_fab.backends import PyBulletClient # noqa: F401 from compas_fab.backends import PyBulletPlanner # noqa: F401 @@ -107,7 +107,8 @@ def inverse_kinematics(self, target, robot_cell_state, group=None, options=None) """ # Set default group name planner = self # type: PyBulletPlanner - group = group or planner.client.robot.main_group_name + client = planner.client # type: PyBulletClient + group = group or client.robot_cell.main_group_name # Make a copy of the options because we will modify it # Note: Modifying the options dict accidentally will break the hashing function in the inverse_kinematics() @@ -161,8 +162,8 @@ def iter_inverse_kinematics(self, target, robot_cell_state, group=None, options= # =================================================================================== planner = self # type: PyBulletPlanner - robot = planner.client.robot # type: Robot - group = group or robot.main_group_name + robot_cell = planner.client.robot_cell # type: RobotCell + group = group or robot_cell.main_group_name # Unit conversion from user scale to meter scale can be done here because they are shared. # This will be triggered too, when entering from the inverse_kinematics method. @@ -172,7 +173,7 @@ def iter_inverse_kinematics(self, target, robot_cell_state, group=None, options= planner.ensure_robot_cell_state_supports_target_mode(robot_cell_state, target.target_mode, group) # Check if the planning group is supported by the planner - if group not in robot.group_names: + if group not in robot_cell.group_names: raise PlanningGroupNotExistsError("Planning group '{}' is not supported by PyBullet planner.".format(group)) # =================================================================================== @@ -269,10 +270,10 @@ def iter_inverse_kinematics_frame_target(self, target, robot_cell_state, group, # Housekeeping for intellisense planner = self # type: PyBulletPlanner client = planner.client # type: PyBulletClient - robot = client.robot # type: Robot + robot_cell = client.robot_cell # type: RobotCell # NOTE: group is not optional in this inner function. - if group not in robot.semantics.groups: + if group not in robot_cell.robot_semantics.groups: raise ValueError("Planning group '{}' not found in the robot's semantics.".format(group)) # Default options @@ -300,7 +301,7 @@ def iter_inverse_kinematics_frame_target(self, target, robot_cell_state, group, # Formatting input for PyBullet body_id = client.robot_puid # Note: The target link is the last link in semantics.groups[group]["links"][-1] - link_id = client.robot_link_puids[robot.get_end_effector_link_name(group)] + link_id = client.robot_link_puids[robot_cell.get_end_effector_link_name(group)] point, orientation = pose_from_frame(target_pcf) # Get list of joint_name and puids in order @@ -313,7 +314,7 @@ def iter_inverse_kinematics_frame_target(self, target, robot_cell_state, group, # The order of the values needs to match with pybullet's joint id order # Start configuration needs to be a full_configuration, not negotiable. start_configuration = robot_cell_state.robot_configuration - all_joint_names = robot.model.get_configurable_joint_names() + all_joint_names = robot_cell.get_configurable_joint_names() assert set(all_joint_names) == set(start_configuration.keys()), "Robot configuration is missing some joints" rest_poses = client.build_pose_for_pybullet(start_configuration) @@ -322,7 +323,7 @@ def iter_inverse_kinematics_frame_target(self, target, robot_cell_state, group, lower_limits = [] upper_limits = [] for joint_name in joint_names_sorted: - joint = robot.get_joint_by_name(joint_name) + joint = robot_cell.robot_model.get_joint_by_name(joint_name) lower_limits.append(joint.limit.lower if joint.type != Joint.CONTINUOUS else 0) upper_limits.append(joint.limit.upper if joint.type != Joint.CONTINUOUS else 2 * math.pi) @@ -351,7 +352,7 @@ def iter_inverse_kinematics_frame_target(self, target, robot_cell_state, group, def set_random_config(): # Function for setting random joint values for randomized search - config = robot.random_configuration(group) + config = robot_cell.random_configuration(group) client.set_robot_configuration(config) # The uniqueness checker keep track of past results @@ -529,10 +530,10 @@ def iter_inverse_kinematics_point_axis_target(self, target, robot_cell_state, gr # Housekeeping for intellisense planner = self # type: PyBulletPlanner client = planner.client # type: PyBulletClient - robot = client.robot # type: Robot + robot_cell = client.robot_cell # type: RobotCell # NOTE: group is not optional in this inner function. - if group not in robot.semantics.groups: + if group not in robot_cell.robot_semantics.groups: raise ValueError("Planning group '{}' not found in the robot's semantics.".format(group)) # Default options (specific to PointAxisTarget) @@ -602,7 +603,7 @@ def compute_initial_frame(configuration): def set_random_config(): # Function for setting random joint values for randomized search - start_configuration = robot.random_configuration(group) + start_configuration = robot_cell.random_configuration(group) client.set_robot_configuration(start_configuration) # Options dict for passing to the FrameTarget function @@ -781,9 +782,9 @@ def _check_configuration_match_group(self, start_configuration, configuration, g ValueError If the configuration has changed joints that are not in the group. """ - robot = self.client.robot # type: Robot + robot_cell = self.client.robot_cell # type: RobotCell - configurable_joints = robot.get_configurable_joint_names(group) + configurable_joints = robot_cell.get_configurable_joint_names(group) for joint_name, joint_value in configuration.items(): if joint_name not in start_configuration: raise KeyError( @@ -792,8 +793,8 @@ def _check_configuration_match_group(self, start_configuration, configuration, g if joint_name in configurable_joints: continue if not TOL.is_close(joint_value, start_configuration[joint_name]): - joint_names = robot.semantics.groups[group]["joints"] - link_names = robot.semantics.groups[group]["links"] + joint_names = robot_cell.robot_semantics.groups[group]["joints"] + link_names = robot_cell.robot_semantics.groups[group]["links"] print("Configuration changed joint '{}' that is not in the group.".format(joint_name)) raise PlanningGroupNotSupported(group, joint_names, link_names) diff --git a/src/compas_fab/backends/pybullet/backend_features/pybullet_plan_cartesian_motion.py b/src/compas_fab/backends/pybullet/backend_features/pybullet_plan_cartesian_motion.py index 5e7c653bd..26a82af80 100644 --- a/src/compas_fab/backends/pybullet/backend_features/pybullet_plan_cartesian_motion.py +++ b/src/compas_fab/backends/pybullet/backend_features/pybullet_plan_cartesian_motion.py @@ -44,7 +44,7 @@ from compas_fab.backends import PyBulletClient # noqa: F401 from compas_fab.backends import PyBulletPlanner # noqa: F401 - from compas_fab.robots import Robot # noqa: F401 + from compas_fab.robots import RobotCell # noqa: F401 from compas_fab.robots import RobotCellState # noqa: F401 from compas_fab.robots import Waypoints # noqa: F401 @@ -153,8 +153,8 @@ def plan_cartesian_motion(self, waypoints, start_state, group=None, options=None planner = self # type: PyBulletPlanner client = planner.client # type: PyBulletClient - robot = client.robot # type: Robot - group = group or robot.main_group_name + robot_cell = client.robot_cell # type: RobotCell + group = group or robot_cell.main_group_name # Unit conversion from user scale to meter scale can be done here because they are shared by all planners. waypoints = waypoints.normalized_to_meters() @@ -172,7 +172,7 @@ def plan_cartesian_motion(self, waypoints, start_state, group=None, options=None # Get default group name if not provided # Do not skip this line because some functions do not default to main_group_name when group input is None. - group = group or robot.main_group_name + group = group or robot_cell.main_group_name # =================================================================================== # End of common lines @@ -313,7 +313,7 @@ def plan_cartesian_motion_point_axis_waypoints(self, waypoints, start_state, gro # Housekeeping for intellisense planner = self # type: PyBulletPlanner client = planner.client # type: PyBulletClient - robot = client.robot # type: Robot + robot_cell = client.robot_cell # type: RobotCell # Setting robot cell state planner.set_robot_cell_state(start_state) @@ -336,8 +336,8 @@ def plan_cartesian_motion_point_axis_waypoints(self, waypoints, start_state, gro ik_options["max_random_restart"] = 1 # Getting the joint names this way ensures that the joint order is consistent with Semantics - joint_names = robot.get_configurable_joint_names(group) - joint_types = robot.get_configurable_joint_types(group) + joint_names = robot_cell.get_configurable_joint_names(group) + joint_types = robot_cell.get_configurable_joint_types(group) # Iterate over the waypoints as segments intermediate_state = deepcopy(start_state) # type: RobotCellState @@ -346,7 +346,7 @@ def plan_cartesian_motion_point_axis_waypoints(self, waypoints, start_state, gro # Recreate the first frame for the beginning of the interpolation fk_options = deepcopy(options) - fk_options["link"] = robot.get_end_effector_link_name(group) + fk_options["link"] = robot_cell.get_end_effector_link_name(group) # This frame reference need to match with the target_mode of the waypoints start_frame = planner.forward_kinematics(start_state, waypoints.target_mode, group=group, options=fk_options) @@ -670,7 +670,7 @@ def plan_cartesian_motion_frame_waypoints(self, waypoints, start_state, group, o # Housekeeping for intellisense planner = self # type: PyBulletPlanner client = planner.client # type: PyBulletClient - robot = client.robot # type: Robot + robot_cell = client.robot_cell # type: RobotCell # Setting robot cell state planner.set_robot_cell_state(start_state) @@ -729,8 +729,8 @@ def plan_cartesian_motion_frame_waypoints(self, waypoints, start_state, group, o # The ["check_collision"] in options is passed also to the ik_options # Getting the joint names this way ensures that the joint order is consistent with Semantics - joint_names = robot.get_configurable_joint_names(group) - joint_types = robot.get_configurable_joint_types(group) + joint_names = robot_cell.get_configurable_joint_names(group) + joint_types = robot_cell.get_configurable_joint_types(group) # Iterate over the waypoints as segments intermediate_state = deepcopy(start_state) # type: RobotCellState @@ -751,7 +751,7 @@ def plan_cartesian_motion_frame_waypoints(self, waypoints, start_state, group, o # Recreate the first frame for the beginning of the interpolation # First frame is obtained from the start configuration with forward kinematics fk_options = deepcopy(options) - fk_options["link"] = robot.get_end_effector_link_name(group) + fk_options["link"] = robot_cell.get_end_effector_link_name(group) # This frame reference need to match with the target_mode of the waypoints start_frame = planner.forward_kinematics(start_state, waypoints.target_mode, group=group, options=fk_options) diff --git a/src/compas_fab/backends/pybullet/backend_features/pybullet_set_robot_cell.py b/src/compas_fab/backends/pybullet/backend_features/pybullet_set_robot_cell.py index 5251bc1da..9d788fbd4 100644 --- a/src/compas_fab/backends/pybullet/backend_features/pybullet_set_robot_cell.py +++ b/src/compas_fab/backends/pybullet/backend_features/pybullet_set_robot_cell.py @@ -51,10 +51,13 @@ def set_robot_cell(self, robot_cell, robot_cell_state=None, options=None): # client.convert_mesh_to_body(rigid_body.visual_meshes[0], Frame.worldXY()) # Feed the robot to the client - if robot_cell.robot: - robot_cell.robot.ensure_geometry() - robot_cell.robot.ensure_semantics() - client.set_robot(robot_cell.robot) + if robot_cell.robot_model: + robot_cell.ensure_geometry() + robot_cell.ensure_semantics() + client.set_robot( + robot_cell.robot_model, + robot_cell.robot_semantics, + ) # Keep a copy of the robot cell in the client client._robot_cell = robot_cell.copy() diff --git a/src/compas_fab/backends/pybullet/client.py b/src/compas_fab/backends/pybullet/client.py index 15250ffcf..ac57281dc 100644 --- a/src/compas_fab/backends/pybullet/client.py +++ b/src/compas_fab/backends/pybullet/client.py @@ -41,6 +41,7 @@ import pybullet from compas_robots import Configuration # noqa: F401 from compas_robots import ToolModel # noqa: F401 + from compas_robots import RobotModel # noqa: F401 from compas_robots.model import Mimic # noqa: F401 from compas_fab.backends.kinematics import AnalyticalInverseKinematics # noqa: F401 @@ -49,6 +50,7 @@ from compas_fab.robots import Robot # noqa: F401 from compas_fab.robots import RobotCell # noqa: F401 from compas_fab.robots import RobotCellState # noqa: F401 + from compas_fab.robots import RobotSemantics # noqa: F401 # If Pybullet is not defined, load it from LazyLoader @@ -277,9 +279,9 @@ def step_simulation(self): # Functions for loading and removing objects # ------------------------------------------ - def set_robot(self, robot, concavity=False): - # type: (Robot, Optional[bool]) -> Robot - """Load an existing robot to PyBullet. + def set_robot(self, robot_model, robot_semantics, concavity=False): + # type: (RobotModel, RobotSemantics, Optional[bool]) -> Robot + """Send a robot to PyBullet. This function is used by SetRobotCell and should not be called directly by user. The robot must contain geometry and semantics. @@ -288,23 +290,19 @@ def set_robot(self, robot, concavity=False): Parameters ---------- - robot : :class:`compas_fab.robots.Robot` + robot_cell : :class:`compas_fab.robots.RobotCell` The robot to be saved for use with PyBullet. concavity : :obj:`bool`, optional - Returns - ------- - :class:`compas_fab.robots.Robot` - A robot instance. """ if self.robot_puid is not None: self.remove_robot() - urdf_fp = self.robot_model_to_urdf(robot.model, concavity=concavity) + urdf_fp = self.robot_model_to_urdf(robot_model, concavity=concavity) self.robot_puid = robot_puid = self.pybullet_load_urdf(urdf_fp) # The root link of the robot have index of `-1` (refer to PyBullet API) - self.robot_link_puids[robot.model.root.name] = -1 + self.robot_link_puids[robot_model.root.name] = -1 # For the rest of the joints and links, their numbers are the same # The id of the joint is the same as the id of its child link @@ -318,9 +316,7 @@ def set_robot(self, robot, concavity=False): # that accept poses will require passing also the mimic joints. # In compas_fab, mimic joints are not considered as DOF and are not included in get_configurable_joints(). - self.disabled_collisions = robot.semantics.disabled_collisions - - return robot + self.disabled_collisions = robot_semantics.disabled_collisions def remove_robot(self): """Remove the robot from the PyBullet server if it exists.""" @@ -626,7 +622,7 @@ def build_pose_for_pybullet(self, configuration): joint_values.append(configuration[joint_name]) else: # Check if this is mimic joint - joint = self.robot.model.get_joint_by_name(joint_name) + joint = self.robot_cell.robot_model.get_joint_by_name(joint_name) mimic = joint.mimic # type: Mimic # Get the value of the joint that is being mimicked (works only for non-cascaded mimic) if mimic: @@ -741,7 +737,7 @@ def set_robot_configuration(self, configuration): self._set_joint_position(joint_puid, configuration[joint_name], self.robot_puid) else: # Check if this is mimic joint - joint = self.robot.model.get_joint_by_name(joint_name) + joint = self.robot_cell.robot_model.get_joint_by_name(joint_name) mimic = joint.mimic # type: Mimic # Get the value of the joint that is being mimicked (works only for non-cascaded mimic) if mimic: diff --git a/src/compas_fab/backends/ros/backend_features/move_it_forward_kinematics.py b/src/compas_fab/backends/ros/backend_features/move_it_forward_kinematics.py index 08d88125c..623b6dcb8 100644 --- a/src/compas_fab/backends/ros/backend_features/move_it_forward_kinematics.py +++ b/src/compas_fab/backends/ros/backend_features/move_it_forward_kinematics.py @@ -22,7 +22,7 @@ if TYPE_CHECKING: from compas_fab.backends.ros.client import RosClient # noqa: F401 from compas_fab.backends.ros.planner import MoveItPlanner # noqa: F401 - from compas_fab.robots import Robot # noqa: F401 + from compas_fab.robots import RobotCell # noqa: F401 from compas_fab.robots import TargetMode # noqa: F401 from compas_fab.robots import RobotCellState # noqa: F401 from compas.geometry import Frame # noqa: F401 @@ -70,7 +70,7 @@ def forward_kinematics(self, robot_cell_state, target_mode, group=None, options= """ planner = self # type: MoveItPlanner client = planner.client # type: RosClient - robot = client.robot # type: Robot + robot_cell = client.robot_cell # type: RobotCell options = options or {} planner.set_robot_cell_state(robot_cell_state) @@ -81,13 +81,13 @@ def forward_kinematics(self, robot_cell_state, target_mode, group=None, options= kwargs["errback_name"] = "errback" # Use base_link or fallback to model's root link - options["base_link"] = options.get("base_link", robot.model.root.name) + options["base_link"] = options.get("base_link", robot_cell.root_name) - group = group or robot.main_group_name + group = group or robot_cell.main_group_name # Use selected link or default to group's end effector - options["link"] = options.get("link", options.get("tool0")) or robot.get_end_effector_link_name(group) - if options["link"] not in robot.get_link_names(group): + options["link"] = options.get("link", options.get("tool0")) or robot_cell.get_end_effector_link_name(group) + if options["link"] not in robot_cell.get_link_names(group): raise ValueError("Link name {} does not exist in planning group".format(options["link"])) pcf_frame = await_callback(self.forward_kinematics_async, **kwargs) diff --git a/src/compas_fab/backends/ros/backend_features/move_it_inverse_kinematics.py b/src/compas_fab/backends/ros/backend_features/move_it_inverse_kinematics.py index f5846a8d7..fb83995bb 100644 --- a/src/compas_fab/backends/ros/backend_features/move_it_inverse_kinematics.py +++ b/src/compas_fab/backends/ros/backend_features/move_it_inverse_kinematics.py @@ -17,7 +17,8 @@ from typing import Dict # noqa: F401 from compas_fab.robots import RobotCellState # noqa: F401 from compas_fab.robots import FrameTarget # noqa: F401 - from compas_fab.robots import AttachedCollisionMesh # noqa: F401 + from compas_fab.backends import MoveItPlanner # noqa: F401 + from compas_fab.backends import RosClient # noqa: F401 from compas.tolerance import TOL from compas.utilities import await_callback @@ -37,7 +38,6 @@ from compas_fab.backends.ros.messages import RobotState from compas_fab.backends.ros.messages import RosDistro from compas_fab.backends.ros.service_description import ServiceDescription -from compas_fab.utilities import from_tcf_to_t0cf __all__ = [ "MoveItInverseKinematics", @@ -66,12 +66,12 @@ def __init__(self): # kwargs = {} # kwargs["options"] = options # kwargs["frame_WCF"] = frame_WCF - # kwargs["group"] = group or self.client.robot.main_group_name - # kwargs["start_configuration"] = start_configuration or self.client.robot.zero_configuration() + # kwargs["group"] = group or self.client.robot_cell.main_group_name + # kwargs["start_configuration"] = start_configuration or self.client.robot_cell.zero_configuration() # kwargs["errback_name"] = "errback" # # Use base_link or fallback to model's root link - # options["base_link"] = options.get("base_link", self.client.robot.model.root.name) + # options["base_link"] = options.get("base_link", self.client.robot_cell.robot_model.root.name) # return await_callback(self.inverse_kinematics_async, **kwargs) # # return_full_configuration = options.get("return_full_configuration", False) @@ -145,51 +145,38 @@ def iter_inverse_kinematics(self, target, start_state=None, group=None, options= """ options = options or {} planner = self # type: MoveItPlanner - robot = planner.client.robot - client = planner.client + client = planner.client # type: RosClient + robot_cell = client.robot_cell + # Group - group = group or robot.main_group_name + group = group or robot_cell.main_group_name # Set scene - # TODO: Implement start_state client.robot_cell.assert_cell_state_match(start_state) - start_state = start_state or RobotCellState.from_robot_configuration(robot) + planner.set_robot_cell_state(start_state) # Start configuration start_configuration = start_state.robot_configuration # Merge with zero configuration ensure all joints are present - start_configuration = robot.zero_configuration(group).merged(start_configuration) + start_configuration = robot_cell.zero_configuration(group).merged(start_configuration) # Target frame and Tool Coordinate Frame target = target.normalized_to_meters() target_frame = target.target_frame - - # Attached tool frames does not need scaling because Tools are modelled in meter scale - attached_tool = client.robot_cell.get_attached_tool(start_state, group) - if attached_tool: - target_frame = from_tcf_to_t0cf(target_frame, attached_tool.frame) - - # Scale Attached Collision Meshes - attached_collision_meshes = client.robot_cell.get_attached_rigid_bodies_as_attached_collision_meshes( - start_state, group - ) - # TODO: ACM will always be modeled in meters too. This is a temporary solution - if robot.need_scaling: - for acm in attached_collision_meshes: - acm.collision_mesh.scale(1.0 / robot.scale_factor) + planner_frame = client.robot_cell.target_frames_to_pcf(start_state, target_frame, target.target_mode, group) # Compose the kwargs for the async function kwargs = {} kwargs["options"] = options - kwargs["frame_WCF"] = target_frame + kwargs["frame_WCF"] = planner_frame kwargs["group"] = group - kwargs["start_configuration"] = start_configuration or robot.zero_configuration() + kwargs["start_configuration"] = start_configuration kwargs["errback_name"] = "errback" max_results = options.get("max_results", 100) # Use base_link or fallback to model's root link - options["base_link"] = options.get("base_link", self.client.robot.model.root.name) + options["base_link"] = options.get("base_link", robot_cell.root_name) all_yielded_joint_positions = [] # Yield up to max_results configurations @@ -209,7 +196,7 @@ def iter_inverse_kinematics(self, target, start_state=None, group=None, options= all_yielded_joint_positions.append(joint_positions) # Generate random start configuration for next iteration - random_configuration = self.client.robot.random_configuration(kwargs["group"]) + random_configuration = robot_cell.random_configuration(kwargs["group"]) kwargs["start_configuration"] = random_configuration def inverse_kinematics_async( @@ -223,12 +210,9 @@ def inverse_kinematics_async( joint_state = JointState( name=start_configuration.joint_names, position=start_configuration.joint_values, header=header ) - start_state = RobotState(joint_state, MultiDOFJointState(header=header)) - if options.get("attached_collision_meshes"): - for acm in options["attached_collision_meshes"]: - aco = AttachedCollisionObject.from_attached_collision_mesh(acm) - start_state.attached_collision_objects.append(aco) + # The start state being in diff mode allows it to keep previously set Attached Collision Objects + start_state = RobotState(joint_state, MultiDOFJointState(header=header), is_diff=True) # Filter needs to happen after all objects have been added start_state.filter_fields_for_distro(self.client.ros_distro) diff --git a/src/compas_fab/backends/ros/backend_features/move_it_plan_cartesian_motion.py b/src/compas_fab/backends/ros/backend_features/move_it_plan_cartesian_motion.py index 3797f81f3..b234175c9 100644 --- a/src/compas_fab/backends/ros/backend_features/move_it_plan_cartesian_motion.py +++ b/src/compas_fab/backends/ros/backend_features/move_it_plan_cartesian_motion.py @@ -31,6 +31,7 @@ from compas_fab.robots import JointTrajectory # noqa: F401 from compas_fab.robots import RobotCellState # noqa: F401 from compas_fab.robots import Waypoints # noqa: F401 + from compas_fab.robots import RobotCell # noqa: F401 __all__ = [ "MoveItPlanCartesianMotion", @@ -95,24 +96,24 @@ def plan_cartesian_motion(self, waypoints, start_state, group=None, options=None """ planner = self # type: MoveItPlanner # noqa: F841 client = planner.client # type: RosClient # noqa: F841 - robot = client.robot + robot_cell = client.robot_cell # type: RobotCell # noqa: F841 options = options or {} kwargs = {} kwargs["options"] = options kwargs["waypoints"] = waypoints kwargs["start_state"] = start_state - group = group or robot.main_group_name + group = group or robot_cell.main_group_name kwargs["group"] = group kwargs["errback_name"] = "errback" # Use base_link or fallback to model's root link - options["base_link"] = options.get("base_link", robot.model.root.name) - options["joints"] = {j.name: j.type for j in robot.model.joints} + options["base_link"] = options.get("base_link", robot_cell.root_name) + options["joints"] = {j.name: j.type for j in robot_cell.robot_model.joints} - options["link"] = options.get("link") or robot.get_end_effector_link_name(group) - if options["link"] not in robot.get_link_names(group): + options["link"] = options.get("link") or robot_cell.get_end_effector_link_name(group) + if options["link"] not in robot_cell.get_link_names(group): raise ValueError("Link name {} does not exist in planning group".format(options["link"])) # This function wraps multiple implementations depending on the type of waypoints diff --git a/src/compas_fab/backends/ros/client.py b/src/compas_fab/backends/ros/client.py index 0721b7144..51164ac1a 100644 --- a/src/compas_fab/backends/ros/client.py +++ b/src/compas_fab/backends/ros/client.py @@ -25,11 +25,17 @@ from compas_fab.backends.ros.messages import RobotTrajectory from compas_fab.backends.ros.messages import Time from compas_fab.backends.tasks import CancellableFutureResult -from compas_fab.robots import Robot from compas_fab.robots import RobotSemantics from compas_fab.robots import RobotCell from compas_fab.robots import RobotCellState +from compas import IPY + +if not IPY: + from typing import TYPE_CHECKING + + if TYPE_CHECKING: + from typing import Optional __all__ = [ "RosClient", @@ -153,7 +159,7 @@ def ros_distro(self): return self._ros_distro - def load_robot( + def load_robot_cell( self, load_geometry=False, urdf_param_name="/robot_description", @@ -161,7 +167,9 @@ def load_robot( precision=None, local_cache_directory=None, ): - """Load an entire robot instance -including model and semantics- directly from ROS. + # type: (bool, str, str, int, Optional[str]) -> RobotCell + """Load the robot cell (including model and semantics) from ROS. + The robot celL is set in `client.robot_cell` and returned. Parameters ---------- @@ -182,8 +190,8 @@ def load_robot( Examples -------- >>> with RosClient() as client: - ... robot = client.load_robot() - ... print(robot.name) + ... robot_cell = client.load_robot_cell() + ... print(robot_cell.robot_model.name) ur5_robot """ @@ -208,10 +216,10 @@ def load_robot( if load_geometry: model.load_geometry(loader, precision=precision) - robot = Robot(model, semantics=semantics) - self._robot_cell = RobotCell(robot) + self._robot_cell = RobotCell(robot_model=model, robot_semantics=semantics) self._robot_cell_state = RobotCellState.from_robot_cell(self._robot_cell) - return robot + + return self._robot_cell # ========================================================================== # executing diff --git a/src/compas_fab/ghpython/components/Cf_VisualizeRobot/code.py b/src/compas_fab/ghpython/components/Cf_VisualizeRobot/code.py index 143c75ff0..7289f43c4 100644 --- a/src/compas_fab/ghpython/components/Cf_VisualizeRobot/code.py +++ b/src/compas_fab/ghpython/components/Cf_VisualizeRobot/code.py @@ -15,9 +15,9 @@ from scriptcontext import sticky as st from compas_fab.backends import BackendFeatureNotSupportedError -from compas_fab.robots import PlanningScene +# TODO: This component needs to be migrated to visualize RobotCell class RobotVisualize(component): def RunScript( self, diff --git a/src/compas_fab/ghpython/components/Cf_VisualizeTrajectory/code.py b/src/compas_fab/ghpython/components/Cf_VisualizeTrajectory/code.py index f27f15402..2759596ff 100644 --- a/src/compas_fab/ghpython/components/Cf_VisualizeTrajectory/code.py +++ b/src/compas_fab/ghpython/components/Cf_VisualizeTrajectory/code.py @@ -10,7 +10,7 @@ class TrajectoryVisualize(component): - def RunScript(self, robot, group, trajectory): + def RunScript(self, robot_cell, group, trajectory): start_configuration = None configurations = [] fraction = 0.0 @@ -21,8 +21,9 @@ def RunScript(self, robot, group, trajectory): velocities = [] accelerations = [] - if robot and trajectory: - group = group or robot.main_group_name + # TODO: Fix this + if robot_cell and trajectory: + group = group or robot_cell.main_group_name for c in trajectory.points: configurations.append( diff --git a/src/compas_fab/robots/__init__.py b/src/compas_fab/robots/__init__.py index b1f460798..dcb1f8ea6 100644 --- a/src/compas_fab/robots/__init__.py +++ b/src/compas_fab/robots/__init__.py @@ -109,9 +109,6 @@ DeviationVectorsGenerator, OrthonormalVectorsFromAxisGenerator, ) -from .robot import ( - Robot, -) from .robot_cell import ( RobotCell, @@ -173,8 +170,6 @@ "ReachabilityMap", "DeviationVectorsGenerator", "OrthonormalVectorsFromAxisGenerator", - # Robot - "Robot", # Robot Cell "RobotCell", # Rigid Body diff --git a/src/compas_fab/robots/conftest.py b/src/compas_fab/robots/conftest.py index 8cc64da3e..33278f7ec 100644 --- a/src/compas_fab/robots/conftest.py +++ b/src/compas_fab/robots/conftest.py @@ -12,8 +12,7 @@ import compas_fab from compas_fab.backends import RosClient -from compas_fab.robots import RobotLibrary -from compas_fab.robots.tool import Tool +from compas_fab.robots import RobotCellLibrary @pytest.fixture(autouse=True) @@ -30,4 +29,3 @@ def add_imports(doctest_namespace): doctest_namespace["RobotLibrary"] = RobotLibrary doctest_namespace["RosClient"] = RosClient doctest_namespace["Rotation"] = Rotation - doctest_namespace["Tool"] = Tool diff --git a/src/compas_fab/robots/constraints.py b/src/compas_fab/robots/constraints.py index b83063ffc..0da401123 100644 --- a/src/compas_fab/robots/constraints.py +++ b/src/compas_fab/robots/constraints.py @@ -8,8 +8,6 @@ from compas.geometry import Scale from compas.geometry import Sphere -from compas_fab.utilities import from_tcf_to_t0cf - if not IPY: from typing import TYPE_CHECKING @@ -605,18 +603,18 @@ def from_frame(cls, frame_WCF, tolerances_orientation, link_name, tool_coordinat Examples -------- - >>> robot = RobotLibrary.ur5() + >>> robot_cell, robot_cell_state = RobotCellLibrary.ur5() >>> frame = Frame([0.4, 0.3, 0.4], [0, 1, 0], [0, 0, 1]) >>> tolerances_orientation = [math.radians(1)] * 3 - >>> group = robot.main_group_name + >>> group = robot_cell.main_group_name >>> end_effector_link_name = robot.get_end_effector_link_name(group) >>> OrientationConstraint.from_frame(frame, tolerances_orientation, end_effector_link_name) OrientationConstraint('tool0', [0.5, 0.5, 0.5, 0.5], [0.017453292519943295, 0.017453292519943295, 0.017453292519943295], 1.0) """ # TODO: Add support for tool_state.attachment_frame, call from_tcf_to_pcf. - if tool_coordinate_frame: - frame_WCF = from_tcf_to_t0cf(frame_WCF, tool_coordinate_frame) + # if tool_coordinate_frame: + # frame_WCF = from_tcf_to_t0cf(frame_WCF, tool_coordinate_frame) tolerances_orientation = list(tolerances_orientation) if len(tolerances_orientation) == 1: @@ -708,10 +706,10 @@ def from_frame(cls, frame_WCF, tolerance_position, link_name, tool_coordinate_fr Examples -------- - >>> robot = RobotLibrary.ur5() + >>> robot_cell, robot_cell_state = RobotCellLibrary.ur5() >>> frame = Frame([0.4, 0.3, 0.4], [0, 1, 0], [0, 0, 1]) >>> tolerance_position = 0.001 - >>> group = robot.main_group_name + >>> group = robot_cell.main_group_name >>> end_effector_link_name = robot.get_end_effector_link_name(group) >>> PositionConstraint.from_frame(frame, tolerance_position, end_effector_link_name) # doctest: +SKIP PositionConstraint('tool0', BoundingVolume(2, Sphere(Point(0.400, 0.300, 0.400), 0.001)), 1.0) # doctest: +SKIP @@ -719,8 +717,8 @@ def from_frame(cls, frame_WCF, tolerance_position, link_name, tool_coordinate_fr # TODO: Add support for tool_state.attachment_frame, call from_tcf_to_pcf. - if tool_coordinate_frame: - frame_WCF = from_tcf_to_t0cf(frame_WCF, tool_coordinate_frame) + # if tool_coordinate_frame: + # frame_WCF = from_tcf_to_t0cf(frame_WCF, tool_coordinate_frame) radius = float(tolerance_position) sphere = Sphere(radius, point=frame_WCF.point) diff --git a/src/compas_fab/robots/robot.py b/src/compas_fab/robots/robot.py index 8895005ff..e11b4a1b9 100644 --- a/src/compas_fab/robots/robot.py +++ b/src/compas_fab/robots/robot.py @@ -2,7 +2,6 @@ from __future__ import division from __future__ import print_function -import random from compas import IPY from compas.data import Data @@ -12,11 +11,9 @@ from compas_robots import Configuration from compas_robots import RobotModel from compas_robots.model import Joint -from compas_robots.resources import LocalPackageMeshLoader -import compas_fab + from compas_fab.robots.constraints import Constraint -from compas_fab.utilities import from_t0cf_to_tcf if not IPY: from typing import TYPE_CHECKING @@ -40,7 +37,6 @@ from compas_fab.robots import Target # noqa: F401 from compas_fab.robots import Tool # noqa: F401 from compas_fab.robots import Waypoints # noqa: F401 - from compas_fab.robots.planning_scene import AttachedCollisionMesh # noqa: F401 __all__ = [ @@ -105,75 +101,6 @@ def __init__(self, model=None, scene_object=None, semantics=None): self.semantics = semantics self.attributes = {} - @property - def __data__(self): - data = { - "scale_factor": self._scale_factor, - "attached_tools": self._attached_tools, - # The current_ik is an extrinsic state that is not serialized with the robot - # "current_ik": self._current_ik, - "model": self.model.__data__, - "semantics": self.semantics, - "attributes": self.attributes, - # The following attributes cannot be serialized: scene_object - } - return data - - @classmethod - def __from_data__(cls, data): - _scale_factor = data.get("scale_factor", 1.0) - _attached_tools = data.get("attached_tools", {}) - model = RobotModel.__from_data__(data["model"]) - semantics = data.get("semantics") - attributes = data.get("attributes", {}) - - robot = cls(model, None, semantics=semantics) - robot._scale_factor = _scale_factor - robot._attached_tools = _attached_tools - robot.attributes = attributes - return robot - - @classmethod - def from_urdf(cls, urdf_filename, srdf_filename=None, local_package_mesh_folder=None): - # type: (str, Optional[str], Optional[str]) -> Robot - """Create a robot from URDF. - Optionally, SRDF can be provided to load semantics and a local package mesh folder to load mesh geometry. - - Parameters - ---------- - urdf_filename : :obj:`str` - Path to the URDF file. - srdf_filename : :obj:`str`, optional - Path to the SRDF file to load semantics. Default is `None`. - local_package_mesh_folder : :obj:`str`, optional - Path to the local package mesh folder. - If the path is provided, the geometry of the robot is loaded from this folder. - Default is `None`, which means that the geometry is not loaded. - - Returns - ------- - :class:`compas_fab.robots.Robot` - Newly created instance of the robot. - - """ - # NOTE: This import is here to avoid circular imports - from compas_fab.robots import RobotSemantics # noqa: F811 - - model = RobotModel.from_urdf_file(urdf_filename) - - if srdf_filename: - semantics = RobotSemantics.from_srdf_file(srdf_filename, model) - else: - semantics = None - - if local_package_mesh_folder: - loader = LocalPackageMeshLoader(compas_fab.get(local_package_mesh_folder), "") - model.load_geometry(loader) - - robot = cls(model, semantics=semantics) - - return robot - @property def scene_object(self): # type: () -> BaseRobotModelObject | None @@ -191,1656 +118,110 @@ def scene_object(self, value): for tool in self.attached_tools.values(): self.scene_object.attach_tool_model(tool.tool_model) - @classmethod - def basic(cls, name, joints=None, links=None, materials=None, **kwargs): - # type: (str, Optional[List[Joint]], Optional[List[Link]], Optional[List[Material]], **Any) -> Robot - """Create the most basic instance of a robot, based only on name. - - Parameters - ---------- - name : :obj:`str` - Name of the robot - joints : :obj:`list` of :class:`compas_robots.Joint`, optional - Robot's joints. - links : :obj:`list` of :class:`compas_robots.Link`, optional - Robot's links. - materials : :obj:`list` of :class:`compas_robots.Material`, optional - Material description of the robot. - kwargs : :obj:`dict` - Keyword arguments passed to the :class:`compas_robots.RobotModel` - `attr` :obj:`dict`. Accessible from `Robot.model.attr`. - - - Returns - ------- - :class:`Robot` - Newly created instance of a robot. - - Examples - -------- - >>> robot = Robot.basic('A robot') - >>> robot.name - 'A robot' - """ - model = RobotModel(name, joints=joints or [], links=links or [], materials=materials or [], **kwargs) - return cls(model, None) - - @property - def name(self): - # type: () -> str - """Name of the robot, as defined by its model. - - Returns - ------- - :obj:`str` - - Examples - -------- - >>> robot = RobotLibrary.ur5() - >>> robot.name - 'ur5_robot' - """ - return self.model.name - - @name.setter - def name(self, value): - if value != self.model.name: - assert False, "Changing the name of the robot is not supported." - - @property - def group_names(self): - # type: () -> List[str] - """All planning groups of the robot, only available if semantics is set. - - Returns - ------- - :obj:`list` of :obj:`str` - - Examples - -------- - >>> robot = RobotLibrary.ur5() - >>> robot.group_names - ['manipulator', 'endeffector'] - - """ - self.ensure_semantics() - return self.semantics.group_names - - @property - def main_group_name(self): - # type: () -> str - """ - Robot's main planning group, only available if semantics is set. - - Returns - ------- - :obj:`str` - - """ - self.ensure_semantics() - return self.semantics.main_group_name - - @property - def root_name(self): - """Robot's root name.""" - return self.model.root.name - - @property - def group_states(self): - # type: () -> Dict[str, dict] - """All group states of the robot, only available if semantics is set. - - Returns - ------- - :obj:`dict` of :obj:`dict` - - Examples - -------- - >>> robot = RobotLibrary.ur5() - >>> sorted(robot.group_states['manipulator'].keys()) - ['home', 'up'] - - """ - self.ensure_semantics() - return self.semantics.group_states - - @property - def attached_tools(self): - # type: () -> Dict[str, Tool] - """Dictionary of tools and the planning groups that the tools are currently attached to, if any.""" - return self._attached_tools - - @property - def attached_tool(self): - # type: () -> Tool | None - """Returns the tool attached to the default main planning group, if any.""" - return self._attached_tools.get(self.main_group_name, None) - - def get_end_effector_link_name(self, group=None): - # type: (Optional[str]) -> str - """Get the name of the robot's end effector link. - - Parameters - ---------- - group : :obj:`str`, optional - The name of the group. Defaults to the main planning group. - - Returns - ------- - :obj:`str` - - Examples - -------- - >>> robot = RobotLibrary.ur5() - >>> robot.get_end_effector_link_name() - 'tool0' - """ - if not self.semantics: - return self.model.get_end_effector_link_name() - else: - return self.semantics.get_end_effector_link_name(group) - - def get_end_effector_link(self, group=None): - # type: (Optional[str]) -> Link - """Get the robot's end effector link. - - Parameters - ---------- - group : :obj:`str`, optional - The name of the planning group. Defaults to the main planning group. - - Returns - ------- - :class:`compas_robots.Link` - - Examples - -------- - >>> robot = RobotLibrary.ur5() - >>> link = robot.get_end_effector_link() - >>> link.name - 'tool0' - """ - name = self.get_end_effector_link_name(group) - return self.model.get_link_by_name(name) - - def get_end_effector_frame(self, group=None, full_configuration=None): - # type: (Optional[str], Optional[Configuration]) -> Frame - """Get the frame of the robot's end effector. - - Parameters - ---------- - group : :obj:`str`, optional - The name of the planning group. Defaults to the main planning group. - full_configuration : :class:`compas_robots.Configuration`, optional - The robot's full configuration, i.e. values for all configurable - joints of the entire robot. Defaults to the all-zero configuration. - - Returns - ------- - :class:`compas.geometry.Frame` - """ - if not full_configuration: - full_configuration = self.zero_configuration() - return self.model.forward_kinematics(full_configuration, link_name=self.get_end_effector_link_name(group)) - - def get_base_link_name(self, group=None): - # type: (Optional[str]) -> str - """Get the name of the robot's base link. - - Parameters - ---------- - group : :obj:`str`, optional - The name of the planning group. Defaults to the main planning group. - - Returns - ------- - str - - Examples - -------- - >>> robot = RobotLibrary.ur5() - >>> robot.get_base_link_name() - 'base_link' - """ - if not self.semantics: - return self.model.get_base_link_name() - else: - return self.semantics.get_base_link_name(group) - - def get_base_link(self, group=None): - # type: (Optional[str]) -> Link - """Get the robot's base link. - - Parameters - ---------- - group : :obj:`str`, optional - The name of the planning group. Defaults to the main planning group. - - Returns - ------- - :class:`compas_robots.Link` - - Examples - -------- - >>> robot = RobotLibrary.ur5() - >>> link = robot.get_base_link() - >>> link.name - 'base_link' - """ - name = self.get_base_link_name(group) - return self.model.get_link_by_name(name) - - def get_base_frame(self, group=None, full_configuration=None): - # type: (Optional[str], Optional[Configuration]) -> Frame - """Get the frame of the robot's base link, which is the robot's origin frame. - - Parameters - ---------- - group : :obj:`str`, optional - The name of the planning group. Defaults to the main planning group. - full_configuration : :class:`compas_robots.Configuration`, optional - The robot's full configuration, i.e. values for all configurable - joints of the entire robot. Defaults to the all-zero configuration. - - Returns - ------- - :class:`compas.geometry.Frame` - """ - if not full_configuration: - full_configuration = self.zero_configuration() - return self.model.forward_kinematics(full_configuration, link_name=self.get_base_link_name(group)) - - def get_link_names(self, group=None): - # type: (Optional[str]) -> List[str] - """Get the names of the links in the kinematic chain. - - Parameters - ---------- - group : :obj:`str`, optional - The name of the planning group. Defaults to the main planning group. - - Returns - ------- - :obj:`list` of :obj:`str` - - Examples - -------- - >>> robot = RobotLibrary.ur5() - >>> robot.get_link_names('manipulator') - ['base_link', 'base_link_inertia', 'shoulder_link', 'upper_arm_link', 'forearm_link', 'wrist_1_link', 'wrist_2_link', 'wrist_3_link', 'flange', 'tool0'] - """ - base_link_name = self.get_base_link_name(group) - end_effector_link_name = self.get_end_effector_link_name(group) - link_names = [] - for link in self.model.iter_link_chain(base_link_name, end_effector_link_name): - link_names.append(link.name) - return link_names - - def get_link_names_with_collision_geometry(self): - # type: () -> List[str] - """Get the names of the links with collision geometry. - - Note that returned names does not imply that the link has collision geometry loaded. - Use :meth:`ensure_geometry()` to ensure that collision geometry is loaded. - - Returns - ------- - :obj:`list` of :obj:`str` - - Examples - -------- - >>> robot = RobotLibrary.ur5() - >>> robot.get_link_names_with_collision_geometry() - ['base_link_inertia', 'shoulder_link', 'upper_arm_link', 'forearm_link', 'wrist_1_link', 'wrist_2_link', 'wrist_3_link'] - """ - return [link.name for link in self.model.iter_links() if link.collision] - - def get_configurable_joints(self, group=None): - # type: (Optional[str]) -> List[Joint] - """Get the robot's configurable joints. - - If group is provided and robot.semantics is set, it returns the configurable joints of the group. - If group is None, all configurable joints of the robot are returned. - - Parameters - ---------- - group : :obj:`str`, optional - The name of the planning group. - - Returns - ------- - :obj:`list` of :class:`compas_robots.Joint` - - Notes - ----- - If semantics is set and no group is passed, it returns all configurable - joints of all groups. - - Examples - -------- - >>> robot = RobotLibrary.ur5() - >>> joints = robot.get_configurable_joints('manipulator') - >>> [j.name for j in joints] - ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'] - """ - if self.semantics: - if group: - return self.semantics.get_configurable_joints(self.model, group) - else: - return self.semantics.get_all_configurable_joints(self.model) - else: - return self.model.get_configurable_joints() - - def get_joint_types_by_names(self, names): - # type: (List[str]) -> List[int] - """Get a list of joint types given a list of joint names. - - Parameters - ---------- - names : :obj:`list` of :obj:`str` - The names of the joints. - - Returns - ------- - :obj:`list` of :attr:`compas_robots.Joint.SUPPORTED_TYPES` - List of joint types. - """ - return self.model.get_joint_types_by_names(names) + # ========================================================================== + # configurations + # ========================================================================== - def get_joint_by_name(self, name): - # type: (str) -> Joint | None - """RGet the joint in the robot model matching the given name. + # ========================================================================== + # transformations, coordinate frames + # ========================================================================== - Parameters - ---------- - name : :obj:`str` - The name of the joint. + # ========================================================================== + # checks + # ========================================================================== - Returns - ------- - :class:`compas_robots.Joint` - """ - return self.model.get_joint_by_name(name) + # ========================================================================== + # services + # ========================================================================== - def get_configurable_joint_names(self, group=None): - # type: (Optional[str]) -> List[str] - """Get the configurable joint names. + def transformed_frames(self, configuration, group=None): + # type: (Configuration, Optional[str]) -> List[Frame] + """Get the robot's transformed frames. Parameters ---------- + configuration : :class:`compas_robots.Configuration` + Configuration to compute transformed frames for. group : :obj:`str`, optional - The name of the planning group. Defaults to the main planning group. + The planning group used for the calculation. Defaults to the robot's + main planning group. Returns ------- - :obj:`list` of :obj:`str` - - Notes - ----- - If semantics is set and no group is passed, it returns all configurable - joints of all groups. - - Examples - -------- - >>> robot = RobotLibrary.ur5() - >>> robot.get_configurable_joint_names('manipulator') - ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', \ - 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'] + :obj:`list` of :class:`compas.geometry.Frame` + Transformed frames. """ - configurable_joints = self.get_configurable_joints(group) - return [j.name for j in configurable_joints] + if not len(configuration.joint_names): + configuration.joint_names = self.get_configurable_joint_names(group) + return self.model.transformed_frames(configuration) - def get_configurable_joint_types(self, group=None): - # type: (Optional[str]) -> List[int] - """Get the configurable joint types. + def transformed_axes(self, configuration, group=None): + # type: (Configuration, Optional[str]) -> List[Vector] + """Get the robot's transformed axes. Parameters ---------- + configuration : :class:`compas_robots.Configuration` + Configuration to compute transformed axes for. group : :obj:`str`, optional - The name of the planning group. Defaults to the main planning group. + The planning group used for the calculation. Defaults to the robot's + main planning group. Returns ------- - :obj:`list` of :attr:`compas_robots.Joint.SUPPORTED_TYPES` - - Notes - ----- - If :attr:`semantics` is set and no group is passed, it returns all - configurable joint types of all groups. - - Examples - -------- - >>> robot = RobotLibrary.ur5() - >>> robot.get_configurable_joint_types('manipulator') - [0, 0, 0, 0, 0, 0] + :obj:`list` of :class:`compas.geometry.Vector` + Transformed axes. """ - configurable_joints = self.get_configurable_joints(group) - return [j.type for j in configurable_joints] + if not len(configuration.joint_names): + configuration.joint_names = self.get_configurable_joint_names(group) + return self.model.transformed_axes(configuration) # ========================================================================== - # configurations + # drawing # ========================================================================== - def zero_configuration(self, group=None): - # type: (Optional[str]) -> Configuration - """Get the zero joint configuration of the specified planning group. - If group is None, all configurable joints are returned. - - If zero is out of joint limits ``(upper, lower)`` then - ``(upper + lower) / 2`` is used as joint value. - - Parameters - ---------- - group : :obj:`str`, optional - The name of the planning group. Defaults to the main planning group. - - Examples - -------- - >>> robot = RobotLibrary.ur5() - >>> robot.zero_configuration('manipulator') - Configuration((0.000, 0.000, 0.000, 0.000, 0.000, 0.000), (0, 0, 0, 0, 0, 0), \ - ('shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint')) - """ - values = [] - joint_names = [] - joint_types = [] - for joint in self.get_configurable_joints(group): - if joint.limit and not (0 <= joint.limit.upper and 0 >= joint.limit.lower): - values.append((joint.limit.upper + joint.limit.lower) / 2.0) - else: - values.append(0) - joint_names.append(joint.name) - joint_types.append(joint.type) - return Configuration(values, joint_types, joint_names) - - def random_configuration(self, group=None): - # type: (Optional[str]) -> Configuration - """Get a random configuration for the specified planning group. + def update(self, configuration, group=None, visual=True, collision=True): + # type: (Configuration, Optional[str], Optional[bool], Optional[bool]) -> None + """Update the robot's geometry. Parameters ---------- + configuration : :class:`compas_robots.Configuration` + Instance of the configuration (joint state) to move to. group : :obj:`str`, optional - The name of the planning group. Defaults to the main planning group. - - Returns - ------- - :class:`compas_robots.Configuration` - - Notes - ----- - No collision checking is involved, the configuration may be invalid. + The name of the group to plan for. Defaults to the robot's main + planning group. + visual : :obj:`bool`, optional + ``True`` if the visual geometry should be also updated, otherwise ``False``. + Defaults to ``True``. + collision : :obj:`bool`, optional + ``True`` if the collision geometry should be also updated, otherwise ``False``. + Defaults to ``True``. """ - configurable_joints = self.get_configurable_joints(group) - values = [] - for joint in configurable_joints: - if joint.limit: - values.append(joint.limit.lower + (joint.limit.upper - joint.limit.lower) * random.random()) - else: - values.append(0) - joint_names = self.get_configurable_joint_names(group) - joint_types = self.get_joint_types_by_names(joint_names) - return Configuration(values, joint_types, joint_names) + group = group or self.main_group_name if self.semantics else None - def get_group_configuration(self, group, full_configuration): - # type: (str, Configuration) -> Configuration - """Filter a full configuration and return only the joints of the specified group. + if not len(configuration.joint_names): + configuration.joint_names = self.get_configurable_joint_names(group) - Parameters - ---------- - group : :obj:`str` - The name of the planning group. - full_configuration : :class:`compas_robots.Configuration` - A full configuration (with all configurable joints of the robot). - Note that this object is not modified. + self.scene_object.update(configuration, visual, collision) - Returns - ------- - :class:`compas_robots.Configuration` - The configuration with only the joints of the specified group. - """ - # adds joint_names to full_configuration and makes copy - full_configuration = self._check_full_configuration(full_configuration) + def draw_visual(self): + # type: () -> None + """Draw the robot's visual geometry using the defined :attr:`Robot.scene_object`.""" + return self.scene_object.draw_visual() - group_joint_names = self.get_configurable_joint_names(group) - values = [full_configuration[name] for name in group_joint_names] - return Configuration(values, self.get_configurable_joint_types(group), group_joint_names) + def draw_collision(self): + # type: () -> None + """Draw the robot's collision geometry using the defined :attr:`Robot.scene_object`.""" + return self.scene_object.draw_collision() - def merge_group_with_full_configuration(self, group_configuration, full_configuration, group): - # type: (Configuration, Configuration, str) -> Configuration - """Get the robot's full configuration by merging a group's configuration with a full configuration. - The group configuration takes precedence over the full configuration in - case a joint value is present in both. - - Parameters - ---------- - group_configuration : :class:`compas_robots.Configuration` - The configuration for one of the robot's planning groups. - full_configuration : :class:`compas_robots.Configuration` - The configuration for all configurable joints of the robot. - group : :obj:`str` - The name of the planning group. - - Returns - ------- - :class:`compas_robots.Configuration` - A full configuration with values for all configurable joints. - - Raises - ------ - :exc:`ValueError` - If the `full_configuration` does not specify positions for all - configurable joints, or if the `group_configuration` does not - specify positions for all configurable joints of the given group. - """ - if not group_configuration.joint_names: - group_configuration.joint_names = self.get_configurable_joint_names(group) - - # adds joint_names to full_configuration and makes copy - full_configuration = self._check_full_configuration(full_configuration) - - full_configuration = full_configuration.merged(group_configuration) - return full_configuration - - def get_group_names_from_link_name(self, link_name): - # type: (str) -> List[str] - """Get the names of the groups `link_name` belongs to. - - Parameters - ---------- - link_name : :obj:`str` - The name of a link - - Returns - ------- - :obj:`list` of :obj:`str` - A list of group names. - """ - group_names = [] - for group in self.group_names: - if link_name in self.get_link_names(group): - group_names.append(group) - return group_names - - def get_position_by_joint_name(self, configuration, joint_name, group=None): - # type: (Configuration, str, Optional[str]) -> float - """Get the position of named joint in given configuration. - - Parameters - ---------- - configuration : :class:`compas_robots.Configuration` - The configuration of the configurable joints. - joint_name : :obj:`str` - Name of joint. - group : :obj:`str`, optional - The name of the planning group. Defaults to the main planning group. - - Returns - ------- - :obj:`float` - Joint position for the given joint. - - Raises - ------ - :exc:`ValueError` - If the number of joints in the `configuration` parameter does not - match the configurable joints of the given `group`. - """ - names = self.get_configurable_joint_names(group) - if len(names) != len(configuration.joint_values): - raise ValueError("Please pass a configuration with {} joint_values or specify group".format(len(names))) - return configuration.joint_values[names.index(joint_name)] - - def _check_full_configuration(self, full_configuration=None): - # type: (Optional[Configuration]) -> Tuple[Configuration, Configuration] - """Either create a full configuration or check if the passed full configuration is valid. - - Returned Configuration is a new object and contain `.joint_names` attribute. - - Parameters - ---------- - full_configuration : :class:`compas_robots.Configuration`, optional - The full configuration of the whole robot, including values for all configurable joints. - - Returns - ------- - ::class:`compas_robots.Configuration` - The full configuration - """ - if not full_configuration: - configuration = self.zero_configuration() # with joint_names - else: - joint_names = self.get_configurable_joint_names() # full configuration - # full_configuration might have passive joints specified as well, we allow this. - if len(joint_names) > len(full_configuration.joint_values): - raise ValueError( - "Please pass a configuration with {} values, for all configurable joints of the robot.".format( - len(joint_names) - ) - ) - configuration = full_configuration.copy() - if not configuration.joint_names: - configuration.joint_names = joint_names - return configuration - - def get_configuration_from_group_state(self, group, group_state): - # type: (str, str) -> Configuration - """Get a :class:`compas_robots.Configuration` from a group's group state. - - Parameters - ---------- - group : :obj:`str` - The name of the planning group. - group_state : :obj:`str` - The name of the group_state. - - Returns - ------- - :class:`compas_robots.Configuration` - The configuration specified by the :attr:`group_state`. - """ - joint_dict = self.group_states[group][group_state] - group_joint_names = self.get_configurable_joint_names(group) - group_joint_types = self.get_configurable_joint_types(group) - values = [joint_dict[name] for name in group_joint_names] - return Configuration(values, group_joint_types, group_joint_names) - - # ========================================================================== - # transformations, coordinate frames - # ========================================================================== - - def transformation_RCF_WCF(self, group=None): - # type: (Optional[str]) -> Transformation - """Get the transformation from the robot's coordinate system (RCF) to the world coordinate system (WCF). - - Parameters - ---------- - group : :obj:`str`, optional - The name of the planning group. Defaults to the main planning group. - - Returns - ------- - :class:`compas.geometry.Transformation` - - """ - base_frame = self.get_base_frame(group) - return Transformation.from_change_of_basis(base_frame, Frame.worldXY()) - - def transformation_WCF_RCF(self, group=None): - # type: (Optional[str]) -> Transformation - """Get the transformation from the world coordinate system (WCF) to the robot's coordinate system (RCF). - - Parameters - ---------- - group : :obj:`str`, optional - The name of the planning group. Defaults to the main planning group. - - Returns - ------- - :class:`compas.geometry.Transformation` - - """ - base_frame = self.get_base_frame(group) - return Transformation.from_change_of_basis(Frame.worldXY(), base_frame) - - def set_RCF(self, robot_coordinate_frame, group=None): - # type: (Frame, Optional[str]) -> None - """Move the origin frame of the robot to the robot_coordinate_frame. - - Raises - ------ - :exc:`NotImplementedError` - Not implemented yet. - """ - # TODO: must be applied to the model, so that base_frame is RCF - # Problem: check if conversion wcf/rcf still works with backend - raise NotImplementedError - - def get_RCF(self, group=None): - # type: (Optional[str]) -> Frame - """Get the origin frame of the robot. - - Parameters - ---------- - group : :obj:`str`, optional - The name of the planning group. Defaults to the main planning group. - - Returns - ------- - :class:`compas.geometry.Frame` - Origin frame of the robot. - """ - return self.get_base_frame(group) - - def to_local_coordinates(self, frame_WCF, group=None): - # type: (Frame, Optional[str]) -> Frame - """Represent a frame from the world coordinate system (WCF) in the robot's coordinate system (RCF). - - Parameters - ---------- - frame_WCF : :class:`compas.geometry.Frame` - A frame in the world coordinate frame. - group : :obj:`str`, optional - The name of the planning group. Defaults to the main planning group. - - Returns - ------- - :class:`compas.geometry.Frame` - A frame in the robot's coordinate frame. - - Examples - -------- - >>> robot = RobotLibrary.ur5() - >>> frame_WCF = Frame([-0.363, 0.003, -0.147], [0.388, -0.351, -0.852], [0.276, 0.926, -0.256]) - >>> frame_RCF = robot.to_local_coordinates(frame_WCF) - >>> frame_RCF # doctest: +SKIP - Frame(Point(-0.363, 0.003, -0.147), Vector(0.388, -0.351, -0.852), Vector(0.276, 0.926, -0.256)) # doctest: +SKIP - """ - frame_RCF = frame_WCF.transformed(self.transformation_WCF_RCF(group)) - return frame_RCF - - def to_world_coordinates(self, frame_RCF, group=None): - # type: (Frame, Optional[str]) -> Frame - """Represent a frame from the robot's coordinate system (RCF) in the world coordinate system (WCF). - - Parameters - ---------- - frame_RCF : :class:`compas.geometry.Frame` - A frame in the robot's coordinate frame. - group : :obj:`str`, optional - The name of the planning group. Defaults to the main planning group. - - Returns - ------- - :class:`compas.geometry.Frame` - A frame in the world coordinate frame. - - Examples - -------- - >>> robot = RobotLibrary.ur5() - >>> frame_RCF = Frame([-0.363, 0.003, -0.147], [0.388, -0.351, -0.852], [0.276, 0.926, -0.256]) - >>> frame_WCF = robot.to_world_coordinates(frame_RCF) - >>> frame_WCF # doctest: +SKIP - Frame(Point(-0.363, 0.003, -0.147), Vector(0.388, -0.351, -0.852), Vector(0.276, 0.926, -0.256)) # doctest: +SKIP - """ - frame_WCF = frame_RCF.transformed(self.transformation_RCF_WCF(group)) - return frame_WCF - - def _get_attached_tool_for_group(self, group_name=None): - # type: (Optional[str]) -> Tool - """Get the tool attached to the given planning group. Group name defaults to main_group_name. - Raises ValueError if group name is unknown or there is no tool currently attached to it""" - group = group_name or self.main_group_name - if group not in self.attached_tools: - raise ValueError("No tool attached to group {}".format(group)) - - return self.attached_tools[group] - - def from_tcf_to_t0cf(self, frames_tcf, group=None): - # type: (List[Frame], Optional[str]) -> List[Frame] - """Convert a list of frames at the robot's tool tip (tcf frame) to frames at the robot's flange (tool0 frame) using the attached tool. - - Parameters - ---------- - frames_tcf : :obj:`list` of :class:`compas.geometry.Frame` - Frames (in WCF) at the robot's tool tip (tcf). - - group : :obj:`str`, optional - The planning group whose tool to use. Defaults to the main - planning group. - - Returns - ------- - :obj:`list` of :class:`compas.geometry.Frame` - Frames (in WCF) at the robot's flange (tool0). - - Raises - ------ - Exception - If the attached tool is not set. - - Examples - -------- - >>> robot = RobotLibrary.ur5() - >>> mesh = Mesh.from_stl(compas_fab.get('planning_scene/cone.stl')) - >>> frame = Frame([0.14, 0, 0], [0, 1, 0], [0, 0, 1]) - >>> robot.attach_tool(Tool(mesh, frame)) - >>> frames_tcf = [Frame((-0.309, -0.046, -0.266), (0.276, 0.926, -0.256), (0.879, -0.136, 0.456))] - >>> robot.from_tcf_to_t0cf(frames_tcf) # doctest: +SKIP - [Frame(Point(-0.363, 0.003, -0.147), Vector(0.388, -0.351, -0.852), Vector(0.276, 0.926, -0.256))] # doctest: +SKIP - >>> robot.from_tcf_to_t0cf(frames_tcf, group=robot.main_group_name) # doctest: +SKIP - [Frame(Point(-0.363, 0.003, -0.147), Vector(0.388, -0.351, -0.852), Vector(0.276, 0.926, -0.256))] # doctest: +SKIP - """ - tool = self._get_attached_tool_for_group(group_name=group) - return tool.from_tcf_to_t0cf(frames_tcf) - - def from_t0cf_to_tcf(self, frames_t0cf, group=None): - # type: (List[Frame], Optional[str]) -> List[Frame] - """Convert frames at the robot's flange (tool0 frame) to frames at the robot's tool tip (tcf frame) using the attached tool. - - Parameters - ---------- - frames_t0cf : :obj:`list` of :class:`compas.geometry.Frame` - Frames (in WCF) at the robot's flange (tool0). - - group : :obj:`str`, optional - The planning group to attach this tool to. Defaults to the main - planning group. - - Returns - ------- - :obj:`list` of :class:`compas.geometry.Frame` - Frames (in WCF) at the robot's tool tip (tcf). - - Raises - ------ - :exc:`Exception` - If the robot has no attached tool defined. - - Examples - -------- - >>> robot = RobotLibrary.ur5() - >>> mesh = Mesh.from_stl(compas_fab.get('planning_scene/cone.stl')) - >>> frame = Frame([0.14, 0, 0], [0, 1, 0], [0, 0, 1]) - >>> robot.attach_tool(Tool(mesh, frame)) - >>> frames_t0cf = [Frame((-0.363, 0.003, -0.147), (0.388, -0.351, -0.852), (0.276, 0.926, -0.256))] - >>> robot.from_t0cf_to_tcf(frames_t0cf) # doctest: +SKIP - [Frame(Point(-0.309, -0.046, -0.266), Vector(0.276, 0.926, -0.256), Vector(0.879, -0.136, 0.456))] # doctest: +SKIP - >>> robot.from_t0cf_to_tcf(frames_t0cf, group=robot.main_group_name) # doctest: +SKIP - [Frame(Point(-0.309, -0.046, -0.266), Vector(0.276, 0.926, -0.256), Vector(0.879, -0.136, 0.456))] # doctest: +SKIP - """ - tool = self._get_attached_tool_for_group(group_name=group) - return tool.from_t0cf_to_tcf(frames_t0cf) - - def attach_tool(self, tool, group=None, touch_links=None): - # type: (Tool, Optional[str], Optional[List[str]]) -> None - """Attach a tool to the robot independently of the model definition. - - Parameters - ---------- - tool : :class:`Tool` - The tool that should be attached to the robot's flange. - group : :obj:`str`, optional - The planning group to attach this tool to. Defaults to the main - planning group. - touch_links : :obj:`list` of :obj:`str`, optional - A list of link names the end-effector is allowed to touch. Defaults - to the end-effector link. - - Returns - ------- - ``None`` - - See Also - -------- - :meth:`Robot.detach_tool` - - Examples - -------- - >>> robot = RobotLibrary.ur5() - >>> mesh = Mesh.from_stl(compas_fab.get('planning_scene/cone.stl')) - >>> frame = Frame([0.14, 0, 0], [0, 1, 0], [0, 0, 1]) - >>> tool = Tool(mesh, frame) - >>> robot.attach_tool(tool) - """ - group = group or self.main_group_name - if group not in self.semantics.group_names: - raise ValueError("No such group: {}".format(group)) - - if not tool.connected_to: - tool.connected_to = self.get_end_effector_link_name(group) - tool.update_touch_links(touch_links) - self.attached_tools[group] = tool - - if self.scene_object: - self.scene_object.attach_tool_model(tool.tool_model) - - def detach_tool(self, group=None): - # type: (Optional[str]) -> None - """Detach the attached tool. - - Parameters - ---------- - group : :obj:`str`, optional - The planning group to attach this tool to. Defaults to the main - planning group. - - See Also - -------- - :meth:`Robot.attach_tool` - """ - group = group or self.main_group_name - if group not in self.attached_tools: - raise ValueError("No tool attached to group {}".format(group)) - - tool_to_remove = self.attached_tools[group] - if self.scene_object and tool_to_remove: - self.scene_object.detach_tool_model(tool_to_remove.tool_model) - self.attached_tools.pop(group) - - # ========================================================================== - # checks - # ========================================================================== - - def ensure_semantics(self): - # type: () -> None - """Check if semantics is set. - - Raises - ------ - :exc:`Exception` - If :attr:`semantics` is not set. - """ - if not self.semantics: - raise Exception("This method is only callable once a semantic model is assigned.") - - def ensure_geometry(self): - # type: () -> None - """Check if the model's geometry has been loaded. - - Raises - ------ - :exc:`Exception` - If geometry has not been loaded. - """ - self.model.ensure_geometry() - - # ========================================================================== - # services - # ========================================================================== - - def inverse_kinematics( - self, - frame_WCF, - start_configuration=None, - group=None, - return_full_configuration=False, - use_attached_tool_frame=True, - options=None, - ): - # type: (Frame, Optional[Configuration], Optional[str], Optional[bool], Optional[bool], Optional[Dict[str, Any]]) -> Configuration - """Calculate the robot's inverse kinematic for a given frame. - - The inverse kinematic solvers are implemented as generators in order to fit both analytic - and numerical solver approaches. However, this method abstracts that away and returns one - configuration at a time to simplify its usage. - - To keep the usefulness of the generator, calls to this method will recall the last retrieved - iterator and keep returning results yielded by it, one at a time. This is true only as long as - the request is identical to the last one. If the arguments change, the last generator - is discarded and the client IK implementation is invoked again. - - Parameters - ---------- - frame_WCF : :class:`compas.geometry.Frame` - The frame to calculate the inverse kinematic for. - start_configuration : :class:`compas_robots.Configuration`, optional - If passed, the inverse will be calculated such that the calculated - joint positions differ the least from the start_configuration. - Defaults to the zero configuration. - group: :obj:`str`, optional - The planning group used for calculation. Defaults to the robot's - main planning group. - return_full_configuration : :obj:`bool`, optional - If ``True``, returns a full configuration with all joint values - specified, including passive ones if available. Defaults to ``False``. - use_attached_tool_frame : :obj:`bool`, optional - If ``True`` and there is a tool attached to the planning group, it will use its TCF - instead of the T0CF to calculate IK. Defaults to ``True``. - options: :obj:`dict`, optional - Dictionary containing the key-value pairs of additional options. - The valid options are specific to the backend in use. - Check the API reference of the IK backend implementation for more details. - - Raises - ------ - :exc:`compas_fab.backends.BackendError` - If no configuration can be found. - - Returns - ------- - :class:`compas_robots.Configuration` - An inverse kinematic solution represented as a configuration. - - Examples - -------- - >>> robot = RobotLibrary.ur5() - >>> frame_WCF = Frame([0.3, 0.1, 0.5], [1, 0, 0], [0, 1, 0]) - >>> start_configuration = robot.zero_configuration() - >>> group = robot.main_group_name - >>> robot.inverse_kinematics(frame_WCF, start_configuration, group) # doctest: +SKIP - Configuration((4.045, 5.130, -2.174, -6.098, -5.616, 6.283), (0, 0, 0, 0, 0, 0)) # doctest: +SKIP - """ - # TODO: Planning function wrappers in Robot class will be removed. - # Pseudo-memoized sequential calls will re-use iterator if not exhausted - request_id = "{}-{}-{}-{}-{}".format( - str(frame_WCF), str(start_configuration), str(group), str(return_full_configuration), str(options) - ) - - if self._current_ik["request_id"] == request_id and self._current_ik["solutions"] is not None: - solution = next(self._current_ik["solutions"], None) - if solution is not None: - return solution - - solutions = self.iter_inverse_kinematics( - frame_WCF, start_configuration, group, return_full_configuration, use_attached_tool_frame, options - ) - self._current_ik["request_id"] = request_id - self._current_ik["solutions"] = solutions - - return next(solutions) - - def iter_inverse_kinematics( - self, - frame_WCF, - start_configuration=None, - group=None, - return_full_configuration=False, - use_attached_tool_frame=True, - options=None, - ): - # type: (Frame, Optional[Configuration], Optional[str], Optional[bool], Optional[bool], Optional[Dict[str, Any]]) -> Generator[Configuration] - """Iterate over the inverse kinematic solutions of a robot. - - This method exposes the generator-based inverse kinematic solvers. Analytics solvers will return - generators that include all possible solutions, hence exhausting the iterator indicates there are - no more solutions to be found. Numerical solvers, on the other hand, will keep returning solutions - until one fails to be found (usually caused by a timeout rather than infeasibility) or until the user - code stops the iteration. - - Parameters - ---------- - frame_WCF : :class:`compas.geometry.Frame` - The frame to calculate the inverse kinematic for. - start_configuration : :class:`compas_robots.Configuration`, optional - If passed, the inverse will be calculated such that the calculated - joint positions differ the least from the start_configuration. - Defaults to the zero configuration. - group: :obj:`str`, optional - The planning group used for calculation. Defaults to the robot's - main planning group. - return_full_configuration : :obj:`bool`, optional - If ``True``, returns a full configuration with all joint values - specified, including passive ones if available. Defaults to ``False``. - use_attached_tool_frame : :obj:`bool`, optional - If ``True`` and there is a tool attached to the planning group, it will use its TCF - instead of the T0CF to calculate IK. Defaults to ``True``. - options: :obj:`dict`, optional - Dictionary containing the key-value pairs of additional options. - The valid options are specific to the backend in use. - Check the API reference of the IK backend implementation for more details. - - Raises - ------ - :exc:`compas_fab.backends.BackendError` - If no configuration can be found. - - Yields - ------ - :class:`compas_robots.Configuration` - An inverse kinematic solution represented as a configuration. - - Examples - -------- - >>> robot = RobotLibrary.ur5() - >>> frame_WCF = Frame([0.3, 0.1, 0.5], [1, 0, 0], [0, 1, 0]) - >>> start_configuration = robot.zero_configuration() - >>> group = robot.main_group_name - >>> next(robot.iter_inverse_kinematics(frame_WCF, start_configuration, group)) # doctest: +SKIP - Configuration((4.045, 5.130, -2.174, -6.098, -5.616, 6.283), (0, 0, 0, 0, 0, 0)) # doctest: +SKIP - """ - # TODO: Planning function wrappers in Robot class will be removed. - options = options or {} - attached_collision_meshes = options.get("attached_collision_meshes") or [] - - group = group or self.main_group_name if self.semantics else None - - start_configuration = self._check_full_configuration(start_configuration) - - attached_tool = self.attached_tools.get(group) - if use_attached_tool_frame and attached_tool: - frame_WCF = self.from_tcf_to_t0cf([frame_WCF], group)[0] - - frame_WCF_scaled = frame_WCF.copy() - frame_WCF_scaled.point /= self.scale_factor # must be in meters - - for tool in self.attached_tools.values(): - if tool: - attached_collision_meshes.extend(tool.attached_collision_meshes) - - options["attached_collision_meshes"] = attached_collision_meshes - - # TODO: self.client is removed, the following line needs fixed. - solutions = self.client.inverse_kinematics(self, frame_WCF_scaled, start_configuration, group, options) - - # The returned joint names might be more than the requested ones if there are passive joints present - for joint_positions, joint_names in solutions: - if joint_positions: - yield self._build_configuration(joint_positions, joint_names, group, return_full_configuration) - else: - yield None # to accommodate analytic ik with keeping the order of solutions - - def _build_configuration(self, joint_positions, joint_names, group, return_full_configuration): - # type: (List[float], List[str], str | None, bool) -> Configuration - # TODO: This will go obsolete - if return_full_configuration: - # build configuration including passive joints, but no sorting - joint_types = self.get_joint_types_by_names(joint_names) - configuration = Configuration(joint_positions, joint_types, joint_names) - else: - # sort values for group configuration - joint_state = dict(zip(joint_names, joint_positions)) - group_joint_names = self.get_configurable_joint_names(group) - values = [joint_state[name] for name in group_joint_names] - configuration = Configuration(values, self.get_configurable_joint_types(group), group_joint_names) - - return configuration.scaled(self.scale_factor) - - def forward_kinematics(self, configuration, group=None, tool_coordinate_frame=None, options=None): - # type: (Configuration, Optional[str], Optional[bool], Optional[Dict[str, Any]]) -> Frame - """Calculate the robot's forward kinematic. - - Parameters - ---------- - configuration: :class:`compas_fab.robots.Configuration` - The full configuration to calculate the forward kinematic for. If no - full configuration is passed, the zero-joint state for the other - configurable joints is assumed. - group: obj:`str`, optional - The planning group used for the calculation. Defaults to the robot's - main planning group. - tool_coordinate_frame : :class:`compas.geometry.Frame`, optional - The tool tip coordinate frame relative to the flange of the robot. - Units must be in meters. - If set, forward kinematics will return the TCF of the attached tool - instead of the T0CF. Defaults to ``None``. - options: obj:`dict`, optional - Dictionary containing the following key-value pairs: - - - ``"link"``: (:obj:`str`, optional) The name of the link to - calculate the forward kinematics for. Defaults to the group's end - effector link. - Backwards compatibility note: if there's no ``link`` option, the - planner will try also ``tool0`` as fallback before defaulting - to the end effector's link. - - There are additional options that are specific to the backend in use. - Check the API reference of the FK backend implementation for more details. - - Returns - ------- - :class:`compas.geometry.Frame` - The frame in the world's coordinate system (WCF). - - Raises - ------ - :exc:`ValueError` - If `link_name` doesn't match any of the :class:`Robot` instance's links. - :exc:`NotImplementedError` - If forward kinematic method for given `backend` is not implemented. - - Examples - -------- - >>> robot = RobotLibrary.ur5() - >>> configuration = Configuration.from_revolute_values([-2.238, -1.153, -2.174, 0.185, 0.667, 0.000]) - >>> group = robot.main_group_name - >>> frame_WCF_c = robot.forward_kinematics(configuration, group) - >>> options = {'solver': 'model'} - >>> frame_WCF_m = robot.forward_kinematics(configuration, group, options) - >>> frame_WCF_c == frame_WCF_m - True - """ - # TODO: Planning function wrappers in Robot class will be removed. - options = options or {} - - group = group or self.main_group_name if self.semantics else None - - full_configuration = self.merge_group_with_full_configuration(configuration, self.zero_configuration(), group) - full_configuration = self._check_full_configuration(full_configuration) - - # Calling forward_kinematics from Robot class will use the RobotModel's function to calculate FK - - link = options.get("link", options.get("tool0")) - link = link or self.get_end_effector_link_name(group) - if link not in self.get_link_names(group): - raise ValueError("Link name {} does not exist in planning group".format(link)) - - frame_WCF = self.model.forward_kinematics(full_configuration, link) - - if tool_coordinate_frame: - frame_WCF = from_t0cf_to_tcf(frame_WCF, tool_coordinate_frame) - - # Scale and return - frame_WCF.point *= self.scale_factor - - return frame_WCF - - def plan_cartesian_motion(self, waypoints, start_configuration=None, group=None, options=None): - # type: (Waypoints, Optional[Configuration], Optional[str], Optional[Dict[str, Any]]) -> JointTrajectory - """Calculate a cartesian motion path (linear in tool space). - - Parameters - ---------- - waypoints : :class:`compas_fab.robots.Waypoints` - The waypoints for the robot to follow. - For more information on how to define waypoints, see :ref:`waypoints`. - In addition, note that not all planning backends support all waypoint types, - check documentation of the backend in use for more details. - start_configuration : :class:`compas_robots.Configuration`, optional - The robot's full configuration, i.e. values for all configurable - joints of the entire robot, at the start of the motion. - Defaults to the all-zero configuration. - group : :obj:`str`, optional - The name of the planning group used for motion planning. - Defaults to the robot's main planning group. - options : :obj:`dict`, optional - Dictionary containing the following key-value pairs: - - - max_step :: :obj:`float`, optional - The approximate distance between the - calculated points. (Defined in the robot's units.) Defaults to ``0.01``. - - path_constraints :: :obj:`list` of :class:`compas_fab.robots.Constraint`, optional - Optional constraints that can be imposed along the solution path. - Note that path calculation won't work if the start_configuration - violates these constraints. Defaults to ``None``. - - attached_collision_meshes :: :obj:`list` of :class:`compas_fab.robots.AttachedCollisionMesh` - Defaults to ``None``. - - There are additional options that are specific to the backend in use. - Check the API reference of the cartesian motion planner backend implementation - for more details. - - Returns - ------- - :class:`JointTrajectory` - The calculated trajectory. - - Examples - -------- - - >>> with RosClient() as client: # doctest: +SKIP - #: This doctest can pass locally but persistently fails on CI in GitHub. "roslibpy.core.RosTimeoutError: Failed to connect to ROS" - ... robot = client.load_robot() - ... target_frames = [Frame([0.3, 0.1, 0.5], [1, 0, 0], [0, 1, 0]),\ - Frame([0.5, 0.1, 0.6], [1, 0, 0], [0, 1, 0])] - ... waypoints = FrameWaypoints(target_frames) - ... start_configuration = Configuration.from_revolute_values([-0.042, 0.033, -2.174, 5.282, -1.528, 0.000]) - ... group = robot.main_group_name - ... options = {'max_step': 0.01,\ - 'jump_threshold': 1.57,\ - 'avoid_collisions': True} - ... trajectory = robot.plan_cartesian_motion(waypoints,\ - start_configuration,\ - group=group,\ - options=options) - ... len(trajectory.points) > 1 - True - """ - # TODO: Planning function wrappers in Robot class will be removed. - # The plan_cartesian_motion method in the Robot class is a wrapper around planing backend's - # plan_cartesian_motion method. This method is responsible for scaling the waypoints, start_configuration and the planned trajectory. - # Some options may also need scaling, like max_step. This should be removed in the future. - # TODO: Discuss if we can have all options passed with a fixed unit to avoid scaling. - # Attached tools are also managed by this function. - - options = options or {} - - if not group: - group = self.main_group_name # ensure semantics - - # ======= - # Scaling - # ======= - need_scaling = not TOL.is_close(self.scale_factor, 1.0, rtol=1e-8) - - if need_scaling: - waypoints = waypoints.scaled(1.0 / self.scale_factor) - - max_step = options.get("max_step") - if need_scaling and max_step: - options["max_step"] = max_step / self.scale_factor - - # NOTE: start_configuration has to be a full robot configuration, such - # that all configurable joints of the whole robot are defined for planning. - start_configuration = self._check_full_configuration(start_configuration) - - # Path constraints are only relevant to ROS Backend - path_constraints = options.get("path_constraints") - if need_scaling and path_constraints: - path_constraints_WCF_scaled = [] - for c in path_constraints: - cp = c.copy() - if c.type == Constraint.JOINT: - joint = self.get_joint_by_name(c.joint_name) - if joint.is_scalable(): - cp.scale(1.0 / self.scale_factor) - else: - cp.scale(1.0 / self.scale_factor) - path_constraints_WCF_scaled.append(cp) - options["path_constraints"] = path_constraints_WCF_scaled - - # ===================== - # Attached CM and Tools - # ===================== - - attached_collision_meshes = options.get("attached_collision_meshes") or [] - # Collect attached collision meshes from attached tools - # TODO: Discuss why scaling is not happening here? - for _, tool in self.attached_tools.items(): - if tool: - attached_collision_meshes.extend(tool.attached_collision_meshes) - options["attached_collision_meshes"] = attached_collision_meshes - - # ======== - # Planning - # ======== - - # TODO: self.client is removed, the following line needs fixed. - trajectory = self.client.plan_cartesian_motion( - robot=self, - waypoints=waypoints, - start_configuration=start_configuration, - group=group, - options=options, - ) - - # ========================= - # Scaling result trajectory - # ========================= - - # Scale everything back to robot's scale - for pt in trajectory.points: - pt.scale(self.scale_factor) - - trajectory.start_configuration.scale(self.scale_factor) - - return trajectory - - def plan_motion(self, target, start_configuration=None, group=None, options=None): - # type: (Target, Optional[Configuration], Optional[str], Optional[Dict[str, Any]]) -> JointTrajectory - """Calculate a motion path. - - Parameters - ---------- - target : :class:`compas_fab.robots.Target` - The target to be achieved by the robot at the end of the motion. - See :ref:`targets`. - start_configuration : :class:`compas_robots.Configuration`, optional - The robot's full configuration, i.e. values for all configurable - joints of the entire robot, at the starting position. Defaults to - the all-zero configuration. - group : :obj:`str`, optional - The name of the planning group used to define the movable joints. - The planning group must match with one of the groups defined in the robot's semantics. - Defaults to the robot's main planning group. - options : :obj:`dict`, optional - Dictionary containing the following key-value pairs: - - - path_constraints :: :obj:`list` of :class:`Constraint`, optional - Optional constraints that can be imposed along the solution path. - Note that path calculation won't work if the start_configuration - violates these constraints. Defaults to ``None``. - - attached_collision_meshes :: :obj:`list` of :class:`compas_fab.robots.AttachedCollisionMesh` - Defaults to ``None``. - - There are additional options that are specific to the backend in use. - Check the API reference of the motion planner backend implementation - for more details. - - Returns - ------- - :class:`JointTrajectory` - The calculated trajectory. - - Examples - --------ssssss - - Using a :class:`~compas_fab.robots.FrameTarget` : - - >>> with RosClient() as client: # doctest: +SKIP - #: This doctest can pass locally but persistently fails on CI in GitHub. "roslibpy.core.RosTimeoutError: Failed to connect to ROS" - ... robot = client.load_robot() - ... frame = Frame([0.4, 0.3, 0.4], [0, 1, 0], [0, 0, 1]) - ... tolerance_position = 0.001 - ... tolerance_orientation = math.radians(1) - ... start_configuration = Configuration.from_revolute_values([-0.042, 4.295, 0, -3.327, 4.755, 0.]) - ... group = robot.main_group_name - ... target = FrameTarget(frame, TargetMode.ROBOT, tolerance_position, tolerance_orientation) - ... goal_constraints = robot.constraints_from_frame(frame, tolerance_position, tolerances_axes, group) - ... trajectory = robot.plan_motion((target, start_configuration, group, {'planner_id': 'RRTConnect'}) - ... print(trajectory.fraction) - 1.0 - - - Using a :class:`~compas_fab.robots.ConfigurationTarget` : - - >>> with RosClient() as client: # doctest: +SKIP - #: This doctest can pass locally but persistently fails on CI in GitHub. "roslibpy.core.RosTimeoutError: Failed to connect to ROS" - ... robot = client.load_robot() - ... joint_names = robot.get_configurable_joint_names() - ... configuration = Configuration.from_revolute_values([0.0, -1.5707, 0.0, -1.5707, 0.0, 0.0], joint_names) - ... tolerances_above = [math.radians(5)] * len(configuration.joint_values) - ... tolerances_below = [math.radians(5)] * len(configuration.joint_values) - ... group = robot.main_group_name - ... target = ConfigurationTarget(configuration, tolerances_above, tolerances_below) - ... trajectory = robot.plan_motion(target, start_configuration, group, {'planner_id': 'RRTConnect'}) - ... print(trajectory.fraction) - 1.0 - - """ - # TODO: Planning function wrappers in Robot class will be removed. - options = options or {} - path_constraints = options.get("path_constraints") - attached_collision_meshes = options.get("attached_collision_meshes") or [] - - # TODO: for the motion plan request a list of possible goal constraints - # can be passed, from which the planner will try to find a path that - # satisfies at least one of the specified goal constraints. For now only - # one set of goal constraints is supported. - - # TODO: add workspace_parameters - - if not group: - group = self.main_group_name - - # NOTE: start_configuration has to be a full robot configuration, such - # that all configurable joints of the whole robot are defined for planning. - start_configuration = self._check_full_configuration(start_configuration) - - # Scale Target Definitions - if self.scale_factor != 1.0: - target = target.scaled(1.0 / self.scale_factor) - - # Transform path constraints to RCF and scale - if path_constraints: - path_constraints_WCF_scaled = [] - for c in path_constraints: - cp = c.copy() - if c.type == Constraint.JOINT: - joint = self.get_joint_by_name(c.joint_name) - if joint.is_scalable(): - cp.scale(1.0 / self.scale_factor) - else: - cp.scale(1.0 / self.scale_factor) - path_constraints_WCF_scaled.append(cp) - else: - path_constraints_WCF_scaled = None - - for tool in self.attached_tools.values(): - if tool: - attached_collision_meshes.extend(tool.attached_collision_meshes) - - options["attached_collision_meshes"] = attached_collision_meshes - options["path_constraints"] = path_constraints_WCF_scaled - - # TODO: self.client is removed, the following line needs fixed. - trajectory = self.client.plan_motion( - robot=self, - target=target, - start_configuration=start_configuration, - group=group, - options=options, - ) - - # Scale everything back to robot's scale - for pt in trajectory.points: - pt.scale(self.scale_factor) - - trajectory.start_configuration.scale(self.scale_factor) - - return trajectory - - def transformed_frames(self, configuration, group=None): - # type: (Configuration, Optional[str]) -> List[Frame] - """Get the robot's transformed frames. - - Parameters - ---------- - configuration : :class:`compas_robots.Configuration` - Configuration to compute transformed frames for. - group : :obj:`str`, optional - The planning group used for the calculation. Defaults to the robot's - main planning group. - - Returns - ------- - :obj:`list` of :class:`compas.geometry.Frame` - Transformed frames. - """ - if not len(configuration.joint_names): - configuration.joint_names = self.get_configurable_joint_names(group) - return self.model.transformed_frames(configuration) - - def transformed_axes(self, configuration, group=None): - # type: (Configuration, Optional[str]) -> List[Vector] - """Get the robot's transformed axes. - - Parameters - ---------- - configuration : :class:`compas_robots.Configuration` - Configuration to compute transformed axes for. - group : :obj:`str`, optional - The planning group used for the calculation. Defaults to the robot's - main planning group. - - Returns - ------- - :obj:`list` of :class:`compas.geometry.Vector` - Transformed axes. - """ - if not len(configuration.joint_names): - configuration.joint_names = self.get_configurable_joint_names(group) - return self.model.transformed_axes(configuration) - - # ========================================================================== - # drawing - # ========================================================================== - - def update(self, configuration, group=None, visual=True, collision=True): - # type: (Configuration, Optional[str], Optional[bool], Optional[bool]) -> None - """Update the robot's geometry. - - Parameters - ---------- - configuration : :class:`compas_robots.Configuration` - Instance of the configuration (joint state) to move to. - group : :obj:`str`, optional - The name of the group to plan for. Defaults to the robot's main - planning group. - visual : :obj:`bool`, optional - ``True`` if the visual geometry should be also updated, otherwise ``False``. - Defaults to ``True``. - collision : :obj:`bool`, optional - ``True`` if the collision geometry should be also updated, otherwise ``False``. - Defaults to ``True``. - """ - group = group or self.main_group_name if self.semantics else None - - if not len(configuration.joint_names): - configuration.joint_names = self.get_configurable_joint_names(group) - - self.scene_object.update(configuration, visual, collision) - - def draw_visual(self): - # type: () -> None - """Draw the robot's visual geometry using the defined :attr:`Robot.scene_object`.""" - return self.scene_object.draw_visual() - - def draw_collision(self): - # type: () -> None - """Draw the robot's collision geometry using the defined :attr:`Robot.scene_object`.""" - return self.scene_object.draw_collision() - - def draw(self): - # type: () -> None - """Alias of :meth:`draw_visual`.""" - return self.draw_visual() + def draw(self): + # type: () -> None + """Alias of :meth:`draw_visual`.""" + return self.draw_visual() # TODO: add scene_object.draw_attached_tool # def draw_attached_tool(self): # """Draw the attached tool using the defined :attr:`Robot.scene_object`.""" # if self.scene_object and self.attached_tool: # return self.scene_object.draw_attached_tool() - - def scale(self, factor): - # type: (float) -> None - """Scale the robot geometry by a factor (absolute). - - Parameters - ---------- - factor : :obj:`float` - The factor to scale the robot with. - - Returns - ------- - ``None`` - """ - self.model.scale(factor) - if self.scene_object: - self.scene_object.scale(factor) - else: - self._scale_factor = factor - - @property - def scale_factor(self): - # type: () -> float - """A scale factor affecting planning targets, results and visualization. Typically, robot models are defined in meters, - if used in CAD environment where units is mm, use a scale_factor of 1000. - - """ - if self.scene_object: - return self.scene_object.scale_factor - else: - return self._scale_factor - - @property - def need_scaling(self): - # type: () -> bool - """Check if the robot's geometry needs scaling.""" - return not TOL.is_close(self.scale_factor, 1.0, rtol=1e-8) - - def info(self): - # type: () -> None - """Print information about the robot.""" - print("The robot's name is '{}'.".format(self.name)) - if self.semantics: - print("The planning groups are:", self.group_names) - print("The main planning group is '{}'.".format(self.main_group_name)) - configurable_joints = self.get_configurable_joints(self.main_group_name) - else: - configurable_joints = self.get_configurable_joints() - print("The end-effector's name is '{}'.".format(self.get_end_effector_link_name())) - if self.attached_tools: - for tool in self.attached_tools.values(): - print("The robot has a tool at the {} link attached.".format(tool.connected_to)) - else: - print("The robot has NO tool attached.") - print("The base link's name is '{}'".format(self.get_base_link_name())) - print("The base_frame is:", self.get_base_frame()) - print("The robot's joints are:") - for joint in configurable_joints: - info = "\t* '{}' is of type '{}'".format(joint.name, list(Joint.SUPPORTED_TYPES)[joint.type]) - if joint.limit: - info += " and has limits [{:.3f}, {:.3f}]".format(joint.limit.upper, joint.limit.lower) - print(info) - print("The robot's links are:") - print([link.name for link in self.model.links]) diff --git a/src/compas_fab/robots/robot_cell.py b/src/compas_fab/robots/robot_cell.py index afe811d87..74b4afa6a 100644 --- a/src/compas_fab/robots/robot_cell.py +++ b/src/compas_fab/robots/robot_cell.py @@ -246,8 +246,8 @@ def group_names(self): Examples -------- - >>> robot = RobotLibrary.ur5() - >>> robot.group_names + >>> robot_cell, robot_cell_state = RobotCellLibrary.ur5() + >>> robot_cell.group_names ['manipulator', 'endeffector'] """ @@ -268,8 +268,8 @@ def group_states(self): Examples -------- - >>> robot = RobotLibrary.ur5() - >>> sorted(robot.group_states['manipulator'].keys()) + >>> robot_cell, robot_cell_state = RobotCellLibrary.ur5() + >>> sorted(robot_cell.group_states['manipulator'].keys()) ['home', 'up'] """ @@ -291,8 +291,8 @@ def get_end_effector_link_name(self, group=None): Examples -------- - >>> robot = RobotLibrary.ur5() - >>> robot.get_end_effector_link_name() + >>> robot_cell, robot_cell_state = RobotCellLibrary.ur5() + >>> robot_cell.get_end_effector_link_name() 'tool0' """ return self.robot_semantics.get_end_effector_link_name(group or self.main_group_name) @@ -312,8 +312,8 @@ def get_end_effector_link(self, group=None): Examples -------- - >>> robot = RobotLibrary.ur5() - >>> link = robot.get_end_effector_link() + >>> robot_cell, robot_cell_state = RobotCellLibrary.ur5() + >>> link = robot_cell.get_end_effector_link() >>> link.name 'tool0' """ @@ -335,8 +335,8 @@ def get_base_link_name(self, group=None): Examples -------- - >>> robot = RobotLibrary.ur5() - >>> robot.get_base_link_name() + >>> robot_cell, robot_cell_state = RobotCellLibrary.ur5() + >>> robot_cell.get_base_link_name() 'base_link' """ return self.robot_semantics.get_base_link_name(group or self.main_group_name) @@ -356,8 +356,8 @@ def get_base_link(self, group=None): Examples -------- - >>> robot = RobotLibrary.ur5() - >>> link = robot.get_base_link() + >>> robot_cell, robot_cell_state = RobotCellLibrary.ur5() + >>> link = robot_cell.get_base_link() >>> link.name 'base_link' """ @@ -379,8 +379,8 @@ def get_link_names(self, group=None): Examples -------- - >>> robot = RobotLibrary.ur5() - >>> robot.get_link_names('manipulator') + >>> robot_cell, robot_cell_state = RobotCellLibrary.ur5() + >>> robot_cell.get_link_names('manipulator') ['base_link', 'base_link_inertia', 'shoulder_link', 'upper_arm_link', 'forearm_link', 'wrist_1_link', 'wrist_2_link', 'wrist_3_link', 'flange', 'tool0'] """ group = group or self.main_group_name @@ -404,8 +404,8 @@ def get_link_names_with_collision_geometry(self): Examples -------- - >>> robot = RobotLibrary.ur5() - >>> robot.get_link_names_with_collision_geometry() + >>> robot_cell, robot_cell_state = RobotCellLibrary.ur5() + >>> robot_cell.get_link_names_with_collision_geometry() ['base_link_inertia', 'shoulder_link', 'upper_arm_link', 'forearm_link', 'wrist_1_link', 'wrist_2_link', 'wrist_3_link'] """ return [link.name for link in self.robot_model.iter_links() if link.collision] @@ -498,8 +498,8 @@ def get_configurable_joint_types(self, group=None): Examples -------- - >>> robot = RobotLibrary.ur5() - >>> robot.get_configurable_joint_types('manipulator') + >>> robot_cell, robot_cell_state = RobotCellLibrary.ur5() + >>> robot_cell.get_configurable_joint_types('manipulator') [0, 0, 0, 0, 0, 0] """ group = group or self.main_group_name @@ -589,8 +589,8 @@ def zero_configuration(self, group=None): Examples -------- - >>> robot = RobotLibrary.ur5() - >>> robot.zero_configuration('manipulator') + >>> robot_cell, robot_cell_state = RobotCellLibrary.ur5() + >>> robot_cell.zero_configuration('manipulator') Configuration((0.000, 0.000, 0.000, 0.000, 0.000, 0.000), (0, 0, 0, 0, 0, 0), \ ('shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint')) """ @@ -1148,7 +1148,7 @@ def pcf_to_target_frames(self, robot_cell_state, frame_or_frames, target_mode, g # Debug Functions # ---------------------------------------- - def info(self): + def print_info(self): # type: () -> None """Print information about the robot.""" print("The robot's name is '{}'.".format(self.robot_model.name)) diff --git a/src/compas_fab/robots/robot_library.py b/src/compas_fab/robots/robot_library.py index 119e4a499..3b9eedf07 100644 --- a/src/compas_fab/robots/robot_library.py +++ b/src/compas_fab/robots/robot_library.py @@ -336,9 +336,13 @@ class RobotCellLibrary(object): >>> robot_cell_state.get """ + # --------------------------------------------------------------------- + # Robot Cells with only Robots + # --------------------------------------------------------------------- + @classmethod def rfl(cls, load_geometry=True): - # type: (Optional[bool]) -> RobotCell + # type: (Optional[bool]) -> Tuple[RobotCell, RobotCellState] """Create and return the RFL robot with 4 ABB irb 4600 and twin-gantry setup. The returned :class:`compas_fab.robots.Robot` object contains the robot model and semantics. @@ -360,12 +364,13 @@ def rfl(cls, load_geometry=True): srdf_filename=compas_fab.get("robot_library/rfl/robot_description_semantic.srdf"), local_package_mesh_folder="robot_library/rfl" if load_geometry else None, ) + robot_cell_state = RobotCellState.from_robot_cell(robot_cell) - return robot_cell + return robot_cell, robot_cell_state @classmethod def ur5(cls, load_geometry=True): - # type: (Optional[bool]) -> RobotCell + # type: (Optional[bool]) -> Tuple[RobotCell, RobotCellState] """Returns a UR5 robot. The returned :class:`compas_fab.robots.Robot` object contains the robot model and semantics. @@ -390,12 +395,13 @@ def ur5(cls, load_geometry=True): srdf_filename=compas_fab.get("robot_library/ur5_robot/robot_description_semantic.srdf"), local_package_mesh_folder="robot_library/ur5_robot" if load_geometry else None, ) + robot_cell_state = RobotCellState.from_robot_cell(robot_cell) - return robot_cell + return robot_cell, robot_cell_state @classmethod def ur10e(cls, load_geometry=True): - # type: (Optional[bool]) -> RobotCell + # type: (Optional[bool]) -> Tuple[RobotCell, RobotCellState] """Returns a UR10e robot. The returned :class:`compas_fab.robots.Robot` object contains the robot model and semantics. @@ -417,12 +423,13 @@ def ur10e(cls, load_geometry=True): srdf_filename=compas_fab.get("robot_library/ur10e_robot/robot_description_semantic.srdf"), local_package_mesh_folder="robot_library/ur10e_robot" if load_geometry else None, ) + robot_cell_state = RobotCellState.from_robot_cell(robot_cell) - return robot_cell + return robot_cell, robot_cell_state @classmethod def abb_irb4600_40_255(cls, load_geometry=True): - # type: (Optional[bool]) -> RobotCell + # type: (Optional[bool]) -> Tuple[RobotCell, RobotCellState] """Returns a ABB irb4600-40/2.55 robot. The returned :class:`compas_fab.robots.Robot` object contains the robot model and semantics. @@ -444,12 +451,13 @@ def abb_irb4600_40_255(cls, load_geometry=True): srdf_filename=compas_fab.get("robot_library/abb_irb4600_40_255/robot_description_semantic.srdf"), local_package_mesh_folder="robot_library/abb_irb4600_40_255" if load_geometry else None, ) + robot_cell_state = RobotCellState.from_robot_cell(robot_cell) - return robot_cell + return robot_cell, robot_cell_state @classmethod def abb_irb120_3_58(cls, load_geometry=True): - # type: (Optional[bool]) -> RobotCell + # type: (Optional[bool]) -> Tuple[RobotCell, RobotCellState] """Returns a ABB irb120-3/58 robot. The returned :class:`compas_fab.robots.Robot` object contains the robot model and semantics. @@ -471,12 +479,13 @@ def abb_irb120_3_58(cls, load_geometry=True): srdf_filename=compas_fab.get("robot_library/abb_irb120_3_58/robot_description_semantic.srdf"), local_package_mesh_folder="robot_library/abb_irb120_3_58" if load_geometry else None, ) + robot_cell_state = RobotCellState.from_robot_cell(robot_cell) - return robot_cell + return robot_cell, robot_cell_state @classmethod def panda(cls, load_geometry=True): - # type: (Optional[bool]) -> RobotCell + # type: (Optional[bool]) -> Tuple[RobotCell, RobotCellState] """Returns a Panda robot. The returned :class:`compas_fab.robots.Robot` object contains the robot model and semantics. @@ -509,7 +518,13 @@ def panda(cls, load_geometry=True): robot_model.remove_link(link.name) robot_model.remove_joint(link.parent_joint.name) - return robot_cell + robot_cell_state = RobotCellState.from_robot_cell(robot_cell) + + return robot_cell, robot_cell_state + + # --------------------------------------------------------------------- + # Robot Cells with Tools and Rigid Bodies + # --------------------------------------------------------------------- @classmethod def ur5_cone_tool(cls, load_geometry=True): @@ -535,7 +550,7 @@ def ur5_cone_tool(cls, load_geometry=True): # --------------------------------------------------------------------- # Load Robot and create RobotCell # --------------------------------------------------------------------- - robot_cell = RobotCellLibrary.ur5(load_geometry=load_geometry) + robot_cell, robot_cell_state = RobotCellLibrary.ur5(load_geometry=load_geometry) # --------------------------------------------------------------------- # Load Tools @@ -555,7 +570,7 @@ def ur5_cone_tool(cls, load_geometry=True): robot_cell.rigid_body_models["floor"] = RigidBody.from_mesh(floor_mesh) # ------------------------------------------------------------------------ - # Create RobotCellState + # Re-Create RobotCellState after modifying the RobotCell # ------------------------------------------------------------------------ robot_cell_state = RobotCellState.from_robot_cell(robot_cell) @@ -603,7 +618,7 @@ def abb_irb4600_40_255_gripper_one_beam(cls, load_geometry=True): # --------------------------------------------------------------------- # Load Robot and create RobotCell # --------------------------------------------------------------------- - robot_cell = RobotCellLibrary.abb_irb4600_40_255(load_geometry=load_geometry) + robot_cell, robot_cell_state = RobotCellLibrary.abb_irb4600_40_255(load_geometry=load_geometry) # --------------------------------------------------------------------- # Load Tools @@ -629,7 +644,7 @@ def abb_irb4600_40_255_gripper_one_beam(cls, load_geometry=True): robot_cell.rigid_body_models["floor"] = RigidBody.from_mesh(floor_mesh) # ------------------------------------------------------------------------ - # Create RobotCellState + # Re-Create RobotCellState after modifying the RobotCell # ------------------------------------------------------------------------ robot_cell_state = RobotCellState.from_robot_cell(robot_cell) @@ -670,7 +685,7 @@ def ur10e_gripper_one_beam(cls, load_geometry=True): # --------------------------------------------------------------------- # Load Robot and create RobotCell # --------------------------------------------------------------------- - robot_cell = RobotCellLibrary.ur10e(load_geometry=load_geometry) + robot_cell, robot_cell_state = RobotCellLibrary.ur10e(load_geometry=load_geometry) # --------------------------------------------------------------------- # Load Tools @@ -696,7 +711,7 @@ def ur10e_gripper_one_beam(cls, load_geometry=True): robot_cell.rigid_body_models["floor"] = RigidBody.from_mesh(floor_mesh) # ------------------------------------------------------------------------ - # Create RobotCellState + # Re-Create RobotCellState after modifying the RobotCell # ------------------------------------------------------------------------ robot_cell_state = RobotCellState.from_robot_cell(robot_cell) @@ -710,7 +725,7 @@ def ur10e_gripper_one_beam(cls, load_geometry=True): # Gripper is allowed to touch the last link of the robot. # However, do not use the following line to get the end effector link name, # because it is not guaranteed to be the last link that has geometry in the robot chain. - # ee_link_name = robot.get_end_effector_link_name(robot.main_group_name) + # ee_link_name = robot.get_end_effector_link_name(robot_cell.main_group_name) # Instead, check the robot model and hard code the actual link name. touch_links = ["wrist_3_link"] @@ -764,7 +779,7 @@ def abb_irb4600_40_255_printing_tool(cls, load_geometry=True): # --------------------------------------------------------------------- # Load Robot and create RobotCell # --------------------------------------------------------------------- - robot_cell = RobotCellLibrary.abb_irb4600_40_255(load_geometry=load_geometry) + robot_cell, robot_cell_state = RobotCellLibrary.abb_irb4600_40_255(load_geometry=load_geometry) # --------------------------------------------------------------------- # Load Tools @@ -782,7 +797,7 @@ def abb_irb4600_40_255_printing_tool(cls, load_geometry=True): robot_cell.rigid_body_models["floor"] = RigidBody.from_mesh(floor_mesh) # ------------------------------------------------------------------------ - # Create RobotCellState + # Re-Create RobotCellState after modifying the RobotCell # ------------------------------------------------------------------------ robot_cell_state = RobotCellState.from_robot_cell(robot_cell) @@ -809,18 +824,6 @@ def abb_irb4600_40_255_printing_tool(cls, load_geometry=True): if __name__ == "__main__": - # TODO: Clean up the development code below - - # robot = RobotLibrary.rfl(load_geometry=True) - # robot.info() - - # robot = RobotLibrary.ur5(load_geometry=True) - # robot.info() - - # robot_cell, robot_cell_state = RobotCellLibrary.ur5_cone_tool(load_geometry=True) - # robot_cell.robot.info() - # robot_cell_state.get_attached_tool_id(robot_cell.robot.main_group_name) - # ---------------------------- # Visualize Tool with compas_viewer # ---------------------------- @@ -831,7 +834,6 @@ def abb_irb4600_40_255_printing_tool(cls, load_geometry=True): viewer = Viewer() viewer.renderer.rendermode = "lighted" - # model = robot.model model = ToolLibrary.static_gripper(load_geometry=True) robot_object = viewer.scene.add(model, show_lines=False) # type: RobotModelObject diff --git a/src/compas_fab/robots/semantics.py b/src/compas_fab/robots/semantics.py index e3a9a4f32..5c508d8cd 100644 --- a/src/compas_fab/robots/semantics.py +++ b/src/compas_fab/robots/semantics.py @@ -373,10 +373,10 @@ def _get_disabled_collisions(root): # it prints the data object as a json string using the UR5 robot as example. if __name__ == "__main__": - from compas_fab.robots import RobotLibrary + from compas_fab.robots import RobotCellLibrary - robot = RobotLibrary.ur5() - semantics = robot.semantics + robot_cell, robot_cell_state = RobotCellLibrary.ur5() + semantics = robot_cell.robot_semantics json = semantics.to_jsonstring(pretty=True) print(json) diff --git a/src/compas_fab/robots/trajectory.py b/src/compas_fab/robots/trajectory.py index 55132a991..859a645c0 100644 --- a/src/compas_fab/robots/trajectory.py +++ b/src/compas_fab/robots/trajectory.py @@ -9,7 +9,6 @@ from compas_robots import Configuration from compas_robots.configuration import FixedLengthList -from compas_fab.robots import AttachedCollisionMesh from compas_fab.robots.time_ import Duration if not IPY: @@ -311,8 +310,6 @@ class JointTrajectory(Trajectory): fraction : :obj:`float`, optional Indicates the percentage of requested trajectory that was calculated, e.g. ``1`` means the full trajectory was found. - attached_collision_meshes : :obj:`list` of :class:`compas_fab.robots.AttachedCollisionMesh` - The attached collision meshes included in the calculation of this trajectory. attributes : :obj:`dict` Custom attributes of the trajectory. @@ -327,8 +324,6 @@ class JointTrajectory(Trajectory): fraction : :obj:`float` Indicates the percentage of requested trajectory that was calculated, e.g. ``1`` means the full trajectory was found. - attached_collision_meshes : :obj:`list` of :class:`compas_fab.robots.AttachedCollisionMesh` - The attached collision meshes included in the calculation of this trajectory. attributes : :obj:`dict` data : :obj:`dict` The data representing the trajectory. @@ -340,16 +335,14 @@ def __init__( joint_names=None, start_configuration=None, fraction=None, - attached_collision_meshes=None, attributes=None, ): - # type: (List[JointTrajectoryPoint], List[str], Configuration, float, List[AttachedCollisionMesh], Dict[str, Any]) -> None + # type: (List[JointTrajectoryPoint], List[str], Configuration, float, Dict[str, Any]) -> None super(JointTrajectory, self).__init__(attributes=attributes) self.points = trajectory_points or [] self.joint_names = joint_names or [] self.start_configuration = start_configuration self.fraction = fraction - self.attached_collision_meshes = attached_collision_meshes or [] @property def __data__(self): @@ -359,7 +352,6 @@ def __data__(self): data_obj["joint_names"] = self.joint_names or [] data_obj["start_configuration"] = self.start_configuration.__data__ if self.start_configuration else None data_obj["fraction"] = self.fraction - data_obj["attached_collision_meshes"] = [acm.__data__ for acm in self.attached_collision_meshes] data_obj["planning_time"] = self.planning_time data_obj["attributes"] = self.attributes @@ -373,16 +365,12 @@ def __from_data__(cls, data): start_configuration = data.get("start_configuration", None) start_configuration = Configuration.__from_data__(start_configuration) if start_configuration else None fraction = data.get("fraction") - attached_collision_meshes = [ - AttachedCollisionMesh.__from_data__(acm_data) for acm_data in data.get("attached_collision_meshes", []) - ] trajectory = cls( trajectory_points=points, joint_names=joint_names, start_configuration=start_configuration, fraction=fraction, - attached_collision_meshes=attached_collision_meshes, attributes=data["attributes"], ) trajectory.planning_time = data["planning_time"] diff --git a/tests/backends/kinematics/test_analytical_kinematics_planner.py b/tests/backends/kinematics/test_analytical_kinematics_planner.py index a8804e00d..e2fe770ba 100644 --- a/tests/backends/kinematics/test_analytical_kinematics_planner.py +++ b/tests/backends/kinematics/test_analytical_kinematics_planner.py @@ -14,7 +14,7 @@ from compas_fab.robots import FrameTarget from compas_fab.robots import RobotCell from compas_fab.robots import RobotCellState -from compas_fab.robots import RobotLibrary +from compas_fab.robots import RobotCellLibrary from compas_fab.robots import TargetMode if not IPY: @@ -36,12 +36,11 @@ def ur5_planner_robot_only(): planner = AnalyticalKinematicsPlanner(solver) # Set up the robot cell with only the robot - robot = RobotLibrary.ur5(load_geometry=False) # No need to load the geometry because no CC - robot_cell = RobotCell(robot) + # (No need to load the geometry because no CC) + robot_cell, robot_cell_state = RobotCellLibrary.ur5(load_geometry=False) planner.set_robot_cell(robot_cell) # Set Initial RobotCellState - robot_cell_state = RobotCellState.from_robot_cell(robot_cell) planner.set_robot_cell_state(robot_cell_state) return planner diff --git a/tests/backends/kinematics/test_analytical_pybullet_planner.py b/tests/backends/kinematics/test_analytical_pybullet_planner.py index 860a63a2c..71c709f77 100644 --- a/tests/backends/kinematics/test_analytical_pybullet_planner.py +++ b/tests/backends/kinematics/test_analytical_pybullet_planner.py @@ -75,9 +75,7 @@ def forward_inverse_agreement(planner, start_state, options=None): # client = analytical_pybullet_client # planner = AnalyticalPyBulletPlanner(client, UR5Kinematics()) -# robot = RobotLibrary.ur5() -# robot_cell = RobotCell(robot) -# robot_cell_state = RobotCellState.from_robot_cell(robot_cell) +# robot_cell, robot_cell_state = RobotCellLibrary.ur5() # planner.set_robot_cell(robot_cell, robot_cell_state) # # TODO: This problem will be solved after we improved the AnalyticalKinematics class diff --git a/tests/backends/pybullet/test_pybullet_client.py b/tests/backends/pybullet/test_pybullet_client.py index aa3e1e5e4..0ae746d77 100644 --- a/tests/backends/pybullet/test_pybullet_client.py +++ b/tests/backends/pybullet/test_pybullet_client.py @@ -3,9 +3,9 @@ import compas_fab from compas_fab.backends import PyBulletClient -from compas_fab.robots import Robot +from compas_fab.robots import RobotSemantics from compas_fab.robots import RobotCell -from compas_fab.robots import RobotLibrary +from compas_fab.robots import RobotCellLibrary def test_pybullet_client_connection_direct(): @@ -13,31 +13,31 @@ def test_pybullet_client_connection_direct(): assert client.is_connected -def test_pybullet_client_set_robot_from_urdf(): +def test_pybullet_client_set_robot(): # Testing workflow of loading robot from URDF with PyBulletClient(connection_type="direct") as client: urdf_filename = compas_fab.get("robot_library/ur5_robot/urdf/robot_description.urdf") srdf_filename = compas_fab.get("robot_library/ur5_robot/robot_description_semantic.srdf") - robot = Robot.from_urdf(urdf_filename, srdf_filename) + robot_cell = RobotCell.from_urdf_and_srdf(urdf_filename, srdf_filename) # Assert that set_robot can only be performed with geometry with pytest.raises(Exception): - robot = client.set_robot(robot) + client.set_robot(robot_cell.robot_model, robot_cell.robot_semantics) # Load robot with geometry mesh_folder = "robot_library/ur5_robot" - robot = Robot.from_urdf(urdf_filename, srdf_filename, mesh_folder) - robot = client.set_robot(robot) + robot_cell = RobotCell.from_urdf_and_srdf(urdf_filename, srdf_filename, mesh_folder) + client.set_robot(robot_cell.robot_model, robot_cell.robot_semantics) - assert isinstance(robot, Robot) - assert robot.name == "ur5_robot" # Check that the RobotModel is present - assert isinstance(robot.model, RobotModel) + assert isinstance(robot_cell.robot_model, RobotModel) + assert isinstance(robot_cell.robot_semantics, RobotSemantics) + assert robot_cell.name == "ur5_robot" # Check that the robot have geometry - robot.ensure_geometry() + robot_cell.ensure_geometry() # Check that the robot have semantics - robot.ensure_semantics() - link_names = robot.get_link_names_with_collision_geometry() + robot_cell.ensure_semantics() + link_names = robot_cell.get_link_names_with_collision_geometry() assert set(link_names) == set( [ "base_link_inertia", @@ -51,55 +51,33 @@ def test_pybullet_client_set_robot_from_urdf(): ) -def test_pybullet_client_set_robot_from_robot_library(): - # Testing workflow of using robot from RobotLibrary - with PyBulletClient(connection_type="direct") as client: - robot = RobotLibrary.ur5(load_geometry=False) - # Assert that set_robot can only be performed with geometry - with pytest.raises(Exception): - robot = client.set_robot(robot) - - # Load robot with geometry - robot = RobotLibrary.ur5(load_geometry=True) - robot = client.set_robot(robot) - - assert isinstance(robot, Robot) - assert robot.name == "ur5_robot" - # Check that the RobotModel is present - assert isinstance(robot.model, RobotModel) - # Check that the robot have geometry - robot.ensure_geometry() - # Check that the robot have semantics - robot.ensure_semantics() - - def test_pybullet_client_set_all_robots_from_robot_library(): - # Testing workflow of using robot from RobotLibrary + # Testing workflow of using robot from RobotCellLibrary with PyBulletClient(connection_type="direct") as client: - def set_and_check_robot(robot): - robot = client.set_robot(robot) - assert isinstance(robot, Robot) - assert isinstance(robot.model, RobotModel) + def set_and_check_robot_cell(robot_cell): + client.set_robot(robot_cell.robot_model, robot_cell.robot_semantics) + assert isinstance(robot_cell.robot_model, RobotModel) + assert isinstance(robot_cell.robot_semantics, RobotSemantics) client.remove_robot() - set_and_check_robot(RobotLibrary.ur5()) - set_and_check_robot(RobotLibrary.ur10e()) - set_and_check_robot(RobotLibrary.panda()) - set_and_check_robot(RobotLibrary.abb_irb120_3_58()) - set_and_check_robot(RobotLibrary.abb_irb4600_40_255()) - set_and_check_robot(RobotLibrary.rfl()) + set_and_check_robot_cell(RobotCellLibrary.ur5()) + set_and_check_robot_cell(RobotCellLibrary.ur10e()) + set_and_check_robot_cell(RobotCellLibrary.panda()) + set_and_check_robot_cell(RobotCellLibrary.abb_irb120_3_58()) + set_and_check_robot_cell(RobotCellLibrary.abb_irb4600_40_255()) + set_and_check_robot_cell(RobotCellLibrary.rfl()) def test_pybullet_client_set_robot_configuration(): with PyBulletClient(connection_type="direct") as client: - robot = RobotLibrary.panda(load_geometry=True) + robot_cell, robot_cell_state = RobotCellLibrary.panda(load_geometry=True) # Typically user would call planner.set_robot_cell() directly - robot = client.set_robot(robot) - client._robot_cell = RobotCell(robot) + client.set_robot(robot_cell.robot_model, robot_cell.robot_semantics) + client._robot_cell = robot_cell # Set configuration - configuration = robot.model.zero_configuration() + configuration = robot_cell.robot_model.zero_configuration() client.set_robot_configuration(configuration) # Check that the configuration is set assert client.get_robot_configuration().close_to(configuration) @@ -118,18 +96,18 @@ def test_pybullet_client_internal_puids(): assert len(client.robot_link_puids) == 0 assert len(client.robot_joint_puids) == 0 - robot = RobotLibrary.ur5(load_geometry=True) - robot = client.set_robot(robot) + robot_cell, robot_cell_state = RobotCellLibrary.panda(load_geometry=True) + client.set_robot(robot_cell.robot_model, robot_cell.robot_semantics) # Check that all links and joints are loaded - link_names_in_model = [link.name for link in robot.model.iter_links()] - joint_names_in_model = [joint.name for joint in robot.model.iter_joints()] + link_names_in_model = [link.name for link in robot_cell.robot_model.iter_links()] + joint_names_in_model = [joint.name for joint in robot_cell.robot_model.iter_joints()] assert set(link_names_in_model) == set(client.robot_link_puids.keys()) assert set(joint_names_in_model) == set(client.robot_joint_puids.keys()) assert len(client.robot_link_puids) == len(client.robot_joint_puids) + 1 # The first link has a puid of -1 - links_in_model = list(robot.model.iter_links()) + links_in_model = list(robot_cell.robot_model.iter_links()) first_link_name = links_in_model[0].name assert client.robot_link_puids[first_link_name] == -1 @@ -139,8 +117,8 @@ def test_pybullet_client_internal_puids(): if link_puid == -1: # Skip the base link continue - link = robot.model.get_link_by_name(link_name) - joint = robot.model.find_parent_joint(link) + link = robot_cell.robot_model.get_link_by_name(link_name) + joint = robot_cell.robot_model.find_parent_joint(link) assert client.robot_joint_puids[joint.name] == link_puid @@ -149,18 +127,18 @@ def test_pybullet_client_internal_puids_abb(): assert len(client.robot_link_puids) == 0 assert len(client.robot_joint_puids) == 0 - robot = RobotLibrary.abb_irb4600_40_255(load_geometry=True) - robot = client.set_robot(robot) + robot_cell, robot_cell_state = RobotCellLibrary.abb_irb4600_40_255(load_geometry=True) + client.set_robot(robot_cell.robot_model, robot_cell.robot_semantics) # Check that all links and joints are loaded - link_names_in_model = [link.name for link in robot.model.iter_links()] - joint_names_in_model = [joint.name for joint in robot.model.iter_joints()] + link_names_in_model = [link.name for link in robot_cell.robot_model.iter_links()] + joint_names_in_model = [joint.name for joint in robot_cell.robot_model.iter_joints()] assert set(link_names_in_model) == set(client.robot_link_puids.keys()) assert set(joint_names_in_model) == set(client.robot_joint_puids.keys()) assert len(client.robot_link_puids) == len(client.robot_joint_puids) + 1 # The first link has a puid of -1 - links_in_model = list(robot.model.iter_links()) + links_in_model = list(robot_cell.robot_model.iter_links()) first_link_name = links_in_model[0].name assert client.robot_link_puids[first_link_name] == -1 @@ -170,8 +148,8 @@ def test_pybullet_client_internal_puids_abb(): if link_puid == -1: # Skip the base link continue - link = robot.model.get_link_by_name(link_name) - joint = robot.model.find_parent_joint(link) + link = robot_cell.robot_model.get_link_by_name(link_name) + joint = robot_cell.robot_model.find_parent_joint(link) assert client.robot_joint_puids[joint.name] == link_puid @@ -179,25 +157,25 @@ def test_pybullet_client_internal_puids_abb(): def test_pybullet_client_link_names(): - robot = RobotLibrary.ur5() + robot_cell, robot_cell_state = RobotCellLibrary.ur5() print("Print tree of Robot Model:") # Start with the root link and iterate through all joints recursively - for joint in robot.model.joints: + for joint in robot_cell.robot_model.joints: print("Name:{}, Parent:{}, Child:{}".format(joint.name, joint.parent.link, joint.child.link)) print("Links in Model:") - print([link.name for link in robot.model.links]) + print([link.name for link in robot_cell.robot_model.links]) print("Joints in Model:") - print([link.name for link in robot.model.joints]) + print([link.name for link in robot_cell.robot_model.joints]) print("Base Link in Model:") - print(robot.model.get_base_link_name()) + print(robot_cell.robot_model.get_base_link_name()) print("Link name in Robot with main group:") - print(robot.get_link_names()) + print(robot_cell.get_link_names()) print("Base Link Name in Robot with main group:") - print(robot.get_base_link_name()) + print(robot_cell.get_base_link_name()) with PyBulletClient(connection_type="direct") as client: - client.set_robot(robot) + client.set_robot(robot_cell.robot_model, robot_cell.robot_semantics) print("Links in Backend:") print(client.robot_link_puids) print("Joints in Backend:") diff --git a/tests/backends/pybullet/test_pybullet_planner_fk_ik.py b/tests/backends/pybullet/test_pybullet_planner_fk_ik.py index ac3cfe49b..e93c7eb20 100644 --- a/tests/backends/pybullet/test_pybullet_planner_fk_ik.py +++ b/tests/backends/pybullet/test_pybullet_planner_fk_ik.py @@ -7,6 +7,9 @@ from compas_fab.backends import PyBulletClient from compas_fab.backends import PyBulletPlanner + from typing import List # noqa: F401 + from typing import Tuple # noqa: F401 + from compas.geometry import Frame from compas.geometry import Point from compas.geometry import Vector @@ -26,7 +29,7 @@ from compas_fab.robots import RobotCell from compas_fab.robots import RobotCellLibrary from compas_fab.robots import RobotCellState -from compas_fab.robots import RobotLibrary +from compas_fab.robots import RobotCellLibrary from compas_fab.robots import TargetMode @@ -79,19 +82,18 @@ def compare_configuration(config1, config2, tolerance_linear, tolerance_angular) ###################################################### -def compare_planner_fk_model_fk(robot, pybullet_client, known_zero_frame): - # type: (Robot, PyBulletClient, Frame) -> None +def compare_planner_fk_model_fk(robot_cell, robot_cell_state, pybullet_client, known_zero_frame): + # type: (RobotCell, RobotCellState, PyBulletClient, Frame) -> None """A test of forward kinematics using the zero configuration of the robot.""" planner = PyBulletPlanner(pybullet_client) - planner.set_robot_cell(RobotCell(robot)) + planner.set_robot_cell(robot_cell) - planning_group = robot.main_group_name - end_effector_link = robot.get_end_effector_link_name(planning_group) + planning_group = robot_cell.main_group_name + end_effector_link = robot_cell.get_end_effector_link_name(planning_group) - robot_cell_state = RobotCellState.from_robot_configuration(robot, robot.zero_configuration()) planner_fk_result = planner.forward_kinematics(robot_cell_state, TargetMode.ROBOT, group=planning_group) print(planner_fk_result) - model_fk_result = robot.model.forward_kinematics(robot.zero_configuration(), end_effector_link) + model_fk_result = robot_cell.robot_model.forward_kinematics(robot_cell.zero_configuration(), end_effector_link) print(model_fk_result) assert compare_frames(planner_fk_result, model_fk_result, 1e-3, 1e-3) @@ -99,43 +101,43 @@ def compare_planner_fk_model_fk(robot, pybullet_client, known_zero_frame): def test_fk_ur5(pybullet_client): - robot = RobotLibrary.ur5(load_geometry=True) + robot_cell, robot_cell_state = RobotCellLibrary.ur5(load_geometry=True) known_zero_frame = Frame( point=Point(x=0.8172500133514404, y=0.19144999980926514, z=-0.005491000134497881), xaxis=Vector(x=-1.0, y=0.0, z=0.0), yaxis=Vector(x=0.0, y=0.0, z=1.0), ) - compare_planner_fk_model_fk(robot, pybullet_client, known_zero_frame) + compare_planner_fk_model_fk(robot_cell, robot_cell_state, pybullet_client, known_zero_frame) def test_fk_abb_irb4600_40_255(pybullet_client): - robot = RobotLibrary.abb_irb4600_40_255(load_geometry=True) + robot_cell, robot_cell_state = RobotCellLibrary.abb_irb4600_40_255(load_geometry=True) known_zero_frame = Frame( point=Point(x=1.58, y=0.0, z=1.765), xaxis=Vector(x=0.0, y=0.0, z=-1.0), yaxis=Vector(x=0.0, y=1.0, z=0.0), ) - compare_planner_fk_model_fk(robot, pybullet_client, known_zero_frame) + compare_planner_fk_model_fk(robot_cell, robot_cell_state, pybullet_client, known_zero_frame) def test_fk_ur10e(pybullet_client): - robot = RobotLibrary.ur10e(load_geometry=True) + robot_cell, robot_cell_state = RobotCellLibrary.ur10e(load_geometry=True) known_zero_frame = Frame( point=Point(x=1.18425, y=0.2907, z=0.0608), xaxis=Vector(x=-1.0, y=0.0, z=-0.0), yaxis=Vector(x=0.0, y=0.0, z=1.0), ) - compare_planner_fk_model_fk(robot, pybullet_client, known_zero_frame) + compare_planner_fk_model_fk(robot_cell, robot_cell_state, pybullet_client, known_zero_frame) def test_fk_panda(pybullet_client): - robot = RobotLibrary.panda(load_geometry=True) + robot_cell, robot_cell_state = RobotCellLibrary.panda(load_geometry=True) known_zero_frame = Frame( point=Point(x=0.256, y=-0.000, z=0.643), xaxis=Vector(x=-0.000, y=0.707, z=-0.707), yaxis=Vector(x=-0.000, y=-0.707, z=-0.707), ) - compare_planner_fk_model_fk(robot, pybullet_client, known_zero_frame) + compare_planner_fk_model_fk(robot_cell, robot_cell_state, pybullet_client, known_zero_frame) ###################################################### @@ -143,7 +145,8 @@ def test_fk_panda(pybullet_client): ###################################################### -def ik_fk_agreement(robot, pybullet_client, ik_target_frames): +def ik_fk_agreement(robot_cell, robot_cell_state, pybullet_client, ik_target_frames): + # type: (RobotCell, RobotCellState, PyBulletClient, List[Frame]) -> None """Helper function to test the IK-FK agreement for the PyBullet backend The function takes a robot and a list of target frames for IK and FK queries. @@ -161,8 +164,8 @@ def ik_fk_agreement(robot, pybullet_client, ik_target_frames): } planner = PyBulletPlanner(pybullet_client) - planner.set_robot_cell(RobotCell(robot)) - planning_group = robot.main_group_name + planner.set_robot_cell(robot_cell) + planning_group = robot_cell.main_group_name for ik_target_frame in ik_target_frames: # IK Query to the planner (Frame to Configuration) @@ -177,7 +180,7 @@ def ik_fk_agreement(robot, pybullet_client, ik_target_frames): tolerance_position=tolerance_position, tolerance_orientation=tolerance_orientation, ), - RobotCellState.from_robot_configuration(robot), + robot_cell_state, group=planning_group, options=ik_options, ) @@ -187,7 +190,7 @@ def ik_fk_agreement(robot, pybullet_client, ik_target_frames): assert False, "No IK Solution found for frame" # FK Query to the planner (Configuration to Frame) - robot_cell_state = RobotCellState.from_robot_configuration(robot, ik_result) + robot_cell_state.robot_configuration = ik_result fk_result = planner.forward_kinematics(robot_cell_state, TargetMode.ROBOT, group=planning_group) # Compare the frames @@ -195,7 +198,7 @@ def ik_fk_agreement(robot, pybullet_client, ik_target_frames): def test_ik_fk_agreement_ur5(pybullet_client): - robot = RobotLibrary.ur5(load_geometry=True) + robot_cell, robot_cell_state = RobotCellLibrary.ur5(load_geometry=True) ik_center_frame = Frame( Point(x=0.4, y=0.1, z=0.3), @@ -211,11 +214,11 @@ def test_ik_fk_agreement_ur5(pybullet_client): ik_target_frames.append(ik_center_frame.translated(Vector(0.1, 0.1, 0.2))) ik_target_frames.append(ik_center_frame.translated(Vector(-0.1, -0.1, 0.0))) - ik_fk_agreement(robot, pybullet_client, ik_target_frames) + ik_fk_agreement(robot_cell, robot_cell_state, pybullet_client, ik_target_frames) def test_ik_fk_agreement_ur10e(pybullet_client): - robot = RobotLibrary.ur10e(load_geometry=True) + robot_cell, robot_cell_state = RobotCellLibrary.ur10e(load_geometry=True) ik_center_frame = Frame( Point(x=0.4, y=0.1, z=0.3), @@ -231,11 +234,11 @@ def test_ik_fk_agreement_ur10e(pybullet_client): ik_target_frames.append(ik_center_frame.translated(Vector(0.1, 0.1, 0.2))) ik_target_frames.append(ik_center_frame.translated(Vector(-0.1, -0.1, 0.0))) - ik_fk_agreement(robot, pybullet_client, ik_target_frames) + ik_fk_agreement(robot_cell, robot_cell_state, pybullet_client, ik_target_frames) def test_ik_fk_agreement_abb_irb4600_40_255(pybullet_client): - robot = RobotLibrary.abb_irb4600_40_255(load_geometry=True) + robot_cell, robot_cell_state = RobotCellLibrary.abb_irb4600_40_255(load_geometry=True) ik_center_frame = Frame( Point(x=1.0, y=0.3, z=1.3), @@ -251,12 +254,12 @@ def test_ik_fk_agreement_abb_irb4600_40_255(pybullet_client): ik_target_frames.append(ik_center_frame.translated(Vector(0.1, 0.1, 0.2))) ik_target_frames.append(ik_center_frame.translated(Vector(-0.3, -0.1, -0.3))) - ik_fk_agreement(robot, pybullet_client, ik_target_frames) + ik_fk_agreement(robot_cell, robot_cell_state, pybullet_client, ik_target_frames) def test_ik_fk_agreement_panda(pybullet_client): # The panda robot has mimic joints for testing purposes - robot = RobotLibrary.panda(load_geometry=True) + robot_cell, robot_cell_state = RobotCellLibrary.panda(load_geometry=True) ik_center_frame = Frame( point=Point(x=0.2, y=-0.0, z=0.6), @@ -272,7 +275,7 @@ def test_ik_fk_agreement_panda(pybullet_client): ik_target_frames.append(ik_center_frame.translated(Vector(0.1, 0.1, -0.2))) ik_target_frames.append(ik_center_frame.translated(Vector(-0.05, -0.05, -0.03))) - ik_fk_agreement(robot, pybullet_client, ik_target_frames) + ik_fk_agreement(robot_cell, robot_cell_state, pybullet_client, ik_target_frames) ################################################## @@ -281,7 +284,7 @@ def test_ik_fk_agreement_panda(pybullet_client): def test_ik_out_of_reach_ur5(pybullet_client): - robot = RobotLibrary.ur5(load_geometry=True) + robot_cell, robot_cell_state = RobotCellLibrary.ur5(load_geometry=True) ik_target_frames = [] @@ -306,15 +309,15 @@ def test_ik_out_of_reach_ur5(pybullet_client): } planner = PyBulletPlanner(pybullet_client) - planner.set_robot_cell(RobotCell(robot)) - planning_group = robot.main_group_name + planner.set_robot_cell(robot_cell) + planning_group = robot_cell.main_group_name for ik_target_frame in ik_target_frames: # IK Query to the planner (Frame to Configuration) try: # Note: The inverse_kinematics method returns a generator planner.inverse_kinematics( FrameTarget(ik_target_frame, target_mode=TargetMode.ROBOT), - RobotCellState.from_robot_configuration(robot), + robot_cell_state, group=planning_group, options=ik_options, ) @@ -327,7 +330,7 @@ def test_ik_out_of_reach_ur5(pybullet_client): def test_ik_return_full_configuration(pybullet_client): - robot = RobotLibrary.panda(load_geometry=True) + robot_cell, robot_cell_state = RobotCellLibrary.panda(load_geometry=True) ik_target_frame = Frame( point=Point(x=0.2, y=-0.0, z=0.6), @@ -337,8 +340,7 @@ def test_ik_return_full_configuration(pybullet_client): target = FrameTarget(ik_target_frame, TargetMode.ROBOT) planner = PyBulletPlanner(pybullet_client) - planner.set_robot_cell(RobotCell(robot)) - robot_cell_state = RobotCellState.from_robot_configuration(robot, robot.zero_configuration()) + planner.set_robot_cell(robot_cell) config = planner.inverse_kinematics(target, robot_cell_state) assert "panda_finger_joint1" not in config.joint_names assert len(config.joint_names) == 7 @@ -351,11 +353,11 @@ def test_ik_return_full_configuration(pybullet_client): def test_ik_modified_only_joints_in_group(pybullet_client): # Test the IK solver see if it moved only the joints in the group - robot = RobotLibrary.panda(load_geometry=True) + robot_cell, robot_cell_state = RobotCellLibrary.panda(load_geometry=True) # Create a custom group with joint 1 locked, `panda_joint1` and `panda_link0` are not included. # This is not supported because the starting part of the chain is not included - robot.semantics.groups["locked_j1"] = { + robot_cell.robot_semantics.groups["locked_j1"] = { "links": [ "panda_link1", "panda_link2", @@ -381,7 +383,7 @@ def test_ik_modified_only_joints_in_group(pybullet_client): } # The following group is supported because only the right side of the chain is cut short - robot.semantics.groups["locked_j7"] = { + robot_cell.robot_semantics.groups["locked_j7"] = { "links": [ "panda_link0", "panda_link1", @@ -401,7 +403,7 @@ def test_ik_modified_only_joints_in_group(pybullet_client): ], } # The following group does not work because the links bridged by the joints are not included - robot.semantics.groups["locked_j7_further"] = { + robot_cell.robot_semantics.groups["locked_j7_further"] = { "links": [ "panda_link0", "panda_link1", @@ -434,12 +436,12 @@ def test_ik_modified_only_joints_in_group(pybullet_client): options = {"return_full_configuration": True} planner = PyBulletPlanner(pybullet_client) - planner.set_robot_cell(RobotCell(robot)) - initial_configuration = robot.zero_configuration() + planner.set_robot_cell(robot_cell) + initial_configuration = robot_cell.zero_configuration() initial_configuration["panda_joint1"] = 0.123 initial_configuration["panda_joint7"] = 0.123 print(initial_configuration) - robot_cell_state = RobotCellState.from_robot_configuration(robot, initial_configuration) + robot_cell_state.robot_configuration = initial_configuration # The main planning group should work as expected config = planner.inverse_kinematics(target, robot_cell_state, options=options) @@ -492,6 +494,7 @@ def test_ik_fk_with_wrong_group_name_raises_exception(pybullet_client): @pytest.fixture def planner_with_test_cell(pybullet_client): + # type: (PyBulletClient) -> Tuple[PyBulletPlanner, RobotCell, RobotCellState] planner = PyBulletPlanner(pybullet_client) # Setup the robot cell @@ -513,7 +516,7 @@ def test_frame_target_ik_tolerance(planner_with_test_cell): This makes sure that the IK solver is not spending more time than necessary to find a solution. """ planner, robot_cell, robot_cell_state = planner_with_test_cell - group = robot_cell.robot.main_group_name + group = robot_cell.main_group_name result_cell_state = robot_cell_state.copy() # type: RobotCellState target_frame = Frame([0.5, 0.5, 0.5], [1.0, 0.0, 0.0], [0.0, 1.0, 0.0]) @@ -554,7 +557,7 @@ def test_frame_target_ik_with_tools_and_workpieces(planner_with_test_cell): planner, robot_cell, robot_cell_state = planner_with_test_cell # Testing conditions - group = robot_cell.robot.main_group_name + group = robot_cell.main_group_name target_frame = Frame([0.5, 0.5, 0.5], [1.0, 0.0, 0.0], [0.0, 1.0, 0.0]) options = {"check_collision": False} tolerance_position = 1e-3 @@ -678,7 +681,7 @@ def test_iter_ik_frame_target(planner_with_test_cell): with pytest.raises(ValueError): next(generator) - group = robot_cell.robot.main_group_name + group = robot_cell.main_group_name generator = planner.iter_inverse_kinematics_frame_target(target, robot_cell_state, group, options) result = next(generator) # type: Configuration @@ -717,7 +720,7 @@ def test_iter_ik_returns_different_results(planner_with_test_cell): planner, robot_cell, robot_cell_state = planner_with_test_cell # Testing conditions - group = robot_cell.robot.main_group_name + group = robot_cell.main_group_name options = { "check_collision": True, "solution_uniqueness_threshold_prismatic": 1e-3, @@ -764,7 +767,7 @@ def test_iter_ik_point_axis_target(planner_with_test_cell): with pytest.raises(ValueError): next(generator) - group = robot_cell.robot.main_group_name + group = robot_cell.main_group_name generator = planner.iter_inverse_kinematics_point_axis_target(target, robot_cell_state, group, options) result = next(generator) # type: Configuration diff --git a/tests/backends/pybullet/test_pybullet_planner_plan_cartesian_motion.py b/tests/backends/pybullet/test_pybullet_planner_plan_cartesian_motion.py index c26be764e..1431b0b35 100644 --- a/tests/backends/pybullet/test_pybullet_planner_plan_cartesian_motion.py +++ b/tests/backends/pybullet/test_pybullet_planner_plan_cartesian_motion.py @@ -10,7 +10,7 @@ # from compas_fab.backends.exceptions import CollisionCheckError # from compas_fab.robots import RobotCellLibrary -# from compas_fab.robots import RobotLibrary +# from compas_fab.robots import RobotCellLibrary # from compas_fab.robots import RobotCellState # from compas_fab.robots import FrameTarget # from compas_fab.robots import PointAxisTarget diff --git a/tests/robots/obsolete_test_robot.py b/tests/robots/obsolete_test_robot.py index 9c817fedd..a87239f05 100644 --- a/tests/robots/obsolete_test_robot.py +++ b/tests/robots/obsolete_test_robot.py @@ -11,10 +11,7 @@ import compas_fab from compas_fab.backends.interfaces import ClientInterface from compas_fab.backends.interfaces import InverseKinematics -from compas_fab.robots import Robot -from compas_fab.robots import RobotLibrary from compas_fab.robots import RobotSemantics -from compas_fab.robots import Tool BASE_FOLDER = os.path.dirname(__file__) @@ -157,8 +154,8 @@ def test_group_names(ur5_robot_instance): def test_main_group_name(panda_robot_instance): - robot = panda_robot_instance - assert robot.main_group_name == "panda_arm_hand" + robot_cell = panda_robot_instance + assert robot_cell.main_group_name == "panda_arm_hand" def test_root_name(ur5_robot_instance): @@ -477,4 +474,4 @@ def test_attached_tools_no_assigning(ur5_robot_instance, robot_tool1): def test_print_robot_info(ur5_robot_instance): robot = ur5_robot_instance - robot.info() + robot_cell.print_info() diff --git a/tests/robots/test_robot_library.py b/tests/robots/test_robot_library.py index 5eaf17e08..0fe0ececc 100644 --- a/tests/robots/test_robot_library.py +++ b/tests/robots/test_robot_library.py @@ -1,4 +1,4 @@ -from compas_fab.robots import RobotLibrary +from compas_fab.robots import RobotCellLibrary import pytest @@ -17,4 +17,4 @@ def test_robot_semantics_and_geometry(all_robots): robot.ensure_semantics() robot.ensure_geometry() assert robot.name - robot.info() + robot_cell.print_info() diff --git a/tests/robots/test_semantics.py b/tests/robots/test_semantics.py index d7e0dbf75..2527be527 100644 --- a/tests/robots/test_semantics.py +++ b/tests/robots/test_semantics.py @@ -4,7 +4,7 @@ from compas_robots import RobotModel from compas_fab.robots import RobotSemantics -from compas_fab.robots import RobotLibrary +from compas_fab.robots import RobotCellLibrary BASE_FOLDER = os.path.dirname(__file__) @@ -61,13 +61,13 @@ def test_panda_srdf_file(panda_srdf, panda_urdf): def test_ur5_semantics(): - robot = RobotLibrary.ur5(load_geometry=False) - semantics = robot.semantics + robot_cell, robot_cell_state = RobotCellLibrary.ur5(load_geometry=False) + semantics = robot_cell.robot_semantics assert sorted(semantics.group_names) == sorted(["manipulator", "endeffector"]) assert semantics.main_group_name == "manipulator" assert semantics.get_base_link_name() == "base_link" assert semantics.get_end_effector_link_name() == "tool0" - assert semantics.get_configurable_joint_names(robot.model) == [ + assert robot_cell.get_configurable_joint_names() == [ "shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint",