Updated documentation for collision handling

docs/collision_handling.rst

@@ -1,138 +1,137 @@
 Collision Handling
 =====================
 
-Armer implements collision handling features that extend the capabilities provided by the roboticstoolbox-python and spatialgeometry libraries.
-A description of each core functionality is provided below. 
+Armer implements collision handling features that extend the capabilities provided by the roboticstoolbox-python and spatialgeometry libraries. 
+Upon launch of Armer, the default robot_description (i.e., robot model) is parsed, with each collision shape associated with each link captured in a collision dictionary for checking.
+If a valid collision is found in every step of the Armer driver, a preemption occurs to safely stop the robot.
+
+The following high-level features are now available:
+- Cython build of the global checking method for fast execution
+- A slice of target links (named slice links) that can be configured by user input for limiting the number of links to be evaluated per time step.
+- K-Dimensional (KD) tree creation is conducted each step to: (a) capture the distance between all links to target slice links; and (b) find the closest 4 links per each slice link to confirm for collision. This ensures that the collision checking process remains stable even with an increase in the number of shapes in the scene. 
+- Multiple arm instances per Armer driver (two tested) can be conducted with the global collision checking method, where each individual arm checks and preempts upon collision in a common scene.
+- A new recover_move service to move the arm back to a pre-defined safe joint state along its recently conducted trajectory. This ensures the arm comes free of collision preemption for fast recovery.
+- The dynamic addition/removal of collision objects (simple primitives, such as spheres, cuboids, or cylinders) during runtime for custom collision object setup
+- Each added collision object is interactive through RVIZ for easy positioning/orienting based on the use case.
+- A collision scene (list of added dynamic collision shapes) can be saved to a config yaml file for easy reload on a new run 
 
 URDF Parsed Collision Objects
 ------------------------------
 
+Armer supports loading in a robot_description; and as such, will parse and extract collision shapes to conduct collision checking.
 
+Note that, these collision shapes will not be interactive and are a part of the robot's collision tree. This is especially useful when considering
+the end-effector's collision shapes (in a custom configuration for example). Please refer to https://wiki.ros.org/urdf/Tutorials for an indepth explanation on
+what a URDF is and how to create a custom version.
 
-Moving to a Named Pose
+Adding Collision Objects
 ------------------------
 
-The Armer interface uses a ROS action server to listen for requests to move to a named pose.
+Collision primitives (i.e., spheres, cylinders, and cuboids) can be added in at runtime via the ``/arm/add_collision_object`` service. 
 
-Requests to the ROS action server can be made by creating a ROS action client in Python. This client is used to send requests to the action server.
-
-This example shows a request to move to named pose `my_pose`:
-
-.. code-block:: python
-
-    from armer_msgs.msg import MoveToNamedPoseAction, MoveToNamedPoseGoal 
-    import actionlib
-    import rospy
-
-    rospy.init_node('armer_example', disable_signals=True)
-
-    named_pose="my_pose"
-
-    servo_cli = actionlib.SimpleActionClient('/arm/joint/named', MoveToNamedPoseAction)
-    servo_cli.wait_for_server()
-
-    goal = MoveToNamedPoseGoal()
-    goal.pose_name=named_pose
-
-    # Send goal and wait for it to finish
-    servo_cli.send_goal(goal)
-    servo_cli.wait_for_result()
-    servo_cli.get_result()
-
-Removing a Named Pose
------------------------
-
-Named poses can be removed via the ``/arm/remove_named_pose`` service. This can be called via Python:
+This can be called via Python:
 
 .. code-block:: python
 
-    from armer_msgs.srv import RemoveNamedPose
+    from armer_msgs.srv import AddCollisionObject
     import rospy
 
     rospy.init_node('armer_example', disable_signals=True)
-    remove_pose_service = rospy.ServiceProxy('/arm/remove_named_pose', RemoveNamedPose)
+    service = rospy.ServiceProxy('/arm/add_collision_object', AddCollisionObject)
+    # Create a pose object and populate
+    pose = Pose()
+    pose.position.x = 0.3
+    pose.position.y = 0
+    pose.position.z = 0.15
+    pose.orientation.w = 0
+    pose.orientation.x = 1
+    pose.orientation.y = 0
+    pose.orientation.z = 0
     
-    named_pose="my_pose"
-    remove_pose_service(named_pose)    
+    # Configure Shape Properties
+    name="test_object"
+    shape_type='cylinder'
+    length=0.3
+    radius=0.05
 
-Or bash: 
+    # Send via service
+    service(name, shape_type, length, radius, pose)  
 
-.. code-block:: bash
+Or via Bash:
 
-    rosservice call /arm/remove_named_pose "pose_name: {POSE_TO_REMOVE}"
+.. code-block:: bash
 
-Getting Saved Named Poses
---------------------------
+    rosservice call /arm/add_collision_object "name: 'test_object'
+    type: 'cylinder'
+    radius: 0.05
+    length: 0.3
+    scale_x: 0.0
+    scale_y: 0.0
+    scale_z: 0.0
+    pose:
+        position: {x: 0.3, y: 0.0, z: 0.15}
+        orientation: {x: 1.0, y: 0.0, z: 0.0, w: 0.0}
+    overwrite: false" 
 
-To see a list of poses are saved, use the ``/arm/get_named_poses`` service.
+Getting Collision Object Details
+----------------------------------
 
-Via Python:
+Added collision object details (specifically the ``name``, its ``type``, and the ``x,y,z translation``) can be requested through the ``/arm/get_collision_objects`` service.
 
-.. code-block:: python
+This can be called via Bash:
 
-    from armer_msgs.srv import GetNamedPoses
-    import rospy
-
-    rospy.init_node('armer_example', disable_signals=True)
-    get_poses_service = rospy.ServiceProxy('/arm/get_named_poses', GetNamedPoses)
+.. code-block:: bash
 
-    get_poses_service()    
+    rosservice call /arm/get_collision_objects "{}"
 
-Bash: 
+which will return a list of information. As an example, you may see the following:
 
 .. code-block:: bash
 
-    rosservice call /arm/get_named_poses
+    rosservice call /arm/get_collision_objects "{}"
+    objects: 
+        - 'block_1 -> (shape: cylinder, pose (x,y,z): [0.43330675 0.         0.16029578])'
+        - 'ceil -> (shape: cuboid, pose (x,y,z): [0.90051776 0.         0.55649769])'
+        - 'floor -> (shape: cuboid, pose (x,y,z): [3.72529030e-09 0.00000000e+00 2.11520195e-02]
 
+Interacting with Collision Objects
+----------------------------------
 
-Loading Named Poses from Config files
----------------------------------------
+With RVIZ open, simply add the ``RobotModel`` and the ``InteractiveMarkers`` modules to view currently configured shapes.
 
-To load configs from a YAML other than the default Armer config, the ``/arm/add_named_pose_config`` service can be used.
+Removing Collision Objects
+--------------------------
 
-It can be summoned via Python or Bash.
+Added objects (excluding those that are a part of the robot_description) can be done through the ``/arm/remove_collision_object`` service.
 
-Python:
+This can be called via Python:
 
 .. code-block:: python
 
-    from armer_msgs.srv import AddNamedPoseConfig
+    from armer_msgs.srv import RemoveCollisionObject
     import rospy
 
     rospy.init_node('armer_example', disable_signals=True)
-    save_config_service = rospy.ServiceProxy('/arm/add_named_pose_config', AddNamedPoseConfig)
+    service = rospy.ServiceProxy('/arm/remove_collision_object', RemoveCollisionObject)
     
-    config_path="/home/user/saved_poses.yaml"
-    save_config_service(config_path)    
-
-Bash: 
-
-.. code-block:: bash
-
-    rosservice call /arm/add_named_pose_config "config_path: {PATH_TO_CONFIG.yaml}"
-
+    # Set the name of the object to be removed
+    name="test_object"
 
-Removing Named Poses from Config files
------------------------------------------
+    # Send via service
+    service(name)  
 
-To remove the config poses, the ``/arm/remove_named_pose_config`` service can be called. 
+Or via Bash:
 
-Via Python:
-
-.. code-block:: python
+.. code-block:: bash
 
-    from armer_msgs.srv import RemoveNamedPoseConfig
-    import rospy
+    rosservice call /arm/remove_collision_object "name: 'test_object'"
 
-    rospy.init_node('armer_example', disable_signals=True)
-    remove_config_service = rospy.ServiceProxy('/arm/remove_named_pose_config', RemoveNamedPoseConfig)
-    
-    config_path="/home/user/saved_poses.yaml"
-    remove_config_service(config_path)    
+NOTE: that the name must be within the existing dictionary of collision objects added. If this name is not in this dictionary, then you will receive an error.
 
-Bash: 
+Saving a Collision Scene
+------------------------
 
-.. code-block:: bash
+Loading a Collision Scene
+-------------------------
 
-    rosservice call /arm/remove_named_pose_config "config_path: {PATH_TO_CONFIG.yaml}"