diff --git a/config/chomp_planning.yaml b/config/chomp_planning.yaml new file mode 100644 index 0000000000000000000000000000000000000000..176c0d49ed61d3c1d469ae10066dd33531f16d86 --- /dev/null +++ b/config/chomp_planning.yaml @@ -0,0 +1,25 @@ +planning_time_limit: 10.0 +max_iterations: 200 +max_iterations_after_collision_free: 5 +smoothness_cost_weight: 0.1 +obstacle_cost_weight: 1.0 +learning_rate: 0.01 +animate_path: true +add_randomness: false +smoothness_cost_velocity: 0.0 +smoothness_cost_acceleration: 1.0 +smoothness_cost_jerk: 0.0 +hmc_discretization: 0.01 +hmc_stochasticity: 0.01 +hmc_annealing_factor: 0.99 +use_hamiltonian_monte_carlo: false +ridge_factor: 0.0 +use_pseudo_inverse: false +pseudo_inverse_ridge_factor: 1e-4 +animate_endeffector: false +animate_endeffector_segment: "panda_rightfinger" +joint_update_limit: 0.1 +collision_clearence: 0.2 +collision_threshold: 0.07 +random_jump_amount: 1.0 +use_stochastic_descent: true diff --git a/launch/chomp_planning_pipeline.launch.xml b/launch/chomp_planning_pipeline.launch.xml new file mode 100644 index 0000000000000000000000000000000000000000..ffbc72ea63ba52bbbeb1b3dc8fbb4ecd7700e73e --- /dev/null +++ b/launch/chomp_planning_pipeline.launch.xml @@ -0,0 +1,15 @@ +<launch> + + <!-- CHOMP Plugin for MoveIt! --> + <arg name="planning_plugin" value="chomp_interface/CHOMPPlanner" /> + + <arg name="start_state_max_bounds_error" value="0.1" /> + + <param name="planning_plugin" value="$(arg planning_plugin)" /> + <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" /> + + <param name="collision_detector" value="Hybrid" /> + + <rosparam command="load" file="$(find panda_moveit_config)/config/chomp_planning.yaml" /> + +</launch> diff --git a/launch/demo_chomp.launch b/launch/demo_chomp.launch new file mode 100644 index 0000000000000000000000000000000000000000..78815d9dd477d64d5eef479a67256c6ea622eb33 --- /dev/null +++ b/launch/demo_chomp.launch @@ -0,0 +1,58 @@ +<launch> + + <!-- By default, we do not start a database (it can be large) --> + <arg name="db" default="false" /> + <!-- Allow user to specify database location --> + <arg name="db_path" default="$(find panda_moveit_config)/default_warehouse_mongo_db" /> + + <!-- By default, we are not in debug mode --> + <arg name="debug" default="false" /> + + <!-- + By default, hide joint_state_publisher's GUI + + MoveIt!'s "demo" mode replaces the real robot driver with the joint_state_publisher. + The latter one maintains and publishes the current joint configuration of the simulated robot. + It also provides a GUI to move the simulated robot around "manually". + This corresponds to moving around the real robot without the use of MoveIt. + --> + <arg name="use_gui" default="false" /> + + <!-- Load the URDF, SRDF and other .yaml configuration files on the param server --> + <include file="$(find panda_moveit_config)/launch/planning_context.launch"> + <arg name="load_robot_description" value="true"/> + </include> + + <!-- If needed, broadcast static tf for robot root --> + + + <!-- We do not have a robot connected, so publish fake joint states --> + <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher"> + <param name="/use_gui" value="$(arg use_gui)"/> + <rosparam param="/source_list">[/move_group/fake_controller_joint_states]</rosparam> + </node> + + <!-- Given the published joint states, publish tf for the robot links --> + <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" /> + + <!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) --> + <include file="$(find panda_moveit_config)/launch/move_group.launch"> + <arg name="allow_trajectory_execution" value="true"/> + <arg name="fake_execution" value="true"/> + <arg name="info" value="true"/> + <arg name="debug" value="$(arg debug)"/> + <arg name="planner" value="chomp" /> + </include> + + <!-- Run Rviz and load the default config to see the state of the move_group node --> + <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch"> + <arg name="config" value="true"/> + <arg name="debug" value="$(arg debug)"/> + </include> + + <!-- If database loading was enabled, start mongodb as well --> + <include file="$(find panda_moveit_config)/launch/default_warehouse_db.launch" if="$(arg db)"> + <arg name="moveit_warehouse_database_path" value="$(arg db_path)"/> + </include> + +</launch> diff --git a/launch/move_group.launch b/launch/move_group.launch index 3030e368be0e816cb53720966cae13e045408fe4..d2c63b56753c18f2902fe422eb4cf2508aabc434 100644 --- a/launch/move_group.launch +++ b/launch/move_group.launch @@ -2,6 +2,8 @@ <include file="$(find panda_moveit_config)/launch/planning_context.launch" /> + <arg name="planner" default="ompl" /> + <!-- GDB Debug Option --> <arg name="debug" default="false" /> <arg unless="$(arg debug)" name="launch_prefix" value="" /> @@ -22,7 +24,7 @@ <!-- Planning Functionality --> <include ns="move_group" file="$(find panda_moveit_config)/launch/planning_pipeline.launch.xml"> - <arg name="pipeline" value="ompl" /> + <arg name="pipeline" value="$(arg planner)" /> </include> <!-- Trajectory Execution Functionality --> diff --git a/package.xml b/package.xml index a364a429fd16995e56c2c238d3c4089368a99b01..09098fa90b13227d3589c52b6fdc37d7d34bb8df 100644 --- a/package.xml +++ b/package.xml @@ -20,6 +20,7 @@ <run_depend>moveit_fake_controller_manager</run_depend> <run_depend>moveit_kinematics</run_depend> <run_depend>moveit_planners_ompl</run_depend> + <run_depend>moveit_planners_chomp</run_depend> <run_depend>moveit_ros_visualization</run_depend> <run_depend>joint_state_publisher</run_depend> <run_depend>robot_state_publisher</run_depend>