Ros example 01 and 02

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!*
+

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
+
+

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)

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...")

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...")

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...")