Define the robot model and collision environment based on ROS parameters.
planning_environment is a library that allows users to instantiate robot models and collision models based on data from the parameter server with minimal user input. Additionally, state information for both robot models and collision environments can be monitored.
A URDF robot description is assumed to be loaded on the parameter server. The name of the parameter under which this description exists is taken as an argument to class constructors. Additionally, .yaml files describing the collision and planning information should be loaded on the parameter server under the same prefix as the robot description.
The collision.yaml file should define the follwing:
An example collision.yaml file:
## links for which collision checking with the environment should be performed collision_links: base_link torso_lift_link head_pan_link head_tilt_link laser_tilt_mount_link base_laser_link ... ## self collision is performed between groups of links; the names for the groups can be anything, but they must contain 'a' and 'b' as subgroups self_collision_groups: scg_r scg_l ## -- for right arm; self-collision if any link in 'a' collides with some link in 'b' scg_r: a: r_forearm_link r_gripper_palm_link r_gripper_l_finger_link r_gripper_r_finger_link r_gripper_l_finger_tip_link r_gripper_r_finger_tip_link b: base_link base_laser_link torso_lift_link laser_tilt_mount_link head_tilt_link ## -- for left arm; self-collision if any link in 'a' collides with some link in 'b' scg_l: a: l_forearm_link l_gripper_palm_link l_gripper_l_finger_link l_gripper_r_finger_link l_gripper_l_finger_tip_link l_gripper_r_finger_tip_link b: base_link base_laser_link torso_lift_link laser_tilt_mount_link head_tilt_link ## list of links that the robot can see with its sensors (used to remove ## parts of the robot from scanned data) self_see: r_upper_arm_link r_upper_arm_roll_link r_elbow_flex_link r_forearm_link r_forearm_roll_link ... ## The padding to be added for the body parts the robot can see self_see_padd: 0.1 ## The scaling to be considered for the body parts the robot can see self_see_scale: 1.0 ## The padding for the robot body parts to be considered when collision checking with the environment robot_padd: 0.01 ## The scaling for the robot body parts to be considered when collision checking with the environment robot_scale: 1.0
The planning.yaml file defines the groups of links/joints motion planning can be performed for and contains the following:
An example planning.yaml file:
## the list of groups for which motion planning can be performed
group_list:
base
left_arm
...
## the definition of each group
groups:
base:
links:
base_link
planner_configs:
RRTkConfig1 RRTdConfig1 SBLkConfig1 KPIECEkConfig1
left_arm:
links:
l_shoulder_pan_link
l_shoulder_lift_link
l_upper_arm_roll_link
l_upper_arm_link
l_elbow_flex_link
l_forearm_roll_link
l_forearm_link
l_wrist_flex_link
l_wrist_roll_link
planner_configs:
SBLkConfig2 LBKPIECEkConfig2l
...
## the planner configurations; each config must have a type, which specifies
## the planner to be used; other parameters can be specified as well, depending
## on the planner
planner_configs:
RRTkConfig1:
type: kinematic::RRT
range: 0.75
RRTdConfig1:
type: dynamic::RRT
range: 0.75
SBLkConfig1:
type: kinematic::SBL
projection: 0 1
celldim: 1 1
range: 0.5
KPIECEkConfig1:
type: kinematic::KPIECE
projection: 0 1
celldim: 1 1
range: 0.5
SBLkConfig2:
type: kinematic::SBL
projection: 5 6
celldim: 0.1 0.1
LBKPIECEkConfig2l:
type: kinematic::LBKPIECE
projection: link l_wrist_roll_link
celldim: 0.1 0.1 0.1
The intended use for this package is to instantiante one of the two model classes and potentially one of the monitor classes.
The model classes are:
The monitor classes are:
Subscribes to:
Reads the following parameters from the parameter server
A robot description and its corresponding planning and collision descriptions are assumed to be loaded on the parameter server as well.
Sets the following parameters on the parameter server
view_state_validity is a node that uses planning environment to check whether the current state of the robot is valid or not. This information is also broadcasted on the 'state_validity' topic as a Byte value (1 for collision, 0 for valid state). Verbosity is enabled, so one should be able to see which robot parts cause collision (if any).
ROS topics and parameters are as above.
display_planner_collision_model is a node that monitors the collision space maintained by the planning environment and broadcasts it as visualization markers. Bodies attached to the robot are also displayed. An additional boolean parameter "~skip_collision_map" can be used tp disable sending markers produced by the collision map. This is useful if the use wants to only see what other objects are in the map (on the 'object_in_map' topic, for instance).
ROS topics and parameters are as above.
clear_known_objects is a node that filters robot_msgs::PointCloud clouds. The purpose is to remove points in the cloud that correspond to objects that have already been identified.
A robot description and its corresponding planning and collision descriptions are assumed to be loaded on the parameter server as well.