I have a trajectory ie. vector of `joint_angles` for an arm. The path is generated by using a evolutionary algorithm but it may be invalid and result in collisions. I want to check whether there is a collision using moveit or fcl.
Based on the tutorials, I think one way of doing this would be to
joint_angles = [[j1, j2, j3....], [j1, j2, j3....], [j1, j2, j3....], ....]
for each point in joint_angles
robot_state::RobotState& current_state = planning_scene.getCurrentStateNonConst();
current_state.setJointGroupPositions(joint_model_group, point);
collision_result.clear();
planning_scene.checkSelfCollision(collision_request, collision_result);
Is this the right way of doing it or is there a better way of passing the entire trajectory and checking collision.
↧