copied RRTConnect from ompl to CRRTConnect
removed unused member from HolonomicConstraint
added simple constrained rrt-connect implementation