python version of the planning scene tutorial

Author: PeterMitrano

doc/planning_scene/src/planning_scene_tutorial.py

@@ -0,0 +1,265 @@
+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
+
+
+# BEGIN_SUB_TUTORIAL stateFeasibilityTestExample
+#
+# User defined constraints can also be specified to the PlanningScene
+# class. This is done by specifying a callback using the
+# setStateFeasibilityPredicate function. Here's a simple example of a
+# user-defined callback that checks whether the "panda_joint1" of
+# the Panda robot is at a positive or negative angle:
+def stateFeasibilityTestExample(kinematic_state, verbose):
+    joint_values = kinematic_state.getJointPositions("panda_joint1")
+    return joint_values[0] > 0.0
+
+
+# END_SUB_TUTORIAL
+
+def main():
+    rospy.init_node("planning_scene_tutorial")
+    moveit_commander.roscpp_initialize(sys.argv)
+
+    robot_state_pub = rospy.Publisher("display_robot_state", DisplayRobotState, queue_size=10)
+
+    # BEGIN_TUTORIAL
+    #
+    # Setup
+    # ^^^^^
+    #
+    # The :planning_scene:`PlanningScene` class can be easily setup and
+    # configured using a :moveit_core:`RobotModel` or a URDF and
+    # SRDF. This is, however, not the recommended way to instantiate a
+    # PlanningScene. The :planning_scene_monitor:`PlanningSceneMonitor`
+    # is the recommended method to create and maintain the current
+    # planning scene (and is discussed in detail in the next tutorial)
+    # using data from the robot's joints and the sensors on the robot. In
+    # this tutorial, we will instantiate a PlanningScene class directly,
+    # but this method of instantiation is only intended for illustration.
+
+    urdf_path = '/opt/ros/noetic/share/moveit_resources_panda_description/urdf/panda.urdf'
+    srdf_path = '/opt/ros/noetic/share/moveit_resources_panda_moveit_config/config/panda.srdf'
+    kinematic_model = robot_model.load_robot_model(urdf_path, srdf_path)
+    planning_scene = PlanningScene(kinematic_model, collision_detection.World())
+    current_state = planning_scene.getCurrentStateNonConst()
+    joint_model_group = current_state.getJointModelGroup("panda_arm")
+
+    # Collision Checking
+    # ^^^^^^^^^^^^^^^^^^
+    #
+    # Self-collision checking
+    # ~~~~~~~~~~~~~~~~~~~~~~~
+    #
+    # The first thing we will do is check whether the robot in its
+    # current state is in *self-collision*, i.e. whether the current
+    # configuration of the robot would result in the robot's parts
+    # hitting each other. To do this, we will construct a
+    # :collision_detection_struct:`CollisionRequest` object and a
+    # :collision_detection_struct:`CollisionResult` object and pass them
+    # into the collision checking function. Note that the result of
+    # whether the robot is in self-collision or not is contained within
+    # the result. Self collision checking uses an *unpadded* version of
+    # the robot, i.e. it directly uses the collision meshes provided in
+    # the URDF with no extra padding added on.
+
+    collision_request = collision_detection.CollisionRequest()
+    collision_result = collision_detection.CollisionResult()
+    planning_scene.checkSelfCollision(collision_request, collision_result)
+    print(f"Test 1: Current state is {'in' if collision_result.collision else 'not in'} self collision")
+
+    # Change the state
+    # ~~~~~~~~~~~~~~~~
+    #
+    # Now, let's change the current state of the robot. The planning
+    # scene maintains the current state internally. We can get a
+    # reference to it and change it and then check for collisions for the
+    # new robot configuration. Note in particular that we need to clear
+    # the collision_result before making a new collision checking
+    # request.
+
+    rng = random_numbers.RandomNumbers(0)
+    current_state.setToRandomPositions(joint_model_group, rng)
+
+    collision_result.clear()
+    planning_scene.checkSelfCollision(collision_request, collision_result)
+    print(f"Test 2: Current state is {'in' if collision_result.collision else 'not in'} self collision")
+
+    # Checking for a group
+    # ~~~~~~~~~~~~~~~~~~~~
+    #
+    # Now, we will do collision checking only for the hand of the
+    # Panda, i.e. we will check whether there are any collisions between
+    # the hand and other parts of the body of the robot. We can ask
+    # for this specifically by adding the group name "hand" to the
+    # collision request.
+
+    collision_request.group_name = "hand"
+    current_state.setToRandomPositions(joint_model_group, rng)
+    collision_result.clear()
+    planning_scene.checkSelfCollision(collision_request, collision_result)
+    print(f"Test 3: Current state is {'in' if collision_result.collision else 'not in'} self collision")
+
+    # Getting Contact Information
+    # ~~~~~~~~~~~~~~~~~~~~~~~~~~~
+    #
+    # First, manually set the Panda arm to a position where we know
+    # internal (self) collisions do happen. Note that this state is now
+    # actually outside the joint limits of the Panda, which we can also
+    # check for directly.
+
+    joint_values = np.array([0.0, 0.0, 0.0, -2.9, 0.0, 1.4, 0.0])
+    joint_model_group = current_state.getJointModelGroup("panda_arm")
+    current_state.setJointGroupPositions(joint_model_group, joint_values)
+    print(
+        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)
+    print(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:
+        print(f"Contact between {first_name} and {second_name}")
+
+    # Modifying the Allowed Collision Matrix
+    # ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+    #
+    # The :collision_detection_class:`AllowedCollisionMatrix` (ACM)
+    # provides a mechanism to tell the collision world to ignore
+    # collisions between certain object: both parts of the robot and
+    # objects in the world. We can tell the collision checker to ignore
+    # all collisions between the links reported above, i.e. even though
+    # the links are actually in collision, the collision checker will
+    # ignore those collisions and return not in collision for this
+    # particular state of the robot.
+    #
+    # Note also in this example how we are making copies of both the
+    # allowed collision matrix and the current state and passing them in
+    # to the collision checking function.
+
+    acm = planning_scene.getAllowedCollisionMatrix()
+    copied_state = planning_scene.getCurrentState()
+
+    # for (it2 = collision_result.contacts.begin() it2 != collision_result.contacts.end() + +it2)
+    for (first_name, second_name), contacts in collision_result.contacts:
+        acm.setEntry(first_name, second_name, True)
+    collision_result.clear()
+    planning_scene.checkSelfCollision(collision_request, collision_result, copied_state, acm)
+    print(f"Test 6: Current state is {'in' if collision_result.collision else 'not in'} self collision")
+
+    # Full Collision Checking
+    # ~~~~~~~~~~~~~~~~~~~~~~~
+    #
+    # While we have been checking for self-collisions, we can use the
+    # checkCollision functions instead which will check for both
+    # self-collisions and for collisions with the environment (which is
+    # currently empty).  This is the set of collision checking
+    # functions that you will use most often in a planner. Note that
+    # collision checks with the environment will use the padded version
+    # of the robot. Padding helps in keeping the robot further away
+    # from obstacles in the environment.
+    collision_result.clear()
+    planning_scene.checkCollision(collision_request, collision_result, copied_state, acm)
+    print(f"Test 7: Current state is {'in' if collision_result.collision else 'not in'} self collision")
+
+    # Constraint Checking
+    # ^^^^^^^^^^^^^^^^^^^
+    #
+    # The PlanningScene class also includes easy to use function calls
+    # for checking constraints. The constraints can be of two types:
+    # (a) constraints chosen from the
+    # :kinematic_constraints:`KinematicConstraint` set:
+    # i.e. :kinematic_constraints:`JointConstraint`,
+    # :kinematic_constraints:`PositionConstraint`,
+    # :kinematic_constraints:`OrientationConstraint` and
+    # :kinematic_constraints:`VisibilityConstraint` and (b) user
+    # defined constraints specified through a callback. We will first
+    # look at an example with a simple KinematicConstraint.
+    #
+    # Checking Kinematic Constraints
+    # ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+    #
+    # We will first define a simple position and orientation constraint
+    # on the end-effector of the panda_arm group of the Panda robot. Note the
+    # use of convenience functions for filling up the constraints
+    # (these functions are found in the :moveit_core_files:`utils.h<utils_8h>` file from the
+    # kinematic_constraints directory in moveit_core).
+
+    end_effector_name = joint_model_group.getLinkModelNames()[-1]
+
+    desired_pose = PoseStamped()
+    desired_pose.pose.orientation.w = 1.0
+    desired_pose.pose.position.x = 0.3
+    desired_pose.pose.position.y = -0.185
+    desired_pose.pose.position.z = 0.5
+    desired_pose.header.frame_id = "panda_link0"
+    goal_constraint = kinematic_constraints.constructGoalConstraints(end_effector_name, desired_pose)
+
+    # Now, we can check a state against this constraint using the
+    # isStateConstrained functions in the PlanningScene class.
+    copied_state.setToRandomPositions(joint_model_group, rng)
+    copied_state.update()
+    constrained = planning_scene.isStateConstrained(copied_state, goal_constraint)
+    print(f"Test 8: Random state is {'constrained' if constrained else 'not constrained'}")
+
+    # There's a more efficient way of checking constraints (when you want
+    # to check the same constraint over and over again, e.g. inside a
+    # planner). We first construct a KinematicConstraintSet which
+    # pre-processes the ROS Constraints messages and sets it up for quick
+    # processing.
+
+    kinematic_constraint_set = kinematic_constraints.KinematicConstraintSet(kinematic_model)
+    kinematic_constraint_set.add(goal_constraint, planning_scene.getTransforms())
+    constrained_2 = planning_scene.isStateConstrained(copied_state, kinematic_constraint_set)
+    print(f"Test 9: Random state is {'constrained' if constrained_2 else 'not constrained'}")
+
+    # There's a direct way to do this using the KinematicConstraintSet
+    # class.
+
+    constraint_eval_result = kinematic_constraint_set.decide(copied_state)
+    print(
+        f"Test 10: Random state is {'constrained' if constraint_eval_result.satisfied else 'not constrained'}")
+
+    # User-defined constraints
+    # ~~~~~~~~~~~~~~~~~~~~~~~~
+    #
+    # CALL_SUB_TUTORIAL stateFeasibilityTestExample
+
+    # Now, whenever isStateFeasible is called, this user-defined callback
+    # will be called.
+
+    # planning_scene.setStateFeasibilityPredicate(stateFeasibilityTestExample)
+    # state_feasible = planning_scene.isStateFeasible(copied_state)
+    # print(f"Test 11: Random state is {'feasible' if state_feasible else 'not feasible'}")
+
+    # Whenever isStateValid is called, three checks are conducted: (a)
+    # collision checking (b) constraint checking and (c) feasibility
+    # checking using the user-defined callback.
+
+    state_valid = planning_scene.isStateValid(copied_state, kinematic_constraint_set, "panda_arm")
+    print(f"Test 12: Random state is {'valid' if state_valid else 'not valid'}")
+
+    # Note that all the planners available through MoveIt and OMPL will
+    # currently perform collision checking, constraint checking and
+    # feasibility checking using user-defined callbacks.
+    # END_TUTORIAL
+
+    moveit_commander.roscpp_shutdown()
+
+
+if __name__ == '__main__':
+    main()