From c1c3ea235e693e26f9ca7d932b55bd5fd6c1fbd0 Mon Sep 17 00:00:00 2001 From: Raghavender Sahdev Date: Fri, 25 May 2018 11:02:24 +1000 Subject: [PATCH 01/16] added tests for running CHOMP in scenes with obstacles into the CHOMP tutorial --- .../chomp_interface_tutorial.rst | 24 ++++++-- doc/chomp_interface/collision_scene_test_1.py | 60 +++++++++++++++++++ doc/chomp_interface/collision_scene_test_2.py | 45 ++++++++++++++ 3 files changed, 124 insertions(+), 5 deletions(-) create mode 100644 doc/chomp_interface/collision_scene_test_1.py create mode 100644 doc/chomp_interface/collision_scene_test_2.py diff --git a/doc/chomp_interface/chomp_interface_tutorial.rst b/doc/chomp_interface/chomp_interface_tutorial.rst index e0efbd17a..90ef5b3e1 100644 --- a/doc/chomp_interface/chomp_interface_tutorial.rst +++ b/doc/chomp_interface/chomp_interface_tutorial.rst @@ -1,10 +1,10 @@ -CHOMP Interface +CHOMP Planner =============== .. image:: chomp.png :width: 700px -Covariant Hamiltonian optimization for motion planning (CHOMP) is a novel gradient-based trajectory optimization procedure that makes many everyday motion planning problems both simple and trainable (Ratliff et al., 2009c). While most high-dimensional motion planners separate trajectory generation into distinct planning and optimization stages, this algorithm capitalizes on covariant gradient and functional gradient approaches to the optimization stage to design a motion planning algorithm based entirely on trajectory optimization. Given an infeasible naive trajectory, CHOMP reacts to the surrounding environment to quickly pull the trajectory out of collision while simultaneously optimizing dynamical quantities such as joint velocities and accelerations. It rapidly converges to a smooth collision-free trajectory that can be executed efficiently on the robot. Integration into latest version of MoveIt! is work in progress. `More info `_ +Covariant Hamiltonian optimization for motion planning (CHOMP) is a novel gradient-based trajectory optimization procedure that makes many everyday motion planning problems both simple and trainable (Ratliff et al., 2009c). While most high-dimensional motion planners separate trajectory generation into distinct planning and optimization stages, this algorithm capitalizes on covariant gradient and functional gradient approaches to the optimization stage to design a motion planning algorithm based entirely on trajectory optimization. Given an infeasible naive trajectory, CHOMP reacts to the surrounding environment to quickly pull the trajectory out of collision while simultaneously optimizing dynamical quantities such as joint velocities and accelerations. It rapidly converges to a smooth collision-free trajectory that can be executed efficiently on the robot. Integration into latest version of MoveIt! is `work in progress `_. `More info `_ Getting Started --------------- @@ -14,7 +14,7 @@ You should also have gone through the steps in `Visualization with MoveIt! RViz Prerequisites -------------- - 1. You must have the latest version of MoveIt! installed. On ROS Kinetic you will need to build MoveIt! from source. We will go through the steps for doing this below. + 1. You must have the latest version of MoveIt! installed. On ROS Kinetic you will need to build MoveIt! from source. A build from source is required as CHOMP is not officially released so ``apt-get install`` for moveIt would not be appropriate here. We will go through the steps for doing this below. 2. To use CHOMP with your robot you must already have a MoveIt! configuration package for your robot already. For example, if you have a Panda robot, it's probably called ``panda_moveit_config``. This is typically built using the `MoveIt! Setup Assistant <../setup_assistant/setup_assistant_tutorial.html>`_. Installing MoveIt! from Source @@ -49,10 +49,24 @@ Using CHOMP with Your Robot -#. Open the ``move_group.launch`` file in your ``/launch/`` folder and make two changes. First, add ```` just under the ```` tag and second, within the ```` tag replace ```` with ````. +#. Open the ``move_group.launch`` file in your ``/launch/`` folder and make two changes. + + 7.1. First, add ```` just under the ```` tag, and, + + 7.2. Second, within the ```` tag replace ```` with ````. Running the Demo ---------------- If you have the ``panda_moveit_config`` from the `ros-planning/panda_moveit_config `_ repository you should be able to simply run the demo: :: - roslaunch panda_moveit_config demo_chomp.launch \ No newline at end of file + roslaunch panda_moveit_config demo_chomp.launch + +Testing CHOMP with Obstacles in the Scene +----------------------------------------- +To test CHOMP in an evironment with obstacles, you can run any of the sample python scripts (`collision_scene_test_1.py <./collision_scene_test1.py>`_ or `collision_scene_test_2.py <./collision_scene_test2.py>`_). The first scripts creates a complex scene with four ostacles. The second script creates a simple environment with one obstacle. One can change the position/size of the obstacles to change the scene. + + +To run the CHOMP planner with obstacles, do the following in two seperate terminals: :: + + roslaunch panda_moveit_config demo_chomp.launch + python collision_scene_test_1.py OR python collision_scene_test_2.py diff --git a/doc/chomp_interface/collision_scene_test_1.py b/doc/chomp_interface/collision_scene_test_1.py new file mode 100644 index 000000000..c19d1e8a2 --- /dev/null +++ b/doc/chomp_interface/collision_scene_test_1.py @@ -0,0 +1,60 @@ +import rospy +from moveit_commander import RobotCommander, PlanningSceneInterface +import geometry_msgs.msg +import time + + +class CreateScene1(object): + def __init__(self): + self._scene = PlanningSceneInterface() + + # clear the scene + self._scene.remove_world_object() + + self.robot = RobotCommander() + + # pause to wait for rviz to load + rospy.sleep(4) + + floor_pose = [0, 0, -1.12, 0, 0, 0, 1] + floor_dimensions = [4, 4, 0.02] + + box1_pose = [0.20, 0.50, 0.25, 0, 0, 0, 1] + box1_dimensions = [0.2, 0.2, 0.5] + + box2_pose = [-0.55, -0.55, 0, 0, 0, 0, 1] + box2_dimensions = [0.25, 0.25, 1.75] + + box3_pose = [0.5, -0.55, 0.14, 0, 0, 0, 1] + box3_dimensions = [0.28, 0.28, 0.22] + + box4_pose = [-0.4, 0.4, 0.5, 0, 0, 0, 1] + box4_dimensions = [0.25, 0.25, 1.1] + + self.add_box_object("floor", floor_dimensions, floor_pose) + self.add_box_object("box1", box1_dimensions, box1_pose) + self.add_box_object("box2", box2_dimensions, box2_pose) + self.add_box_object("box3", box3_dimensions, box3_pose) + self.add_box_object("box4", box4_dimensions, box4_pose) + + def add_box_object(self, name, dimensions, pose): + p = geometry_msgs.msg.PoseStamped() + p.header.frame_id = self.robot.get_planning_frame() + p.header.stamp = rospy.Time.now() + p.pose.position.x = pose[0] + p.pose.position.y = pose[1] + p.pose.position.z = pose[2] + p.pose.orientation.x = pose[3] + p.pose.orientation.y = pose[4] + p.pose.orientation.z = pose[5] + p.pose.orientation.w = pose[6] + self._scene.add_box(name, p, (dimensions[0], dimensions[1], dimensions[2])) + rospy.sleep(0.2) + + +if __name__ == "__main__": + rospy.init_node("collision_scene_1") + while not rospy.search_param('robot_description_semantic') and not rospy.is_shutdown(): + time.sleep(0.5) + load_scene = CreateScene1() + rospy.spin() diff --git a/doc/chomp_interface/collision_scene_test_2.py b/doc/chomp_interface/collision_scene_test_2.py new file mode 100644 index 000000000..c71332e63 --- /dev/null +++ b/doc/chomp_interface/collision_scene_test_2.py @@ -0,0 +1,45 @@ +import rospy +from moveit_commander import RobotCommander, PlanningSceneInterface +import geometry_msgs.msg +import time + + +class CreateScene1(object): + def __init__(self): + self._scene = PlanningSceneInterface() + + # clear the scene + self._scene.remove_world_object() + + self.robot = RobotCommander() + + # pause to wait for rviz to load + rospy.sleep(4) + + box2_pose = [0.25, 0.25, 0.0, 0, 0, 0, 1] + box2_dimensions = [0.25, 0.25, 1.75] + + self.add_box_object("box2", box2_dimensions, box2_pose) + + + def add_box_object(self, name, dimensions, pose): + p = geometry_msgs.msg.PoseStamped() + p.header.frame_id = self.robot.get_planning_frame() + p.header.stamp = rospy.Time.now() + p.pose.position.x = pose[0] + p.pose.position.y = pose[1] + p.pose.position.z = pose[2] + p.pose.orientation.x = pose[3] + p.pose.orientation.y = pose[4] + p.pose.orientation.z = pose[5] + p.pose.orientation.w = pose[6] + self._scene.add_box(name, p, (dimensions[0], dimensions[1], dimensions[2])) + rospy.sleep(0.2) + + +if __name__ == "__main__": + rospy.init_node("collision_scene_2") + while not rospy.search_param('robot_description_semantic') and not rospy.is_shutdown(): + time.sleep(0.5) + load_scene = CreateScene1() + rospy.spin() From 9720788b742f37c4a1c8f6afa5305b371e6bcc81 Mon Sep 17 00:00:00 2001 From: Raghavender Sahdev Date: Mon, 4 Jun 2018 01:56:55 -0400 Subject: [PATCH 02/16] added section on Tweaking some of the parameters for CHOMP_under-construction commit --- .../chomp_interface_tutorial.rst | 54 +++++++++++++++++++ 1 file changed, 54 insertions(+) diff --git a/doc/chomp_interface/chomp_interface_tutorial.rst b/doc/chomp_interface/chomp_interface_tutorial.rst index 90ef5b3e1..544f986cc 100644 --- a/doc/chomp_interface/chomp_interface_tutorial.rst +++ b/doc/chomp_interface/chomp_interface_tutorial.rst @@ -70,3 +70,57 @@ To run the CHOMP planner with obstacles, do the following in two seperate termin roslaunch panda_moveit_config demo_chomp.launch python collision_scene_test_1.py OR python collision_scene_test_2.py + + +Tweaking some of the parameters for CHOMP +----------------------------------------- +Chomp has some optimization parameters associated with it. These can be modified for the given environment/robot you are working with and is normally present in the `chomp_planning.yaml `_ file in config folder of the robot you are working with. If this file does not exist for your robot, you can create it and set the parameter values as you want. Following are some of the insights to set up these parameter values for some of them: + +- *planning_time_limit*: the maximum time the optimizer can take to find a solution before terminating + +- *max_iterations*: this is the maximum number of iterations that the planner can take to find a good solution while optimization + +- *max_iterations_after_collision_free*: maximum iterations to be performed after a collision free path is found. + +- *smoothness_cost_weight*: The smoothness_cost_weight parameters controls its weight in the final cost that CHOMP is actually optimizing over + +- *obstacle_cost_weight*: This controls the weight to be given to obstacles towards the final cost CHOMP optimizes over. e.g., 0.0 would have obstacles to be ignored, 1.0 would be a hard constraint + +- *learning_rate*: this is the learning rate used by the optimizer to find the local / global minima while reducing the total cost. + +- *add_randomness*: adds some random noise to the costVeclocity + +- *smoothness_cost_velocity, smoothness_cost_acceleration, smoothness_cost_jerk*: variables associated with the cost in velocity, acceleration and jerk. + +- *hmc_discretization*: TODO + +- *ridge_factor*: the noise added to the diagnal of the total `quadratic cost matrix _` in the objective function. + +- *use_pseudo_inverse*: TODO + +- *pseudo_inverse_ridge_factor*: TODO + +- *joint_update_limit*: set the update limit for the robot joints + +- *collision_clearance*: the minimum distance that needs to be maintained to avoid obstacles. + +- *collision_threshold*: the collision threshold cost that needs to be mainted to avoid collisions + +- *use_stochastic_descent*: set this to true/false if you want to use stochastic descent while optimizing the cost. In stochastic descent, a random point from the trajectory is used, rather than all the trajectory points. This is faster and guaranteed to converge, but it may take more iterations in the worst case. + + +Some of the unused/commented parameters are + +- hmc_stochasticity: +- hmc_annealing_factor: +- use_hamiltonian_montecarlo: +- animate_endeffector: +- animate_endeffector_segment +- animate_path: +- random_jump_amount: + + + +Difference between plans obtained by CHOMP and OMPL +--------------------------------------------------- +Optimizing planners optimize a cost function that may sometimes lead to surprising results: moving through a thin obstacle might be lower cost than a long, winding trajectory that avoids all collisions. \ No newline at end of file From 9077fd9d2e80d7db7dd76ac5ccadac228c1f1b15 Mon Sep 17 00:00:00 2001 From: Raghavender Sahdev Date: Thu, 7 Jun 2018 08:11:12 -0400 Subject: [PATCH 03/16] added: CHOMP parameters, difference in plans OMPL/CHOMP, python scripts edited --- .../chomp_interface_tutorial.rst | 39 ++++++------ .../scripts/collision_scene_test_1.py | 62 +++++++++++++++++++ .../scripts/collision_scene_test_2.py | 46 ++++++++++++++ 3 files changed, 129 insertions(+), 18 deletions(-) create mode 100644 doc/chomp_interface/scripts/collision_scene_test_1.py create mode 100644 doc/chomp_interface/scripts/collision_scene_test_2.py diff --git a/doc/chomp_interface/chomp_interface_tutorial.rst b/doc/chomp_interface/chomp_interface_tutorial.rst index 544f986cc..e544ab63c 100644 --- a/doc/chomp_interface/chomp_interface_tutorial.rst +++ b/doc/chomp_interface/chomp_interface_tutorial.rst @@ -14,7 +14,7 @@ You should also have gone through the steps in `Visualization with MoveIt! RViz Prerequisites -------------- - 1. You must have the latest version of MoveIt! installed. On ROS Kinetic you will need to build MoveIt! from source. A build from source is required as CHOMP is not officially released so ``apt-get install`` for moveIt would not be appropriate here. We will go through the steps for doing this below. + 1. You must have the latest version of MoveIt! installed. On ROS Kinetic you will need to build MoveIt! from source. A build from source is required as CHOMP is not part of the official release yet. It is therefore not included in the binary packages. We will go through the steps for doing this below. 2. To use CHOMP with your robot you must already have a MoveIt! configuration package for your robot already. For example, if you have a Panda robot, it's probably called ``panda_moveit_config``. This is typically built using the `MoveIt! Setup Assistant <../setup_assistant/setup_assistant_tutorial.html>`_. Installing MoveIt! from Source @@ -63,7 +63,12 @@ If you have the ``panda_moveit_config`` from the `ros-planning/panda_moveit_conf Testing CHOMP with Obstacles in the Scene ----------------------------------------- -To test CHOMP in an evironment with obstacles, you can run any of the sample python scripts (`collision_scene_test_1.py <./collision_scene_test1.py>`_ or `collision_scene_test_2.py <./collision_scene_test2.py>`_). The first scripts creates a complex scene with four ostacles. The second script creates a simple environment with one obstacle. One can change the position/size of the obstacles to change the scene. +To test CHOMP in an evironment with obstacles, you can run any of the sample python scripts + + - `collision_scene_test_1.py <./scripts/collision_scene_test1.py>`_ or + - `collision_scene_test_2.py <./scripts/collision_scene_test2.py>`_). + + The first scripts creates a complex scene with four ostacles. The second script creates a simple environment with one obstacle. One can change the position/size of the obstacles to change the scene. To run the CHOMP planner with obstacles, do the following in two seperate terminals: :: @@ -74,7 +79,7 @@ To run the CHOMP planner with obstacles, do the following in two seperate termin Tweaking some of the parameters for CHOMP ----------------------------------------- -Chomp has some optimization parameters associated with it. These can be modified for the given environment/robot you are working with and is normally present in the `chomp_planning.yaml `_ file in config folder of the robot you are working with. If this file does not exist for your robot, you can create it and set the parameter values as you want. Following are some of the insights to set up these parameter values for some of them: +CHOMP has some optimization parameters associated with it. These can be modified for the given environment/robot you are working with and is normally present in the `chomp_planning.yaml `_ file in config folder of the robot you are working with. If this file does not exist for your robot, you can create it and set the parameter values as you want. The following are some of the insights to set up these parameter values for some of them: - *planning_time_limit*: the maximum time the optimizer can take to find a solution before terminating @@ -92,13 +97,11 @@ Chomp has some optimization parameters associated with it. These can be modified - *smoothness_cost_velocity, smoothness_cost_acceleration, smoothness_cost_jerk*: variables associated with the cost in velocity, acceleration and jerk. -- *hmc_discretization*: TODO - -- *ridge_factor*: the noise added to the diagnal of the total `quadratic cost matrix _` in the objective function. +- *ridge_factor*: the noise added to the diagnal of the total `quadratic cost matrix `_ in the objective function. Addition of small noise (e.g., 0.001) allows CHOMP to avoid obstacles at the cost of smoothness in trajectory. -- *use_pseudo_inverse*: TODO +- *use_pseudo_inverse*: enable pseudo inverse calculations or not. -- *pseudo_inverse_ridge_factor*: TODO +- *pseudo_inverse_ridge_factor*: set the ridge factor if pseudo inverse is enabled. - *joint_update_limit*: set the update limit for the robot joints @@ -108,19 +111,19 @@ Chomp has some optimization parameters associated with it. These can be modified - *use_stochastic_descent*: set this to true/false if you want to use stochastic descent while optimizing the cost. In stochastic descent, a random point from the trajectory is used, rather than all the trajectory points. This is faster and guaranteed to converge, but it may take more iterations in the worst case. +Choosing Parameters for CHOMP requires some sort of intuition based on the environment we are working in. One can have the default parameters for CHOMP and this works well in environments without obstacles. However in cases where the scene is populated with obstacles, we need to vary some parameters to ensure that CHOMP is not stuck in local minima, or quickly finds optimal solutions, prefering trajectories which ovoids obstacles. Some parameters like increasing the *ridge_factor* to say 0.001 makes CHOMP avoids obstacles by not prefering smooth trajectories, so there is a trade-off between smoothness and CHOMP's ability to avoid obstacles. Choosing the correct number of *max_iterations*, *learning_rate* is important based on the environment we are working in. Not choosing the appropriate CHOMP parameters might lead to CHOMP reporting not finding a collision free path. *collision_clearance*, *collision_threshold* parameters are useful in specifying the minimum distance to be kept from obstacles to avoid collisions. -Some of the unused/commented parameters are - -- hmc_stochasticity: -- hmc_annealing_factor: -- use_hamiltonian_montecarlo: -- animate_endeffector: -- animate_endeffector_segment -- animate_path: -- random_jump_amount: +Some of the unused/commented parameters are *hmc_stochasticity*, *hmc_annealing_factor*, *hmc_discretization*, *use_hamiltonian_montecarlo*, *animate_endeffector*, *animate_endeffector_segment*, *animate_path*, *random_jump_amount*. Difference between plans obtained by CHOMP and OMPL --------------------------------------------------- -Optimizing planners optimize a cost function that may sometimes lead to surprising results: moving through a thin obstacle might be lower cost than a long, winding trajectory that avoids all collisions. \ No newline at end of file +Optimizing planners optimize a cost function that may sometimes lead to surprising results: moving through a thin obstacle might be lower cost than a long, winding trajectory that avoids all collisions. In this section we make a distinction between paths obtained from CHOMP and contrast it to those obtained from OMPL. + +OMPL is a open source library for sampling based / randomized motion planning algorithms. Sampling based algorithms are probabilistically complete: a solution would be eventually found if one exists, however non-existence of a solution cannot be reported. These algorithms are efficient and usually find a solution quickly. OMPL does not contain any code related to collision checking or visualization as the designers of OMPL did not want to tie it to a any particular colision checker or visualization front end. The library is designed so it can be easily integrated into systems that provide the additional components. MoveIt integrates directly with OMPL and uses the motion planners from OMP as its default set of planners. The planners in OMPL are abstract; i.e. OMPL has no concept of a robot. Instead, MoveIt! configures OMPL and provides the back-end for OMPL to work with problems in Robotics. + + +CHOMP: While most high-dimensional motion planners separate trajectory generation into distinct planning and optimization stages, CHOMP capitalizes on covariant gradient and functional gradient approaches to the optimization stage to design a motion planning algorithm based entirely on trajectory optimization. Given an infeasible naive trajectory, CHOMP reacts to the surrounding environment to quickly pull the trajectory out of collision while simultaneously optimizing dynamical quantities such as joint velocities and accelerations. It rapidly converges to a smooth collision-free trajectory that can be executed efficiently on the robot. A covaraint update rule ensures that CHOMP quickly converges to a locally optimal trajectory. + +For scenes containing obstacles, CHOMP often generates paths which do not prefer smooth trajectories by addition of some noise (*ridge_factor*) in the cost function for the dynamical quantities of the robot (like acceleration, velocity). CHOMP is able to avoid obstacle in most cases but also fails in some if it gets stuck in the local minima and might report a solution not found due to a naive initial guess for the trajectory. OMPL on the other hand generates collision free smooth paths in the presence of obstacles too. \ No newline at end of file diff --git a/doc/chomp_interface/scripts/collision_scene_test_1.py b/doc/chomp_interface/scripts/collision_scene_test_1.py new file mode 100644 index 000000000..1b15d5d70 --- /dev/null +++ b/doc/chomp_interface/scripts/collision_scene_test_1.py @@ -0,0 +1,62 @@ +import rospy +from moveit_commander import RobotCommander, PlanningSceneInterface +import geometry_msgs.msg +import time + + +class CreateScene(object): + def __init__(self): + self._scene = PlanningSceneInterface() + + # clear the scene + self._scene.remove_world_object() + + self.robot = RobotCommander() + + # pause to wait for rviz to load + rospy.sleep(4) + + floor_pose = [0, 0, -1.12, 0, 0, 0, 1] + floor_dimensions = [0, 0, 0.0] + + box1_pose = [0.20, 0.50, 0.25, 0, 0, 0, 1] + box1_dimensions = [0.2, 0.2, 0.5] + + box2_pose = [-0.55, -0.55, 0, 0, 0, 0, 1] + box2_dimensions = [0.25, 0.25, 1.75] + + box3_pose = [0.5, -0.55, 0.14, 0, 0, 0, 1] + box3_dimensions = [0.28, 0.28, 0.22] + + box4_pose = [-0.4, 0.4, 0.5, 0, 0, 0, 1] + box4_dimensions = [0.25, 0.25, 1.1] + + self.add_box_object("floor", floor_dimensions, floor_pose) + self.add_box_object("box1", box1_dimensions, box1_pose) + self.add_box_object("box2", box2_dimensions, box2_pose) + self.add_box_object("box3", box3_dimensions, box3_pose) + self.add_box_object("box4", box4_dimensions, box4_pose) + + def add_box_object(self, name, dimensions, pose): + p = geometry_msgs.msg.PoseStamped() + p.header.frame_id = self.robot.get_planning_frame() + p.header.stamp = rospy.Time.now() + p.pose.position.x = pose[0] + p.pose.position.y = pose[1] + p.pose.position.z = pose[2] + p.pose.orientation.x = pose[3] + p.pose.orientation.y = pose[4] + p.pose.orientation.z = pose[5] + p.pose.orientation.w = pose[6] + + self._scene.add_box(name, p, (dimensions[0], dimensions[1], dimensions[2])) + print "============ Waiting while RVIZ displays the scene with four obstacles..." + rospy.sleep(1) + + +if __name__ == "__main__": + rospy.init_node("collision_scene_1") + while not rospy.search_param('robot_description_semantic') and not rospy.is_shutdown(): + time.sleep(0.5) + load_scene = CreateScene() + rospy.spin() diff --git a/doc/chomp_interface/scripts/collision_scene_test_2.py b/doc/chomp_interface/scripts/collision_scene_test_2.py new file mode 100644 index 000000000..6941f9118 --- /dev/null +++ b/doc/chomp_interface/scripts/collision_scene_test_2.py @@ -0,0 +1,46 @@ +import rospy +from moveit_commander import RobotCommander, PlanningSceneInterface +import geometry_msgs.msg +import time + + +class CreateScene(object): + def __init__(self): + self._scene = PlanningSceneInterface() + + # clear the scene + self._scene.remove_world_object() + + self.robot = RobotCommander() + + # pause to wait for rviz to load + rospy.sleep(4) + + box2_pose = [0.25, 0.25, 0.0, 0, 0, 0, 1] + box2_dimensions = [0.25, 0.25, 0.45] + + self.add_box_object("box2", box2_dimensions, box2_pose) + + + def add_box_object(self, name, dimensions, pose): + p = geometry_msgs.msg.PoseStamped() + p.header.frame_id = self.robot.get_planning_frame() + p.header.stamp = rospy.Time.now() + p.pose.position.x = pose[0] + p.pose.position.y = pose[1] + p.pose.position.z = pose[2] + p.pose.orientation.x = pose[3] + p.pose.orientation.y = pose[4] + p.pose.orientation.z = pose[5] + p.pose.orientation.w = pose[6] + self._scene.add_box(name, p, (dimensions[0], dimensions[1], dimensions[2])) + print "============ Waiting while RVIZ displays the scene with one obstacle..." + rospy.sleep(1) + + +if __name__ == "__main__": + rospy.init_node("collision_scene_2") + while not rospy.search_param('robot_description_semantic') and not rospy.is_shutdown(): + time.sleep(0.5) + load_scene = CreateScene() + rospy.spin() From 2e2b27deedbe6fdaf7e7c8eb7eb0e2a5c1a3e0ec Mon Sep 17 00:00:00 2001 From: Raghavender Sahdev Date: Thu, 7 Jun 2018 08:14:55 -0400 Subject: [PATCH 04/16] corrected scripts link --- doc/chomp_interface/chomp_interface_tutorial.rst | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/doc/chomp_interface/chomp_interface_tutorial.rst b/doc/chomp_interface/chomp_interface_tutorial.rst index e544ab63c..d4132d9d0 100644 --- a/doc/chomp_interface/chomp_interface_tutorial.rst +++ b/doc/chomp_interface/chomp_interface_tutorial.rst @@ -65,8 +65,8 @@ Testing CHOMP with Obstacles in the Scene ----------------------------------------- To test CHOMP in an evironment with obstacles, you can run any of the sample python scripts - - `collision_scene_test_1.py <./scripts/collision_scene_test1.py>`_ or - - `collision_scene_test_2.py <./scripts/collision_scene_test2.py>`_). + - `collision_scene_test_1.py <./scripts/collision_scene_test_1.py>`_ or + - `collision_scene_test_2.py <./scripts/collision_scene_test_2.py>`_). The first scripts creates a complex scene with four ostacles. The second script creates a simple environment with one obstacle. One can change the position/size of the obstacles to change the scene. From b144d6c38e386b223aeec2bed36693f65e29a03c Mon Sep 17 00:00:00 2001 From: Raghavender Sahdev Date: Thu, 7 Jun 2018 16:56:48 -0400 Subject: [PATCH 05/16] fixed formatting issues --- .../chomp_interface_tutorial.rst | 14 ++--- doc/chomp_interface/collision_scene_test_1.py | 60 ------------------- doc/chomp_interface/collision_scene_test_2.py | 45 -------------- 3 files changed, 5 insertions(+), 114 deletions(-) delete mode 100644 doc/chomp_interface/collision_scene_test_1.py delete mode 100644 doc/chomp_interface/collision_scene_test_2.py diff --git a/doc/chomp_interface/chomp_interface_tutorial.rst b/doc/chomp_interface/chomp_interface_tutorial.rst index d4132d9d0..72548ad98 100644 --- a/doc/chomp_interface/chomp_interface_tutorial.rst +++ b/doc/chomp_interface/chomp_interface_tutorial.rst @@ -63,20 +63,19 @@ If you have the ``panda_moveit_config`` from the `ros-planning/panda_moveit_conf Testing CHOMP with Obstacles in the Scene ----------------------------------------- -To test CHOMP in an evironment with obstacles, you can run any of the sample python scripts - - - `collision_scene_test_1.py <./scripts/collision_scene_test_1.py>`_ or - - `collision_scene_test_2.py <./scripts/collision_scene_test_2.py>`_). +To test CHOMP in an evironment with obstacles, you can run any of the sample python scripts: - The first scripts creates a complex scene with four ostacles. The second script creates a simple environment with one obstacle. One can change the position/size of the obstacles to change the scene. +- `collision_scene_test_1.py <./scripts/collision_scene_test_1.py>`_ or +- `collision_scene_test_2.py <./scripts/collision_scene_test_2.py>`_. + +The first scripts creates a complex scene with four ostacles. The second script creates a simple environment with one obstacle. One can change the position/size of the obstacles to change the scene. To run the CHOMP planner with obstacles, do the following in two seperate terminals: :: roslaunch panda_moveit_config demo_chomp.launch python collision_scene_test_1.py OR python collision_scene_test_2.py - Tweaking some of the parameters for CHOMP ----------------------------------------- CHOMP has some optimization parameters associated with it. These can be modified for the given environment/robot you are working with and is normally present in the `chomp_planning.yaml `_ file in config folder of the robot you are working with. If this file does not exist for your robot, you can create it and set the parameter values as you want. The following are some of the insights to set up these parameter values for some of them: @@ -115,15 +114,12 @@ Choosing Parameters for CHOMP requires some sort of intuition based on the envir Some of the unused/commented parameters are *hmc_stochasticity*, *hmc_annealing_factor*, *hmc_discretization*, *use_hamiltonian_montecarlo*, *animate_endeffector*, *animate_endeffector_segment*, *animate_path*, *random_jump_amount*. - - Difference between plans obtained by CHOMP and OMPL --------------------------------------------------- Optimizing planners optimize a cost function that may sometimes lead to surprising results: moving through a thin obstacle might be lower cost than a long, winding trajectory that avoids all collisions. In this section we make a distinction between paths obtained from CHOMP and contrast it to those obtained from OMPL. OMPL is a open source library for sampling based / randomized motion planning algorithms. Sampling based algorithms are probabilistically complete: a solution would be eventually found if one exists, however non-existence of a solution cannot be reported. These algorithms are efficient and usually find a solution quickly. OMPL does not contain any code related to collision checking or visualization as the designers of OMPL did not want to tie it to a any particular colision checker or visualization front end. The library is designed so it can be easily integrated into systems that provide the additional components. MoveIt integrates directly with OMPL and uses the motion planners from OMP as its default set of planners. The planners in OMPL are abstract; i.e. OMPL has no concept of a robot. Instead, MoveIt! configures OMPL and provides the back-end for OMPL to work with problems in Robotics. - CHOMP: While most high-dimensional motion planners separate trajectory generation into distinct planning and optimization stages, CHOMP capitalizes on covariant gradient and functional gradient approaches to the optimization stage to design a motion planning algorithm based entirely on trajectory optimization. Given an infeasible naive trajectory, CHOMP reacts to the surrounding environment to quickly pull the trajectory out of collision while simultaneously optimizing dynamical quantities such as joint velocities and accelerations. It rapidly converges to a smooth collision-free trajectory that can be executed efficiently on the robot. A covaraint update rule ensures that CHOMP quickly converges to a locally optimal trajectory. For scenes containing obstacles, CHOMP often generates paths which do not prefer smooth trajectories by addition of some noise (*ridge_factor*) in the cost function for the dynamical quantities of the robot (like acceleration, velocity). CHOMP is able to avoid obstacle in most cases but also fails in some if it gets stuck in the local minima and might report a solution not found due to a naive initial guess for the trajectory. OMPL on the other hand generates collision free smooth paths in the presence of obstacles too. \ No newline at end of file diff --git a/doc/chomp_interface/collision_scene_test_1.py b/doc/chomp_interface/collision_scene_test_1.py deleted file mode 100644 index c19d1e8a2..000000000 --- a/doc/chomp_interface/collision_scene_test_1.py +++ /dev/null @@ -1,60 +0,0 @@ -import rospy -from moveit_commander import RobotCommander, PlanningSceneInterface -import geometry_msgs.msg -import time - - -class CreateScene1(object): - def __init__(self): - self._scene = PlanningSceneInterface() - - # clear the scene - self._scene.remove_world_object() - - self.robot = RobotCommander() - - # pause to wait for rviz to load - rospy.sleep(4) - - floor_pose = [0, 0, -1.12, 0, 0, 0, 1] - floor_dimensions = [4, 4, 0.02] - - box1_pose = [0.20, 0.50, 0.25, 0, 0, 0, 1] - box1_dimensions = [0.2, 0.2, 0.5] - - box2_pose = [-0.55, -0.55, 0, 0, 0, 0, 1] - box2_dimensions = [0.25, 0.25, 1.75] - - box3_pose = [0.5, -0.55, 0.14, 0, 0, 0, 1] - box3_dimensions = [0.28, 0.28, 0.22] - - box4_pose = [-0.4, 0.4, 0.5, 0, 0, 0, 1] - box4_dimensions = [0.25, 0.25, 1.1] - - self.add_box_object("floor", floor_dimensions, floor_pose) - self.add_box_object("box1", box1_dimensions, box1_pose) - self.add_box_object("box2", box2_dimensions, box2_pose) - self.add_box_object("box3", box3_dimensions, box3_pose) - self.add_box_object("box4", box4_dimensions, box4_pose) - - def add_box_object(self, name, dimensions, pose): - p = geometry_msgs.msg.PoseStamped() - p.header.frame_id = self.robot.get_planning_frame() - p.header.stamp = rospy.Time.now() - p.pose.position.x = pose[0] - p.pose.position.y = pose[1] - p.pose.position.z = pose[2] - p.pose.orientation.x = pose[3] - p.pose.orientation.y = pose[4] - p.pose.orientation.z = pose[5] - p.pose.orientation.w = pose[6] - self._scene.add_box(name, p, (dimensions[0], dimensions[1], dimensions[2])) - rospy.sleep(0.2) - - -if __name__ == "__main__": - rospy.init_node("collision_scene_1") - while not rospy.search_param('robot_description_semantic') and not rospy.is_shutdown(): - time.sleep(0.5) - load_scene = CreateScene1() - rospy.spin() diff --git a/doc/chomp_interface/collision_scene_test_2.py b/doc/chomp_interface/collision_scene_test_2.py deleted file mode 100644 index c71332e63..000000000 --- a/doc/chomp_interface/collision_scene_test_2.py +++ /dev/null @@ -1,45 +0,0 @@ -import rospy -from moveit_commander import RobotCommander, PlanningSceneInterface -import geometry_msgs.msg -import time - - -class CreateScene1(object): - def __init__(self): - self._scene = PlanningSceneInterface() - - # clear the scene - self._scene.remove_world_object() - - self.robot = RobotCommander() - - # pause to wait for rviz to load - rospy.sleep(4) - - box2_pose = [0.25, 0.25, 0.0, 0, 0, 0, 1] - box2_dimensions = [0.25, 0.25, 1.75] - - self.add_box_object("box2", box2_dimensions, box2_pose) - - - def add_box_object(self, name, dimensions, pose): - p = geometry_msgs.msg.PoseStamped() - p.header.frame_id = self.robot.get_planning_frame() - p.header.stamp = rospy.Time.now() - p.pose.position.x = pose[0] - p.pose.position.y = pose[1] - p.pose.position.z = pose[2] - p.pose.orientation.x = pose[3] - p.pose.orientation.y = pose[4] - p.pose.orientation.z = pose[5] - p.pose.orientation.w = pose[6] - self._scene.add_box(name, p, (dimensions[0], dimensions[1], dimensions[2])) - rospy.sleep(0.2) - - -if __name__ == "__main__": - rospy.init_node("collision_scene_2") - while not rospy.search_param('robot_description_semantic') and not rospy.is_shutdown(): - time.sleep(0.5) - load_scene = CreateScene1() - rospy.spin() From 5d747ce95f936d78db72d7960e50f861eac39195 Mon Sep 17 00:00:00 2001 From: Raghavender Sahdev Date: Thu, 7 Jun 2018 23:37:06 -0400 Subject: [PATCH 06/16] PR changes requested --- doc/chomp_interface/chomp_interface_tutorial.rst | 12 ++++++------ .../scripts/collision_scene_test_1.py | 3 ++- .../scripts/collision_scene_test_2.py | 6 ++---- 3 files changed, 10 insertions(+), 11 deletions(-) mode change 100644 => 100755 doc/chomp_interface/scripts/collision_scene_test_1.py mode change 100644 => 100755 doc/chomp_interface/scripts/collision_scene_test_2.py diff --git a/doc/chomp_interface/chomp_interface_tutorial.rst b/doc/chomp_interface/chomp_interface_tutorial.rst index 72548ad98..f428ac5ab 100644 --- a/doc/chomp_interface/chomp_interface_tutorial.rst +++ b/doc/chomp_interface/chomp_interface_tutorial.rst @@ -51,9 +51,9 @@ Using CHOMP with Your Robot #. Open the ``move_group.launch`` file in your ``/launch/`` folder and make two changes. - 7.1. First, add ```` just under the ```` tag, and, + * First, add ```` just under the ```` tag, and, - 7.2. Second, within the ```` tag replace ```` with ````. + * Second, within the ```` tag replace ```` with ````. Running the Demo ---------------- @@ -74,7 +74,7 @@ The first scripts creates a complex scene with four ostacles. The second script To run the CHOMP planner with obstacles, do the following in two seperate terminals: :: roslaunch panda_moveit_config demo_chomp.launch - python collision_scene_test_1.py OR python collision_scene_test_2.py + rosrun moveit_tutorials collision_scene_test_1.py OR rosrun moveit_tutorials collision_scene_test_2.py Tweaking some of the parameters for CHOMP ----------------------------------------- @@ -86,9 +86,9 @@ CHOMP has some optimization parameters associated with it. These can be modified - *max_iterations_after_collision_free*: maximum iterations to be performed after a collision free path is found. -- *smoothness_cost_weight*: The smoothness_cost_weight parameters controls its weight in the final cost that CHOMP is actually optimizing over +- *smoothness_cost_weight*: the smoothness_cost_weight parameters controls its weight in the final cost that CHOMP is actually optimizing over -- *obstacle_cost_weight*: This controls the weight to be given to obstacles towards the final cost CHOMP optimizes over. e.g., 0.0 would have obstacles to be ignored, 1.0 would be a hard constraint +- *obstacle_cost_weight*: this controls the weight to be given to obstacles towards the final cost CHOMP optimizes over. e.g., 0.0 would have obstacles to be ignored, 1.0 would be a hard constraint - *learning_rate*: this is the learning rate used by the optimizer to find the local / global minima while reducing the total cost. @@ -110,7 +110,7 @@ CHOMP has some optimization parameters associated with it. These can be modified - *use_stochastic_descent*: set this to true/false if you want to use stochastic descent while optimizing the cost. In stochastic descent, a random point from the trajectory is used, rather than all the trajectory points. This is faster and guaranteed to converge, but it may take more iterations in the worst case. -Choosing Parameters for CHOMP requires some sort of intuition based on the environment we are working in. One can have the default parameters for CHOMP and this works well in environments without obstacles. However in cases where the scene is populated with obstacles, we need to vary some parameters to ensure that CHOMP is not stuck in local minima, or quickly finds optimal solutions, prefering trajectories which ovoids obstacles. Some parameters like increasing the *ridge_factor* to say 0.001 makes CHOMP avoids obstacles by not prefering smooth trajectories, so there is a trade-off between smoothness and CHOMP's ability to avoid obstacles. Choosing the correct number of *max_iterations*, *learning_rate* is important based on the environment we are working in. Not choosing the appropriate CHOMP parameters might lead to CHOMP reporting not finding a collision free path. *collision_clearance*, *collision_threshold* parameters are useful in specifying the minimum distance to be kept from obstacles to avoid collisions. +Choosing parameters for CHOMP requires some sort of intuition based on the environment we are working in. One can have the default parameters for CHOMP and this works well in environments without obstacles. However in cases where the scene is populated with obstacles, we need to vary some parameters to ensure that CHOMP is not stuck in local minima, or quickly finds optimal solutions, prefering trajectories which ovoids obstacles. Some parameters like increasing the *ridge_factor* to say 0.001 makes CHOMP avoids obstacles by not prefering smooth trajectories, so there is a trade-off between smoothness and CHOMP's ability to avoid obstacles. Choosing the correct number of *max_iterations*, *learning_rate* is important based on the environment we are working in. Not choosing the appropriate CHOMP parameters might lead to CHOMP reporting not finding a collision free path. *collision_clearance*, *collision_threshold* parameters are useful in specifying the minimum distance to be kept from obstacles to avoid collisions. Some of the unused/commented parameters are *hmc_stochasticity*, *hmc_annealing_factor*, *hmc_discretization*, *use_hamiltonian_montecarlo*, *animate_endeffector*, *animate_endeffector_segment*, *animate_path*, *random_jump_amount*. diff --git a/doc/chomp_interface/scripts/collision_scene_test_1.py b/doc/chomp_interface/scripts/collision_scene_test_1.py old mode 100644 new mode 100755 index 1b15d5d70..bbce33d0a --- a/doc/chomp_interface/scripts/collision_scene_test_1.py +++ b/doc/chomp_interface/scripts/collision_scene_test_1.py @@ -1,3 +1,4 @@ +#!/usr/bin/env python import rospy from moveit_commander import RobotCommander, PlanningSceneInterface import geometry_msgs.msg @@ -5,6 +6,7 @@ class CreateScene(object): + def __init__(self): self._scene = PlanningSceneInterface() @@ -53,7 +55,6 @@ def add_box_object(self, name, dimensions, pose): print "============ Waiting while RVIZ displays the scene with four obstacles..." rospy.sleep(1) - if __name__ == "__main__": rospy.init_node("collision_scene_1") while not rospy.search_param('robot_description_semantic') and not rospy.is_shutdown(): diff --git a/doc/chomp_interface/scripts/collision_scene_test_2.py b/doc/chomp_interface/scripts/collision_scene_test_2.py old mode 100644 new mode 100755 index 6941f9118..c406138ec --- a/doc/chomp_interface/scripts/collision_scene_test_2.py +++ b/doc/chomp_interface/scripts/collision_scene_test_2.py @@ -1,16 +1,16 @@ +#!/usr/bin/env python import rospy from moveit_commander import RobotCommander, PlanningSceneInterface import geometry_msgs.msg import time - class CreateScene(object): + def __init__(self): self._scene = PlanningSceneInterface() # clear the scene self._scene.remove_world_object() - self.robot = RobotCommander() # pause to wait for rviz to load @@ -21,7 +21,6 @@ def __init__(self): self.add_box_object("box2", box2_dimensions, box2_pose) - def add_box_object(self, name, dimensions, pose): p = geometry_msgs.msg.PoseStamped() p.header.frame_id = self.robot.get_planning_frame() @@ -37,7 +36,6 @@ def add_box_object(self, name, dimensions, pose): print "============ Waiting while RVIZ displays the scene with one obstacle..." rospy.sleep(1) - if __name__ == "__main__": rospy.init_node("collision_scene_2") while not rospy.search_param('robot_description_semantic') and not rospy.is_shutdown(): From d7cfd8a8f345ffb104f29d329bce79f5e5f6b2f9 Mon Sep 17 00:00:00 2001 From: Raghavender Sahdev Date: Fri, 8 Jun 2018 13:34:26 -0400 Subject: [PATCH 07/16] add_randomness is not used in the CHOMP code --- doc/chomp_interface/chomp_interface_tutorial.rst | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/doc/chomp_interface/chomp_interface_tutorial.rst b/doc/chomp_interface/chomp_interface_tutorial.rst index f428ac5ab..9c945504f 100644 --- a/doc/chomp_interface/chomp_interface_tutorial.rst +++ b/doc/chomp_interface/chomp_interface_tutorial.rst @@ -92,8 +92,6 @@ CHOMP has some optimization parameters associated with it. These can be modified - *learning_rate*: this is the learning rate used by the optimizer to find the local / global minima while reducing the total cost. -- *add_randomness*: adds some random noise to the costVeclocity - - *smoothness_cost_velocity, smoothness_cost_acceleration, smoothness_cost_jerk*: variables associated with the cost in velocity, acceleration and jerk. - *ridge_factor*: the noise added to the diagnal of the total `quadratic cost matrix `_ in the objective function. Addition of small noise (e.g., 0.001) allows CHOMP to avoid obstacles at the cost of smoothness in trajectory. @@ -112,7 +110,7 @@ CHOMP has some optimization parameters associated with it. These can be modified Choosing parameters for CHOMP requires some sort of intuition based on the environment we are working in. One can have the default parameters for CHOMP and this works well in environments without obstacles. However in cases where the scene is populated with obstacles, we need to vary some parameters to ensure that CHOMP is not stuck in local minima, or quickly finds optimal solutions, prefering trajectories which ovoids obstacles. Some parameters like increasing the *ridge_factor* to say 0.001 makes CHOMP avoids obstacles by not prefering smooth trajectories, so there is a trade-off between smoothness and CHOMP's ability to avoid obstacles. Choosing the correct number of *max_iterations*, *learning_rate* is important based on the environment we are working in. Not choosing the appropriate CHOMP parameters might lead to CHOMP reporting not finding a collision free path. *collision_clearance*, *collision_threshold* parameters are useful in specifying the minimum distance to be kept from obstacles to avoid collisions. -Some of the unused/commented parameters are *hmc_stochasticity*, *hmc_annealing_factor*, *hmc_discretization*, *use_hamiltonian_montecarlo*, *animate_endeffector*, *animate_endeffector_segment*, *animate_path*, *random_jump_amount*. +Some of the unused/commented parameters are *hmc_stochasticity*, *hmc_annealing_factor*, *hmc_discretization*, *use_hamiltonian_montecarlo*, *animate_endeffector*, *animate_endeffector_segment*, *animate_path*, *random_jump_amount*, *add_randomness*. Difference between plans obtained by CHOMP and OMPL --------------------------------------------------- From 928ce995acff49ca6e8e96bae195da2a423dc51b Mon Sep 17 00:00:00 2001 From: Raghavender Sahdev Date: Fri, 8 Jun 2018 14:27:59 -0400 Subject: [PATCH 08/16] addressed the PR changes requested --- .../chomp_interface_tutorial.rst | 19 +++++++++++++------ ...y => collision_scene_example_cluttered.py} | 6 +++--- ...2.py => collision_scene_example_sparse.py} | 4 ++-- 3 files changed, 18 insertions(+), 11 deletions(-) rename doc/chomp_interface/scripts/{collision_scene_test_1.py => collision_scene_example_cluttered.py} (92%) rename doc/chomp_interface/scripts/{collision_scene_test_2.py => collision_scene_example_sparse.py} (91%) diff --git a/doc/chomp_interface/chomp_interface_tutorial.rst b/doc/chomp_interface/chomp_interface_tutorial.rst index 9c945504f..6ad0e5cbd 100644 --- a/doc/chomp_interface/chomp_interface_tutorial.rst +++ b/doc/chomp_interface/chomp_interface_tutorial.rst @@ -62,19 +62,26 @@ If you have the ``panda_moveit_config`` from the `ros-planning/panda_moveit_conf roslaunch panda_moveit_config demo_chomp.launch Testing CHOMP with Obstacles in the Scene ------------------------------------------ ++++++++++++++++++++++++++++++++++++++++++ To test CHOMP in an evironment with obstacles, you can run any of the sample python scripts: -- `collision_scene_test_1.py <./scripts/collision_scene_test_1.py>`_ or +- `collision_scene_cluttered.py <./scripts/collision_scene_example_cluttered.py>`_ or -- `collision_scene_test_2.py <./scripts/collision_scene_test_2.py>`_. +- `collision_scene_sparse.py <./scripts/collision_scene_example_sparse.py>`_. -The first scripts creates a complex scene with four ostacles. The second script creates a simple environment with one obstacle. One can change the position/size of the obstacles to change the scene. +The first scripts creates a cluttered scene with four ostacles. The second script creates a simple scene with one obstacle. One can change the position/size of the obstacles to change the scene. -To run the CHOMP planner with obstacles, do the following in two seperate terminals: :: +To run the CHOMP planner with obstacles, open two shells. In the first shell start RViz and wait for everything to finish loading: :: roslaunch panda_moveit_config demo_chomp.launch - rosrun moveit_tutorials collision_scene_test_1.py OR rosrun moveit_tutorials collision_scene_test_2.py + +In the second shell, run either of the two commands: :: + + rosrun moveit_tutorials collision_scene_example_cluttered.py + +or: :: + + rosrun moveit_tutorials collision_scene_example_sparse.py Tweaking some of the parameters for CHOMP ----------------------------------------- diff --git a/doc/chomp_interface/scripts/collision_scene_test_1.py b/doc/chomp_interface/scripts/collision_scene_example_cluttered.py similarity index 92% rename from doc/chomp_interface/scripts/collision_scene_test_1.py rename to doc/chomp_interface/scripts/collision_scene_example_cluttered.py index bbce33d0a..a21e4572e 100755 --- a/doc/chomp_interface/scripts/collision_scene_test_1.py +++ b/doc/chomp_interface/scripts/collision_scene_example_cluttered.py @@ -5,7 +5,7 @@ import time -class CreateScene(object): +class CreateClutteredScene(object): def __init__(self): self._scene = PlanningSceneInterface() @@ -52,12 +52,12 @@ def add_box_object(self, name, dimensions, pose): p.pose.orientation.w = pose[6] self._scene.add_box(name, p, (dimensions[0], dimensions[1], dimensions[2])) - print "============ Waiting while RVIZ displays the scene with four obstacles..." + print "============ Waiting while RVIZ displays the scene with four obstacles..." rospy.sleep(1) if __name__ == "__main__": rospy.init_node("collision_scene_1") while not rospy.search_param('robot_description_semantic') and not rospy.is_shutdown(): time.sleep(0.5) - load_scene = CreateScene() + load_scene = CreateClutteredScene() rospy.spin() diff --git a/doc/chomp_interface/scripts/collision_scene_test_2.py b/doc/chomp_interface/scripts/collision_scene_example_sparse.py similarity index 91% rename from doc/chomp_interface/scripts/collision_scene_test_2.py rename to doc/chomp_interface/scripts/collision_scene_example_sparse.py index c406138ec..8862fc474 100755 --- a/doc/chomp_interface/scripts/collision_scene_test_2.py +++ b/doc/chomp_interface/scripts/collision_scene_example_sparse.py @@ -4,7 +4,7 @@ import geometry_msgs.msg import time -class CreateScene(object): +class CreateSparseScene(object): def __init__(self): self._scene = PlanningSceneInterface() @@ -33,7 +33,7 @@ def add_box_object(self, name, dimensions, pose): p.pose.orientation.z = pose[5] p.pose.orientation.w = pose[6] self._scene.add_box(name, p, (dimensions[0], dimensions[1], dimensions[2])) - print "============ Waiting while RVIZ displays the scene with one obstacle..." + print "============ Waiting while RVIZ displays the scene with one obstacle..." rospy.sleep(1) if __name__ == "__main__": From be0a37f04250b7caad3a281ee16a674c2773a54f Mon Sep 17 00:00:00 2001 From: Raghavender Sahdev Date: Fri, 8 Jun 2018 15:02:50 -0400 Subject: [PATCH 09/16] fixed python script bugs, removed sleep --- .../scripts/collision_scene_example_cluttered.py | 9 ++++----- .../scripts/collision_scene_example_sparse.py | 7 +++---- 2 files changed, 7 insertions(+), 9 deletions(-) diff --git a/doc/chomp_interface/scripts/collision_scene_example_cluttered.py b/doc/chomp_interface/scripts/collision_scene_example_cluttered.py index a21e4572e..f10f77389 100755 --- a/doc/chomp_interface/scripts/collision_scene_example_cluttered.py +++ b/doc/chomp_interface/scripts/collision_scene_example_cluttered.py @@ -15,7 +15,8 @@ def __init__(self): self.robot = RobotCommander() - # pause to wait for rviz to load + # pause to wait for rviz to load + print "============ Waiting while RVIZ displays the scene with four obstacles..." rospy.sleep(4) floor_pose = [0, 0, -1.12, 0, 0, 0, 1] @@ -52,11 +53,9 @@ def add_box_object(self, name, dimensions, pose): p.pose.orientation.w = pose[6] self._scene.add_box(name, p, (dimensions[0], dimensions[1], dimensions[2])) - print "============ Waiting while RVIZ displays the scene with four obstacles..." - rospy.sleep(1) - + if __name__ == "__main__": - rospy.init_node("collision_scene_1") + rospy.init_node("collision_cluttered_scene") while not rospy.search_param('robot_description_semantic') and not rospy.is_shutdown(): time.sleep(0.5) load_scene = CreateClutteredScene() diff --git a/doc/chomp_interface/scripts/collision_scene_example_sparse.py b/doc/chomp_interface/scripts/collision_scene_example_sparse.py index 8862fc474..bc91306fe 100755 --- a/doc/chomp_interface/scripts/collision_scene_example_sparse.py +++ b/doc/chomp_interface/scripts/collision_scene_example_sparse.py @@ -14,6 +14,7 @@ def __init__(self): self.robot = RobotCommander() # pause to wait for rviz to load + print "============ Waiting while RVIZ displays the scene with one obstacle..." rospy.sleep(4) box2_pose = [0.25, 0.25, 0.0, 0, 0, 0, 1] @@ -33,12 +34,10 @@ def add_box_object(self, name, dimensions, pose): p.pose.orientation.z = pose[5] p.pose.orientation.w = pose[6] self._scene.add_box(name, p, (dimensions[0], dimensions[1], dimensions[2])) - print "============ Waiting while RVIZ displays the scene with one obstacle..." - rospy.sleep(1) if __name__ == "__main__": - rospy.init_node("collision_scene_2") + rospy.init_node("collision_sparse_scene") while not rospy.search_param('robot_description_semantic') and not rospy.is_shutdown(): time.sleep(0.5) - load_scene = CreateScene() + load_scene = CreateSparseScene() rospy.spin() From df76ba8e2cb806a3ecef43fae65674b14dc84847 Mon Sep 17 00:00:00 2001 From: Raghavender Sahdev Date: Fri, 8 Jun 2018 15:09:46 -0400 Subject: [PATCH 10/16] minor bug changed the file name --- doc/chomp_interface/chomp_interface_tutorial.rst | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/doc/chomp_interface/chomp_interface_tutorial.rst b/doc/chomp_interface/chomp_interface_tutorial.rst index 6ad0e5cbd..9f0d1dffa 100644 --- a/doc/chomp_interface/chomp_interface_tutorial.rst +++ b/doc/chomp_interface/chomp_interface_tutorial.rst @@ -65,9 +65,9 @@ Testing CHOMP with Obstacles in the Scene +++++++++++++++++++++++++++++++++++++++++ To test CHOMP in an evironment with obstacles, you can run any of the sample python scripts: -- `collision_scene_cluttered.py <./scripts/collision_scene_example_cluttered.py>`_ or +- `collision_scene_example_cluttered.py <./scripts/collision_scene_example_cluttered.py>`_ or -- `collision_scene_sparse.py <./scripts/collision_scene_example_sparse.py>`_. +- `collision_scene_example_sparse.py <./scripts/collision_scene_example_sparse.py>`_. The first scripts creates a cluttered scene with four ostacles. The second script creates a simple scene with one obstacle. One can change the position/size of the obstacles to change the scene. From b804fd5e7474005c50a18e3ba3ead13a9576b5dc Mon Sep 17 00:00:00 2001 From: Raghavender Sahdev Date: Mon, 11 Jun 2018 01:38:49 -0400 Subject: [PATCH 11/16] changed class-names to be same as file-name --- .../scripts/collision_scene_example_cluttered.py | 6 +++--- .../scripts/collision_scene_example_sparse.py | 8 ++++---- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/doc/chomp_interface/scripts/collision_scene_example_cluttered.py b/doc/chomp_interface/scripts/collision_scene_example_cluttered.py index f10f77389..e181b2b49 100755 --- a/doc/chomp_interface/scripts/collision_scene_example_cluttered.py +++ b/doc/chomp_interface/scripts/collision_scene_example_cluttered.py @@ -5,7 +5,7 @@ import time -class CreateClutteredScene(object): +class CollisionSceneExampleCluttered(object): def __init__(self): self._scene = PlanningSceneInterface() @@ -55,8 +55,8 @@ def add_box_object(self, name, dimensions, pose): self._scene.add_box(name, p, (dimensions[0], dimensions[1], dimensions[2])) if __name__ == "__main__": - rospy.init_node("collision_cluttered_scene") + rospy.init_node("collision_scene_example_cluttered") while not rospy.search_param('robot_description_semantic') and not rospy.is_shutdown(): time.sleep(0.5) - load_scene = CreateClutteredScene() + load_scene = CollisionSceneExampleCluttered() rospy.spin() diff --git a/doc/chomp_interface/scripts/collision_scene_example_sparse.py b/doc/chomp_interface/scripts/collision_scene_example_sparse.py index bc91306fe..6c0e4f40c 100755 --- a/doc/chomp_interface/scripts/collision_scene_example_sparse.py +++ b/doc/chomp_interface/scripts/collision_scene_example_sparse.py @@ -4,7 +4,7 @@ import geometry_msgs.msg import time -class CreateSparseScene(object): +class CollisionSceneExampleSparse(object): def __init__(self): self._scene = PlanningSceneInterface() @@ -18,7 +18,7 @@ def __init__(self): rospy.sleep(4) box2_pose = [0.25, 0.25, 0.0, 0, 0, 0, 1] - box2_dimensions = [0.25, 0.25, 0.45] + box2_dimensions = [0.25, 0.25, 0.75] self.add_box_object("box2", box2_dimensions, box2_pose) @@ -36,8 +36,8 @@ def add_box_object(self, name, dimensions, pose): self._scene.add_box(name, p, (dimensions[0], dimensions[1], dimensions[2])) if __name__ == "__main__": - rospy.init_node("collision_sparse_scene") + rospy.init_node("collision_scene_exmaple_sparse") while not rospy.search_param('robot_description_semantic') and not rospy.is_shutdown(): time.sleep(0.5) - load_scene = CreateSparseScene() + load_scene = CollisionSceneExampleSparse() rospy.spin() From 1bcf99078118b5644d2eab5a327269e022c7202a Mon Sep 17 00:00:00 2001 From: Raghavender Sahdev Date: Mon, 11 Jun 2018 23:52:30 -0400 Subject: [PATCH 12/16] addressed PR requested changes for python script and tutorial document --- .../scripts/collision_scene_example_sparse.py | 43 ------------------ .../chomp.png | Bin .../chomp_planner_tutorial.rst} | 16 +++---- .../scripts/collision_scene_example.py} | 37 +++++++++++---- 4 files changed, 35 insertions(+), 61 deletions(-) delete mode 100755 doc/chomp_interface/scripts/collision_scene_example_sparse.py rename doc/{chomp_interface => chomp_planner}/chomp.png (100%) rename doc/{chomp_interface/chomp_interface_tutorial.rst => chomp_planner/chomp_planner_tutorial.rst} (93%) rename doc/{chomp_interface/scripts/collision_scene_example_cluttered.py => chomp_planner/scripts/collision_scene_example.py} (68%) diff --git a/doc/chomp_interface/scripts/collision_scene_example_sparse.py b/doc/chomp_interface/scripts/collision_scene_example_sparse.py deleted file mode 100755 index 6c0e4f40c..000000000 --- a/doc/chomp_interface/scripts/collision_scene_example_sparse.py +++ /dev/null @@ -1,43 +0,0 @@ -#!/usr/bin/env python -import rospy -from moveit_commander import RobotCommander, PlanningSceneInterface -import geometry_msgs.msg -import time - -class CollisionSceneExampleSparse(object): - - def __init__(self): - self._scene = PlanningSceneInterface() - - # clear the scene - self._scene.remove_world_object() - self.robot = RobotCommander() - - # pause to wait for rviz to load - print "============ Waiting while RVIZ displays the scene with one obstacle..." - rospy.sleep(4) - - box2_pose = [0.25, 0.25, 0.0, 0, 0, 0, 1] - box2_dimensions = [0.25, 0.25, 0.75] - - self.add_box_object("box2", box2_dimensions, box2_pose) - - def add_box_object(self, name, dimensions, pose): - p = geometry_msgs.msg.PoseStamped() - p.header.frame_id = self.robot.get_planning_frame() - p.header.stamp = rospy.Time.now() - p.pose.position.x = pose[0] - p.pose.position.y = pose[1] - p.pose.position.z = pose[2] - p.pose.orientation.x = pose[3] - p.pose.orientation.y = pose[4] - p.pose.orientation.z = pose[5] - p.pose.orientation.w = pose[6] - self._scene.add_box(name, p, (dimensions[0], dimensions[1], dimensions[2])) - -if __name__ == "__main__": - rospy.init_node("collision_scene_exmaple_sparse") - while not rospy.search_param('robot_description_semantic') and not rospy.is_shutdown(): - time.sleep(0.5) - load_scene = CollisionSceneExampleSparse() - rospy.spin() diff --git a/doc/chomp_interface/chomp.png b/doc/chomp_planner/chomp.png similarity index 100% rename from doc/chomp_interface/chomp.png rename to doc/chomp_planner/chomp.png diff --git a/doc/chomp_interface/chomp_interface_tutorial.rst b/doc/chomp_planner/chomp_planner_tutorial.rst similarity index 93% rename from doc/chomp_interface/chomp_interface_tutorial.rst rename to doc/chomp_planner/chomp_planner_tutorial.rst index 9f0d1dffa..bf61d70ac 100644 --- a/doc/chomp_interface/chomp_interface_tutorial.rst +++ b/doc/chomp_planner/chomp_planner_tutorial.rst @@ -61,15 +61,13 @@ If you have the ``panda_moveit_config`` from the `ros-planning/panda_moveit_conf roslaunch panda_moveit_config demo_chomp.launch -Testing CHOMP with Obstacles in the Scene +Running CHOMP with Obstacles in the Scene +++++++++++++++++++++++++++++++++++++++++ -To test CHOMP in an evironment with obstacles, you can run any of the sample python scripts: +To run CHOMP in an evironment with obstacles, you can run the sample python script: -- `collision_scene_example_cluttered.py <./scripts/collision_scene_example_cluttered.py>`_ or + `collision_scene_example.py <./scripts/collision_scene_example.py>`_ -- `collision_scene_example_sparse.py <./scripts/collision_scene_example_sparse.py>`_. - -The first scripts creates a cluttered scene with four ostacles. The second script creates a simple scene with one obstacle. One can change the position/size of the obstacles to change the scene. +This scripts creates a cluttered scene with four ostacles or a simple scene with one obstacle depending on the argument given to the script. One can also change the position/size of the obstacles to change the scene. To run the CHOMP planner with obstacles, open two shells. In the first shell start RViz and wait for everything to finish loading: :: @@ -77,11 +75,13 @@ To run the CHOMP planner with obstacles, open two shells. In the first shell sta In the second shell, run either of the two commands: :: - rosrun moveit_tutorials collision_scene_example_cluttered.py + rosrun moveit_tutorials collision_scene_example.py cluttered or: :: - rosrun moveit_tutorials collision_scene_example_sparse.py + rosrun moveit_tutorials collision_scene_example.py sparse + +Next, in RViz, select CHOMP in the MotionPlanning pannel under the Context tab. Set the desired start and goal states by moving the end-effector around with the imarker and then click on the Plan button under the Planning tab in the MotionPlanning pannel to start planning. The planner will now attempt to find a feasible solution between the given start and end position. Tweaking some of the parameters for CHOMP ----------------------------------------- diff --git a/doc/chomp_interface/scripts/collision_scene_example_cluttered.py b/doc/chomp_planner/scripts/collision_scene_example.py similarity index 68% rename from doc/chomp_interface/scripts/collision_scene_example_cluttered.py rename to doc/chomp_planner/scripts/collision_scene_example.py index e181b2b49..57457fb4d 100755 --- a/doc/chomp_interface/scripts/collision_scene_example_cluttered.py +++ b/doc/chomp_planner/scripts/collision_scene_example.py @@ -3,9 +3,9 @@ from moveit_commander import RobotCommander, PlanningSceneInterface import geometry_msgs.msg import time +import sys - -class CollisionSceneExampleCluttered(object): +class CollisionSceneExample(object): def __init__(self): self._scene = PlanningSceneInterface() @@ -17,11 +17,19 @@ def __init__(self): # pause to wait for rviz to load print "============ Waiting while RVIZ displays the scene with four obstacles..." - rospy.sleep(4) + + # TODO: need to replace this sleep by explicitly waiting for the scene to be updated. + rospy.sleep(2) + + def add_one_box(self): + box1_pose = [0.25, 0.25, 0.0, 0, 0, 0, 1] + box1_dimensions = [0.25, 0.25, 0.75] - floor_pose = [0, 0, -1.12, 0, 0, 0, 1] - floor_dimensions = [0, 0, 0.0] + self.add_box_object("box1", box1_dimensions, box1_pose) + + print "============ Added one obstacle to RViz!!" + def add_four_boxes(self): box1_pose = [0.20, 0.50, 0.25, 0, 0, 0, 1] box1_dimensions = [0.2, 0.2, 0.5] @@ -34,12 +42,13 @@ def __init__(self): box4_pose = [-0.4, 0.4, 0.5, 0, 0, 0, 1] box4_dimensions = [0.25, 0.25, 1.1] - self.add_box_object("floor", floor_dimensions, floor_pose) self.add_box_object("box1", box1_dimensions, box1_pose) self.add_box_object("box2", box2_dimensions, box2_pose) self.add_box_object("box3", box3_dimensions, box3_pose) self.add_box_object("box4", box4_dimensions, box4_pose) - + + print "========== Added 4 obstacles to the scene!!" + def add_box_object(self, name, dimensions, pose): p = geometry_msgs.msg.PoseStamped() p.header.frame_id = self.robot.get_planning_frame() @@ -52,11 +61,19 @@ def add_box_object(self, name, dimensions, pose): p.pose.orientation.z = pose[5] p.pose.orientation.w = pose[6] - self._scene.add_box(name, p, (dimensions[0], dimensions[1], dimensions[2])) - + self._scene.add_box(name, p, (dimensions[0], dimensions[1], dimensions[2])) + if __name__ == "__main__": rospy.init_node("collision_scene_example_cluttered") while not rospy.search_param('robot_description_semantic') and not rospy.is_shutdown(): time.sleep(0.5) - load_scene = CollisionSceneExampleCluttered() + load_scene = CollisionSceneExample() + + if sys.argv[1] == "cluttered": + load_scene.add_four_boxes(); + elif sys.argv[1] == "sparse": + load_scene.add_one_box(); + else: + print "Please specify correct type of scene as cluttered or sparse" + exit() rospy.spin() From bdf3cce95e2fe712b49dc5b2bb474f417956bbab Mon Sep 17 00:00:00 2001 From: Raghavender Sahdev Date: Tue, 12 Jun 2018 00:11:21 -0400 Subject: [PATCH 13/16] addition of correct number of arguments check --- doc/chomp_planner/scripts/collision_scene_example.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/doc/chomp_planner/scripts/collision_scene_example.py b/doc/chomp_planner/scripts/collision_scene_example.py index 57457fb4d..c75637653 100755 --- a/doc/chomp_planner/scripts/collision_scene_example.py +++ b/doc/chomp_planner/scripts/collision_scene_example.py @@ -69,11 +69,14 @@ def add_box_object(self, name, dimensions, pose): time.sleep(0.5) load_scene = CollisionSceneExample() + if (len(sys.argv) != 2): + print "Correct usage:: \n\"rosrun moveit_tutorials collision_scene_example.py cluttered\" OR \n\"rosrun moveit_tutorials collision_scene_example.py sparse\"" + sys.exit() if sys.argv[1] == "cluttered": load_scene.add_four_boxes(); elif sys.argv[1] == "sparse": load_scene.add_one_box(); else: print "Please specify correct type of scene as cluttered or sparse" - exit() + sys.exit() rospy.spin() From ee26935cb871320e39ff8429ec7e02fb28ec8760 Mon Sep 17 00:00:00 2001 From: Raghavender Sahdev Date: Mon, 18 Jun 2018 23:22:31 -0400 Subject: [PATCH 14/16] added chomp_planner into index.rst and removed ros spin call --- doc/chomp_planner/scripts/collision_scene_example.py | 1 - index.rst | 2 +- 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/doc/chomp_planner/scripts/collision_scene_example.py b/doc/chomp_planner/scripts/collision_scene_example.py index c75637653..13c940e77 100755 --- a/doc/chomp_planner/scripts/collision_scene_example.py +++ b/doc/chomp_planner/scripts/collision_scene_example.py @@ -79,4 +79,3 @@ def add_box_object(self, name, dimensions, pose): else: print "Please specify correct type of scene as cluttered or sparse" sys.exit() - rospy.spin() diff --git a/index.rst b/index.rst index 929c8e198..c6749f4de 100644 --- a/index.rst +++ b/index.rst @@ -66,7 +66,7 @@ Configuration doc/kinematics_configuration/kinematics_configuration_tutorial doc/custom_constraint_samplers/custom_constraint_samplers_tutorial doc/ompl_interface/ompl_interface_tutorial - doc/chomp_interface/chomp_interface_tutorial + doc/chomp_planner/chomp_planner_tutorial Miscellaneous ---------------------------- From ab5a3062d6c406b4a1ded5d4cbf2c78287cb77b9 Mon Sep 17 00:00:00 2001 From: Raghavender Sahdev Date: Tue, 19 Jun 2018 00:36:45 -0400 Subject: [PATCH 15/16] fixed print statement typo --- doc/chomp_planner/scripts/collision_scene_example.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/chomp_planner/scripts/collision_scene_example.py b/doc/chomp_planner/scripts/collision_scene_example.py index 13c940e77..8edffb4ce 100755 --- a/doc/chomp_planner/scripts/collision_scene_example.py +++ b/doc/chomp_planner/scripts/collision_scene_example.py @@ -16,7 +16,7 @@ def __init__(self): self.robot = RobotCommander() # pause to wait for rviz to load - print "============ Waiting while RVIZ displays the scene with four obstacles..." + print "============ Waiting while RVIZ displays the scene with obstacles..." # TODO: need to replace this sleep by explicitly waiting for the scene to be updated. rospy.sleep(2) From 237c3d8b8daa67cdab4c735f16dcdd93d32de0b5 Mon Sep 17 00:00:00 2001 From: Raghavender Sahdev Date: Tue, 19 Jun 2018 14:19:47 -0400 Subject: [PATCH 16/16] fixed travis build errors --- doc/chomp_planner/chomp_planner_tutorial.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/chomp_planner/chomp_planner_tutorial.rst b/doc/chomp_planner/chomp_planner_tutorial.rst index bf61d70ac..ddedb7da3 100644 --- a/doc/chomp_planner/chomp_planner_tutorial.rst +++ b/doc/chomp_planner/chomp_planner_tutorial.rst @@ -65,7 +65,7 @@ Running CHOMP with Obstacles in the Scene +++++++++++++++++++++++++++++++++++++++++ To run CHOMP in an evironment with obstacles, you can run the sample python script: - `collision_scene_example.py <./scripts/collision_scene_example.py>`_ + :codedir:`collision_scene_example.py`. This scripts creates a cluttered scene with four ostacles or a simple scene with one obstacle depending on the argument given to the script. One can also change the position/size of the obstacles to change the scene.