motion_planners: chomp_motion_planner | ompl | ompl_planning | ompl_ros | ompl_search | sbpl
Package Summary
CHOMP is a motion planner based on trajectory optimization. It generates smooth, collision-free trajectories. For technical, algorithmic details, please refer to: "CHOMP: Gradient Optimization Techniques for Efficient Motion Planning". Nathan Ratliff, Matthew Zucker, J. Andrew Bagnell anhd Siddhartha Srinivasa. IEEE International Conference on Robotics and Automation, May 2009.
- Author: Mrinal Kalakrishnan / mail@mrinal.net
- License: BSD
- Repository: wg-ros-pkg (https://code.ros.org/svn/wg-ros-pkg)
This package implements the motion planner interface that the move_arm package requires. This interface is described in detail in the arm navigation tutorials. To use CHOMP, simply launch the arm_navigation_pr2/launch/planning/chomp_planning.launch file instead of any of the other planners.
Video
Development Notes
- Main class - chomp_planner_node.cpp
- takes only joint space goals
- generates the initial straight line trajectory
- sends it to chomp_optimizer class along with
- chomp_robot_model.cpp - CHOMP specific
- class automatically loads URDF and creates a robot model
- also creates its own collision model using configurations from a config file
- config/chomp_pr2_right_arm.yaml
- name of the link, radius of the link (radius of the cylinder that forms the collision model for that link)
- link_extension - hack to make the gripper longer
- used to create the sphere models - length of cylinder comes from joint-joint distance
- config/chomp_pr2_right_arm.yaml
- chomp_collision_space.cpp - create CHOMP specific collision environment
- special numbers
collision clearance: 0.15 (how much clearance from obstacles) - used to specify the collision cost, cost = 0 when outside of link > collision clearance distance from obstacle.
- joint costs: weighting factors - higher number penalizes motion of that joint
- joint velocity limits : not using the ones from the URDF since they are pretty high
- reference_frame - used for everything - also the reference frame in which collision map exists, also where the kinematics are performed
- collision space parameters - defines the size of the collision space and its resolution - origin is specified in reference_frame
- cuboids - to fill up static pre-defined obstacles completely - e.g. used for the base
- special numbers
ChompPlanningGroup - chomp_robot_model.cpp
- specifies whether right arm, left arm, etc.
- contains all the parameters that the optimizer uses
- joint update limit - max amount of update per iteration - controls the rate of the optimization, should be small enough to be linear within the region around this point.
- contains all the parameters that the optimizer uses
- chomp_robot_model.cpp - CHOMP specific
- chomp_optimizer class
- input - initial trajectory
- output - optimized, collision free trajectories
- chomp_cost_server - creates a collision space and returns costs for configurations
- collision space will give you the cartesian gradient - for any point in the environment
- iterate over links in robot model, iterate over point on links - add up the costs to get cost for link
- Also gets the gradient - use the robot model to convert this to joint space gradient - done in the robot model
- in chomp_optimizer - (line 313) getJacobian which needs joint position and joint axes, get that from the forward kinematics in the robot model (treefksolverjointposaxis.cpp, treefksolverjointposaxis_partial.cpp - solves for only one of the planning groups)
- make sure you get the right indexing from the planning group to KDL for kinematics
- in chomp_optimizer - (line 313) getJacobian which needs joint position and joint axes, get that from the forward kinematics in the robot model (treefksolverjointposaxis.cpp, treefksolverjointposaxis_partial.cpp - solves for only one of the planning groups)
- RIGHT NOW REFERENCE FRAME HAS TO BE A FRAME ON THE ROBOT