diff --git a/docs/backends/ros/01_ros_examples.rst b/docs/backends/ros/01_ros_examples.rst new file mode 100644 index 0000000000..7fbfbb9ebe --- /dev/null +++ b/docs/backends/ros/01_ros_examples.rst @@ -0,0 +1,17 @@ +.. _ros_examples: + +******************************************************************************* +Check ROS Connections +******************************************************************************* + +To connect to ROS and to verify that the system is working. + +Copy and paste the following example into any Python environment +(a standalone script, a CAD environment, etc) and run it, you should +see the output ``Connected: True`` if everything is working properly: + +.. code-block:: python + + +*Yay! Our first connection to ROS!* + diff --git a/docs/backends/ros/02_robot_models.rst b/docs/backends/ros/02_robot_models.rst new file mode 100644 index 0000000000..15bc8ca858 --- /dev/null +++ b/docs/backends/ros/02_robot_models.rst @@ -0,0 +1,80 @@ +******************************************************************************* +Robot Cells in ROS +******************************************************************************* + +Once ROS is running and MoveIt! planner has started with a robot, we can +start interacting with the robot model. + +Load robot cell from ROS +======================== + +Load the initial robot cell from ROS using +:meth:`compas_fab.backends.RosClient.load_robot_cell`. +It will return a RobotCell object that contains the RobotModel and RobotSemantics +but without any ToolModel or RigidBody objects. This method is unique to the RosClient +because the RobotModel loaded in the backend cannot be modified. + +This function allows you to load the running robot cell from ROS and be sure that +the RobotModel and RobotSemantics in the RobotCell is the same as the one in the backend. + +.. literalinclude :: files/02_load_robot_cell.py + :language: python + +Customize the robot cell +======================== + +Users can customize the RobotCell by adding :class:`compas_robots.ToolModel` and +:class:`compas_fab.robots.RigidBody` objects to the robot cell. +After modifying the RobotCell, :meth:`compas_fab.backends.MoveItPlanner.set_robot_cell` +must be called to send the robot cell to the backend. + +.. literalinclude :: files/02_set_robot_cell.py + :language: python + +Visualize the robot cell in RViz +=============================== + +If GUI is enabled, the robot cell will be visualized in RViz after calling +:meth:`compas_fab.backends.MoveItPlanner.set_robot_cell`. However, the position +of the objects may not be in a meaningful position. Users can pass a +:class:`compas_fab.robots.RobotCellState` object to the method, for RViz to +display the robot cell in a specific state. + +The following example shows the effect of attaching a tool, attaching a workpiece +and detaching them. Note that the `ToolState.attachment_frame` attribute take effect +when the tool is attached to the robot and the `ToolState.frame` attribute take effect +when the tool is detached from the robot. The same applies to the `RigidBodyState`. + +Note that the example make use of `MoveItPlanner.set_robot_cell_state` to set the +robot cell state in the backend. This method is generally not needed to be called +by the user, as it is called internally by the `MoveItPlanner` when a planning +function is called. It is called here only to update the visualization in RViz. + +.. literalinclude :: files/02_set_robot_cell_state_attach_objects.py + :language: python + +The following example shows the effect of changing the robot configuration, +which causes the attached tool to move with the robot. + +.. literalinclude :: files/02_set_robot_cell_state_with_kinematic_tools.py + :language: python + + +Serializing the robot cell +======================== + +Users can choose to serialize their customized robot cell, which can be used +in another session with the same backend that have been initialized with the +same MoveIt configuration. For example, if the RobotCell comes from a ROS +with a UR5 robot, it will not work with a ROS with a Panda robot. The user +should not modify the RobotModel and RobotSemantics in the serialized file. + +All tools and rigid bodies are serialized with the RobotCell. The user can +simply start a new session with the same backend and load the serialized +RobotCell to continue working with the same set of tools and rigid bodies. + +.. todo +.. literalinclude :: files/02_serialize_robot_cell.py + :language: python + + diff --git a/docs/backends/ros/files/01_ros_connection.py b/docs/backends/ros/files/01_ros_connection.py new file mode 100644 index 0000000000..ea77578c8d --- /dev/null +++ b/docs/backends/ros/files/01_ros_connection.py @@ -0,0 +1,4 @@ +from compas_fab.backends import RosClient + +with RosClient() as client: + print("Connected: ", client.is_connected) diff --git a/docs/backends/ros/files/02_set_robot_cell.py b/docs/backends/ros/files/02_set_robot_cell.py new file mode 100644 index 0000000000..e4f58a2ddf --- /dev/null +++ b/docs/backends/ros/files/02_set_robot_cell.py @@ -0,0 +1,31 @@ +from compas_fab.backends import RosClient +from compas_fab.backends import MoveItPlanner + +from compas_fab.robots import ToolLibrary +from compas_fab.robots import RigidBody +from compas_fab.robots import RigidBodyLibrary + +from compas.geometry import Box + +with RosClient() as client: + robot_cell = client.load_robot_cell() + planner = MoveItPlanner(client) + + # Add a gripper to the robot cell + gripper = ToolLibrary.static_gripper_small() + robot_cell.tool_models["my_gripper"] = gripper + + # Add a beam to the robot cell + # Use triangulated mesh + beam_mesh = Box(1, 0.1, 0.1).to_mesh(triangulated=True) + beam = RigidBody.from_mesh(beam_mesh) + robot_cell.rigid_body_models["my_beam"] = beam + + # Add a floor to the robot cell + floor = RigidBodyLibrary.floor() + robot_cell.rigid_body_models["my_floor"] = floor + + # Set the robot cell back to the client + planner.set_robot_cell(robot_cell) + # If you are running ROS with UI, you should see the objects in the PyBullet world + input("Press Enter to terminate...") diff --git a/docs/backends/ros/files/02_set_robot_cell_state_attach_objects.py b/docs/backends/ros/files/02_set_robot_cell_state_attach_objects.py new file mode 100644 index 0000000000..62428b5239 --- /dev/null +++ b/docs/backends/ros/files/02_set_robot_cell_state_attach_objects.py @@ -0,0 +1,68 @@ +from compas.geometry import Frame + +from compas_fab.backends import MoveItPlanner +from compas_fab.backends import RosClient +from compas_fab.robots import RobotCellLibrary + +with RosClient() as client: + planner = MoveItPlanner(client) + + # ========= + # Step 1 + # ========= + + # Load a robot cell that contains a gripper and a beam + robot_cell, _ = RobotCellLibrary.ur5_gripper_one_beam() + gripper = robot_cell.tool_models["gripper"] + beam = robot_cell.rigid_body_models["beam"] + + # The objects are simply added to the backend, their positions are not set + planner.set_robot_cell(robot_cell) + + # If you are running ROS with UI, you should see the added object + # positioned around the world origin + input("Press Enter to continue...") + + # ========= + # Step 2 + # ========= + # Set the position of the beam + robot_cell_state = robot_cell.default_cell_state() + robot_cell_state.rigid_body_states["beam"].frame = Frame([1.2, 0, 0], [0, 0, 1], [1, 0, 0]) + + # Attach the gripper to the robot + robot_cell_state.tool_states["gripper"].attached_to_group = robot_cell.main_group_name + robot_cell_state.tool_states["gripper"].attachment_frame = Frame([0, 0, 0], [0, 0, 1], [1, 0, 0]) + + # Set the robot cell state in the planner + planner.set_robot_cell_state(robot_cell_state) + + # If you are running ROS with UI, you should see the gripper attached to the robot + input("Press Enter to continue...") + + # ========= + # Step 3 + # ========= + + # Attach the beam to the robot + robot_cell_state.rigid_body_states["beam"].attached_to_tool = "gripper" + robot_cell_state.rigid_body_states["beam"].attachment_frame = Frame([0, 0, 0], [1, 0, 0], [0, 1, 0]) + + # Set the robot cell state in the planner + planner.set_robot_cell_state(robot_cell_state) + + # If you are running ROS with UI, you should see the beam attached to the gripper + input("Press Enter to continue...") + + # ========= + # Step 4 + # ========= + + # Move the robot to a specific configuration + robot_cell_state.robot_configuration._joint_values[1] = -1.0 + robot_cell_state.robot_configuration._joint_values[2] = 0.5 + planner.set_robot_cell_state(robot_cell_state) + + # If you are running ROS with UI, you should see the robot in a different configuration + # where the gripper and the beam are still attached to the robot. + input("Press Enter to terminate...") diff --git a/docs/backends/ros/files/02_set_robot_cell_state_with_kinematic_tools.py b/docs/backends/ros/files/02_set_robot_cell_state_with_kinematic_tools.py new file mode 100644 index 0000000000..b49df9c241 --- /dev/null +++ b/docs/backends/ros/files/02_set_robot_cell_state_with_kinematic_tools.py @@ -0,0 +1,50 @@ +from compas.geometry import Frame + +from compas_fab.backends import MoveItPlanner +from compas_fab.backends import RosClient +from compas_fab.robots import ToolLibrary + +with RosClient() as client: + robot_cell = client.load_robot_cell() + planner = MoveItPlanner(client) + + # ========= + # Step 1 + # ========= + + # Add a kinematic gripper tool to the robot cell + gripper = ToolLibrary.kinematic_gripper() + robot_cell.tool_models[gripper.name] = gripper + # Attach the gripper to the robot + robot_cell_state = robot_cell.default_cell_state() + robot_cell_state.set_tool_attached_to_group( + gripper.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 + ) + + # Move the robot to a specific configuration + robot_cell_state.robot_configuration._joint_values[1] = -1.0 + robot_cell_state.robot_configuration._joint_values[2] = 0.5 + result = planner.set_robot_cell(robot_cell, robot_cell_state) + + # The gripper tool's default configuration is a closed gripper state at [0, 0] + print(gripper.zero_configuration()) + + # If you are running ROS with UI, you should see the gripper attached to the robot. + input("Press Enter to continue...") + + # ========= + # Step 2 + # ========= + + # Kinematic tools can be moved with it's Configuration + # The gripper tool's open gripper state is at [0.025, 0.025] + robot_cell_state.tool_states[gripper.name].configuration.joint_values = [0.025, 0.025] + + # Calling `set_robot_cell_state` updates the robot cell in the planner + result = planner.set_robot_cell_state(robot_cell_state) + + # If you are running ROS with UI, you should see the gripper fingers in an opened state. + input("Press Enter to terminate...")