{"id":18488098,"url":"https://github.com/cpaxton/grid_planning","last_synced_at":"2025-06-28T11:33:00.766Z","repository":{"id":151500753,"uuid":"58165466","full_name":"cpaxton/grid_planning","owner":"cpaxton","description":"Integrated task and motion planning using grounded actions learned from observing expert demonstrations.","archived":false,"fork":false,"pushed_at":"2017-02-25T23:12:14.000Z","size":29230,"stargazers_count":6,"open_issues_count":0,"forks_count":0,"subscribers_count":3,"default_branch":"master","last_synced_at":"2025-03-23T17:51:28.769Z","etag":null,"topics":["cross-entropy","lfd","machine-learning","motion-planning","robotics","ros","tree-search","wam"],"latest_commit_sha":null,"homepage":null,"language":"C++","has_issues":true,"has_wiki":null,"has_pages":null,"mirror_url":null,"source_name":null,"license":null,"status":null,"scm":"git","pull_requests_enabled":true,"icon_url":"https://github.com/cpaxton.png","metadata":{"files":{"readme":"Readme.md","changelog":null,"contributing":null,"funding":null,"license":null,"code_of_conduct":null,"threat_model":null,"audit":null,"citation":null,"codeowners":null,"security":null,"support":null,"governance":null,"roadmap":null,"authors":null,"dei":null,"publiccode":null,"codemeta":null}},"created_at":"2016-05-05T22:50:01.000Z","updated_at":"2022-01-11T08:48:09.000Z","dependencies_parsed_at":"2023-05-18T01:46:21.472Z","dependency_job_id":null,"html_url":"https://github.com/cpaxton/grid_planning","commit_stats":null,"previous_names":[],"tags_count":17,"template":false,"template_full_name":null,"repository_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/cpaxton%2Fgrid_planning","tags_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/cpaxton%2Fgrid_planning/tags","releases_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/cpaxton%2Fgrid_planning/releases","manifests_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/cpaxton%2Fgrid_planning/manifests","owner_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/owners/cpaxton","download_url":"https://codeload.github.com/cpaxton/grid_planning/tar.gz/refs/heads/master","host":{"name":"GitHub","url":"https://github.com","kind":"github","repositories_count":247923307,"owners_count":21018968,"icon_url":"https://github.com/github.png","version":null,"created_at":"2022-05-30T11:31:42.601Z","updated_at":"2022-07-04T15:15:14.044Z","host_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub","repositories_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories","repository_names_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repository_names","owners_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/owners"}},"keywords":["cross-entropy","lfd","machine-learning","motion-planning","robotics","ros","tree-search","wam"],"created_at":"2024-11-06T12:51:14.354Z","updated_at":"2025-06-28T11:33:00.760Z","avatar_url":"https://github.com/cpaxton.png","language":"C++","funding_links":[],"categories":[],"sub_categories":[],"readme":"# GRID PLAN\n\nChris Paxton\n\nThis is the GRID-PLAN project (Grounded Robot Instruction from Demonstrations-Planning). The package has code for learning and planning based on human user demonstrations. Unfortunately it's prototype code and a little slow, but maybe it will be helpful for you. If you find this work interesting, please check out the paper and cite as:\n\n```\n@inproceedings{paxton2016want,\n  title={Do what I want, not what I did: Imitation of skills by planning sequences of actions},\n  author={Paxton, Chris and Jonathan, Felix and Kobilarov, Marin and Hager, Gregory D},\n  booktitle={Intelligent Robots and Systems (IROS), 2016 IEEE/RSJ International Conference on},\n  pages={3778--3785},\n  year={2016},\n  organization={IEEE}\n}\n```\n\nInitially this project relies on segmented and labeled human demonstrations.\n\nStart the UR5 with:\n\n```\nroslaunch simple_ur_driver simple_ur_pd.launch \n```\n\n\n## Installation\n\nThis code requires:\n  * Boost\n  * ROS (tested on Indigo)\n  * Orocos KDL (download a new version)\n  * ``scikit-learn`` for different mixture models (python only!)\n  * ``pyhsmm`` for HDP-HMM learning (used in some sexperiments; may not be necessray for most things!)\n  * Dynamic Movement Primitives ROS package\n  * Gazebo\n  * The ``lcsr_assembly`` stack\n\nTo run the experiments, you'll need my ```grid_experiments``` ROS packages, as well as a working version of the ```lcsr_barrett``` simulation.\n\n## Package Overview\n\n  * __grid_tasks__: This package defines different situations.\n  * __grid_robots__: This package defines and launches robots, and helps configure them for different situations.\n  * __grid_experiments__: This contains launch files bringing up a whole test, with robots and objects.\n\n\nIn particular, __grid_experiments__ contains a few notable launch files:\n\n  - **ascent_grasping** starts the assembly task.\n  - **ascent_experiments** brings up particular combinations of parts for the assembly task.\n  - **simple_pegs** brings up the peg-manipulation task.\n\n## Experiments\n\n### Collecting Data\n\nData collection requires a proper configuration of the environment. You're grounding all of the different actions for particular objects, right?\n\nExamples are configured to collect gripper data for the Barrett WAM arms. So just start with ``hydra:=true`` and then run:\n```\nrosrun grid_plan demonstrate.py myaction1.yml\n```\n\nYou can then either use the python (prototype) code or the C++ code.\nThe C++ code is an attempt to build a more fully integrated version of the pipeline, faster and with more features. Jury is out on whether that worked, but it seems ideal because of the large numbers of loops and such. The C++ code reads in rosbag files, which you can create from these demonstration YAML files with:\n\n```\nrosrun grid_plan convert_yml_to_rosbag.py /path/to/training/data/\n```\n\nOf course you replace ``/path/to/training/data/`` with a path to your examples.\n\n### Running Tests\n\nThere are a lot of ways you can run the different experiments provided.\n\nThe Execution test will let you connect two different skills. An example is this:\n\n```\nrosrun grid_plan execution_test _step_size:=0.65 _iter:=10 _ntrajs:=50 _noise:=1e-10 trajectory:=/gazebo/traj_rml/joint_traj_cmd _skill:=approach\n```\n\nThis command runs with the Barrett simulation. It'll tell the arm to approach, with the goal being to grasp.\n\n```\nrosrun grid_plan execution_test _step_size:=0.75 _iter:=10 _ntrajs:=55 _noise:=1e-10 trajectory:=/gazebo/traj_rml/joint_traj_cmd _skill:=disengage\n```\n\nThis one does something a little different. It tries to disengage from a link, and will move to a position a bit farther back away from it.\n\nNote that in both of these examples I set a large number of different parameters. You really don't need to worry about these. They matter a lot more depending on how many samples you're making. As my code is not heavily parallelized, it can be inefficient to create task plans for lots of different examples at once.\n\nRunning the DMP tests is a little different. The basic DMP is actually in joint space, not cartesian space. This means that the trajectories you find will deal with joint limits, singularities, etc., but the configuration space is huge and unintuitive. So we need to search more slowly and draw a lot more samples. Here's an example:\n\n```\nrosrun grid_plan dmp_execution_test _step_size:=0.25 _iter:=50 _ntrajs:=100 _noise:=1e-10 trajectory:=/gazebo/traj_rml/joint_traj_cmd _skill:=approach\n```\n\nResults, for example:\n\n```\n[45] \u003e\u003e\u003e\u003e AVG P = 0.020318\n[46] \u003e\u003e\u003e\u003e AVG P = 0.0203147\n[47] \u003e\u003e\u003e\u003e AVG P = 0.0229661\n[48] \u003e\u003e\u003e\u003e AVG P = 0.0238406\n[49] \u003e\u003e\u003e\u003e AVG P = 0.0240259\n```\n\nAs of 2016-01-15, some that worked:\n\n```\nrosrun grid_plan dmp_execution_test _step_size:=0.75 _iter:=20 _ntrajs:=50 _noise:=1e-10 trajectory:=/gazebo/traj_rml/joint_traj_cmd _skill:=disengage\nrosrun grid_plan dmp_execution_test _step_size:=0.75 _iter:=20 _ntrajs:=50 _noise:=1e-10 trajectory:=/gazebo/traj_rml/joint_traj_cmd _skill:=approach\nrosrun grid_plan dmp_execution_test _step_size:=0.75 _iter:=20 _ntrajs:=50 _noise:=1e-10 trajectory:=/gazebo/traj_rml/joint_traj_cmd _skill:=prepare\n```\n\nThe __prepare__ action is intended to move to the beginning of an approach.\n\nThe __task_test__ node actually executes task planning, and has even more options to consider:\n\n```\nrosrun grid_plan task_test _step_size:=0.75 _iter:=70 _ntrajs:=75 _verbosity:=0 _starting_horizon:=4 _max_horizon:=5 _update_horizon:=0.1\n```\n\nWe may want to set the number of actions we want to start thinking about (root, approach, grasp, and align with ```_starting_horizon:=4``` as above). Or we might want to adjust how confident we need to be to add another action to the skill tree.\n\n### Enabling Collision Detection\n\nCollision detection is disabled by default for now, but you can enable it in any particular node by setting the __detect_collisions__ flag to true. For example:\n\n```\nrosrun grid_plan dmp_execution_test _step_size:=0.75 _iter:=10 _ntrajs:=20 _noise:=1e-10 trajectory:=/gazebo/traj_rml/joint_traj_cmd _skill:=approach _wait:=0.25 _detect_collisions:=1\n```\n\n### Other Notes\n\nThe model normalization term is really important to getting strong results. Setting it to a nonzero value is one of the several ways we prevent the distribution over trajectories from converging too quickly: \"bad\" examples in early iterations may still be helpful for guiding the distribution towards an optimal goal.\n\nFor example, here's a set of parameters that seems to work pretty well as of 2016-02-07 for the assembly task:\n\n```\nrosrun grid_plan task_test _step_size:=0.75 _iter:=50 _ntrajs:=50 _verbosity:=0 _starting_horizon:=5 _max_horizon:=5 _update_horizon:=0.001 _detect_collisions:=true _wait:=0 _base_model_norm:=0.1\n```\n\nAs of 2016-02-09, I have been removing/testing the removal of a lot of the random noise parameters I have included in this file. These were things like the \"base model norm\" and \"model norm step.\" Go ahead and remove those in future executions of the task test:\n\n```\nrosrun grid_plan task_test _step_size:=0.5 _iter:=20 _ntrajs:=100  _starting_horizon:=5 _max_horizon:=5 _update_horizon:=0.01 _detect_collisions:=true _base_model_norm:=0.01 _model_norm_step:=1\n```\n\nAs of 2016-02-11, more changes have made the configuration slightly different. We have options to enable things like verbosity for the collision checker.\n\nOne thing that makes a big difference is the model normalization. See here:\n\n```\nrosrun grid_plan task_test _step_size:=0.5 _iter:=25 _ntrajs:=200 _starting_horizon:=5 _max_horizon:=5 _detect_collisions:=false _wait:=0 _collisions_verbose:=false _base_model_norm:=0.1 _model_norm_step=1 _update_horizon:=0.01\n```\n\nSetting the normalization higher makes convergence harder; setting it lower makes convergence easier (if you find examples). This is actually bad, though: fast convergence means you have a higher chance to end up in a local minimum.\n\nAs of 2016-02-17, this was the best set of commands with the 1-Gaussian code:\n\n```\nrosrun grid_plan task_test _step_size:=0.5 _iter:=25 _ntrajs:=200 _starting_horizon:=5 _max_horizon:=5 _detect_collisions:=false _wait:=0 _collisions_verbose:=false _base_model_norm:=0.1 _model_norm_step=1 _update_horizon:=0.01\nrosrun grid_plan task_test _step_size:=0.5 _iter:=25 _ntrajs:=200 _starting_horizon:=5 _max_horizon:=5 _detect_collisions:=true _wait:=0 _collisions_verbose:=false _base_model_norm:=0.1 _model_norm_step=1 _update_horizon:=0.01\n```\n\nYou should be able to see a roughly exponential improvement:\n\n```\n[5] align \u003e\u003e\u003e\u003e AVG P = 1.2851\n[4] grasp \u003e\u003e\u003e\u003e AVG P = 0.00642552\n[5] align \u003e\u003e\u003e\u003e AVG P = 268.966\n[4] grasp \u003e\u003e\u003e\u003e AVG P = 1.34483\n[5] align \u003e\u003e\u003e\u003e AVG P = 3533.92\n[4] grasp \u003e\u003e\u003e\u003e AVG P = 17.6696\n[5] align \u003e\u003e\u003e\u003e AVG P = 6778.76\n[4] grasp \u003e\u003e\u003e\u003e AVG P = 33.8938\n[5] align \u003e\u003e\u003e\u003e AVG P = 62099.1\n[4] grasp \u003e\u003e\u003e\u003e AVG P = 310.496\n[5] align \u003e\u003e\u003e\u003e AVG P = 88452.6\n[4] grasp \u003e\u003e\u003e\u003e AVG P = 442.263\n[5] align \u003e\u003e\u003e\u003e AVG P = 335451\n[4] grasp \u003e\u003e\u003e\u003e AVG P = 1677.25\n[5] align \u003e\u003e\u003e\u003e AVG P = 561695\n[4] grasp \u003e\u003e\u003e\u003e AVG P = 2808.47\n[5] align \u003e\u003e\u003e\u003e AVG P = 807009\n[4] grasp \u003e\u003e\u003e\u003e AVG P = 4035.04\n[5] align \u003e\u003e\u003e\u003e AVG P = 1.13222e+06\n[4] grasp \u003e\u003e\u003e\u003e AVG P = 5661.09\n[5] align \u003e\u003e\u003e\u003e AVG P = 1.35106e+06\n[4] grasp \u003e\u003e\u003e\u003e AVG P = 6755.32\n[5] align \u003e\u003e\u003e\u003e AVG P = 1.49062e+06\n[4] grasp \u003e\u003e\u003e\u003e AVG P = 7453.1\n[5] align \u003e\u003e\u003e\u003e AVG P = 1.56917e+06\n[4] grasp \u003e\u003e\u003e\u003e AVG P = 7845.86\n[5] align \u003e\u003e\u003e\u003e AVG P = 1.6093e+06\n[4] grasp \u003e\u003e\u003e\u003e AVG P = 8046.48\n[5] align \u003e\u003e\u003e\u003e AVG P = 1.63996e+06\n[4] grasp \u003e\u003e\u003e\u003e AVG P = 8199.78\n\n```\n\n### Current Commands\n\nGood places to start experiments:\n\n```\nrosrun grid_plan task_test _step_size:=0.5 _iter:=25 _ntrajs:=200 _starting_horizon:=5 _max_horizon:=5 _detect_collisions:=true _wait:=0 _collisions_verbose:=false _base_model_norm:=0.001 _model_norm_step=1 _update_horizon:=0.01\nrosrun grid_plan task_test _step_size:=0.5 _iter:=15 _ntrajs:=200 _starting_horizon:=5 _max_horizon:=5 _detect_collisions:=true _wait:=0 _collisions_verbose:=false _base_model_norm:=0.0001 _model_norm_step=1 _update_horizon:=0.0001\nrosrun grid_plan task_test _step_size:=0.5 _iter:=15 _ntrajs:=100 _starting_horizon:=5 _max_horizon:=5 _detect_collisions:=true _wait:=0 _collisions_verbose:=false _base_model_norm:=0.001 _model_norm_step=1 _update_horizon:=0.0001 _replan_depth:=0 _execute_depth:=5 _collision_detection_step:=2 _fixed_distribution_noise:=true _test:=0\n```\n\nNote the decreased model norm from the previous version. I changed back to adding a higher norm after switching to k=3 components. As of 2016-02-24, I am making \"place\" have k=2 since one of the recorded examples was bad.\n\n## Guide to Files\n\n### Executables\n\nThe ***trajectory_test*** exectable generates a set of random trajectories into the pose of the \"node\" object. So of course this will only work if you are running the assembly simulation.\n\n### Directory Structure\n\n#### Config\n\nRVIZ configuration files for debugging.\n\n#### Data\n\nStores files containing demonstrated trajectories in YAML and Rosbag formats. Subfolders contain demonstrations recorded on different platforms.\n\n#### Include\n\nContains the C++ headers for the produced library.\n\n#### Python\n\nThis directory contains grid-planning python code, for collecting demonstrations and for prototyping LfD.\nPython dependencies are changing, but may include:\n  * ``scikit-learn`` for different mixture models\n  * ``pyhsmm`` for HDP-HMM learning\n\n#### Launch\n\nLaunch files are used to either (1) bring up tests for different things, or (2) set up the configuration for skills at run-time.\n\n##### Setup\n\nThese launch files set up the different skills that we want to look at during runtime.\n\n#### Scripts\n\nThis directory contains Python scripts.\n\nScripts will collect data based on examples, segment, and do other things. These let you more or less directly run the different demos.\n\nSome of the most important of these are:\n  * ``working_cpp.py`` and ``run_ur5.py`` for running simulation and UR5 experiments\n  * ``demonstrate.py`` and ``demonstrate_ur5.py`` for recording data\n  * ``create_skills.py`` and ``create_skills_ur5.py`` for creating skill models using the experimental (python) code\n  * ``convert_yml_to_rosbag.py`` for converting yml of examples into rosbag format to read via C++. I decided to go with rosbags for my example data mostly to reduce the number of external dependencies for the c++ code; I already am requiring ROS be installed as it is.\n\n#### Skills\n\nThis directory contains skill files in different formats, in particular as YAML and skill (custom) files.\n\n#### SRC\n\nThis directory contains C++ code for planning.\n\nC++ code includes/will include modifications to DMPs, possibly C++ cross-entropy code.\n","project_url":"https://awesome.ecosyste.ms/api/v1/projects/github.com%2Fcpaxton%2Fgrid_planning","html_url":"https://awesome.ecosyste.ms/projects/github.com%2Fcpaxton%2Fgrid_planning","lists_url":"https://awesome.ecosyste.ms/api/v1/projects/github.com%2Fcpaxton%2Fgrid_planning/lists"}