ompl planning attempts to move through static obstacles
Hello all!
I'm trying to use the ompl planner to get the path for our robotic arm. I have added static obstacles to the collision space, and sometimes when trying to move the arm from one location to another, as seen with rviz, the planned motion will attempt to move the hand at the end of the arm through the obstacles (in this case a table).....I have added the hand to the collision checks, as well as attempting to increase padding. I have also tried using various configurations (SBLk, LBKPIECEk, RRTConnect) but they all have the same result.
The output reports no errors in planning, so I dont understand why the motion doesn't avoid the obstacles....
Here are the settings used in collision_checks.yaml:
default_collision_operations:
- object1: platform
object2: arm
operation: enable
- object1: torso
object2: arm
operation: enable
- object1: tablet
object2: arm
operation: enable
- object1: base_link
object2: arm_0_link
operation: disable
- object1: platform
object2: hand
operation: enable
- object1: torso
object2: hand
operation: enable
- object1: tablet
object2: hand
operation: enable
- object1: arm
object2: hand
operation: enable
- object1: sdh_palm_link
object2: arm_7_link
operation: disable
- object1: sdh_palm_link
object2: arm_6_link
operation: disable
- object1: hand
object2: arm_5_link
operation: disable
Also, here are the ompl_planning_config.yaml settings:
groups:
- arm
arm:
planner_type: JointPlanner
planner_configs:
# - SBLkConfig1
# - LBKPIECEkConfig1
- RRTConnectConfig1
kinematics_solver: pr2_arm_kinematics/PR2ArmKinematicsPlugin
tip_name: arm_7_link
root_name: arm_0_link
robot: cob3-3
projection_evaluator: joint_state
planner_configs:
SBLkConfig1:
type: kinematic::SBL
LBKPIECEkConfig1:
type: kinematic::LBKPIECE
RRTConnectConfig1:
type: kinematic::RRTConnect
publish_diagnostics: true
And finally, the planning_groups.yaml:
groups:
- name: platform
joints:
base_joint
links:
base_link
base_laser_front_link
base_laser_rear_link
base_laser_top_link
- name: torso
joints:
torso_lower_neck_pan_joint
torso_lower_neck_tilt_joint
torso_upper_neck_pan_joint
torso_upper_neck_tilt_joint
links:
torso_lower_neck_tilt_link
torso_upper_neck_tilt_link
head_cover_link
- name: tablet
joints:
torso_tray_joint
links:
torso_tray_link
- name: arm
joints:
arm_1_joint
arm_2_joint
arm_3_joint
arm_4_joint
arm_5_joint
arm_6_joint
arm_7_joint
links:
arm_0_link
arm_1_link
arm_2_link
arm_3_link
arm_4_link
arm_5_link
arm_6_link
arm_7_link
- name: hand
joints:
sdh_finger_11_joint
sdh_finger_12_joint
sdh_finger_13_joint
sdh_finger_21_joint
sdh_finger_22_joint
sdh_finger_23_joint
sdh_thumb_2_joint
sdh_thumb_3_joint
links:
sdh_palm_link
sdh_finger_11_link
sdh_finger_12_link
sdh_finger_13_link
sdh_finger_21_link
sdh_finger_22_link
sdh_finger_23_link
sdh_thumb_1_link
sdh_thumb_2_link
sdh_thumb_3_link
#this group is for the experimental cob_object_manipulation
- name: end_effector
links:
sdh_palm_link
sdh_finger_11_link
sdh_finger_12_link
sdh_finger_13_link
sdh_finger_21_link
sdh_finger_22_link
sdh_finger_23_link
sdh_thumb_1_link
sdh_thumb_2_link
sdh_thumb_3_link
I am currently using the diamondback version.
Someone working previously with this said they had no issues with collisions back in April when they were looking at it....has there been any major changes in planning/collision checking since then?
If there are other configuration files, or launch files that I could include which would be of use, please let me know! Thanks for your time!
Robert
