SmoothPath does not respect collision constraints

[Stefanos19]

Here's the code we use:

std::vector<aikido::constraint::ConstTestablePtr> constraints;
if (m_collisionFree)
  constraints.push_back(m_collisionFree);
auto testable = std::make_shared<aikido::constraint::TestableIntersection>(m_stateSpace, constraints);
std::unique_lock<std::mutex> lock(m_skeleton->getBodyNode(0)->getSkeleton()->getMutex());
aikido::trajectory::TrajectoryPtr timedTrajectory = ada->smoothPath(m_skeleton,
                                                                    trajectory.get(),
                                                                    testable);