From 5a03bf80ab60f386a5784799ee96e7f939cf4776 Mon Sep 17 00:00:00 2001 From: Peter Mitrano Date: Wed, 10 Mar 2021 11:50:36 -0500 Subject: [PATCH 01/18] python version of the planning scene tutorial --- .../src/planning_scene_tutorial.py | 265 ++++++++++++++++++ 1 file changed, 265 insertions(+) create mode 100644 doc/planning_scene/src/planning_scene_tutorial.py diff --git a/doc/planning_scene/src/planning_scene_tutorial.py b/doc/planning_scene/src/planning_scene_tutorial.py new file mode 100644 index 000000000..d15e59a7b --- /dev/null +++ b/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` 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() From 302d849a9e8121625010b613f47aec503b034089 Mon Sep 17 00:00:00 2001 From: Peter Mitrano Date: Wed, 10 Mar 2021 13:38:17 -0500 Subject: [PATCH 02/18] cleanup tutorial --- .../src/planning_scene_tutorial.py | 57 ++++++------------- 1 file changed, 16 insertions(+), 41 deletions(-) diff --git a/doc/planning_scene/src/planning_scene_tutorial.py b/doc/planning_scene/src/planning_scene_tutorial.py index d15e59a7b..efab1e682 100644 --- a/doc/planning_scene/src/planning_scene_tutorial.py +++ b/doc/planning_scene/src/planning_scene_tutorial.py @@ -9,37 +9,21 @@ 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 + # configured using 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) + # PlanningScene. At the time of writing there are not yet python bindings + # for the PlanningSceneMonitoir, which is is the recommended method to + # create and maintain the current planning scene # 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. @@ -72,7 +56,7 @@ def main(): 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") + rospy.loginfo(f"Test 1: Current state is {'in' if collision_result.collision else 'not in'} self collision") # Change the state # ~~~~~~~~~~~~~~~~ @@ -89,7 +73,7 @@ def main(): 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") + rospy.loginfo(f"Test 2: Current state is {'in' if collision_result.collision else 'not in'} self collision") # Checking for a group # ~~~~~~~~~~~~~~~~~~~~ @@ -104,7 +88,7 @@ def main(): 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") + rospy.loginfo(f"Test 3: Current state is {'in' if collision_result.collision else 'not in'} self collision") # Getting Contact Information # ~~~~~~~~~~~~~~~~~~~~~~~~~~~ @@ -117,7 +101,7 @@ def main(): 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( + 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 @@ -131,9 +115,9 @@ def main(): 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") + 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: - print(f"Contact between {first_name} and {second_name}") + rospy.loginfo(f"Contact between {first_name} and {second_name}") # Modifying the Allowed Collision Matrix # ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ @@ -159,7 +143,7 @@ def main(): 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") + rospy.loginfo(f"Test 6: Current state is {'in' if collision_result.collision else 'not in'} self collision") # Full Collision Checking # ~~~~~~~~~~~~~~~~~~~~~~~ @@ -174,7 +158,7 @@ def main(): # 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") + rospy.loginfo(f"Test 7: Current state is {'in' if collision_result.collision else 'not in'} self collision") # Constraint Checking # ^^^^^^^^^^^^^^^^^^^ @@ -214,7 +198,7 @@ def main(): 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'}") + rospy.loginfo(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 @@ -225,33 +209,24 @@ def main(): 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'}") + rospy.loginfo(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( + rospy.loginfo( 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'}") + rospy.loginfo(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 From 8cae58434cf8ec638438bc4af9b5aaac7eaae0d6 Mon Sep 17 00:00:00 2001 From: Peter Mitrano Date: Wed, 10 Mar 2021 14:14:53 -0500 Subject: [PATCH 03/18] continue work on tutorial --- CMakeLists.txt | 1 + doc/planning_scene_python/CMakeLists.txt | 0 .../planning_scene_tutorial.rst | 44 +++++++++++++++++++ .../src/planning_scene_tutorial.py | 35 +++++++-------- 4 files changed, 61 insertions(+), 19 deletions(-) create mode 100644 doc/planning_scene_python/CMakeLists.txt create mode 100644 doc/planning_scene_python/planning_scene_tutorial.rst rename doc/{planning_scene => planning_scene_python}/src/planning_scene_tutorial.py (97%) mode change 100644 => 100755 diff --git a/CMakeLists.txt b/CMakeLists.txt index c394c7c46..3f63c16d5 100644 --- a/CMakeLists.txt +++ b/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) diff --git a/doc/planning_scene_python/CMakeLists.txt b/doc/planning_scene_python/CMakeLists.txt new file mode 100644 index 000000000..e69de29bb diff --git a/doc/planning_scene_python/planning_scene_tutorial.rst b/doc/planning_scene_python/planning_scene_tutorial.rst new file mode 100644 index 000000000..ed5975e40 --- /dev/null +++ b/doc/planning_scene_python/planning_scene_tutorial.rst @@ -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`. + +.. 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 `_. diff --git a/doc/planning_scene/src/planning_scene_tutorial.py b/doc/planning_scene_python/src/planning_scene_tutorial.py old mode 100644 new mode 100755 similarity index 97% rename from doc/planning_scene/src/planning_scene_tutorial.py rename to doc/planning_scene_python/src/planning_scene_tutorial.py index efab1e682..808e59bd0 --- a/doc/planning_scene/src/planning_scene_tutorial.py +++ b/doc/planning_scene_python/src/planning_scene_tutorial.py @@ -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. From 7f365ba42f989bc0809ed5b33827f214e140094d Mon Sep 17 00:00:00 2001 From: Peter Mitrano Date: Sun, 21 Mar 2021 10:22:38 -0400 Subject: [PATCH 04/18] update for new api --- .../move_group_python_interface_tutorial.py | 792 +++++++++--------- .../src/planning_scene_tutorial.py | 14 +- 2 files changed, 397 insertions(+), 409 deletions(-) diff --git a/doc/move_group_python_interface/scripts/move_group_python_interface_tutorial.py b/doc/move_group_python_interface/scripts/move_group_python_interface_tutorial.py index 8926dee7c..71c9fc8a7 100755 --- a/doc/move_group_python_interface/scripts/move_group_python_interface_tutorial.py +++ b/doc/move_group_python_interface/scripts/move_group_python_interface_tutorial.py @@ -32,7 +32,7 @@ # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. # -# Author: Acorn Pooley, Mike Lautman +# Author: Acorn Pooley, Mike Lautman, Peter Mitrano ## BEGIN_SUB_TUTORIAL imports ## @@ -41,30 +41,26 @@ ## and a `RobotCommander`_ class. More on these below. We also import `rospy`_ and some messages that we will use: ## -# Python 2/3 compatibility imports -from __future__ import print_function -from six.moves import input - import sys import copy import rospy -import moveit_commander +from moveit_commander import roscpp_initializer +from moveit.core.conversions import pose_to_list +from moveit.planning_interface import planning_interface import moveit_msgs.msg import geometry_msgs.msg - try: - from math import pi, tau, dist, fabs, cos -except: # For Python 2 compatibility - from math import pi, fabs, cos, sqrt - - tau = 2.0 * pi - - def dist(p, q): - return sqrt(sum((p_i - q_i) ** 2.0 for p_i, q_i in zip(p, q))) - - + from math import pi, tau, dist, fabs, cos +except: # For Python 2 compatibility + from math import pi, fabs, cos, sqrt + tau = 2.0*pi + def dist(p, q): + return sqrt(sum((p_i - q_i)**2.0 for p_i, q_i in zip(p,q))) from std_msgs.msg import String from moveit_commander.conversions import pose_to_list +## END_SUB_TUTORIAL + + ## END_SUB_TUTORIAL @@ -100,375 +96,375 @@ def all_close(goal, actual, tolerance): class MoveGroupPythonInterfaceTutorial(object): - """MoveGroupPythonInterfaceTutorial""" - - def __init__(self): - super(MoveGroupPythonInterfaceTutorial, self).__init__() - - ## BEGIN_SUB_TUTORIAL setup - ## - ## First initialize `moveit_commander`_ and a `rospy`_ node: - moveit_commander.roscpp_initialize(sys.argv) - rospy.init_node("move_group_python_interface_tutorial", anonymous=True) - - ## Instantiate a `RobotCommander`_ object. Provides information such as the robot's - ## kinematic model and the robot's current joint states - robot = moveit_commander.RobotCommander() - - ## Instantiate a `PlanningSceneInterface`_ object. This provides a remote interface - ## for getting, setting, and updating the robot's internal understanding of the - ## surrounding world: - scene = moveit_commander.PlanningSceneInterface() - - ## Instantiate a `MoveGroupCommander`_ object. This object is an interface - ## to a planning group (group of joints). In this tutorial the group is the primary - ## arm joints in the Panda robot, so we set the group's name to "panda_arm". - ## If you are using a different robot, change this value to the name of your robot - ## arm planning group. - ## This interface can be used to plan and execute motions: - group_name = "panda_arm" - move_group = moveit_commander.MoveGroupCommander(group_name) - - ## Create a `DisplayTrajectory`_ ROS publisher which is used to display - ## trajectories in Rviz: - display_trajectory_publisher = rospy.Publisher( - "/move_group/display_planned_path", - moveit_msgs.msg.DisplayTrajectory, - queue_size=20, - ) - - ## END_SUB_TUTORIAL - - ## BEGIN_SUB_TUTORIAL basic_info - ## - ## Getting Basic Information - ## ^^^^^^^^^^^^^^^^^^^^^^^^^ - # We can get the name of the reference frame for this robot: - planning_frame = move_group.get_planning_frame() - print("============ Planning frame: %s" % planning_frame) - - # We can also print the name of the end-effector link for this group: - eef_link = move_group.get_end_effector_link() - print("============ End effector link: %s" % eef_link) - - # We can get a list of all the groups in the robot: - group_names = robot.get_group_names() - print("============ Available Planning Groups:", robot.get_group_names()) - - # Sometimes for debugging it is useful to print the entire state of the - # robot: - print("============ Printing robot state") - print(robot.get_current_state()) - print("") - ## END_SUB_TUTORIAL - - # Misc variables - self.box_name = "" - self.robot = robot - self.scene = scene - self.move_group = move_group - self.display_trajectory_publisher = display_trajectory_publisher - self.planning_frame = planning_frame - self.eef_link = eef_link - self.group_names = group_names - - def go_to_joint_state(self): - # Copy class variables to local variables to make the web tutorials more clear. - # In practice, you should use the class variables directly unless you have a good - # reason not to. - move_group = self.move_group - - ## BEGIN_SUB_TUTORIAL plan_to_joint_state - ## - ## Planning to a Joint Goal - ## ^^^^^^^^^^^^^^^^^^^^^^^^ - ## The Panda's zero configuration is at a `singularity `_, so the first - ## thing we want to do is move it to a slightly better configuration. - ## We use the constant `tau = 2*pi `_ for convenience: - # We get the joint values from the group and change some of the values: - joint_goal = move_group.get_current_joint_values() - joint_goal[0] = 0 - joint_goal[1] = -tau / 8 - joint_goal[2] = 0 - joint_goal[3] = -tau / 4 - joint_goal[4] = 0 - joint_goal[5] = tau / 6 # 1/6 of a turn - joint_goal[6] = 0 - - # The go command can be called with joint values, poses, or without any - # parameters if you have already set the pose or joint target for the group - move_group.go(joint_goal, wait=True) - - # Calling ``stop()`` ensures that there is no residual movement - move_group.stop() - - ## END_SUB_TUTORIAL - - # For testing: - current_joints = move_group.get_current_joint_values() - return all_close(joint_goal, current_joints, 0.01) - - def go_to_pose_goal(self): - # Copy class variables to local variables to make the web tutorials more clear. - # In practice, you should use the class variables directly unless you have a good - # reason not to. - move_group = self.move_group - - ## BEGIN_SUB_TUTORIAL plan_to_pose - ## - ## Planning to a Pose Goal - ## ^^^^^^^^^^^^^^^^^^^^^^^ - ## We can plan a motion for this group to a desired pose for the - ## end-effector: - pose_goal = geometry_msgs.msg.Pose() - pose_goal.orientation.w = 1.0 - pose_goal.position.x = 0.4 - pose_goal.position.y = 0.1 - pose_goal.position.z = 0.4 - - move_group.set_pose_target(pose_goal) - - ## Now, we call the planner to compute the plan and execute it. - plan = move_group.go(wait=True) - # Calling `stop()` ensures that there is no residual movement - move_group.stop() - # It is always good to clear your targets after planning with poses. - # Note: there is no equivalent function for clear_joint_value_targets() - move_group.clear_pose_targets() - - ## END_SUB_TUTORIAL - - # For testing: - # Note that since this section of code will not be included in the tutorials - # we use the class variable rather than the copied state variable - current_pose = self.move_group.get_current_pose().pose - return all_close(pose_goal, current_pose, 0.01) - - def plan_cartesian_path(self, scale=1): - # Copy class variables to local variables to make the web tutorials more clear. - # In practice, you should use the class variables directly unless you have a good - # reason not to. - move_group = self.move_group - - ## BEGIN_SUB_TUTORIAL plan_cartesian_path - ## - ## Cartesian Paths - ## ^^^^^^^^^^^^^^^ - ## You can plan a Cartesian path directly by specifying a list of waypoints - ## for the end-effector to go through. If executing interactively in a - ## Python shell, set scale = 1.0. - ## - waypoints = [] - - wpose = move_group.get_current_pose().pose - wpose.position.z -= scale * 0.1 # First move up (z) - wpose.position.y += scale * 0.2 # and sideways (y) - waypoints.append(copy.deepcopy(wpose)) - - wpose.position.x += scale * 0.1 # Second move forward/backwards in (x) - waypoints.append(copy.deepcopy(wpose)) - - wpose.position.y -= scale * 0.1 # Third move sideways (y) - waypoints.append(copy.deepcopy(wpose)) - - # We want the Cartesian path to be interpolated at a resolution of 1 cm - # which is why we will specify 0.01 as the eef_step in Cartesian - # translation. We will disable the jump threshold by setting it to 0.0, - # ignoring the check for infeasible jumps in joint space, which is sufficient - # for this tutorial. - (plan, fraction) = move_group.compute_cartesian_path( - waypoints, 0.01, 0.0 # waypoints to follow # eef_step - ) # jump_threshold - - # Note: We are just planning, not asking move_group to actually move the robot yet: - return plan, fraction - - ## END_SUB_TUTORIAL - - def display_trajectory(self, plan): - # Copy class variables to local variables to make the web tutorials more clear. - # In practice, you should use the class variables directly unless you have a good - # reason not to. - robot = self.robot - display_trajectory_publisher = self.display_trajectory_publisher - - ## BEGIN_SUB_TUTORIAL display_trajectory - ## - ## Displaying a Trajectory - ## ^^^^^^^^^^^^^^^^^^^^^^^ - ## You can ask RViz to visualize a plan (aka trajectory) for you. But the - ## group.plan() method does this automatically so this is not that useful - ## here (it just displays the same trajectory again): - ## - ## A `DisplayTrajectory`_ msg has two primary fields, trajectory_start and trajectory. - ## We populate the trajectory_start with our current robot state to copy over - ## any AttachedCollisionObjects and add our plan to the trajectory. - display_trajectory = moveit_msgs.msg.DisplayTrajectory() - display_trajectory.trajectory_start = robot.get_current_state() - display_trajectory.trajectory.append(plan) - # Publish - display_trajectory_publisher.publish(display_trajectory) - - ## END_SUB_TUTORIAL - - def execute_plan(self, plan): - # Copy class variables to local variables to make the web tutorials more clear. - # In practice, you should use the class variables directly unless you have a good - # reason not to. - move_group = self.move_group - - ## BEGIN_SUB_TUTORIAL execute_plan - ## - ## Executing a Plan - ## ^^^^^^^^^^^^^^^^ - ## Use execute if you would like the robot to follow - ## the plan that has already been computed: - move_group.execute(plan, wait=True) - - ## **Note:** The robot's current joint state must be within some tolerance of the - ## first waypoint in the `RobotTrajectory`_ or ``execute()`` will fail - ## END_SUB_TUTORIAL - - def wait_for_state_update( - self, box_is_known=False, box_is_attached=False, timeout=4 - ): - # Copy class variables to local variables to make the web tutorials more clear. - # In practice, you should use the class variables directly unless you have a good - # reason not to. - box_name = self.box_name - scene = self.scene - - ## BEGIN_SUB_TUTORIAL wait_for_scene_update - ## - ## Ensuring Collision Updates Are Received - ## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - ## If the Python node dies before publishing a collision object update message, the message - ## could get lost and the box will not appear. To ensure that the updates are - ## made, we wait until we see the changes reflected in the - ## ``get_attached_objects()`` and ``get_known_object_names()`` lists. - ## For the purpose of this tutorial, we call this function after adding, - ## removing, attaching or detaching an object in the planning scene. We then wait - ## until the updates have been made or ``timeout`` seconds have passed - start = rospy.get_time() - seconds = rospy.get_time() - while (seconds - start < timeout) and not rospy.is_shutdown(): - # Test if the box is in attached objects - attached_objects = scene.get_attached_objects([box_name]) - is_attached = len(attached_objects.keys()) > 0 - - # Test if the box is in the scene. - # Note that attaching the box will remove it from known_objects - is_known = box_name in scene.get_known_object_names() - - # Test if we are in the expected state - if (box_is_attached == is_attached) and (box_is_known == is_known): - return True - - # Sleep so that we give other threads time on the processor - rospy.sleep(0.1) - seconds = rospy.get_time() - - # If we exited the while loop without returning then we timed out - return False - ## END_SUB_TUTORIAL - - def add_box(self, timeout=4): - # Copy class variables to local variables to make the web tutorials more clear. - # In practice, you should use the class variables directly unless you have a good - # reason not to. - box_name = self.box_name - scene = self.scene - - ## BEGIN_SUB_TUTORIAL add_box - ## - ## Adding Objects to the Planning Scene - ## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - ## First, we will create a box in the planning scene between the fingers: - box_pose = geometry_msgs.msg.PoseStamped() - box_pose.header.frame_id = "panda_hand" - box_pose.pose.orientation.w = 1.0 - box_pose.pose.position.z = 0.11 # above the panda_hand frame - box_name = "box" - scene.add_box(box_name, box_pose, size=(0.075, 0.075, 0.075)) - - ## END_SUB_TUTORIAL - # Copy local variables back to class variables. In practice, you should use the class - # variables directly unless you have a good reason not to. - self.box_name = box_name - return self.wait_for_state_update(box_is_known=True, timeout=timeout) - - def attach_box(self, timeout=4): - # Copy class variables to local variables to make the web tutorials more clear. - # In practice, you should use the class variables directly unless you have a good - # reason not to. - box_name = self.box_name - robot = self.robot - scene = self.scene - eef_link = self.eef_link - group_names = self.group_names - - ## BEGIN_SUB_TUTORIAL attach_object - ## - ## Attaching Objects to the Robot - ## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - ## Next, we will attach the box to the Panda wrist. Manipulating objects requires the - ## robot be able to touch them without the planning scene reporting the contact as a - ## collision. By adding link names to the ``touch_links`` array, we are telling the - ## planning scene to ignore collisions between those links and the box. For the Panda - ## robot, we set ``grasping_group = 'hand'``. If you are using a different robot, - ## you should change this value to the name of your end effector group name. - grasping_group = "hand" - touch_links = robot.get_link_names(group=grasping_group) - scene.attach_box(eef_link, box_name, touch_links=touch_links) - ## END_SUB_TUTORIAL - - # We wait for the planning scene to update. - return self.wait_for_state_update( - box_is_attached=True, box_is_known=False, timeout=timeout - ) - - def detach_box(self, timeout=4): - # Copy class variables to local variables to make the web tutorials more clear. - # In practice, you should use the class variables directly unless you have a good - # reason not to. - box_name = self.box_name - scene = self.scene - eef_link = self.eef_link - - ## BEGIN_SUB_TUTORIAL detach_object - ## - ## Detaching Objects from the Robot - ## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - ## We can also detach and remove the object from the planning scene: - scene.remove_attached_object(eef_link, name=box_name) - ## END_SUB_TUTORIAL - - # We wait for the planning scene to update. - return self.wait_for_state_update( - box_is_known=True, box_is_attached=False, timeout=timeout - ) - - def remove_box(self, timeout=4): - # Copy class variables to local variables to make the web tutorials more clear. - # In practice, you should use the class variables directly unless you have a good - # reason not to. - box_name = self.box_name - scene = self.scene - - ## BEGIN_SUB_TUTORIAL remove_object - ## - ## Removing Objects from the Planning Scene - ## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - ## We can remove the box from the world. - scene.remove_world_object(box_name) - - ## **Note:** The object must be detached before we can remove it from the world - ## END_SUB_TUTORIAL - - # We wait for the planning scene to update. - return self.wait_for_state_update( - box_is_attached=False, box_is_known=False, timeout=timeout - ) + """MoveGroupPythonInterfaceTutorial""" + def __init__(self): + super(MoveGroupPythonInterfaceTutorial, self).__init__() + + ## BEGIN_SUB_TUTORIAL setup + ## + ## First initialize `moveit_commander`_ and a `rospy`_ node: + moveit_commander.roscpp_initialize(sys.argv) + rospy.init_node('move_group_python_interface_tutorial', anonymous=True) + + ## Instantiate a `RobotCommander`_ object. Provides information such as the robot's + ## kinematic model and the robot's current joint states + robot = moveit_commander.RobotCommander() + + ## Instantiate a `PlanningSceneInterface`_ object. This provides a remote interface + ## for getting, setting, and updating the robot's internal understanding of the + ## surrounding world: + scene = moveit_commander.PlanningSceneInterface() + + ## Instantiate a `MoveGroupCommander`_ object. This object is an interface + ## to a planning group (group of joints). In this tutorial the group is the primary + ## arm joints in the Panda robot, so we set the group's name to "panda_arm". + ## If you are using a different robot, change this value to the name of your robot + ## arm planning group. + ## This interface can be used to plan and execute motions: + group_name = "panda_arm" + move_group = moveit_commander.MoveGroupCommander(group_name) + + ## Create a `DisplayTrajectory`_ ROS publisher which is used to display + ## trajectories in Rviz: + display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', + moveit_msgs.msg.DisplayTrajectory, + queue_size=20) + + ## END_SUB_TUTORIAL + + ## BEGIN_SUB_TUTORIAL basic_info + ## + ## Getting Basic Information + ## ^^^^^^^^^^^^^^^^^^^^^^^^^ + # We can get the name of the reference frame for this robot: + planning_frame = move_group.get_planning_frame() + print("============ Planning frame: %s" % planning_frame) + + # We can also print the name of the end-effector link for this group: + eef_link = move_group.get_end_effector_link() + print("============ End effector link: %s" % eef_link) + + # We can get a list of all the groups in the robot: + group_names = robot.get_group_names() + print("============ Available Planning Groups:", robot.get_group_names()) + + # Sometimes for debugging it is useful to print the entire state of the + # robot: + print("============ Printing robot state") + print(robot.get_current_state()) + print("") + ## END_SUB_TUTORIAL + + # Misc variables + self.box_name = '' + self.robot = robot + self.scene = scene + self.move_group = move_group + self.display_trajectory_publisher = display_trajectory_publisher + self.planning_frame = planning_frame + self.eef_link = eef_link + self.group_names = group_names + + + def go_to_joint_state(self): + # Copy class variables to local variables to make the web tutorials more clear. + # In practice, you should use the class variables directly unless you have a good + # reason not to. + move_group = self.move_group + + ## BEGIN_SUB_TUTORIAL plan_to_joint_state + ## + ## Planning to a Joint Goal + ## ^^^^^^^^^^^^^^^^^^^^^^^^ + ## The Panda's zero configuration is at a `singularity `_, so the first + ## thing we want to do is move it to a slightly better configuration. + ## We use the constant `tau = 2*pi `_ for convenience: + # We get the joint values from the group and change some of the values: + joint_goal = move_group.get_current_joint_values() + joint_goal[0] = 0 + joint_goal[1] = -tau/8 + joint_goal[2] = 0 + joint_goal[3] = -tau/4 + joint_goal[4] = 0 + joint_goal[5] = tau/6 # 1/6 of a turn + joint_goal[6] = 0 + + # The go command can be called with joint values, poses, or without any + # parameters if you have already set the pose or joint target for the group + move_group.go(joint_goal, wait=True) + + # Calling ``stop()`` ensures that there is no residual movement + move_group.stop() + + ## END_SUB_TUTORIAL + + # For testing: + current_joints = move_group.get_current_joint_values() + return all_close(joint_goal, current_joints, 0.01) + + + def go_to_pose_goal(self): + # Copy class variables to local variables to make the web tutorials more clear. + # In practice, you should use the class variables directly unless you have a good + # reason not to. + move_group = self.move_group + + ## BEGIN_SUB_TUTORIAL plan_to_pose + ## + ## Planning to a Pose Goal + ## ^^^^^^^^^^^^^^^^^^^^^^^ + ## We can plan a motion for this group to a desired pose for the + ## end-effector: + pose_goal = geometry_msgs.msg.Pose() + pose_goal.orientation.w = 1.0 + pose_goal.position.x = 0.4 + pose_goal.position.y = 0.1 + pose_goal.position.z = 0.4 + + move_group.set_pose_target(pose_goal) + + ## Now, we call the planner to compute the plan and execute it. + plan = move_group.go(wait=True) + # Calling `stop()` ensures that there is no residual movement + move_group.stop() + # It is always good to clear your targets after planning with poses. + # Note: there is no equivalent function for clear_joint_value_targets() + move_group.clear_pose_targets() + + ## END_SUB_TUTORIAL + + # For testing: + # Note that since this section of code will not be included in the tutorials + # we use the class variable rather than the copied state variable + current_pose = self.move_group.get_current_pose().pose + return all_close(pose_goal, current_pose, 0.01) + + + def plan_cartesian_path(self, scale=1): + # Copy class variables to local variables to make the web tutorials more clear. + # In practice, you should use the class variables directly unless you have a good + # reason not to. + move_group = self.move_group + + ## BEGIN_SUB_TUTORIAL plan_cartesian_path + ## + ## Cartesian Paths + ## ^^^^^^^^^^^^^^^ + ## You can plan a Cartesian path directly by specifying a list of waypoints + ## for the end-effector to go through. If executing interactively in a + ## Python shell, set scale = 1.0. + ## + waypoints = [] + + wpose = move_group.get_current_pose().pose + wpose.position.z -= scale * 0.1 # First move up (z) + wpose.position.y += scale * 0.2 # and sideways (y) + waypoints.append(copy.deepcopy(wpose)) + + wpose.position.x += scale * 0.1 # Second move forward/backwards in (x) + waypoints.append(copy.deepcopy(wpose)) + + wpose.position.y -= scale * 0.1 # Third move sideways (y) + waypoints.append(copy.deepcopy(wpose)) + + # We want the Cartesian path to be interpolated at a resolution of 1 cm + # which is why we will specify 0.01 as the eef_step in Cartesian + # translation. We will disable the jump threshold by setting it to 0.0, + # ignoring the check for infeasible jumps in joint space, which is sufficient + # for this tutorial. + (plan, fraction) = move_group.compute_cartesian_path( + waypoints, # waypoints to follow + 0.01, # eef_step + 0.0) # jump_threshold + + # Note: We are just planning, not asking move_group to actually move the robot yet: + return plan, fraction + + ## END_SUB_TUTORIAL + + + def display_trajectory(self, plan): + # Copy class variables to local variables to make the web tutorials more clear. + # In practice, you should use the class variables directly unless you have a good + # reason not to. + robot = self.robot + display_trajectory_publisher = self.display_trajectory_publisher + + ## BEGIN_SUB_TUTORIAL display_trajectory + ## + ## Displaying a Trajectory + ## ^^^^^^^^^^^^^^^^^^^^^^^ + ## You can ask RViz to visualize a plan (aka trajectory) for you. But the + ## group.plan() method does this automatically so this is not that useful + ## here (it just displays the same trajectory again): + ## + ## A `DisplayTrajectory`_ msg has two primary fields, trajectory_start and trajectory. + ## We populate the trajectory_start with our current robot state to copy over + ## any AttachedCollisionObjects and add our plan to the trajectory. + display_trajectory = moveit_msgs.msg.DisplayTrajectory() + display_trajectory.trajectory_start = robot.get_current_state() + display_trajectory.trajectory.append(plan) + # Publish + display_trajectory_publisher.publish(display_trajectory); + + ## END_SUB_TUTORIAL + + + def execute_plan(self, plan): + # Copy class variables to local variables to make the web tutorials more clear. + # In practice, you should use the class variables directly unless you have a good + # reason not to. + move_group = self.move_group + + ## BEGIN_SUB_TUTORIAL execute_plan + ## + ## Executing a Plan + ## ^^^^^^^^^^^^^^^^ + ## Use execute if you would like the robot to follow + ## the plan that has already been computed: + move_group.execute(plan, wait=True) + + ## **Note:** The robot's current joint state must be within some tolerance of the + ## first waypoint in the `RobotTrajectory`_ or ``execute()`` will fail + ## END_SUB_TUTORIAL + + + def wait_for_state_update(self, box_is_known=False, box_is_attached=False, timeout=4): + # Copy class variables to local variables to make the web tutorials more clear. + # In practice, you should use the class variables directly unless you have a good + # reason not to. + box_name = self.box_name + scene = self.scene + + ## BEGIN_SUB_TUTORIAL wait_for_scene_update + ## + ## Ensuring Collision Updates Are Received + ## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + ## If the Python node dies before publishing a collision object update message, the message + ## could get lost and the box will not appear. To ensure that the updates are + ## made, we wait until we see the changes reflected in the + ## ``get_attached_objects()`` and ``get_known_object_names()`` lists. + ## For the purpose of this tutorial, we call this function after adding, + ## removing, attaching or detaching an object in the planning scene. We then wait + ## until the updates have been made or ``timeout`` seconds have passed + start = rospy.get_time() + seconds = rospy.get_time() + while (seconds - start < timeout) and not rospy.is_shutdown(): + # Test if the box is in attached objects + attached_objects = scene.get_attached_objects([box_name]) + is_attached = len(attached_objects.keys()) > 0 + + # Test if the box is in the scene. + # Note that attaching the box will remove it from known_objects + is_known = box_name in scene.get_known_object_names() + + # Test if we are in the expected state + if (box_is_attached == is_attached) and (box_is_known == is_known): + return True + + # Sleep so that we give other threads time on the processor + rospy.sleep(0.1) + seconds = rospy.get_time() + + # If we exited the while loop without returning then we timed out + return False + ## END_SUB_TUTORIAL + + + def add_box(self, timeout=4): + # Copy class variables to local variables to make the web tutorials more clear. + # In practice, you should use the class variables directly unless you have a good + # reason not to. + box_name = self.box_name + scene = self.scene + + ## BEGIN_SUB_TUTORIAL add_box + ## + ## Adding Objects to the Planning Scene + ## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + ## First, we will create a box in the planning scene between the fingers: + box_pose = geometry_msgs.msg.PoseStamped() + box_pose.header.frame_id = "panda_hand" + box_pose.pose.orientation.w = 1.0 + box_pose.pose.position.z = 0.11 # above the panda_hand frame + box_name = "box" + scene.add_box(box_name, box_pose, size=(0.075, 0.075, 0.075)) + + ## END_SUB_TUTORIAL + # Copy local variables back to class variables. In practice, you should use the class + # variables directly unless you have a good reason not to. + self.box_name=box_name + return self.wait_for_state_update(box_is_known=True, timeout=timeout) + + + def attach_box(self, timeout=4): + # Copy class variables to local variables to make the web tutorials more clear. + # In practice, you should use the class variables directly unless you have a good + # reason not to. + box_name = self.box_name + robot = self.robot + scene = self.scene + eef_link = self.eef_link + group_names = self.group_names + + ## BEGIN_SUB_TUTORIAL attach_object + ## + ## Attaching Objects to the Robot + ## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + ## Next, we will attach the box to the Panda wrist. Manipulating objects requires the + ## robot be able to touch them without the planning scene reporting the contact as a + ## collision. By adding link names to the ``touch_links`` array, we are telling the + ## planning scene to ignore collisions between those links and the box. For the Panda + ## robot, we set ``grasping_group = 'hand'``. If you are using a different robot, + ## you should change this value to the name of your end effector group name. + grasping_group = 'hand' + touch_links = robot.get_link_names(group=grasping_group) + scene.attach_box(eef_link, box_name, touch_links=touch_links) + ## END_SUB_TUTORIAL + + # We wait for the planning scene to update. + return self.wait_for_state_update(box_is_attached=True, box_is_known=False, timeout=timeout) + + + def detach_box(self, timeout=4): + # Copy class variables to local variables to make the web tutorials more clear. + # In practice, you should use the class variables directly unless you have a good + # reason not to. + box_name = self.box_name + scene = self.scene + eef_link = self.eef_link + + ## BEGIN_SUB_TUTORIAL detach_object + ## + ## Detaching Objects from the Robot + ## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + ## We can also detach and remove the object from the planning scene: + scene.remove_attached_object(eef_link, name=box_name) + ## END_SUB_TUTORIAL + + # We wait for the planning scene to update. + return self.wait_for_state_update(box_is_known=True, box_is_attached=False, timeout=timeout) + + + def remove_box(self, timeout=4): + # Copy class variables to local variables to make the web tutorials more clear. + # In practice, you should use the class variables directly unless you have a good + # reason not to. + box_name = self.box_name + scene = self.scene + + ## BEGIN_SUB_TUTORIAL remove_object + ## + ## Removing Objects from the Planning Scene + ## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + ## We can remove the box from the world. + scene.remove_world_object(box_name) + + ## **Note:** The object must be detached before we can remove it from the world + ## END_SUB_TUTORIAL + + # We wait for the planning scene to update. + return self.wait_for_state_update(box_is_attached=False, box_is_known=False, timeout=timeout) def main(): @@ -479,14 +475,10 @@ def main(): print("----------------------------------------------------------") print("Press Ctrl-D to exit at any time") print("") - input( - "============ Press `Enter` to begin the tutorial by setting up the moveit_commander ..." - ) + input("============ Press `Enter` to begin the tutorial by setting up the moveit_commander ...") tutorial = MoveGroupPythonInterfaceTutorial() - input( - "============ Press `Enter` to execute a movement using a joint state goal ..." - ) + input("============ Press `Enter` to execute a movement using a joint state goal ...") tutorial.go_to_joint_state() input("============ Press `Enter` to execute a movement using a pose goal ...") @@ -495,9 +487,7 @@ def main(): input("============ Press `Enter` to plan and display a Cartesian path ...") cartesian_plan, fraction = tutorial.plan_cartesian_path() - input( - "============ Press `Enter` to display a saved trajectory (this will replay the Cartesian path) ..." - ) + input("============ Press `Enter` to display a saved trajectory (this will replay the Cartesian path) ...") tutorial.display_trajectory(cartesian_plan) input("============ Press `Enter` to execute a saved path ...") @@ -509,28 +499,26 @@ def main(): input("============ Press `Enter` to attach a Box to the Panda robot ...") tutorial.attach_box() - input( - "============ Press `Enter` to plan and execute a path with an attached collision object ..." - ) + input("============ Press `Enter` to plan and execute a path with an attached collision object ...") cartesian_plan, fraction = tutorial.plan_cartesian_path(scale=-1) tutorial.execute_plan(cartesian_plan) input("============ Press `Enter` to detach the box from the Panda robot ...") tutorial.detach_box() - input( - "============ Press `Enter` to remove the box from the planning scene ..." - ) + input("============ Press `Enter` to remove the box from the planning scene ...") tutorial.remove_box() print("============ Python tutorial demo complete!") + + roscpp_initializer.roscpp_shutdown() except rospy.ROSInterruptException: return except KeyboardInterrupt: return -if __name__ == "__main__": +if __name__ == '__main__': main() ## BEGIN_TUTORIAL diff --git a/doc/planning_scene_python/src/planning_scene_tutorial.py b/doc/planning_scene_python/src/planning_scene_tutorial.py index 808e59bd0..d04351cdb 100755 --- a/doc/planning_scene_python/src/planning_scene_tutorial.py +++ b/doc/planning_scene_python/src/planning_scene_tutorial.py @@ -5,8 +5,9 @@ import numpy as np import rospy from geometry_msgs.msg import PoseStamped -from pymoveit import kinematic_constraints, collision_detection, robot_model, random_numbers -from pymoveit.planning_scene import PlanningScene +import moveit.core +from moveit.core import kinematic_constraints, collision_detection, robot_model +from moveit.core.planning_scene import PlanningScene def main(): @@ -30,7 +31,7 @@ def main(): 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) + kinematic_model = moveit.core.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") @@ -83,8 +84,7 @@ def main(): # the collision_result before making a new collision checking # request. - rng = random_numbers.RandomNumbers(0) - current_state.setToRandomPositions(joint_model_group, rng) + current_state.setToRandomPositions(joint_model_group) collision_result.clear() planning_scene.checkSelfCollision(collision_request, collision_result) @@ -100,7 +100,7 @@ def main(): # collision request. collision_request.group_name = "hand" - current_state.setToRandomPositions(joint_model_group, rng) + current_state.setToRandomPositions(joint_model_group) collision_result.clear() planning_scene.checkSelfCollision(collision_request, collision_result) rospy.loginfo(f"Test 3: Current state is {'in' if collision_result.collision else 'not in'} self collision") @@ -195,7 +195,7 @@ def main(): # 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.setToRandomPositions(joint_model_group) copied_state.update() constrained = planning_scene.isStateConstrained(copied_state, goal_constraint) rospy.loginfo(f"Test 8: Random state is {'constrained' if constrained else 'not constrained'}") From a5d7b49c393414574b0b86f1840006b6f66445c0 Mon Sep 17 00:00:00 2001 From: Peter Mitrano Date: Sun, 28 Mar 2021 16:17:16 -0400 Subject: [PATCH 05/18] revert other tutorial --- .../move_group_python_interface_tutorial.py | 145 +++++++++--------- 1 file changed, 70 insertions(+), 75 deletions(-) diff --git a/doc/move_group_python_interface/scripts/move_group_python_interface_tutorial.py b/doc/move_group_python_interface/scripts/move_group_python_interface_tutorial.py index 71c9fc8a7..35aa97687 100755 --- a/doc/move_group_python_interface/scripts/move_group_python_interface_tutorial.py +++ b/doc/move_group_python_interface/scripts/move_group_python_interface_tutorial.py @@ -32,7 +32,7 @@ # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. # -# Author: Acorn Pooley, Mike Lautman, Peter Mitrano +# Author: Acorn Pooley, Mike Lautman ## BEGIN_SUB_TUTORIAL imports ## @@ -41,12 +41,14 @@ ## and a `RobotCommander`_ class. More on these below. We also import `rospy`_ and some messages that we will use: ## +# Python 2/3 compatibility imports +from __future__ import print_function +from six.moves import input + import sys import copy import rospy -from moveit_commander import roscpp_initializer -from moveit.core.conversions import pose_to_list -from moveit.planning_interface import planning_interface +import moveit_commander import moveit_msgs.msg import geometry_msgs.msg try: @@ -61,38 +63,34 @@ def dist(p, q): ## END_SUB_TUTORIAL - -## END_SUB_TUTORIAL - - def all_close(goal, actual, tolerance): - """ - Convenience method for testing if the values in two lists are within a tolerance of each other. - For Pose and PoseStamped inputs, the angle between the two quaternions is compared (the angle - between the identical orientations q and -q is calculated correctly). - @param: goal A list of floats, a Pose or a PoseStamped - @param: actual A list of floats, a Pose or a PoseStamped - @param: tolerance A float - @returns: bool - """ - if type(goal) is list: - for index in range(len(goal)): - if abs(actual[index] - goal[index]) > tolerance: - return False - - elif type(goal) is geometry_msgs.msg.PoseStamped: - return all_close(goal.pose, actual.pose, tolerance) - - elif type(goal) is geometry_msgs.msg.Pose: - x0, y0, z0, qx0, qy0, qz0, qw0 = pose_to_list(actual) - x1, y1, z1, qx1, qy1, qz1, qw1 = pose_to_list(goal) - # Euclidean distance - d = dist((x1, y1, z1), (x0, y0, z0)) - # phi = angle between orientations - cos_phi_half = fabs(qx0 * qx1 + qy0 * qy1 + qz0 * qz1 + qw0 * qw1) - return d <= tolerance and cos_phi_half >= cos(tolerance / 2.0) - - return True + """ + Convenience method for testing if the values in two lists are within a tolerance of each other. + For Pose and PoseStamped inputs, the angle between the two quaternions is compared (the angle + between the identical orientations q and -q is calculated correctly). + @param: goal A list of floats, a Pose or a PoseStamped + @param: actual A list of floats, a Pose or a PoseStamped + @param: tolerance A float + @returns: bool + """ + if type(goal) is list: + for index in range(len(goal)): + if abs(actual[index] - goal[index]) > tolerance: + return False + + elif type(goal) is geometry_msgs.msg.PoseStamped: + return all_close(goal.pose, actual.pose, tolerance) + + elif type(goal) is geometry_msgs.msg.Pose: + x0, y0, z0, qx0, qy0, qz0, qw0 = pose_to_list(actual) + x1, y1, z1, qx1, qy1, qz1, qw1 = pose_to_list(goal) + # Euclidean distance + d = dist((x1, y1, z1), (x0, y0, z0)) + # phi = angle between orientations + cos_phi_half = fabs(qx0*qx1 + qy0*qy1 + qz0*qz1 + qw0*qw1) + return d <= tolerance and cos_phi_half >= cos(tolerance / 2.0) + + return True class MoveGroupPythonInterfaceTutorial(object): @@ -177,7 +175,7 @@ def go_to_joint_state(self): ## Planning to a Joint Goal ## ^^^^^^^^^^^^^^^^^^^^^^^^ ## The Panda's zero configuration is at a `singularity `_, so the first - ## thing we want to do is move it to a slightly better configuration. + ## thing we want to do is move it to a slightly better configuration. ## We use the constant `tau = 2*pi `_ for convenience: # We get the joint values from the group and change some of the values: joint_goal = move_group.get_current_joint_values() @@ -468,58 +466,55 @@ def remove_box(self, timeout=4): def main(): - try: - print("") - print("----------------------------------------------------------") - print("Welcome to the MoveIt MoveGroup Python Interface Tutorial") - print("----------------------------------------------------------") - print("Press Ctrl-D to exit at any time") - print("") - input("============ Press `Enter` to begin the tutorial by setting up the moveit_commander ...") - tutorial = MoveGroupPythonInterfaceTutorial() - - input("============ Press `Enter` to execute a movement using a joint state goal ...") - tutorial.go_to_joint_state() - - input("============ Press `Enter` to execute a movement using a pose goal ...") - tutorial.go_to_pose_goal() + try: + print("") + print("----------------------------------------------------------") + print("Welcome to the MoveIt MoveGroup Python Interface Tutorial") + print("----------------------------------------------------------") + print("Press Ctrl-D to exit at any time") + print("") + input("============ Press `Enter` to begin the tutorial by setting up the moveit_commander ...") + tutorial = MoveGroupPythonInterfaceTutorial() - input("============ Press `Enter` to plan and display a Cartesian path ...") - cartesian_plan, fraction = tutorial.plan_cartesian_path() + input("============ Press `Enter` to execute a movement using a joint state goal ...") + tutorial.go_to_joint_state() - input("============ Press `Enter` to display a saved trajectory (this will replay the Cartesian path) ...") - tutorial.display_trajectory(cartesian_plan) + input("============ Press `Enter` to execute a movement using a pose goal ...") + tutorial.go_to_pose_goal() - input("============ Press `Enter` to execute a saved path ...") - tutorial.execute_plan(cartesian_plan) + input("============ Press `Enter` to plan and display a Cartesian path ...") + cartesian_plan, fraction = tutorial.plan_cartesian_path() - input("============ Press `Enter` to add a box to the planning scene ...") - tutorial.add_box() + input("============ Press `Enter` to display a saved trajectory (this will replay the Cartesian path) ...") + tutorial.display_trajectory(cartesian_plan) - input("============ Press `Enter` to attach a Box to the Panda robot ...") - tutorial.attach_box() + input("============ Press `Enter` to execute a saved path ...") + tutorial.execute_plan(cartesian_plan) - input("============ Press `Enter` to plan and execute a path with an attached collision object ...") - cartesian_plan, fraction = tutorial.plan_cartesian_path(scale=-1) - tutorial.execute_plan(cartesian_plan) + input("============ Press `Enter` to add a box to the planning scene ...") + tutorial.add_box() - input("============ Press `Enter` to detach the box from the Panda robot ...") - tutorial.detach_box() + input("============ Press `Enter` to attach a Box to the Panda robot ...") + tutorial.attach_box() - input("============ Press `Enter` to remove the box from the planning scene ...") - tutorial.remove_box() + input("============ Press `Enter` to plan and execute a path with an attached collision object ...") + cartesian_plan, fraction = tutorial.plan_cartesian_path(scale=-1) + tutorial.execute_plan(cartesian_plan) - print("============ Python tutorial demo complete!") + input("============ Press `Enter` to detach the box from the Panda robot ...") + tutorial.detach_box() - roscpp_initializer.roscpp_shutdown() - except rospy.ROSInterruptException: - return - except KeyboardInterrupt: - return + input("============ Press `Enter` to remove the box from the planning scene ...") + tutorial.remove_box() + print("============ Python tutorial demo complete!") + except rospy.ROSInterruptException: + return + except KeyboardInterrupt: + return if __name__ == '__main__': - main() + main() ## BEGIN_TUTORIAL ## .. _moveit_commander: From d05f96d1857c5940de7bd36e2464bee0d6659078 Mon Sep 17 00:00:00 2001 From: Peter Mitrano Date: Sat, 10 Apr 2021 11:42:49 -0400 Subject: [PATCH 06/18] debugging --- conf.py | 7 ++++++ .../planning_scene_tutorial.rst | 8 ++++-- .../src/planning_scene_tutorial.py | 25 ++++++------------- index.rst | 1 + 4 files changed, 22 insertions(+), 19 deletions(-) diff --git a/conf.py b/conf.py index b5be6b75e..d27ed4ded 100644 --- a/conf.py +++ b/conf.py @@ -50,6 +50,7 @@ # Add any paths that contain custom themes here, relative to this directory. # Links +<<<<<<< HEAD ros_distro = "noetic" extlinks = { "codedir": ( @@ -83,6 +84,12 @@ + "/api/moveit_core/html/cpp/classmoveit_1_1core_1_1%s.html", "", ), + "moveit_python": ( + "http://docs.ros.org/" + + ros_distro + + "/api/moveit_core/html/python/_autosummary/moveit.%s.html", + "", + ), "planning_scene": ( "http://docs.ros.org/" + ros_distro diff --git a/doc/planning_scene_python/planning_scene_tutorial.rst b/doc/planning_scene_python/planning_scene_tutorial.rst index ed5975e40..30429d92f 100644 --- a/doc/planning_scene_python/planning_scene_tutorial.rst +++ b/doc/planning_scene_python/planning_scene_tutorial.rst @@ -1,7 +1,7 @@ Planning Scene Python ================================== -The :planning_scene:`PlanningScene` class provides the main interface that you will use +The :moveit_python:`core.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. @@ -17,6 +17,10 @@ The entire code can be seen :codedir:`here in the MoveIt GitHub project`_ in your catkin workspace and build: :: + + roslaunch panda_moveit_config demo.launch + Roslaunch the launch file to run the code directly from moveit_tutorials: :: rosrun moveit_tutorials planning_scene_tutorial.py @@ -41,4 +45,4 @@ joint values so some things may be different. :: [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 `_. +**Note:** Don't worry if your output has different ROS console format. You can customize your ROS console logger by following `this blog post `_. \ No newline at end of file diff --git a/doc/planning_scene_python/src/planning_scene_tutorial.py b/doc/planning_scene_python/src/planning_scene_tutorial.py index d04351cdb..3e8b7d7f0 100755 --- a/doc/planning_scene_python/src/planning_scene_tutorial.py +++ b/doc/planning_scene_python/src/planning_scene_tutorial.py @@ -19,7 +19,7 @@ def main(): # Setup # ^^^^^ # - # The :planning_scene:`PlanningScene` class can be easily setup and + # The :moveit_python:`core.planning_scene.PlanningScene` class can be easily setup and # configured using a URDF and # SRDF. This is, however, not the recommended way to instantiate a # PlanningScene. At the time of writing there are not yet python bindings @@ -46,8 +46,8 @@ def main(): # 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 + # :moveit_python:`core.collision_detection.CollisionRequest` object and a + # :moveit_python:`core.collision_detection.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 @@ -73,7 +73,6 @@ def main(): for (first_name, second_name), contacts in collision_result.contacts.items(): rospy.loginfo(f"Contact between {first_name} and {second_name}") - # Change the state # ~~~~~~~~~~~~~~~~ # @@ -122,7 +121,7 @@ def main(): # Modifying the Allowed Collision Matrix # ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ # - # The :collision_detection_class:`AllowedCollisionMatrix` (ACM) + # The :moveit_python:`core.collision_detection.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 @@ -163,16 +162,10 @@ def main(): # 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. + # The PlanningScene class also includes easy to use function calls for checking constraints. The constraints can be + # of two types: (a) constraints constructed using :moveit_python:`core.kinematic_constraints`, or (b) user defined + # constraints specified through a callback. We will first look at an example with a simple + # KinematicConstraint. # # Checking Kinematic Constraints # ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ @@ -180,8 +173,6 @@ def main(): # 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` file from the - # kinematic_constraints directory in moveit_core). end_effector_name = joint_model_group.getLinkModelNames()[-1] diff --git a/index.rst b/index.rst index 2a6d746d4..ec2f313b2 100644 --- a/index.rst +++ b/index.rst @@ -38,6 +38,7 @@ Building more complex applications with MoveIt often requires developers to dig doc/planning_scene/planning_scene_tutorial doc/planning_scene_monitor/planning_scene_monitor_tutorial doc/planning_scene_ros_api/planning_scene_ros_api_tutorial + doc/planning_scene_python/planning_scene_tutorial doc/motion_planning_api/motion_planning_api_tutorial doc/motion_planning_pipeline/motion_planning_pipeline_tutorial doc/creating_moveit_plugins/plugin_tutorial From 348fd5968966caaf844156fd1f31ffd64f4acb5d Mon Sep 17 00:00:00 2001 From: Peter Mitrano Date: Sat, 10 Apr 2021 13:39:01 -0400 Subject: [PATCH 07/18] figuring out the right URLs --- conf.py | 1 - doc/planning_scene_python/planning_scene_tutorial.rst | 2 +- .../src/planning_scene_tutorial.py | 10 +++++----- 3 files changed, 6 insertions(+), 7 deletions(-) diff --git a/conf.py b/conf.py index d27ed4ded..8adadf04b 100644 --- a/conf.py +++ b/conf.py @@ -50,7 +50,6 @@ # Add any paths that contain custom themes here, relative to this directory. # Links -<<<<<<< HEAD ros_distro = "noetic" extlinks = { "codedir": ( diff --git a/doc/planning_scene_python/planning_scene_tutorial.rst b/doc/planning_scene_python/planning_scene_tutorial.rst index 30429d92f..26f515dc1 100644 --- a/doc/planning_scene_python/planning_scene_tutorial.rst +++ b/doc/planning_scene_python/planning_scene_tutorial.rst @@ -1,7 +1,7 @@ Planning Scene Python ================================== -The :moveit_python:`core.planning_scene.PlanningScene` class provides the main interface that you will use +The :moveit_core_python:`core.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. diff --git a/doc/planning_scene_python/src/planning_scene_tutorial.py b/doc/planning_scene_python/src/planning_scene_tutorial.py index 3e8b7d7f0..f6b641d12 100755 --- a/doc/planning_scene_python/src/planning_scene_tutorial.py +++ b/doc/planning_scene_python/src/planning_scene_tutorial.py @@ -19,7 +19,7 @@ def main(): # Setup # ^^^^^ # - # The :moveit_python:`core.planning_scene.PlanningScene` class can be easily setup and + # The :moveit_core_python:`core.planning_scene.PlanningScene` class can be easily setup and # configured using a URDF and # SRDF. This is, however, not the recommended way to instantiate a # PlanningScene. At the time of writing there are not yet python bindings @@ -46,8 +46,8 @@ def main(): # 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 - # :moveit_python:`core.collision_detection.CollisionRequest` object and a - # :moveit_python:`core.collision_detection.CollisionResult` object and pass them + # :moveit_core_python:`core.collision_detection.CollisionRequest` object and a + # :moveit_core_python:`core.collision_detection.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 @@ -121,7 +121,7 @@ def main(): # Modifying the Allowed Collision Matrix # ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ # - # The :moveit_python:`core.collision_detection.AllowedCollisionMatrix` (ACM) + # The :moveit_core_python:`core.collision_detection.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 @@ -163,7 +163,7 @@ def main(): # ^^^^^^^^^^^^^^^^^^^ # # The PlanningScene class also includes easy to use function calls for checking constraints. The constraints can be - # of two types: (a) constraints constructed using :moveit_python:`core.kinematic_constraints`, or (b) user defined + # of two types: (a) constraints constructed using :moveit_core_python:`core.kinematic_constraints`, or (b) user defined # constraints specified through a callback. We will first look at an example with a simple # KinematicConstraint. # From f7dc7a618d6072c224e11fffbe28e0365a9803d8 Mon Sep 17 00:00:00 2001 From: Peter Mitrano Date: Sat, 10 Apr 2021 14:02:17 -0400 Subject: [PATCH 08/18] formatting --- .../planning_scene_tutorial.rst | 2 +- .../src/planning_scene_tutorial.py | 17 +++++++++-------- 2 files changed, 10 insertions(+), 9 deletions(-) diff --git a/doc/planning_scene_python/planning_scene_tutorial.rst b/doc/planning_scene_python/planning_scene_tutorial.rst index 26f515dc1..e91010baf 100644 --- a/doc/planning_scene_python/planning_scene_tutorial.rst +++ b/doc/planning_scene_python/planning_scene_tutorial.rst @@ -45,4 +45,4 @@ joint values so some things may be different. :: [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 `_. \ No newline at end of file +**Note:** Don't worry if your output has different ROS console format. You can customize your ROS console logger by following `this blog post `_. diff --git a/doc/planning_scene_python/src/planning_scene_tutorial.py b/doc/planning_scene_python/src/planning_scene_tutorial.py index f6b641d12..dcb104ac1 100755 --- a/doc/planning_scene_python/src/planning_scene_tutorial.py +++ b/doc/planning_scene_python/src/planning_scene_tutorial.py @@ -29,8 +29,8 @@ def main(): # 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' + 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 = moveit.core.load_robot_model(urdf_path, srdf_path) planning_scene = PlanningScene(kinematic_model, collision_detection.World()) current_state = planning_scene.getCurrentStateNonConst() @@ -115,8 +115,8 @@ def main(): 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) - rospy.loginfo( - f"Test 4: Current state is {'valid' if current_state.satisfiesBounds(joint_model_group) else 'not valid'}") + satisfied_bounds = current_state.satisfiesBounds(joint_model_group) + rospy.loginfo(f"Test 4: Current state is {'valid' if satisfied_bounds else 'not valid'}") # Modifying the Allowed Collision Matrix # ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ @@ -163,8 +163,8 @@ def main(): # ^^^^^^^^^^^^^^^^^^^ # # The PlanningScene class also includes easy to use function calls for checking constraints. The constraints can be - # of two types: (a) constraints constructed using :moveit_core_python:`core.kinematic_constraints`, or (b) user defined - # constraints specified through a callback. We will first look at an example with a simple + # of two types: (a) constraints constructed using :moveit_core_python:`core.kinematic_constraints`, + # or (b) user defined constraints specified through a callback. We will first look at an example with a simple # KinematicConstraint. # # Checking Kinematic Constraints @@ -207,7 +207,8 @@ def main(): constraint_eval_result = kinematic_constraint_set.decide(copied_state) rospy.loginfo( - f"Test 10: Random state is {'constrained' if constraint_eval_result.satisfied else 'not constrained'}") + f"Test 10: Random state is {'constrained' if constraint_eval_result.satisfied else 'not constrained'}" + ) # Whenever isStateValid is called, three checks are conducted: (a) # collision checking (b) constraint checking and (c) feasibility @@ -224,5 +225,5 @@ def main(): moveit_commander.roscpp_shutdown() -if __name__ == '__main__': +if __name__ == "__main__": main() From f6fa9e35554b71cba4b051bed2685f7603a35bd3 Mon Sep 17 00:00:00 2001 From: Peter Mitrano Date: Sat, 12 Jun 2021 22:22:45 -0400 Subject: [PATCH 09/18] Update doc/planning_scene_python/src/planning_scene_tutorial.py Co-authored-by: Joseph Schornak --- doc/planning_scene_python/src/planning_scene_tutorial.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/planning_scene_python/src/planning_scene_tutorial.py b/doc/planning_scene_python/src/planning_scene_tutorial.py index dcb104ac1..c40c17dda 100755 --- a/doc/planning_scene_python/src/planning_scene_tutorial.py +++ b/doc/planning_scene_python/src/planning_scene_tutorial.py @@ -23,7 +23,7 @@ def main(): # configured using a URDF and # SRDF. This is, however, not the recommended way to instantiate a # PlanningScene. At the time of writing there are not yet python bindings - # for the PlanningSceneMonitoir, which is is the recommended method to + # for the PlanningSceneMonitor, which is is the recommended method to # create and maintain the current planning scene # using data from the robot's joints and the sensors on the robot. In # this tutorial, we will instantiate a PlanningScene class directly, From 5e18b69fe6c71469e35c6e208d94ecc12524b327 Mon Sep 17 00:00:00 2001 From: Peter Mitrano Date: Sat, 12 Jun 2021 22:32:23 -0400 Subject: [PATCH 10/18] adressing reviews a bit --- .../src/planning_scene_tutorial.py | 26 +++++++++---------- 1 file changed, 12 insertions(+), 14 deletions(-) diff --git a/doc/planning_scene_python/src/planning_scene_tutorial.py b/doc/planning_scene_python/src/planning_scene_tutorial.py index c40c17dda..cd6901ad5 100755 --- a/doc/planning_scene_python/src/planning_scene_tutorial.py +++ b/doc/planning_scene_python/src/planning_scene_tutorial.py @@ -1,15 +1,6 @@ #!/usr/bin/env python import sys -import moveit_commander -import numpy as np -import rospy -from geometry_msgs.msg import PoseStamped -import moveit.core -from moveit.core import kinematic_constraints, collision_detection, robot_model -from moveit.core.planning_scene import PlanningScene - - def main(): rospy.init_node("planning_scene_tutorial") moveit_commander.roscpp_initialize(sys.argv) @@ -28,10 +19,18 @@ def main(): # 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 = moveit.core.load_robot_model(urdf_path, srdf_path) + import moveit_commander + import numpy as np + import rospy + from geometry_msgs.msg import PoseStamped + import moveit.core + from moveit.core import kinematic_constraints, collision_detection, robot_model + from moveit.core.planning_scene import PlanningScene + + r = rospkg.RosPack() + urdf_path = pathlib.Path(r.get_path("moveit_resources_panda_description")) / "urdf" / "panda.urdf" + srdf_path = pathlib.Path(r.get_path("moveit_resources_panda_moveit_config")) /"config"/"panda.srdf" + kinematic_model = moveit.core.load_robot_model(urdf_path.as_posix(), srdf_path.as_posix()) planning_scene = PlanningScene(kinematic_model, collision_detection.World()) current_state = planning_scene.getCurrentStateNonConst() joint_model_group = current_state.getJointModelGroup("panda_arm") @@ -137,7 +136,6 @@ def main(): 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() From d7b7c321252a34b1c35dd5e3b27f3511b8350cc4 Mon Sep 17 00:00:00 2001 From: Peter Mitrano Date: Mon, 19 Jul 2021 16:09:52 -0400 Subject: [PATCH 11/18] accept rewording suggestion Co-authored-by: John Stechschulte --- doc/planning_scene_python/planning_scene_tutorial.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/planning_scene_python/planning_scene_tutorial.rst b/doc/planning_scene_python/planning_scene_tutorial.rst index e91010baf..079163a2f 100644 --- a/doc/planning_scene_python/planning_scene_tutorial.rst +++ b/doc/planning_scene_python/planning_scene_tutorial.rst @@ -17,7 +17,7 @@ The entire code can be seen :codedir:`here in the MoveIt GitHub project`_ in your catkin workspace and build: :: +If you haven't already, you need to clone `panda_moveit_config `_ in your catkin workspace and build. Then, open two shells and start RViz and wait for everything to finish loading in the first shell: :: roslaunch panda_moveit_config demo.launch From c3a49d33354a0f9e7e93f712652b9fc7ca5d67b7 Mon Sep 17 00:00:00 2001 From: Peter Mitrano Date: Mon, 19 Jul 2021 16:10:04 -0400 Subject: [PATCH 12/18] accept rewording suggestion Co-authored-by: John Stechschulte --- doc/planning_scene_python/planning_scene_tutorial.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/planning_scene_python/planning_scene_tutorial.rst b/doc/planning_scene_python/planning_scene_tutorial.rst index 079163a2f..cb5294228 100644 --- a/doc/planning_scene_python/planning_scene_tutorial.rst +++ b/doc/planning_scene_python/planning_scene_tutorial.rst @@ -21,7 +21,7 @@ If you haven't already, you need to clone `panda_moveit_config Date: Mon, 19 Jul 2021 16:12:34 -0400 Subject: [PATCH 13/18] fix imports --- .../src/planning_scene_tutorial.py | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/doc/planning_scene_python/src/planning_scene_tutorial.py b/doc/planning_scene_python/src/planning_scene_tutorial.py index cd6901ad5..eca6bcad0 100755 --- a/doc/planning_scene_python/src/planning_scene_tutorial.py +++ b/doc/planning_scene_python/src/planning_scene_tutorial.py @@ -1,10 +1,8 @@ #!/usr/bin/env python import sys +import rospy def main(): - rospy.init_node("planning_scene_tutorial") - moveit_commander.roscpp_initialize(sys.argv) - # BEGIN_TUTORIAL # # Setup @@ -19,13 +17,18 @@ def main(): # 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. - import moveit_commander - import numpy as np - import rospy from geometry_msgs.msg import PoseStamped - import moveit.core from moveit.core import kinematic_constraints, collision_detection, robot_model from moveit.core.planning_scene import PlanningScene + import moveit.core + import moveit_commander + import numpy as np + import pathlib + import rospkg + import rospy + + rospy.init_node("planning_scene_tutorial") + moveit_commander.roscpp_initialize(sys.argv) r = rospkg.RosPack() urdf_path = pathlib.Path(r.get_path("moveit_resources_panda_description")) / "urdf" / "panda.urdf" From d8fd9f2f8852e4757fab446d7bc1c04b1f96e3a1 Mon Sep 17 00:00:00 2001 From: Peter Mitrano Date: Mon, 19 Jul 2021 16:32:53 -0400 Subject: [PATCH 14/18] revert wrong changeset --- .../move_group_python_interface_tutorial.py | 919 +++++++++--------- 1 file changed, 468 insertions(+), 451 deletions(-) diff --git a/doc/move_group_python_interface/scripts/move_group_python_interface_tutorial.py b/doc/move_group_python_interface/scripts/move_group_python_interface_tutorial.py index 35aa97687..8926dee7c 100755 --- a/doc/move_group_python_interface/scripts/move_group_python_interface_tutorial.py +++ b/doc/move_group_python_interface/scripts/move_group_python_interface_tutorial.py @@ -51,470 +51,487 @@ import moveit_commander import moveit_msgs.msg import geometry_msgs.msg + try: - from math import pi, tau, dist, fabs, cos -except: # For Python 2 compatibility - from math import pi, fabs, cos, sqrt - tau = 2.0*pi - def dist(p, q): - return sqrt(sum((p_i - q_i)**2.0 for p_i, q_i in zip(p,q))) + from math import pi, tau, dist, fabs, cos +except: # For Python 2 compatibility + from math import pi, fabs, cos, sqrt + + tau = 2.0 * pi + + def dist(p, q): + return sqrt(sum((p_i - q_i) ** 2.0 for p_i, q_i in zip(p, q))) + + from std_msgs.msg import String from moveit_commander.conversions import pose_to_list + ## END_SUB_TUTORIAL def all_close(goal, actual, tolerance): - """ - Convenience method for testing if the values in two lists are within a tolerance of each other. - For Pose and PoseStamped inputs, the angle between the two quaternions is compared (the angle - between the identical orientations q and -q is calculated correctly). - @param: goal A list of floats, a Pose or a PoseStamped - @param: actual A list of floats, a Pose or a PoseStamped - @param: tolerance A float - @returns: bool - """ - if type(goal) is list: - for index in range(len(goal)): - if abs(actual[index] - goal[index]) > tolerance: - return False - - elif type(goal) is geometry_msgs.msg.PoseStamped: - return all_close(goal.pose, actual.pose, tolerance) - - elif type(goal) is geometry_msgs.msg.Pose: - x0, y0, z0, qx0, qy0, qz0, qw0 = pose_to_list(actual) - x1, y1, z1, qx1, qy1, qz1, qw1 = pose_to_list(goal) - # Euclidean distance - d = dist((x1, y1, z1), (x0, y0, z0)) - # phi = angle between orientations - cos_phi_half = fabs(qx0*qx1 + qy0*qy1 + qz0*qz1 + qw0*qw1) - return d <= tolerance and cos_phi_half >= cos(tolerance / 2.0) - - return True + """ + Convenience method for testing if the values in two lists are within a tolerance of each other. + For Pose and PoseStamped inputs, the angle between the two quaternions is compared (the angle + between the identical orientations q and -q is calculated correctly). + @param: goal A list of floats, a Pose or a PoseStamped + @param: actual A list of floats, a Pose or a PoseStamped + @param: tolerance A float + @returns: bool + """ + if type(goal) is list: + for index in range(len(goal)): + if abs(actual[index] - goal[index]) > tolerance: + return False + + elif type(goal) is geometry_msgs.msg.PoseStamped: + return all_close(goal.pose, actual.pose, tolerance) + + elif type(goal) is geometry_msgs.msg.Pose: + x0, y0, z0, qx0, qy0, qz0, qw0 = pose_to_list(actual) + x1, y1, z1, qx1, qy1, qz1, qw1 = pose_to_list(goal) + # Euclidean distance + d = dist((x1, y1, z1), (x0, y0, z0)) + # phi = angle between orientations + cos_phi_half = fabs(qx0 * qx1 + qy0 * qy1 + qz0 * qz1 + qw0 * qw1) + return d <= tolerance and cos_phi_half >= cos(tolerance / 2.0) + + return True class MoveGroupPythonInterfaceTutorial(object): - """MoveGroupPythonInterfaceTutorial""" - def __init__(self): - super(MoveGroupPythonInterfaceTutorial, self).__init__() - - ## BEGIN_SUB_TUTORIAL setup - ## - ## First initialize `moveit_commander`_ and a `rospy`_ node: - moveit_commander.roscpp_initialize(sys.argv) - rospy.init_node('move_group_python_interface_tutorial', anonymous=True) - - ## Instantiate a `RobotCommander`_ object. Provides information such as the robot's - ## kinematic model and the robot's current joint states - robot = moveit_commander.RobotCommander() - - ## Instantiate a `PlanningSceneInterface`_ object. This provides a remote interface - ## for getting, setting, and updating the robot's internal understanding of the - ## surrounding world: - scene = moveit_commander.PlanningSceneInterface() - - ## Instantiate a `MoveGroupCommander`_ object. This object is an interface - ## to a planning group (group of joints). In this tutorial the group is the primary - ## arm joints in the Panda robot, so we set the group's name to "panda_arm". - ## If you are using a different robot, change this value to the name of your robot - ## arm planning group. - ## This interface can be used to plan and execute motions: - group_name = "panda_arm" - move_group = moveit_commander.MoveGroupCommander(group_name) - - ## Create a `DisplayTrajectory`_ ROS publisher which is used to display - ## trajectories in Rviz: - display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', - moveit_msgs.msg.DisplayTrajectory, - queue_size=20) - - ## END_SUB_TUTORIAL - - ## BEGIN_SUB_TUTORIAL basic_info - ## - ## Getting Basic Information - ## ^^^^^^^^^^^^^^^^^^^^^^^^^ - # We can get the name of the reference frame for this robot: - planning_frame = move_group.get_planning_frame() - print("============ Planning frame: %s" % planning_frame) - - # We can also print the name of the end-effector link for this group: - eef_link = move_group.get_end_effector_link() - print("============ End effector link: %s" % eef_link) - - # We can get a list of all the groups in the robot: - group_names = robot.get_group_names() - print("============ Available Planning Groups:", robot.get_group_names()) - - # Sometimes for debugging it is useful to print the entire state of the - # robot: - print("============ Printing robot state") - print(robot.get_current_state()) - print("") - ## END_SUB_TUTORIAL - - # Misc variables - self.box_name = '' - self.robot = robot - self.scene = scene - self.move_group = move_group - self.display_trajectory_publisher = display_trajectory_publisher - self.planning_frame = planning_frame - self.eef_link = eef_link - self.group_names = group_names - - - def go_to_joint_state(self): - # Copy class variables to local variables to make the web tutorials more clear. - # In practice, you should use the class variables directly unless you have a good - # reason not to. - move_group = self.move_group - - ## BEGIN_SUB_TUTORIAL plan_to_joint_state - ## - ## Planning to a Joint Goal - ## ^^^^^^^^^^^^^^^^^^^^^^^^ - ## The Panda's zero configuration is at a `singularity `_, so the first - ## thing we want to do is move it to a slightly better configuration. - ## We use the constant `tau = 2*pi `_ for convenience: - # We get the joint values from the group and change some of the values: - joint_goal = move_group.get_current_joint_values() - joint_goal[0] = 0 - joint_goal[1] = -tau/8 - joint_goal[2] = 0 - joint_goal[3] = -tau/4 - joint_goal[4] = 0 - joint_goal[5] = tau/6 # 1/6 of a turn - joint_goal[6] = 0 - - # The go command can be called with joint values, poses, or without any - # parameters if you have already set the pose or joint target for the group - move_group.go(joint_goal, wait=True) - - # Calling ``stop()`` ensures that there is no residual movement - move_group.stop() - - ## END_SUB_TUTORIAL - - # For testing: - current_joints = move_group.get_current_joint_values() - return all_close(joint_goal, current_joints, 0.01) - - - def go_to_pose_goal(self): - # Copy class variables to local variables to make the web tutorials more clear. - # In practice, you should use the class variables directly unless you have a good - # reason not to. - move_group = self.move_group - - ## BEGIN_SUB_TUTORIAL plan_to_pose - ## - ## Planning to a Pose Goal - ## ^^^^^^^^^^^^^^^^^^^^^^^ - ## We can plan a motion for this group to a desired pose for the - ## end-effector: - pose_goal = geometry_msgs.msg.Pose() - pose_goal.orientation.w = 1.0 - pose_goal.position.x = 0.4 - pose_goal.position.y = 0.1 - pose_goal.position.z = 0.4 - - move_group.set_pose_target(pose_goal) - - ## Now, we call the planner to compute the plan and execute it. - plan = move_group.go(wait=True) - # Calling `stop()` ensures that there is no residual movement - move_group.stop() - # It is always good to clear your targets after planning with poses. - # Note: there is no equivalent function for clear_joint_value_targets() - move_group.clear_pose_targets() - - ## END_SUB_TUTORIAL - - # For testing: - # Note that since this section of code will not be included in the tutorials - # we use the class variable rather than the copied state variable - current_pose = self.move_group.get_current_pose().pose - return all_close(pose_goal, current_pose, 0.01) - - - def plan_cartesian_path(self, scale=1): - # Copy class variables to local variables to make the web tutorials more clear. - # In practice, you should use the class variables directly unless you have a good - # reason not to. - move_group = self.move_group - - ## BEGIN_SUB_TUTORIAL plan_cartesian_path - ## - ## Cartesian Paths - ## ^^^^^^^^^^^^^^^ - ## You can plan a Cartesian path directly by specifying a list of waypoints - ## for the end-effector to go through. If executing interactively in a - ## Python shell, set scale = 1.0. - ## - waypoints = [] - - wpose = move_group.get_current_pose().pose - wpose.position.z -= scale * 0.1 # First move up (z) - wpose.position.y += scale * 0.2 # and sideways (y) - waypoints.append(copy.deepcopy(wpose)) - - wpose.position.x += scale * 0.1 # Second move forward/backwards in (x) - waypoints.append(copy.deepcopy(wpose)) - - wpose.position.y -= scale * 0.1 # Third move sideways (y) - waypoints.append(copy.deepcopy(wpose)) - - # We want the Cartesian path to be interpolated at a resolution of 1 cm - # which is why we will specify 0.01 as the eef_step in Cartesian - # translation. We will disable the jump threshold by setting it to 0.0, - # ignoring the check for infeasible jumps in joint space, which is sufficient - # for this tutorial. - (plan, fraction) = move_group.compute_cartesian_path( - waypoints, # waypoints to follow - 0.01, # eef_step - 0.0) # jump_threshold - - # Note: We are just planning, not asking move_group to actually move the robot yet: - return plan, fraction - - ## END_SUB_TUTORIAL - - - def display_trajectory(self, plan): - # Copy class variables to local variables to make the web tutorials more clear. - # In practice, you should use the class variables directly unless you have a good - # reason not to. - robot = self.robot - display_trajectory_publisher = self.display_trajectory_publisher - - ## BEGIN_SUB_TUTORIAL display_trajectory - ## - ## Displaying a Trajectory - ## ^^^^^^^^^^^^^^^^^^^^^^^ - ## You can ask RViz to visualize a plan (aka trajectory) for you. But the - ## group.plan() method does this automatically so this is not that useful - ## here (it just displays the same trajectory again): - ## - ## A `DisplayTrajectory`_ msg has two primary fields, trajectory_start and trajectory. - ## We populate the trajectory_start with our current robot state to copy over - ## any AttachedCollisionObjects and add our plan to the trajectory. - display_trajectory = moveit_msgs.msg.DisplayTrajectory() - display_trajectory.trajectory_start = robot.get_current_state() - display_trajectory.trajectory.append(plan) - # Publish - display_trajectory_publisher.publish(display_trajectory); - - ## END_SUB_TUTORIAL - - - def execute_plan(self, plan): - # Copy class variables to local variables to make the web tutorials more clear. - # In practice, you should use the class variables directly unless you have a good - # reason not to. - move_group = self.move_group - - ## BEGIN_SUB_TUTORIAL execute_plan - ## - ## Executing a Plan - ## ^^^^^^^^^^^^^^^^ - ## Use execute if you would like the robot to follow - ## the plan that has already been computed: - move_group.execute(plan, wait=True) - - ## **Note:** The robot's current joint state must be within some tolerance of the - ## first waypoint in the `RobotTrajectory`_ or ``execute()`` will fail - ## END_SUB_TUTORIAL - - - def wait_for_state_update(self, box_is_known=False, box_is_attached=False, timeout=4): - # Copy class variables to local variables to make the web tutorials more clear. - # In practice, you should use the class variables directly unless you have a good - # reason not to. - box_name = self.box_name - scene = self.scene - - ## BEGIN_SUB_TUTORIAL wait_for_scene_update - ## - ## Ensuring Collision Updates Are Received - ## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - ## If the Python node dies before publishing a collision object update message, the message - ## could get lost and the box will not appear. To ensure that the updates are - ## made, we wait until we see the changes reflected in the - ## ``get_attached_objects()`` and ``get_known_object_names()`` lists. - ## For the purpose of this tutorial, we call this function after adding, - ## removing, attaching or detaching an object in the planning scene. We then wait - ## until the updates have been made or ``timeout`` seconds have passed - start = rospy.get_time() - seconds = rospy.get_time() - while (seconds - start < timeout) and not rospy.is_shutdown(): - # Test if the box is in attached objects - attached_objects = scene.get_attached_objects([box_name]) - is_attached = len(attached_objects.keys()) > 0 - - # Test if the box is in the scene. - # Note that attaching the box will remove it from known_objects - is_known = box_name in scene.get_known_object_names() - - # Test if we are in the expected state - if (box_is_attached == is_attached) and (box_is_known == is_known): - return True - - # Sleep so that we give other threads time on the processor - rospy.sleep(0.1) - seconds = rospy.get_time() - - # If we exited the while loop without returning then we timed out - return False - ## END_SUB_TUTORIAL - - - def add_box(self, timeout=4): - # Copy class variables to local variables to make the web tutorials more clear. - # In practice, you should use the class variables directly unless you have a good - # reason not to. - box_name = self.box_name - scene = self.scene - - ## BEGIN_SUB_TUTORIAL add_box - ## - ## Adding Objects to the Planning Scene - ## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - ## First, we will create a box in the planning scene between the fingers: - box_pose = geometry_msgs.msg.PoseStamped() - box_pose.header.frame_id = "panda_hand" - box_pose.pose.orientation.w = 1.0 - box_pose.pose.position.z = 0.11 # above the panda_hand frame - box_name = "box" - scene.add_box(box_name, box_pose, size=(0.075, 0.075, 0.075)) - - ## END_SUB_TUTORIAL - # Copy local variables back to class variables. In practice, you should use the class - # variables directly unless you have a good reason not to. - self.box_name=box_name - return self.wait_for_state_update(box_is_known=True, timeout=timeout) - - - def attach_box(self, timeout=4): - # Copy class variables to local variables to make the web tutorials more clear. - # In practice, you should use the class variables directly unless you have a good - # reason not to. - box_name = self.box_name - robot = self.robot - scene = self.scene - eef_link = self.eef_link - group_names = self.group_names - - ## BEGIN_SUB_TUTORIAL attach_object - ## - ## Attaching Objects to the Robot - ## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - ## Next, we will attach the box to the Panda wrist. Manipulating objects requires the - ## robot be able to touch them without the planning scene reporting the contact as a - ## collision. By adding link names to the ``touch_links`` array, we are telling the - ## planning scene to ignore collisions between those links and the box. For the Panda - ## robot, we set ``grasping_group = 'hand'``. If you are using a different robot, - ## you should change this value to the name of your end effector group name. - grasping_group = 'hand' - touch_links = robot.get_link_names(group=grasping_group) - scene.attach_box(eef_link, box_name, touch_links=touch_links) - ## END_SUB_TUTORIAL - - # We wait for the planning scene to update. - return self.wait_for_state_update(box_is_attached=True, box_is_known=False, timeout=timeout) - - - def detach_box(self, timeout=4): - # Copy class variables to local variables to make the web tutorials more clear. - # In practice, you should use the class variables directly unless you have a good - # reason not to. - box_name = self.box_name - scene = self.scene - eef_link = self.eef_link - - ## BEGIN_SUB_TUTORIAL detach_object - ## - ## Detaching Objects from the Robot - ## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - ## We can also detach and remove the object from the planning scene: - scene.remove_attached_object(eef_link, name=box_name) - ## END_SUB_TUTORIAL - - # We wait for the planning scene to update. - return self.wait_for_state_update(box_is_known=True, box_is_attached=False, timeout=timeout) - - - def remove_box(self, timeout=4): - # Copy class variables to local variables to make the web tutorials more clear. - # In practice, you should use the class variables directly unless you have a good - # reason not to. - box_name = self.box_name - scene = self.scene - - ## BEGIN_SUB_TUTORIAL remove_object - ## - ## Removing Objects from the Planning Scene - ## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - ## We can remove the box from the world. - scene.remove_world_object(box_name) - - ## **Note:** The object must be detached before we can remove it from the world - ## END_SUB_TUTORIAL - - # We wait for the planning scene to update. - return self.wait_for_state_update(box_is_attached=False, box_is_known=False, timeout=timeout) + """MoveGroupPythonInterfaceTutorial""" + + def __init__(self): + super(MoveGroupPythonInterfaceTutorial, self).__init__() + + ## BEGIN_SUB_TUTORIAL setup + ## + ## First initialize `moveit_commander`_ and a `rospy`_ node: + moveit_commander.roscpp_initialize(sys.argv) + rospy.init_node("move_group_python_interface_tutorial", anonymous=True) + + ## Instantiate a `RobotCommander`_ object. Provides information such as the robot's + ## kinematic model and the robot's current joint states + robot = moveit_commander.RobotCommander() + + ## Instantiate a `PlanningSceneInterface`_ object. This provides a remote interface + ## for getting, setting, and updating the robot's internal understanding of the + ## surrounding world: + scene = moveit_commander.PlanningSceneInterface() + + ## Instantiate a `MoveGroupCommander`_ object. This object is an interface + ## to a planning group (group of joints). In this tutorial the group is the primary + ## arm joints in the Panda robot, so we set the group's name to "panda_arm". + ## If you are using a different robot, change this value to the name of your robot + ## arm planning group. + ## This interface can be used to plan and execute motions: + group_name = "panda_arm" + move_group = moveit_commander.MoveGroupCommander(group_name) + + ## Create a `DisplayTrajectory`_ ROS publisher which is used to display + ## trajectories in Rviz: + display_trajectory_publisher = rospy.Publisher( + "/move_group/display_planned_path", + moveit_msgs.msg.DisplayTrajectory, + queue_size=20, + ) + + ## END_SUB_TUTORIAL + + ## BEGIN_SUB_TUTORIAL basic_info + ## + ## Getting Basic Information + ## ^^^^^^^^^^^^^^^^^^^^^^^^^ + # We can get the name of the reference frame for this robot: + planning_frame = move_group.get_planning_frame() + print("============ Planning frame: %s" % planning_frame) + + # We can also print the name of the end-effector link for this group: + eef_link = move_group.get_end_effector_link() + print("============ End effector link: %s" % eef_link) + + # We can get a list of all the groups in the robot: + group_names = robot.get_group_names() + print("============ Available Planning Groups:", robot.get_group_names()) + + # Sometimes for debugging it is useful to print the entire state of the + # robot: + print("============ Printing robot state") + print(robot.get_current_state()) + print("") + ## END_SUB_TUTORIAL + + # Misc variables + self.box_name = "" + self.robot = robot + self.scene = scene + self.move_group = move_group + self.display_trajectory_publisher = display_trajectory_publisher + self.planning_frame = planning_frame + self.eef_link = eef_link + self.group_names = group_names + + def go_to_joint_state(self): + # Copy class variables to local variables to make the web tutorials more clear. + # In practice, you should use the class variables directly unless you have a good + # reason not to. + move_group = self.move_group + + ## BEGIN_SUB_TUTORIAL plan_to_joint_state + ## + ## Planning to a Joint Goal + ## ^^^^^^^^^^^^^^^^^^^^^^^^ + ## The Panda's zero configuration is at a `singularity `_, so the first + ## thing we want to do is move it to a slightly better configuration. + ## We use the constant `tau = 2*pi `_ for convenience: + # We get the joint values from the group and change some of the values: + joint_goal = move_group.get_current_joint_values() + joint_goal[0] = 0 + joint_goal[1] = -tau / 8 + joint_goal[2] = 0 + joint_goal[3] = -tau / 4 + joint_goal[4] = 0 + joint_goal[5] = tau / 6 # 1/6 of a turn + joint_goal[6] = 0 + + # The go command can be called with joint values, poses, or without any + # parameters if you have already set the pose or joint target for the group + move_group.go(joint_goal, wait=True) + + # Calling ``stop()`` ensures that there is no residual movement + move_group.stop() + + ## END_SUB_TUTORIAL + + # For testing: + current_joints = move_group.get_current_joint_values() + return all_close(joint_goal, current_joints, 0.01) + + def go_to_pose_goal(self): + # Copy class variables to local variables to make the web tutorials more clear. + # In practice, you should use the class variables directly unless you have a good + # reason not to. + move_group = self.move_group + + ## BEGIN_SUB_TUTORIAL plan_to_pose + ## + ## Planning to a Pose Goal + ## ^^^^^^^^^^^^^^^^^^^^^^^ + ## We can plan a motion for this group to a desired pose for the + ## end-effector: + pose_goal = geometry_msgs.msg.Pose() + pose_goal.orientation.w = 1.0 + pose_goal.position.x = 0.4 + pose_goal.position.y = 0.1 + pose_goal.position.z = 0.4 + + move_group.set_pose_target(pose_goal) + + ## Now, we call the planner to compute the plan and execute it. + plan = move_group.go(wait=True) + # Calling `stop()` ensures that there is no residual movement + move_group.stop() + # It is always good to clear your targets after planning with poses. + # Note: there is no equivalent function for clear_joint_value_targets() + move_group.clear_pose_targets() + + ## END_SUB_TUTORIAL + + # For testing: + # Note that since this section of code will not be included in the tutorials + # we use the class variable rather than the copied state variable + current_pose = self.move_group.get_current_pose().pose + return all_close(pose_goal, current_pose, 0.01) + + def plan_cartesian_path(self, scale=1): + # Copy class variables to local variables to make the web tutorials more clear. + # In practice, you should use the class variables directly unless you have a good + # reason not to. + move_group = self.move_group + + ## BEGIN_SUB_TUTORIAL plan_cartesian_path + ## + ## Cartesian Paths + ## ^^^^^^^^^^^^^^^ + ## You can plan a Cartesian path directly by specifying a list of waypoints + ## for the end-effector to go through. If executing interactively in a + ## Python shell, set scale = 1.0. + ## + waypoints = [] + + wpose = move_group.get_current_pose().pose + wpose.position.z -= scale * 0.1 # First move up (z) + wpose.position.y += scale * 0.2 # and sideways (y) + waypoints.append(copy.deepcopy(wpose)) + + wpose.position.x += scale * 0.1 # Second move forward/backwards in (x) + waypoints.append(copy.deepcopy(wpose)) + + wpose.position.y -= scale * 0.1 # Third move sideways (y) + waypoints.append(copy.deepcopy(wpose)) + + # We want the Cartesian path to be interpolated at a resolution of 1 cm + # which is why we will specify 0.01 as the eef_step in Cartesian + # translation. We will disable the jump threshold by setting it to 0.0, + # ignoring the check for infeasible jumps in joint space, which is sufficient + # for this tutorial. + (plan, fraction) = move_group.compute_cartesian_path( + waypoints, 0.01, 0.0 # waypoints to follow # eef_step + ) # jump_threshold + + # Note: We are just planning, not asking move_group to actually move the robot yet: + return plan, fraction + + ## END_SUB_TUTORIAL + + def display_trajectory(self, plan): + # Copy class variables to local variables to make the web tutorials more clear. + # In practice, you should use the class variables directly unless you have a good + # reason not to. + robot = self.robot + display_trajectory_publisher = self.display_trajectory_publisher + + ## BEGIN_SUB_TUTORIAL display_trajectory + ## + ## Displaying a Trajectory + ## ^^^^^^^^^^^^^^^^^^^^^^^ + ## You can ask RViz to visualize a plan (aka trajectory) for you. But the + ## group.plan() method does this automatically so this is not that useful + ## here (it just displays the same trajectory again): + ## + ## A `DisplayTrajectory`_ msg has two primary fields, trajectory_start and trajectory. + ## We populate the trajectory_start with our current robot state to copy over + ## any AttachedCollisionObjects and add our plan to the trajectory. + display_trajectory = moveit_msgs.msg.DisplayTrajectory() + display_trajectory.trajectory_start = robot.get_current_state() + display_trajectory.trajectory.append(plan) + # Publish + display_trajectory_publisher.publish(display_trajectory) + + ## END_SUB_TUTORIAL + + def execute_plan(self, plan): + # Copy class variables to local variables to make the web tutorials more clear. + # In practice, you should use the class variables directly unless you have a good + # reason not to. + move_group = self.move_group + + ## BEGIN_SUB_TUTORIAL execute_plan + ## + ## Executing a Plan + ## ^^^^^^^^^^^^^^^^ + ## Use execute if you would like the robot to follow + ## the plan that has already been computed: + move_group.execute(plan, wait=True) + + ## **Note:** The robot's current joint state must be within some tolerance of the + ## first waypoint in the `RobotTrajectory`_ or ``execute()`` will fail + ## END_SUB_TUTORIAL + + def wait_for_state_update( + self, box_is_known=False, box_is_attached=False, timeout=4 + ): + # Copy class variables to local variables to make the web tutorials more clear. + # In practice, you should use the class variables directly unless you have a good + # reason not to. + box_name = self.box_name + scene = self.scene + + ## BEGIN_SUB_TUTORIAL wait_for_scene_update + ## + ## Ensuring Collision Updates Are Received + ## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + ## If the Python node dies before publishing a collision object update message, the message + ## could get lost and the box will not appear. To ensure that the updates are + ## made, we wait until we see the changes reflected in the + ## ``get_attached_objects()`` and ``get_known_object_names()`` lists. + ## For the purpose of this tutorial, we call this function after adding, + ## removing, attaching or detaching an object in the planning scene. We then wait + ## until the updates have been made or ``timeout`` seconds have passed + start = rospy.get_time() + seconds = rospy.get_time() + while (seconds - start < timeout) and not rospy.is_shutdown(): + # Test if the box is in attached objects + attached_objects = scene.get_attached_objects([box_name]) + is_attached = len(attached_objects.keys()) > 0 + + # Test if the box is in the scene. + # Note that attaching the box will remove it from known_objects + is_known = box_name in scene.get_known_object_names() + + # Test if we are in the expected state + if (box_is_attached == is_attached) and (box_is_known == is_known): + return True + + # Sleep so that we give other threads time on the processor + rospy.sleep(0.1) + seconds = rospy.get_time() + + # If we exited the while loop without returning then we timed out + return False + ## END_SUB_TUTORIAL + + def add_box(self, timeout=4): + # Copy class variables to local variables to make the web tutorials more clear. + # In practice, you should use the class variables directly unless you have a good + # reason not to. + box_name = self.box_name + scene = self.scene + + ## BEGIN_SUB_TUTORIAL add_box + ## + ## Adding Objects to the Planning Scene + ## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + ## First, we will create a box in the planning scene between the fingers: + box_pose = geometry_msgs.msg.PoseStamped() + box_pose.header.frame_id = "panda_hand" + box_pose.pose.orientation.w = 1.0 + box_pose.pose.position.z = 0.11 # above the panda_hand frame + box_name = "box" + scene.add_box(box_name, box_pose, size=(0.075, 0.075, 0.075)) + + ## END_SUB_TUTORIAL + # Copy local variables back to class variables. In practice, you should use the class + # variables directly unless you have a good reason not to. + self.box_name = box_name + return self.wait_for_state_update(box_is_known=True, timeout=timeout) + + def attach_box(self, timeout=4): + # Copy class variables to local variables to make the web tutorials more clear. + # In practice, you should use the class variables directly unless you have a good + # reason not to. + box_name = self.box_name + robot = self.robot + scene = self.scene + eef_link = self.eef_link + group_names = self.group_names + + ## BEGIN_SUB_TUTORIAL attach_object + ## + ## Attaching Objects to the Robot + ## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + ## Next, we will attach the box to the Panda wrist. Manipulating objects requires the + ## robot be able to touch them without the planning scene reporting the contact as a + ## collision. By adding link names to the ``touch_links`` array, we are telling the + ## planning scene to ignore collisions between those links and the box. For the Panda + ## robot, we set ``grasping_group = 'hand'``. If you are using a different robot, + ## you should change this value to the name of your end effector group name. + grasping_group = "hand" + touch_links = robot.get_link_names(group=grasping_group) + scene.attach_box(eef_link, box_name, touch_links=touch_links) + ## END_SUB_TUTORIAL + + # We wait for the planning scene to update. + return self.wait_for_state_update( + box_is_attached=True, box_is_known=False, timeout=timeout + ) + + def detach_box(self, timeout=4): + # Copy class variables to local variables to make the web tutorials more clear. + # In practice, you should use the class variables directly unless you have a good + # reason not to. + box_name = self.box_name + scene = self.scene + eef_link = self.eef_link + + ## BEGIN_SUB_TUTORIAL detach_object + ## + ## Detaching Objects from the Robot + ## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + ## We can also detach and remove the object from the planning scene: + scene.remove_attached_object(eef_link, name=box_name) + ## END_SUB_TUTORIAL + + # We wait for the planning scene to update. + return self.wait_for_state_update( + box_is_known=True, box_is_attached=False, timeout=timeout + ) + + def remove_box(self, timeout=4): + # Copy class variables to local variables to make the web tutorials more clear. + # In practice, you should use the class variables directly unless you have a good + # reason not to. + box_name = self.box_name + scene = self.scene + + ## BEGIN_SUB_TUTORIAL remove_object + ## + ## Removing Objects from the Planning Scene + ## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + ## We can remove the box from the world. + scene.remove_world_object(box_name) + + ## **Note:** The object must be detached before we can remove it from the world + ## END_SUB_TUTORIAL + + # We wait for the planning scene to update. + return self.wait_for_state_update( + box_is_attached=False, box_is_known=False, timeout=timeout + ) def main(): - try: - print("") - print("----------------------------------------------------------") - print("Welcome to the MoveIt MoveGroup Python Interface Tutorial") - print("----------------------------------------------------------") - print("Press Ctrl-D to exit at any time") - print("") - input("============ Press `Enter` to begin the tutorial by setting up the moveit_commander ...") - tutorial = MoveGroupPythonInterfaceTutorial() - - input("============ Press `Enter` to execute a movement using a joint state goal ...") - tutorial.go_to_joint_state() - - input("============ Press `Enter` to execute a movement using a pose goal ...") - tutorial.go_to_pose_goal() - - input("============ Press `Enter` to plan and display a Cartesian path ...") - cartesian_plan, fraction = tutorial.plan_cartesian_path() - - input("============ Press `Enter` to display a saved trajectory (this will replay the Cartesian path) ...") - tutorial.display_trajectory(cartesian_plan) - - input("============ Press `Enter` to execute a saved path ...") - tutorial.execute_plan(cartesian_plan) - - input("============ Press `Enter` to add a box to the planning scene ...") - tutorial.add_box() - - input("============ Press `Enter` to attach a Box to the Panda robot ...") - tutorial.attach_box() - - input("============ Press `Enter` to plan and execute a path with an attached collision object ...") - cartesian_plan, fraction = tutorial.plan_cartesian_path(scale=-1) - tutorial.execute_plan(cartesian_plan) - - input("============ Press `Enter` to detach the box from the Panda robot ...") - tutorial.detach_box() - - input("============ Press `Enter` to remove the box from the planning scene ...") - tutorial.remove_box() - - print("============ Python tutorial demo complete!") - except rospy.ROSInterruptException: - return - except KeyboardInterrupt: - return - -if __name__ == '__main__': - main() + try: + print("") + print("----------------------------------------------------------") + print("Welcome to the MoveIt MoveGroup Python Interface Tutorial") + print("----------------------------------------------------------") + print("Press Ctrl-D to exit at any time") + print("") + input( + "============ Press `Enter` to begin the tutorial by setting up the moveit_commander ..." + ) + tutorial = MoveGroupPythonInterfaceTutorial() + + input( + "============ Press `Enter` to execute a movement using a joint state goal ..." + ) + tutorial.go_to_joint_state() + + input("============ Press `Enter` to execute a movement using a pose goal ...") + tutorial.go_to_pose_goal() + + input("============ Press `Enter` to plan and display a Cartesian path ...") + cartesian_plan, fraction = tutorial.plan_cartesian_path() + + input( + "============ Press `Enter` to display a saved trajectory (this will replay the Cartesian path) ..." + ) + tutorial.display_trajectory(cartesian_plan) + + input("============ Press `Enter` to execute a saved path ...") + tutorial.execute_plan(cartesian_plan) + + input("============ Press `Enter` to add a box to the planning scene ...") + tutorial.add_box() + + input("============ Press `Enter` to attach a Box to the Panda robot ...") + tutorial.attach_box() + + input( + "============ Press `Enter` to plan and execute a path with an attached collision object ..." + ) + cartesian_plan, fraction = tutorial.plan_cartesian_path(scale=-1) + tutorial.execute_plan(cartesian_plan) + + input("============ Press `Enter` to detach the box from the Panda robot ...") + tutorial.detach_box() + + input( + "============ Press `Enter` to remove the box from the planning scene ..." + ) + tutorial.remove_box() + + print("============ Python tutorial demo complete!") + except rospy.ROSInterruptException: + return + except KeyboardInterrupt: + return + + +if __name__ == "__main__": + main() ## BEGIN_TUTORIAL ## .. _moveit_commander: From de9a59300efaec23893aab57addf0a5271286776 Mon Sep 17 00:00:00 2001 From: Peter Mitrano Date: Mon, 19 Jul 2021 16:33:32 -0400 Subject: [PATCH 15/18] Update doc/planning_scene_python/src/planning_scene_tutorial.py Co-authored-by: John Stechschulte --- doc/planning_scene_python/src/planning_scene_tutorial.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/doc/planning_scene_python/src/planning_scene_tutorial.py b/doc/planning_scene_python/src/planning_scene_tutorial.py index eca6bcad0..eed176dde 100755 --- a/doc/planning_scene_python/src/planning_scene_tutorial.py +++ b/doc/planning_scene_python/src/planning_scene_tutorial.py @@ -50,9 +50,9 @@ def main(): # hitting each other. To do this, we will construct a # :moveit_core_python:`core.collision_detection.CollisionRequest` object and a # :moveit_core_python:`core.collision_detection.CollisionResult` object and pass them - # into the collision checking function. Note that the result of + # into the collision checking function. Note that # whether the robot is in self-collision or not is contained within - # the result. Self collision checking uses an *unpadded* version of + # the `CollisionResult` object. 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. From 268435b413e0145b7284816bb13d9ba7d380d6f4 Mon Sep 17 00:00:00 2001 From: Peter Mitrano Date: Mon, 19 Jul 2021 16:33:46 -0400 Subject: [PATCH 16/18] Update doc/planning_scene_python/src/planning_scene_tutorial.py Co-authored-by: John Stechschulte --- doc/planning_scene_python/src/planning_scene_tutorial.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/planning_scene_python/src/planning_scene_tutorial.py b/doc/planning_scene_python/src/planning_scene_tutorial.py index eed176dde..cea5e347a 100755 --- a/doc/planning_scene_python/src/planning_scene_tutorial.py +++ b/doc/planning_scene_python/src/planning_scene_tutorial.py @@ -125,7 +125,7 @@ def main(): # # The :moveit_core_python:`core.collision_detection.AllowedCollisionMatrix` (ACM) # provides a mechanism to tell the collision world to ignore - # collisions between certain object: both parts of the robot and + # collisions between certain objects: 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 From edb3143025049ac8a9d0f65e7dfe54c77bf1acbd Mon Sep 17 00:00:00 2001 From: Peter Mitrano Date: Mon, 19 Jul 2021 17:43:59 -0400 Subject: [PATCH 17/18] fix formatting --- .../src/planning_scene_tutorial.py | 73 ++++++++++++++----- 1 file changed, 56 insertions(+), 17 deletions(-) diff --git a/doc/planning_scene_python/src/planning_scene_tutorial.py b/doc/planning_scene_python/src/planning_scene_tutorial.py index cea5e347a..4bf375819 100755 --- a/doc/planning_scene_python/src/planning_scene_tutorial.py +++ b/doc/planning_scene_python/src/planning_scene_tutorial.py @@ -2,6 +2,7 @@ import sys import rospy + def main(): # BEGIN_TUTORIAL # @@ -31,9 +32,19 @@ def main(): moveit_commander.roscpp_initialize(sys.argv) r = rospkg.RosPack() - urdf_path = pathlib.Path(r.get_path("moveit_resources_panda_description")) / "urdf" / "panda.urdf" - srdf_path = pathlib.Path(r.get_path("moveit_resources_panda_moveit_config")) /"config"/"panda.srdf" - kinematic_model = moveit.core.load_robot_model(urdf_path.as_posix(), srdf_path.as_posix()) + urdf_path = ( + pathlib.Path(r.get_path("moveit_resources_panda_description")) + / "urdf" + / "panda.urdf" + ) + srdf_path = ( + pathlib.Path(r.get_path("moveit_resources_panda_moveit_config")) + / "config" + / "panda.srdf" + ) + kinematic_model = moveit.core.load_robot_model( + urdf_path.as_posix(), srdf_path.as_posix() + ) planning_scene = PlanningScene(kinematic_model, collision_detection.World()) current_state = planning_scene.getCurrentStateNonConst() joint_model_group = current_state.getJointModelGroup("panda_arm") @@ -59,7 +70,9 @@ def main(): collision_request = collision_detection.CollisionRequest() collision_result = collision_detection.CollisionResult() 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") + 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 @@ -89,7 +102,9 @@ def main(): collision_result.clear() planning_scene.checkSelfCollision(collision_request, collision_result) - rospy.loginfo(f"Test 2: Current state is {'in' if collision_result.collision else 'not in'} self collision") + rospy.loginfo( + f"Test 2: Current state is {'in' if collision_result.collision else 'not in'} self collision" + ) # Checking for a group # ~~~~~~~~~~~~~~~~~~~~ @@ -104,7 +119,9 @@ def main(): current_state.setToRandomPositions(joint_model_group) collision_result.clear() planning_scene.checkSelfCollision(collision_request, collision_result) - rospy.loginfo(f"Test 3: Current state is {'in' if collision_result.collision else 'not in'} self collision") + rospy.loginfo( + f"Test 3: Current state is {'in' if collision_result.collision else 'not in'} self collision" + ) # Getting Contact Information # ~~~~~~~~~~~~~~~~~~~~~~~~~~~ @@ -118,7 +135,9 @@ def main(): joint_model_group = current_state.getJointModelGroup("panda_arm") current_state.setJointGroupPositions(joint_model_group, joint_values) satisfied_bounds = current_state.satisfiesBounds(joint_model_group) - rospy.loginfo(f"Test 4: Current state is {'valid' if satisfied_bounds else 'not valid'}") + rospy.loginfo( + f"Test 4: Current state is {'valid' if satisfied_bounds else 'not valid'}" + ) # Modifying the Allowed Collision Matrix # ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ @@ -142,8 +161,12 @@ def main(): 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) - rospy.loginfo(f"Test 6: Current state is {'in' if collision_result.collision else 'not in'} self collision") + planning_scene.checkSelfCollision( + collision_request, collision_result, copied_state, acm + ) + rospy.loginfo( + f"Test 6: Current state is {'in' if collision_result.collision else 'not in'} self collision" + ) # Full Collision Checking # ~~~~~~~~~~~~~~~~~~~~~~~ @@ -157,8 +180,12 @@ def main(): # 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) - rospy.loginfo(f"Test 7: Current state is {'in' if collision_result.collision else 'not in'} self collision") + planning_scene.checkCollision( + collision_request, collision_result, copied_state, acm + ) + rospy.loginfo( + f"Test 7: Current state is {'in' if collision_result.collision else 'not in'} self collision" + ) # Constraint Checking # ^^^^^^^^^^^^^^^^^^^ @@ -183,14 +210,18 @@ def main(): 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) + 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) copied_state.update() constrained = planning_scene.isStateConstrained(copied_state, goal_constraint) - rospy.loginfo(f"Test 8: Random state is {'constrained' if constrained else 'not constrained'}") + rospy.loginfo( + 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 @@ -198,10 +229,16 @@ def main(): # pre-processes the ROS Constraints messages and sets it up for quick # processing. - kinematic_constraint_set = kinematic_constraints.KinematicConstraintSet(kinematic_model) + 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) - rospy.loginfo(f"Test 9: Random state is {'constrained' if constrained_2 else 'not constrained'}") + constrained_2 = planning_scene.isStateConstrained( + copied_state, kinematic_constraint_set + ) + rospy.loginfo( + 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. @@ -215,7 +252,9 @@ def main(): # 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") + state_valid = planning_scene.isStateValid( + copied_state, kinematic_constraint_set, "panda_arm" + ) rospy.loginfo(f"Test 12: Random state is {'valid' if state_valid else 'not valid'}") # Note that all the planners available through MoveIt and OMPL will From 3f142f059033573fd8c9b9040f415630db4d5034 Mon Sep 17 00:00:00 2001 From: Peter Mitrano Date: Mon, 19 Jul 2021 17:44:17 -0400 Subject: [PATCH 18/18] fix formmatting --- doc/planning_scene_python/src/planning_scene_tutorial.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/planning_scene_python/src/planning_scene_tutorial.py b/doc/planning_scene_python/src/planning_scene_tutorial.py index 4bf375819..047bcce4e 100755 --- a/doc/planning_scene_python/src/planning_scene_tutorial.py +++ b/doc/planning_scene_python/src/planning_scene_tutorial.py @@ -61,7 +61,7 @@ def main(): # hitting each other. To do this, we will construct a # :moveit_core_python:`core.collision_detection.CollisionRequest` object and a # :moveit_core_python:`core.collision_detection.CollisionResult` object and pass them - # into the collision checking function. Note that + # into the collision checking function. Note that # whether the robot is in self-collision or not is contained within # the `CollisionResult` object. Self collision checking uses an *unpadded* version of # the robot, i.e. it directly uses the collision meshes provided in