fixed small rotation bug, but still issues with planning
fixed safety scale
no rotation previous to collision checking
fixed box issue, fixed replanning collision checking, integrated naive cc
removed debug options| reshaped action set
higher threshold for mocap
minor
minor
geometry privatized
made geometry thread safe
fixed swept volume constraint checker for complete trajectory
added new heuristic for reaching the goal almost exactly | fixed half sitting position at goal
refined and tested new heuristic
clean up
fixed concatenation bug, still some problems with obstacles inside the horizon
sorted our rviz functions
Merge branch 'master' into object_size
merged with object_size | refactored geometry.h