continue work on tutorial

Author: PeterMitrano

CMakeLists.txt

@@ -63,6 +63,7 @@ add_subdirectory(doc/pick_place)
 add_subdirectory(doc/planning)
 add_subdirectory(doc/planning_scene)
 add_subdirectory(doc/planning_scene_ros_api)
+add_subdirectory(doc/planning_scene_python)
 add_subdirectory(doc/robot_model_and_robot_state)
 add_subdirectory(doc/state_display)
 add_subdirectory(doc/subframes)

doc/planning_scene_python/CMakeLists.txt

@@ -0,0 +1,44 @@
+Planning Scene Python
+==================================
+
+The :planning_scene:`PlanningScene` class provides the main interface that you will use
+for collision checking and constraint checking. In this tutorial, we
+will explore the Python interface to this class.
+
+Getting Started
+---------------
+If you haven't already done so, make sure you've completed the steps in `Getting Started <../getting_started/getting_started.html>`_.
+
+The entire code
+---------------
+The entire code can be seen :codedir:`here in the MoveIt GitHub project<planning_scene>`.
+
+.. tutorial-formatter:: ./src/planning_scene_tutorial.py
+
+Running the code
+----------------
+Roslaunch the launch file to run the code directly from moveit_tutorials: ::
+
+ rosrun moveit_tutorials planning_scene_tutorial.py
+
+Expected Output
+---------------
+
+The output should look something like this, though we are using random
+joint values so some things may be different. ::
+
+ [ INFO] [1615403438.643254533]: Loading robot model 'panda'...
+ [INFO] [1615403438.700939]: Test 1: Current state is in self collision
+ [INFO] [1615403438.704516]: Contact between panda_hand and panda_link5
+ [INFO] [1615403438.706369]: Contact between panda_link5 and panda_link7
+ [INFO] [1615403459.455112]: Test 2: Current state is in self collision
+ [INFO] [1615403459.461933]: Test 3: Current state is not in self collision
+ [INFO] [1615403459.470156]: Test 4: Current state is valid
+ [INFO] [1615403459.474089]: Test 6: Current state is not in self collision
+ [INFO] [1615403459.479456]: Test 7: Current state is not in self collision
+ [INFO] [1615403459.488881]: Test 8: Random state is not constrained
+ [INFO] [1615403459.496180]: Test 9: Random state is not constrained
+ [INFO] [1615403459.498652]: Test 10: Random state is not constrained
+ [INFO] [1615403459.503490]: Test 12: Random state is not valid
+
+**Note:** Don't worry if your output has different ROS console format. You can customize your ROS console logger by following `this blog post <http://dav.ee/blog/notes/archives/898>`_.

doc/planning_scene_python/planning_scene_tutorial.rst

@@ -1,10 +1,10 @@
+#!/usr/bin/env python
 import sys
 
 import moveit_commander
 import numpy as np
 import rospy
 from geometry_msgs.msg import PoseStamped
-from moveit_msgs.msg import DisplayRobotState
 from pymoveit import kinematic_constraints, collision_detection, robot_model, random_numbers
 from pymoveit.planning_scene import PlanningScene
 
@@ -58,6 +58,21 @@ def main():
     planning_scene.checkSelfCollision(collision_request, collision_result)
     rospy.loginfo(f"Test 1: Current state is {'in' if collision_result.collision else 'not in'} self collision")
 
+    # Now, we can get contact information for any collisions that might
+    # have happened at a given configuration of the Panda arm. We can ask
+    # for contact information by filling in the appropriate field in the
+    # collision request and specifying the maximum number of contacts to
+    # be returned as a large number.
+
+    collision_request.contacts = True
+    collision_request.max_contacts = 1000
+
+    collision_result.clear()
+    planning_scene.checkSelfCollision(collision_request, collision_result)
+    for (first_name, second_name), contacts in collision_result.contacts.items():
+        rospy.loginfo(f"Contact between {first_name} and {second_name}")
+
+
     # Change the state
     # ~~~~~~~~~~~~~~~~
     #
@@ -104,21 +119,6 @@ def main():
     rospy.loginfo(
         f"Test 4: Current state is {'valid' if current_state.satisfiesBounds(joint_model_group) else 'not valid'}")
 
-    # Now, we can get contact information for any collisions that might
-    # have happened at a given configuration of the Panda arm. We can ask
-    # for contact information by filling in the appropriate field in the
-    # collision request and specifying the maximum number of contacts to
-    # be returned as a large number.
-
-    collision_request.contacts = True
-    collision_request.max_contacts = 1000
-
-    collision_result.clear()
-    planning_scene.checkSelfCollision(collision_request, collision_result)
-    rospy.loginfo(f"Test 5: Current state is {'in' if collision_result.collision else 'not in'} self collision")
-    for (first_name, second_name), contacts in collision_result.contacts:
-        rospy.loginfo(f"Contact between {first_name} and {second_name}")
-
     # Modifying the Allowed Collision Matrix
     # ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
     #
@@ -218,9 +218,6 @@ def main():
     rospy.loginfo(
         f"Test 10: Random state is {'constrained' if constraint_eval_result.satisfied else 'not constrained'}")
 
-    # User-defined constraints
-    # ~~~~~~~~~~~~~~~~~~~~~~~~
-
     # Whenever isStateValid is called, three checks are conducted: (a)
     # collision checking (b) constraint checking and (c) feasibility
     # checking using the user-defined callback.