Added demos for collision avoidance using CHOMP planner #173

raghavendersahdev · 2018-05-25T01:09:22Z

This pull requests has a few collision avoidance demo scripts for CHOMP.

davetcoleman

Great changes!

davetcoleman · 2018-05-29T04:44:22Z

@mlautman please review. Note Travis seems to be failing because of Ruby again

davetcoleman · 2018-05-29T04:44:59Z

Also @raghavendersahdev I would reword the title of your PR in the future to say "demos" instead of "tests" since there is no formal test coverage / integration tests / unit tests

simonschmeisser · 2018-05-30T12:07:39Z

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. 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.


MoveIt! instead of moveIt, "as CHOMP is not part of the official release yet. It is therefore not included in the binary packages."

mlautman · 2018-05-31T15:30:26Z

doc/chomp_interface/chomp_interface_tutorial.rst

-#. Open the ``move_group.launch`` file in your ``<robot_moveit_config>/launch/`` folder and make two changes. First, add ``<arg name="planner" default="ompl" />`` just under the ``<launch>`` tag and second, within the ``<include ns="move_group">`` tag replace ``<arg name="pipeline" value="ompl" />`` with ``<arg name="pipeline" value="$(arg planner)" />``.
+#. Open the ``move_group.launch`` file in your ``<robot_moveit_config>/launch/`` folder and make two changes.
+
+ 7.1. First, add ``<arg name="planner" default="ompl" />`` just under the ``<launch>`` tag, and,


What is the reason for these sub-indices? It seems to me that they should each be additional steps (#.) rather than hardcoded numbers

I requested they be subindices because previously there were multiple steps in the same sentence. I thought it would be nice since they both are edits to one file, but I don't feel strongly about this.

Makes sense. I still don't like mixing auto-generated numbers with hardcoded numbers. If there is a way to do subindices in the markup that would be ideal

@raghavendersahdev please see "mixed list" https://en.support.wordpress.com/markdown-quick-reference/

mlautman · 2018-05-31T15:42:41Z

doc/chomp_interface/chomp_interface_tutorial.rst

@@ -1,10 +1,10 @@
-CHOMP Interface
+CHOMP Planner


In the future it is best to branch off of <distro>-devel on your fork before making changes and opening a PR. This makes it possible to work on multiple features simultaneously and provides additional context to the people reviewing your code. The MoveIt! contributing page makes the recommendation that you name your branch pr-<ros distribution>-<keyword description>

Thanks @mlautman , I just rebased the PR to reflect the latest commits. Is that fine?

If you are changing the name of the tutorial, please also change the filenames and directory names to match. See the contributing guidelines I've been working on for reference: https://github.com/ros-planning/moveit_tutorials/pull/179/files

mlautman · 2018-05-31T16:20:43Z

doc/chomp_interface/chomp_interface_tutorial.rst

+
+  roslaunch panda_moveit_config demo_chomp.launch
+  python collision_scene_test_1.py OR python collision_scene_test_2.py


rosrun for python

@raghavendersahdev

mlautman · 2018-05-31T16:20:45Z

doc/chomp_interface/collision_scene_test_1.py

@@ -0,0 +1,60 @@
+import rospy


Please maintain the directory structure moveit_tutorials/doc/<package>/scripts/<python_code.py>. See move_group_python_interface for how to organize the directory structure

mlautman · 2018-05-31T16:23:37Z

doc/chomp_interface/collision_scene_test_1.py

+        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)


Please avoid using sleeps. See the python move group tutorial for code demonstrating how to wait for the the scene to update.

mlautman · 2018-05-31T16:24:05Z

doc/chomp_interface/collision_scene_test_2.py

+import time
+
+
+class CreateScene1(object):


CreateScene

mlautman · 2018-05-31T16:24:18Z

doc/chomp_interface/collision_scene_test_1.py

+import time
+
+
+class CreateScene1(object):


CreateScene

did you push the changes?

mlautman · 2018-05-31T16:24:54Z

doc/chomp_interface/collision_scene_test_2.py

+        box2_dimensions = [0.25, 0.25, 1.75]
+
+        self.add_box_object("box2", box2_dimensions, box2_pose)
+


one line in between functions within a class

mlautman · 2018-05-31T16:25:08Z

doc/chomp_interface/collision_scene_test_2.py

+        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)


davetcoleman · 2018-06-04T18:29:15Z

doc/chomp_interface/chomp_interface_tutorial.rst

+
+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 <https://github.com/ros-planning/panda_moveit_config/blob/master/config/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 -> "CHOMP"

"The following are some..."

davetcoleman · 2018-06-04T18:30:17Z

doc/chomp_interface/chomp_interface_tutorial.rst

+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. 


can you move the code snippets into their own line, instead of in-line with paragraph?

davetcoleman · 2018-06-07T15:13:45Z

doc/chomp_interface/chomp_interface_tutorial.rst

-#. Open the ``move_group.launch`` file in your ``<robot_moveit_config>/launch/`` folder and make two changes. First, add ``<arg name="planner" default="ompl" />`` just under the ``<launch>`` tag and second, within the ``<include ns="move_group">`` tag replace ``<arg name="pipeline" value="ompl" />`` with ``<arg name="pipeline" value="$(arg planner)" />``.
+#. Open the ``move_group.launch`` file in your ``<robot_moveit_config>/launch/`` folder and make two changes.
+
+ 7.1. First, add ``<arg name="planner" default="ompl" />`` just under the ``<launch>`` tag, and,


@raghavendersahdev please see "mixed list" https://en.support.wordpress.com/markdown-quick-reference/

davetcoleman · 2018-06-07T15:13:58Z

doc/chomp_interface/chomp_interface_tutorial.rst

+
+  roslaunch panda_moveit_config demo_chomp.launch
+  python collision_scene_test_1.py OR python collision_scene_test_2.py


@raghavendersahdev

davetcoleman · 2018-06-07T15:14:10Z

doc/chomp_interface/chomp_interface_tutorial.rst

+ - `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. 


remove space at start of sentence

removed the space at the start of the sentence. "The first scripts...."

davetcoleman · 2018-06-07T15:14:28Z

doc/chomp_interface/chomp_interface_tutorial.rst

+Testing CHOMP with Obstacles in the Scene
+-----------------------------------------
+To test CHOMP in an evironment with obstacles, you can run any of the sample python scripts 


end sentence with : ::

if I end this sentence with : :: the formatting for the bullets below this line gets messed up like:

Currently I just end it with :

davetcoleman · 2018-06-07T15:14:46Z

doc/chomp_interface/chomp_interface_tutorial.rst

+
+
+Tweaking some of the parameters for CHOMP


Capitalize consistently

davetcoleman · 2018-06-07T15:15:06Z

doc/chomp_interface/chomp_interface_tutorial.rst

+- *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.


"Parameters" -> "parameters"

davetcoleman · 2018-06-07T15:15:17Z

doc/chomp_interface/chomp_interface_tutorial.rst

+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.
+
+


remove double line break

davetcoleman · 2018-06-07T15:15:28Z

doc/chomp_interface/collision_scene_test_1.py

+import time
+
+
+class CreateScene1(object):


did you push the changes?

mlautman · 2018-06-08T16:44:16Z

doc/chomp_interface/chomp_interface_tutorial.rst

+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


Broken links

mlautman · 2018-06-08T16:47:58Z

doc/chomp_interface/chomp_interface_tutorial.rst

+
+Testing CHOMP with Obstacles in the Scene
+-----------------------------------------


This should be a subsection. "++++++++++++++++++++"

done and pushed the changes

mlautman · 2018-06-08T17:32:27Z

doc/chomp_interface/chomp_interface_tutorial.rst

+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: ::


For consistency with the other tutorials can you reword to:

Open two shells. In the first shell start RViz and wait for everything to finish loading: ::

roslaunch panda_moveit_config demo_chomp.launch

In the second shell, run either of the two commands: ::

rosrun moveit_tutorials collision_scene_test_1.py

or: ::

rosrun moveit_tutorials collision_scene_test_2.py

done and pushed the changes

mlautman · 2018-06-08T17:42:55Z

doc/chomp_interface/chomp_interface_tutorial.rst

+
+  roslaunch panda_moveit_config demo_chomp.launch
+  rosrun moveit_tutorials collision_scene_test_1.py OR rosrun moveit_tutorials collision_scene_test_2.py


Can you rename these something more descriptive. EG: chomp_collision_scene_example_cluttered.py and chomp_collision_scene_example_sparse.py

done and pushed changes

mlautman · 2018-06-08T17:43:21Z

doc/chomp_interface/chomp_interface_tutorial.rst

+- *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


mlautman · 2018-06-08T17:44:44Z

doc/chomp_interface/scripts/collision_scene_test_1.py

+
+        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)


Please don't use sleep. See the move_group_python_tutorial for explicitly waiting for the ps to update

Thanks for the feedback @mlautman , I removed this particular sleep usage, but I still got the sleep usage over here. to show the user some sort of terminal output while loading the scene.The move_group_python_tutorial for pr2 also used sleep like :

mlautman · 2018-06-08T17:45:04Z

doc/chomp_interface/scripts/collision_scene_test_2.py

+        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..."


same as above

mlautman · 2018-06-08T17:45:11Z

doc/chomp_interface/scripts/collision_scene_test_2.py

+        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)


same as above

replied for sleep as above.

mlautman · 2018-06-08T17:45:31Z

doc/chomp_interface/scripts/collision_scene_test_2.py

+import geometry_msgs.msg
+import time
+
+class CreateScene(object):


better class name. Why is this different from the above file?

changed class name to CreateSparseScene should I keep the classname same as the file name or is this fine as is, filename = collision_scene_example_sparse.py?

currently have class-name same as file-name

mlautman · 2018-06-08T17:45:42Z

doc/chomp_interface/scripts/collision_scene_test_1.py

+import time
+
+
+class CreateScene(object):


better class name.

changed class name to CreateClutteredScene should I keep the classname same as the file name or is this fine as is, filename = collision_scene_example_cluttered.py?

davetcoleman · 2018-06-11T19:18:14Z

See Travis build errors:


- ./build/html/doc/chomp_interface/chomp_interface_tutorial.html
  *  internally linking to ./scripts/collision_scene_example_cluttered.py, which does not exist (line 601)
     <a class="reference external" href="./scripts/collision_scene_example_cluttered.py">collision_scene_example_cluttered.py</a>
  *  internally linking to ./scripts/collision_scene_example_sparse.py, which does not exist (line 602)
     <a class="reference external" href="./scripts/collision_scene_example_sparse.py">collision_scene_example_sparse.py</a>
htmlproofer 3.9.1 | Error:  HTML-Proofer found 2 failures!

mlautman

very close!

mlautman · 2018-06-11T17:35:53Z

doc/chomp_interface/chomp_interface_tutorial.rst

@@ -1,10 +1,10 @@
-CHOMP Interface
+CHOMP Planner


If you are changing the name of the tutorial, please also change the filenames and directory names to match. See the contributing guidelines I've been working on for reference: https://github.com/ros-planning/moveit_tutorials/pull/179/files

mlautman · 2018-06-11T17:36:56Z

doc/chomp_interface/chomp_interface_tutorial.rst

+  roslaunch panda_moveit_config demo_chomp.launch
+
+Testing CHOMP with Obstacles in the Scene


Nit: Change 'Testing' -> 'Running' CHOMP as these aren't really tests

mlautman · 2018-06-11T17:37:53Z

doc/chomp_interface/chomp_interface_tutorial.rst

+Testing CHOMP with Obstacles in the Scene
+++++++++++++++++++++++++++++++++++++++++
+To test CHOMP in an evironment with obstacles, you can run any of the sample python scripts:


Nit: Change 'test' -> 'run' CHOMP as these aren't really tests.

mlautman · 2018-06-11T17:45:38Z

doc/chomp_interface/scripts/collision_scene_example_cluttered.py

+
+        # pause to wait for rviz to load        
+        print "============ Waiting while RVIZ displays the scene with four obstacles..."
+        rospy.sleep(4)


Is this sleep necessary?

Add another print to let the user know that they are good to start planning once the scene has been updated

Hi Mike, if I don't put any sleep, the obstacles do not show up in RViz, hence I used a sleep over here, I could reduce it to rospy.sleep(2), are there any fundamental issues for not using sleep here?

Sleep in this context is a hack that masks the bigger issue. Instead of blindly sleeping, this demo script should wait for the correct topics to be available before publishing to the planning scene interface and it should wait for those changes to be reflected in the planning scene before returning.

Waiting an arbitrary number of seconds is not an approach that we want to encourage among the users of MoveIt

I tried to use the way its done in the move_group_python_interface tutorial, but have not been able to do so with any success, will look into it again tomorrow. Here is the file
collision_scene_example.py.txt

that shows how I am doing it, it does not update the scene, spent a lot of time on it but no success.

mlautman · 2018-06-11T20:02:03Z

doc/chomp_interface/scripts/collision_scene_example_cluttered.py

+        print "============ Waiting while RVIZ displays the scene with four obstacles..."
+        rospy.sleep(4)
+
+        floor_pose = [0, 0, -1.12, 0, 0, 0, 1]


From here on should be a separate method not part of the init.

mlautman · 2018-06-11T20:02:30Z

doc/chomp_interface/scripts/collision_scene_example_sparse.py

+
+        # pause to wait for rviz to load
+        print "============ Waiting while RVIZ displays the scene with one obstacle..."
+        rospy.sleep(4)


same as above

mlautman · 2018-06-11T20:04:17Z

doc/chomp_interface/scripts/collision_scene_example_sparse.py

+import geometry_msgs.msg
+import time
+
+class CollisionSceneExampleSparse(object):


This is a ton of code duplication. You could achieve this in a single script that takes in a command line argument telling it which scene to load

removed duplicate code and merged into single file and behavior now depends on command line argument ("cluttered" or "sparse")

mlautman · 2018-06-11T20:06:48Z

doc/chomp_interface/chomp_interface_tutorial.rst

+
+  rosrun moveit_tutorials collision_scene_example_sparse.py
+


Add instructions telling the user to select CHOMP in the MotionPlanning pannel under the Context tab.
You will also want to tell them to move the endeffector around with the imarker and hit plan

mlautman

My apologies that this is taking so long to merge. Please make the requested changes and we should be good to merge after one last review.

mlautman · 2018-06-19T00:03:36Z

doc/chomp_interface/chomp_interface_tutorial.rst

@@ -1,58 +0,0 @@
-CHOMP Interface


Note: If you use git mv rather than mv when renaming these files then git does a better job of tracking your changes vs the original.

Also, you need to test with the build_locally.sh script before each push. You didn't rename the tutorial in index.rst so the chomp tutorial isn't getting built

done. I tested the build_locally.sh script and renamed the CHOMP tutorial in the index.rst file.

mlautman · 2018-06-19T00:05:36Z

doc/chomp_planner/scripts/collision_scene_example.py

+        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();


mlautman · 2018-06-19T00:13:27Z

doc/chomp_planner/scripts/collision_scene_example.py

+    else:
+        print "Please specify correct type of scene as cluttered or sparse"
+        sys.exit()
+    rospy.spin()


This node does not need to spin. It should die after it has loaded the scene. Please see my earlier comments about waiting for updates in the planning scene.

thanks @mlautman yes this makes sense, no need to call spin. I removed spin call here.

… tutorial

…truction commit

…ts edited

mlautman · 2018-06-19T17:11:21Z

see travis build failure

raghavendersahdev · 2018-06-19T19:24:51Z

@mlautman travis build still fails but I feel once the PR gets merged it should run as its trying to currently find the following 2 files at this error message: https://travis-ci.org/ros-planning/moveit_tutorials/builds/394232202#L1199 Please let me know your thoughts on this.
Once it gets merged this should be fixed right, as it would be able to find the two files:

davetcoleman · 2018-06-20T15:52:35Z

Yes this is a known limitation of our current travis setup - if you add new files / html pages it complains that page is not available on the live website. Would be nice if someone could address.

@mlautman I think we should merge this

davetcoleman · 2018-06-20T15:53:56Z

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


@mlautman lets no do any more changes in the PR, this one is fine, but in the future I think you should avoid asking people to rename folders and filenames in these tutorials, as that breaks hyperlinks on a live website which should be generally avoided

I agree about merging. With regard to filenames and directories, I think that it's important that we maintain the standards in the contributing guidelines. That said, this tutorial could have kept the name chomp_interface_tutorial and I wouldn't have said anything. In the future, I'll probably just suggest that the name of the tutorial stays the same rather than suggest that the file and dir name are also changed to match.

* added tests for running CHOMP in scenes with obstacles into the CHOMP tutorial * added section on Tweaking some of the parameters for CHOMP_under-construction commit * added: CHOMP parameters, difference in plans OMPL/CHOMP, python scripts edited * corrected scripts link * fixed formatting issues * PR changes requested * add_randomness is not used in the CHOMP code * addressed the PR changes requested * fixed python script bugs, removed sleep * minor bug changed the file name * changed class-names to be same as file-name * addressed PR requested changes for python script and tutorial document * addition of correct number of arguments check * added chomp_planner into index.rst and removed ros spin call * fixed print statement typo * fixed travis build errors

raghavendersahdev mentioned this pull request May 25, 2018

Added tests for collision avoidance using CHOMP planner moveit/panda_moveit_config#2

Closed

davetcoleman approved these changes May 29, 2018

View reviewed changes

raghavendersahdev changed the title ~~Added tests for collision avoidance using CHOMP planner~~ Added demos for collision avoidance using CHOMP planner May 29, 2018

raghavendersahdev mentioned this pull request May 30, 2018

Add official support for CHOMP moveit/moveit#702

Closed

simonschmeisser reviewed May 30, 2018

View reviewed changes

mlautman suggested changes May 31, 2018

View reviewed changes

davetcoleman requested changes Jun 4, 2018

View reviewed changes

davetcoleman reviewed Jun 7, 2018

View reviewed changes

mlautman suggested changes Jun 8, 2018

View reviewed changes

davetcoleman approved these changes Jun 11, 2018

View reviewed changes

mlautman suggested changes Jun 11, 2018

View reviewed changes

mlautman suggested changes Jun 19, 2018

View reviewed changes

raghavendersahdev added 15 commits June 18, 2018 23:24

added tests for running CHOMP in scenes with obstacles into the CHOMP…

c1c3ea2

… tutorial

added section on Tweaking some of the parameters for CHOMP_under-cons…

9720788

…truction commit

added: CHOMP parameters, difference in plans OMPL/CHOMP, python scrip…

9077fd9

…ts edited

corrected scripts link

2e2b27d

fixed formatting issues

b144d6c

PR changes requested

5d747ce

add_randomness is not used in the CHOMP code

d7cfd8a

addressed the PR changes requested

928ce99

fixed python script bugs, removed sleep

be0a37f

minor bug changed the file name

df76ba8

changed class-names to be same as file-name

b804fd5

addressed PR requested changes for python script and tutorial document

1bcf990

addition of correct number of arguments check

bdf3cce

added chomp_planner into index.rst and removed ros spin call

ee26935

fixed print statement typo

ab5a306

fixed travis build errors

237c3d8

davetcoleman reviewed Jun 20, 2018

View reviewed changes

mlautman approved these changes Jun 20, 2018

View reviewed changes

davetcoleman merged commit af968de into moveit:kinetic-devel Jun 20, 2018


		roslaunch panda_moveit_config demo_chomp.launch
		python collision_scene_test_1.py OR python collision_scene_test_2.py

		box2_dimensions = [0.25, 0.25, 1.75]

		self.add_box_object("box2", box2_dimensions, box2_pose)

		- `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.

		- 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.

		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.

		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


		Testing CHOMP with Obstacles in the Scene
		-----------------------------------------

		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
		rosrun moveit_tutorials collision_scene_test_1.py OR rosrun moveit_tutorials collision_scene_test_2.py

		- 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

Added demos for collision avoidance using CHOMP planner #173

Added demos for collision avoidance using CHOMP planner #173

Conversation

raghavendersahdev commented May 25, 2018 • edited

davetcoleman left a comment

Choose a reason for hiding this comment

davetcoleman commented May 29, 2018

davetcoleman commented May 29, 2018

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

raghavendersahdev Jun 8, 2018 • edited

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

raghavendersahdev Jun 8, 2018 • edited

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

raghavendersahdev Jun 8, 2018 • edited

Choose a reason for hiding this comment

davetcoleman commented Jun 11, 2018

mlautman left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

raghavendersahdev commented May 25, 2018 •

edited

raghavendersahdev Jun 8, 2018 •

edited

raghavendersahdev Jun 8, 2018 •

edited

raghavendersahdev Jun 8, 2018 •

edited