Skip to content

Instantly share code, notes, and snippets.

@aaronchongth
Created May 19, 2019 07:30
Show Gist options
  • Save aaronchongth/54ee50ba8115912db66b6027c57841f3 to your computer and use it in GitHub Desktop.
Save aaronchongth/54ee50ba8115912db66b6027c57841f3 to your computer and use it in GitHub Desktop.
MoveIt! CHOMP planning adapter with OMPL
aaron@ws:~/scratch/moveit_ws$ roslaunch panda_moveit_config demo.launch
... logging to /home/aaron/.ros/log/a5f6c914-7a07-11e9-86c0-7c67a2ef3c4c/roslaunch-ws-18017.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.
xacro: in-order processing became default in ROS Melodic. You can drop the option.
xacro: in-order processing became default in ROS Melodic. You can drop the option.
xacro: in-order processing became default in ROS Melodic. You can drop the option.
started roslaunch server http://ws:41395/
SUMMARY
========
PARAMETERS
* /joint_state_publisher/use_gui: False
* /move_group/add_randomness: False
* /move_group/allow_trajectory_execution: True
* /move_group/animate_endeffector: False
* /move_group/animate_endeffector_segment: panda_rightfinger
* /move_group/animate_path: True
* /move_group/collision_clearence: 0.2
* /move_group/collision_threshold: 0.07
* /move_group/controller_list: [{'joints': ['pan...
* /move_group/enable_failure_recovery: False
* /move_group/hand/planner_configs: ['SBLkConfigDefau...
* /move_group/hmc_annealing_factor: 0.99
* /move_group/hmc_discretization: 0.01
* /move_group/hmc_stochasticity: 0.01
* /move_group/initial: [{'pose': 'ready'...
* /move_group/jiggle_fraction: 0.05
* /move_group/joint_update_limit: 0.1
* /move_group/learning_rate: 0.01
* /move_group/max_iterations: 200
* /move_group/max_iterations_after_collision_free: 5
* /move_group/max_range: 5.0
* /move_group/max_recovery_attempts: 5
* /move_group/max_safe_path_cost: 1
* /move_group/moveit_controller_manager: moveit_fake_contr...
* /move_group/moveit_manage_controllers: True
* /move_group/obstacle_cost_weight: 1.0
* /move_group/octomap_frame: camera_rgb_optica...
* /move_group/octomap_resolution: 0.05
* /move_group/panda_arm/planner_configs: ['SBLkConfigDefau...
* /move_group/panda_arm_hand/planner_configs: ['SBLkConfigDefau...
* /move_group/planner_configs/BFMTkConfigDefault/balanced: 0
* /move_group/planner_configs/BFMTkConfigDefault/cache_cc: 1
* /move_group/planner_configs/BFMTkConfigDefault/extended_fmt: 1
* /move_group/planner_configs/BFMTkConfigDefault/heuristics: 1
* /move_group/planner_configs/BFMTkConfigDefault/nearest_k: 1
* /move_group/planner_configs/BFMTkConfigDefault/num_samples: 1000
* /move_group/planner_configs/BFMTkConfigDefault/optimality: 1
* /move_group/planner_configs/BFMTkConfigDefault/radius_multiplier: 1.0
* /move_group/planner_configs/BFMTkConfigDefault/type: geometric::BFMT
* /move_group/planner_configs/BKPIECEkConfigDefault/border_fraction: 0.9
* /move_group/planner_configs/BKPIECEkConfigDefault/failed_expansion_score_factor: 0.5
* /move_group/planner_configs/BKPIECEkConfigDefault/min_valid_path_fraction: 0.5
* /move_group/planner_configs/BKPIECEkConfigDefault/range: 0.0
* /move_group/planner_configs/BKPIECEkConfigDefault/type: geometric::BKPIECE
* /move_group/planner_configs/BiESTkConfigDefault/range: 0.0
* /move_group/planner_configs/BiESTkConfigDefault/type: geometric::BiEST
* /move_group/planner_configs/BiTRRTkConfigDefault/cost_threshold: 1e300
* /move_group/planner_configs/BiTRRTkConfigDefault/frountier_node_ratio: 0.1
* /move_group/planner_configs/BiTRRTkConfigDefault/frountier_threshold: 0.0
* /move_group/planner_configs/BiTRRTkConfigDefault/init_temperature: 100
* /move_group/planner_configs/BiTRRTkConfigDefault/range: 0.0
* /move_group/planner_configs/BiTRRTkConfigDefault/temp_change_factor: 0.1
* /move_group/planner_configs/BiTRRTkConfigDefault/type: geometric::BiTRRT
* /move_group/planner_configs/ESTkConfigDefault/goal_bias: 0.05
* /move_group/planner_configs/ESTkConfigDefault/range: 0.0
* /move_group/planner_configs/ESTkConfigDefault/type: geometric::EST
* /move_group/planner_configs/FMTkConfigDefault/cache_cc: 1
* /move_group/planner_configs/FMTkConfigDefault/extended_fmt: 1
* /move_group/planner_configs/FMTkConfigDefault/heuristics: 0
* /move_group/planner_configs/FMTkConfigDefault/nearest_k: 1
* /move_group/planner_configs/FMTkConfigDefault/num_samples: 1000
* /move_group/planner_configs/FMTkConfigDefault/radius_multiplier: 1.1
* /move_group/planner_configs/FMTkConfigDefault/type: geometric::FMT
* /move_group/planner_configs/KPIECEkConfigDefault/border_fraction: 0.9
* /move_group/planner_configs/KPIECEkConfigDefault/failed_expansion_score_factor: 0.5
* /move_group/planner_configs/KPIECEkConfigDefault/goal_bias: 0.05
* /move_group/planner_configs/KPIECEkConfigDefault/min_valid_path_fraction: 0.5
* /move_group/planner_configs/KPIECEkConfigDefault/range: 0.0
* /move_group/planner_configs/KPIECEkConfigDefault/type: geometric::KPIECE
* /move_group/planner_configs/LBKPIECEkConfigDefault/border_fraction: 0.9
* /move_group/planner_configs/LBKPIECEkConfigDefault/min_valid_path_fraction: 0.5
* /move_group/planner_configs/LBKPIECEkConfigDefault/range: 0.0
* /move_group/planner_configs/LBKPIECEkConfigDefault/type: geometric::LBKPIECE
* /move_group/planner_configs/LBTRRTkConfigDefault/epsilon: 0.4
* /move_group/planner_configs/LBTRRTkConfigDefault/goal_bias: 0.05
* /move_group/planner_configs/LBTRRTkConfigDefault/range: 0.0
* /move_group/planner_configs/LBTRRTkConfigDefault/type: geometric::LBTRRT
* /move_group/planner_configs/LazyPRMkConfigDefault/range: 0.0
* /move_group/planner_configs/LazyPRMkConfigDefault/type: geometric::LazyPRM
* /move_group/planner_configs/LazyPRMstarkConfigDefault/type: geometric::LazyPR...
* /move_group/planner_configs/PDSTkConfigDefault/type: geometric::PDST
* /move_group/planner_configs/PRMkConfigDefault/max_nearest_neighbors: 10
* /move_group/planner_configs/PRMkConfigDefault/type: geometric::PRM
* /move_group/planner_configs/PRMstarkConfigDefault/type: geometric::PRMstar
* /move_group/planner_configs/ProjESTkConfigDefault/goal_bias: 0.05
* /move_group/planner_configs/ProjESTkConfigDefault/range: 0.0
* /move_group/planner_configs/ProjESTkConfigDefault/type: geometric::ProjEST
* /move_group/planner_configs/RRTConnectkConfigDefault/range: 0.0
* /move_group/planner_configs/RRTConnectkConfigDefault/type: geometric::RRTCon...
* /move_group/planner_configs/RRTkConfigDefault/goal_bias: 0.05
* /move_group/planner_configs/RRTkConfigDefault/range: 0.0
* /move_group/planner_configs/RRTkConfigDefault/type: geometric::RRT
* /move_group/planner_configs/RRTstarkConfigDefault/delay_collision_checking: 1
* /move_group/planner_configs/RRTstarkConfigDefault/goal_bias: 0.05
* /move_group/planner_configs/RRTstarkConfigDefault/range: 0.0
* /move_group/planner_configs/RRTstarkConfigDefault/type: geometric::RRTstar
* /move_group/planner_configs/SBLkConfigDefault/range: 0.0
* /move_group/planner_configs/SBLkConfigDefault/type: geometric::SBL
* /move_group/planner_configs/SPARSkConfigDefault/dense_delta_fraction: 0.001
* /move_group/planner_configs/SPARSkConfigDefault/max_failures: 1000
* /move_group/planner_configs/SPARSkConfigDefault/sparse_delta_fraction: 0.25
* /move_group/planner_configs/SPARSkConfigDefault/stretch_factor: 3.0
* /move_group/planner_configs/SPARSkConfigDefault/type: geometric::SPARS
* /move_group/planner_configs/SPARStwokConfigDefault/dense_delta_fraction: 0.001
* /move_group/planner_configs/SPARStwokConfigDefault/max_failures: 5000
* /move_group/planner_configs/SPARStwokConfigDefault/sparse_delta_fraction: 0.25
* /move_group/planner_configs/SPARStwokConfigDefault/stretch_factor: 3.0
* /move_group/planner_configs/SPARStwokConfigDefault/type: geometric::SPARStwo
* /move_group/planner_configs/STRIDEkConfigDefault/degree: 16
* /move_group/planner_configs/STRIDEkConfigDefault/estimated_dimension: 0.0
* /move_group/planner_configs/STRIDEkConfigDefault/goal_bias: 0.05
* /move_group/planner_configs/STRIDEkConfigDefault/max_degree: 18
* /move_group/planner_configs/STRIDEkConfigDefault/max_pts_per_leaf: 6
* /move_group/planner_configs/STRIDEkConfigDefault/min_degree: 12
* /move_group/planner_configs/STRIDEkConfigDefault/min_valid_path_fraction: 0.2
* /move_group/planner_configs/STRIDEkConfigDefault/range: 0.0
* /move_group/planner_configs/STRIDEkConfigDefault/type: geometric::STRIDE
* /move_group/planner_configs/STRIDEkConfigDefault/use_projected_distance: 0
* /move_group/planner_configs/TRRTkConfigDefault/frountierNodeRatio: 0.1
* /move_group/planner_configs/TRRTkConfigDefault/frountier_threshold: 0.0
* /move_group/planner_configs/TRRTkConfigDefault/goal_bias: 0.05
* /move_group/planner_configs/TRRTkConfigDefault/init_temperature: 10e-6
* /move_group/planner_configs/TRRTkConfigDefault/k_constant: 0.0
* /move_group/planner_configs/TRRTkConfigDefault/max_states_failed: 10
* /move_group/planner_configs/TRRTkConfigDefault/min_temperature: 10e-10
* /move_group/planner_configs/TRRTkConfigDefault/range: 0.0
* /move_group/planner_configs/TRRTkConfigDefault/temp_change_factor: 2.0
* /move_group/planner_configs/TRRTkConfigDefault/type: geometric::TRRT
* /move_group/planner_configs/TrajOptDefault/type: geometric::TrajOpt
* /move_group/planning_plugin: ompl_interface/OM...
* /move_group/planning_scene_monitor/publish_geometry_updates: True
* /move_group/planning_scene_monitor/publish_planning_scene: True
* /move_group/planning_scene_monitor/publish_state_updates: True
* /move_group/planning_scene_monitor/publish_transforms_updates: True
* /move_group/planning_time_limit: 10.0
* /move_group/pseudo_inverse_ridge_factor: 1e-4
* /move_group/random_jump_amount: 1.0
* /move_group/request_adapters: default_planner_r...
* /move_group/ridge_factor: 0.0
* /move_group/sensors: [{'point_subsampl...
* /move_group/smoothness_cost_acceleration: 1.0
* /move_group/smoothness_cost_jerk: 0.0
* /move_group/smoothness_cost_velocity: 0.0
* /move_group/smoothness_cost_weight: 0.1
* /move_group/start_state_max_bounds_error: 0.1
* /move_group/trajectory_execution/allowed_execution_duration_scaling: 1.2
* /move_group/trajectory_execution/allowed_goal_duration_margin: 0.5
* /move_group/trajectory_execution/allowed_start_tolerance: 0.01
* /move_group/trajectory_initialization_method: fillTrajectory
* /move_group/use_hamiltonian_monte_carlo: False
* /move_group/use_pseudo_inverse: False
* /move_group/use_stochastic_descent: True
* /robot_description: <?xml version="1....
* /robot_description_kinematics/panda_arm/kinematics_solver: kdl_kinematics_pl...
* /robot_description_kinematics/panda_arm/kinematics_solver_search_resolution: 0.005
* /robot_description_kinematics/panda_arm/kinematics_solver_timeout: 0.05
* /robot_description_planning/joint_limits/panda_finger_joint1/has_acceleration_limits: False
* /robot_description_planning/joint_limits/panda_finger_joint1/has_velocity_limits: True
* /robot_description_planning/joint_limits/panda_finger_joint1/max_acceleration: 0
* /robot_description_planning/joint_limits/panda_finger_joint1/max_velocity: 0.1
* /robot_description_planning/joint_limits/panda_finger_joint2/has_acceleration_limits: False
* /robot_description_planning/joint_limits/panda_finger_joint2/has_velocity_limits: True
* /robot_description_planning/joint_limits/panda_finger_joint2/max_acceleration: 0
* /robot_description_planning/joint_limits/panda_finger_joint2/max_velocity: 0.1
* /robot_description_planning/joint_limits/panda_joint1/has_acceleration_limits: True
* /robot_description_planning/joint_limits/panda_joint1/has_velocity_limits: True
* /robot_description_planning/joint_limits/panda_joint1/max_acceleration: 3.75
* /robot_description_planning/joint_limits/panda_joint1/max_velocity: 2.175
* /robot_description_planning/joint_limits/panda_joint2/has_acceleration_limits: True
* /robot_description_planning/joint_limits/panda_joint2/has_velocity_limits: True
* /robot_description_planning/joint_limits/panda_joint2/max_acceleration: 1.875
* /robot_description_planning/joint_limits/panda_joint2/max_velocity: 2.175
* /robot_description_planning/joint_limits/panda_joint3/has_acceleration_limits: True
* /robot_description_planning/joint_limits/panda_joint3/has_velocity_limits: True
* /robot_description_planning/joint_limits/panda_joint3/max_acceleration: 2.5
* /robot_description_planning/joint_limits/panda_joint3/max_velocity: 2.175
* /robot_description_planning/joint_limits/panda_joint4/has_acceleration_limits: True
* /robot_description_planning/joint_limits/panda_joint4/has_velocity_limits: True
* /robot_description_planning/joint_limits/panda_joint4/max_acceleration: 3.125
* /robot_description_planning/joint_limits/panda_joint4/max_velocity: 2.175
* /robot_description_planning/joint_limits/panda_joint5/has_acceleration_limits: True
* /robot_description_planning/joint_limits/panda_joint5/has_velocity_limits: True
* /robot_description_planning/joint_limits/panda_joint5/max_acceleration: 3.75
* /robot_description_planning/joint_limits/panda_joint5/max_velocity: 2.61
* /robot_description_planning/joint_limits/panda_joint6/has_acceleration_limits: True
* /robot_description_planning/joint_limits/panda_joint6/has_velocity_limits: True
* /robot_description_planning/joint_limits/panda_joint6/max_acceleration: 5
* /robot_description_planning/joint_limits/panda_joint6/max_velocity: 2.61
* /robot_description_planning/joint_limits/panda_joint7/has_acceleration_limits: True
* /robot_description_planning/joint_limits/panda_joint7/has_velocity_limits: True
* /robot_description_planning/joint_limits/panda_joint7/max_acceleration: 5
* /robot_description_planning/joint_limits/panda_joint7/max_velocity: 2.61
* /robot_description_semantic: <?xml version="1....
* /rosdistro: melodic
* /rosversion: 1.14.3
* /rviz_ws_18017_8840383221349738648/panda_arm/kinematics_solver: kdl_kinematics_pl...
* /rviz_ws_18017_8840383221349738648/panda_arm/kinematics_solver_search_resolution: 0.005
* /rviz_ws_18017_8840383221349738648/panda_arm/kinematics_solver_timeout: 0.05
* /source_list: ['/move_group/fak...
NODES
/
joint_state_desired_publisher (topic_tools/relay)
joint_state_publisher (joint_state_publisher/joint_state_publisher)
move_group (moveit_ros_move_group/move_group)
robot_state_publisher (robot_state_publisher/robot_state_publisher)
rviz_ws_18017_8840383221349738648 (rviz/rviz)
virtual_joint_broadcaster_1 (tf2_ros/static_transform_publisher)
auto-starting new master
process[master]: started with pid [18037]
ROS_MASTER_URI=http://localhost:11311
setting /run_id to a5f6c914-7a07-11e9-86c0-7c67a2ef3c4c
process[rosout-1]: started with pid [18048]
started core service [/rosout]
process[virtual_joint_broadcaster_1-2]: started with pid [18055]
process[joint_state_publisher-3]: started with pid [18056]
process[joint_state_desired_publisher-4]: started with pid [18058]
process[robot_state_publisher-5]: started with pid [18063]
process[move_group-6]: started with pid [18069]
process[rviz_ws_18017_8840383221349738648-7]: started with pid [18070]
[ INFO] [1558250889.403272725]: Loading robot model 'panda'...
[ INFO] [1558250889.451941951]: rviz version 1.13.3
[ INFO] [1558250889.451989546]: compiled against Qt version 5.9.5
[ INFO] [1558250889.451998253]: compiled against OGRE version 1.9.0 (Ghadamon)
[ INFO] [1558250889.478828149]: Publishing maintained planning scene on 'monitored_planning_scene'
[ INFO] [1558250889.480379191]: MoveGroup debug mode is ON
Starting planning scene monitors...
[ INFO] [1558250889.480432844]: Starting planning scene monitor
[ INFO] [1558250889.481610189]: Listening to '/planning_scene'
[ INFO] [1558250889.481632176]: Starting world geometry update monitor for collision objects, attached objects, octomap updates.
[ INFO] [1558250889.482952328]: Listening to '/collision_object'
[ INFO] [1558250889.485203889]: Listening to '/planning_scene_world' for planning scene world geometry
[ INFO] [1558250889.503169577]: Listening to '/camera/depth_registered/points' using message filter with target frame 'world '
[ INFO] [1558250889.507044415]: Listening to '/attached_collision_object' for attached collision objects
Planning scene monitors started.
[ INFO] [1558250889.532079072]: Initializing OMPL interface using ROS parameters
[ INFO] [1558250889.558135739]: Using planning interface 'OMPL'
[ INFO] [1558250889.560974549]: Param 'default_workspace_bounds' was not set. Using default value: 10
[ INFO] [1558250889.561383950]: Param 'start_state_max_bounds_error' was set to 0.1
[ INFO] [1558250889.561689250]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1558250889.562062135]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1558250889.562317496]: Param 'jiggle_fraction' was set to 0.05
[ INFO] [1558250889.562546770]: Param 'max_sampling_attempts' was not set. Using default value: 100
[ INFO] [1558250889.569509679]: Param pseudo_inverse_ridge_factor was not set. Using default value: 0.0001
[ INFO] [1558250889.569912860]: Param min_clearence was not set. Using default value: 0.2
[ INFO] [1558250889.570487591]: Using planning request adapter 'Fix Workspace Bounds'
[ INFO] [1558250889.570505504]: Using planning request adapter 'Fix Start State Bounds'
[ INFO] [1558250889.570514516]: Using planning request adapter 'Fix Start State In Collision'
[ INFO] [1558250889.570523909]: Using planning request adapter 'Fix Start State Path Constraints'
[ INFO] [1558250889.570532003]: Using planning request adapter 'CHOMP Optimizer'
[ INFO] [1558250889.570543204]: Using planning request adapter 'Add Time Parameterization'
[ INFO] [1558250889.576661944]: Loading robot model 'panda'...
[ INFO] [1558250889.609891854]: Set joints of group 'panda_arm_hand' to pose 'ready'.
[ INFO] [1558250889.610209171]: Fake controller 'fake_panda_arm_controller' with joints [ panda_joint1 panda_joint2 panda_joint3 panda_joint4 panda_joint5 panda_joint6 panda_joint7 ]
[ INFO] [1558250889.610583819]: Fake controller 'fake_hand_controller' with joints [ panda_finger_joint1 panda_finger_joint2 ]
[ INFO] [1558250889.610807265]: Returned 2 controllers in list
[ INFO] [1558250889.617495365]: Trajectory execution is managing controllers
Loading 'move_group/ApplyPlanningSceneService'...
Loading 'move_group/ClearOctomapService'...
Loading 'move_group/MoveGroupCartesianPathService'...
Loading 'move_group/MoveGroupExecuteTrajectoryAction'...
Loading 'move_group/MoveGroupGetPlanningSceneService'...
Loading 'move_group/MoveGroupKinematicsService'...
Loading 'move_group/MoveGroupMoveAction'...
Loading 'move_group/MoveGroupPickPlaceAction'...
Loading 'move_group/MoveGroupPlanService'...
Loading 'move_group/MoveGroupQueryPlannersService'...
Loading 'move_group/MoveGroupStateValidationService'...
[ INFO] [1558250889.656803613]:
********************************************************
* MoveGroup using:
* - ApplyPlanningSceneService
* - ClearOctomapService
* - CartesianPathService
* - ExecuteTrajectoryAction
* - GetPlanningSceneService
* - KinematicsService
* - MoveAction
* - PickPlaceAction
* - MotionPlanService
* - QueryPlannersService
* - StateValidationService
********************************************************
[ INFO] [1558250889.656896604]: MoveGroup context using planning plugin ompl_interface/OMPLPlanner
[ INFO] [1558250889.656948918]: MoveGroup context initialization complete
You can start planning now!
[ INFO] [1558250889.953485172]: Stereo is NOT SUPPORTED
[ INFO] [1558250889.954134287]: OpenGl version: 4.6 (GLSL 4.6).
[ INFO] [1558250890.114022026]: Loading robot model 'panda'...
[ INFO] [1558250890.383072072]: Loading robot model 'panda'...
[ WARN] [1558250890.406517954]: Kinematics solver doesn't support #attempts anymore, but only a timeout.
Please remove the parameter '/rviz_ws_18017_8840383221349738648/panda_arm/kinematics_solver_attempts' from your configuration.
[ INFO] [1558250890.443662134]: Starting planning scene monitor
[ INFO] [1558250890.445854739]: Listening to '/move_group/monitored_planning_scene'
[ INFO] [1558250902.856711614]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1558250902.860153028]: Planner configuration 'panda_arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1558250902.861347046]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1558250902.885576803]: RRTConnect: Created 5 states (2 start + 3 goal)
[ INFO] [1558250902.885672682]: Solution found in 0.024700 seconds
[ INFO] [1558250902.912626541]: SimpleSetup: Path simplification took 0.026778 seconds and changed from 4 to 2 states
[ INFO] [1558250902.914830781]: Configuring Planning Scene for CHOMP ....
[ERROR] [1558250903.065047347]: Start state is empty
[ INFO] [1558250909.156134690]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1558250909.157654371]: Planner configuration 'panda_arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1558250909.158029259]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1558250909.170709650]: RRTConnect: Created 4 states (2 start + 2 goal)
[ INFO] [1558250909.170811784]: Solution found in 0.012933 seconds
[ INFO] [1558250909.181791147]: SimpleSetup: Path simplification took 0.010887 seconds and changed from 3 to 2 states
[ INFO] [1558250909.183040093]: Configuring Planning Scene for CHOMP ....
[ERROR] [1558250909.332360584]: Start state is empty
@aaronchongth
Copy link
Author

I believe I tried to set it up to use OMPL's RRTConnect for an initial solution before using the CHOMP adapter for fine-tuning. I had the issue of it saying Start state is empty as seen on the last line. Sorry this was all quite long ago, and I haven't tried to continue on the effort.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment