Merging changes from the Summer 2019 ADA Demo

include/aikido/common/detail/memory-impl.hpp

@@ -7,7 +7,7 @@ namespace aikido {
 namespace common {
 
 //==============================================================================
-template<typename T, typename... Args>
+template <typename T, typename... Args>
 ::std::unique_ptr<T> make_unique(Args&&... args)
 {
 #if __cplusplus < 201300

include/aikido/io/detail/yaml_extension.hpp

@@ -4,8 +4,8 @@
 #include <sstream>
 #include <unordered_map>
 #include <Eigen/Dense>
-#include "aikido/common/memory.hpp"
 #include <yaml-cpp/yaml.h>
+#include "aikido/common/memory.hpp"
 
 namespace aikido {
 namespace io {

include/aikido/rviz.hpp

@@ -1,10 +1,10 @@
 #include "rviz/BodyNodeMarker.hpp"
 #include "rviz/FrameMarker.hpp"
 #include "rviz/InteractiveMarkerViewer.hpp"
-#include "rviz/pointers.hpp"
 #include "rviz/ResourceServer.hpp"
-#include "rviz/shape_conversions.hpp"
 #include "rviz/ShapeFrameMarker.hpp"
 #include "rviz/SkeletonMarker.hpp"
 #include "rviz/TSRMarker.hpp"
 #include "rviz/WorldInteractiveMarkerViewer.hpp"
+#include "rviz/pointers.hpp"
+#include "rviz/shape_conversions.hpp"

include/aikido/trajectory/util.hpp

@@ -1,6 +1,8 @@
 #ifndef AIKIDO_TRAJECTORY_UTIL_HPP_
 #define AIKIDO_TRAJECTORY_UTIL_HPP_
 
+#include <dart/dart.hpp>
+#include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp"
 #include "aikido/trajectory/Interpolated.hpp"
 #include "aikido/trajectory/Spline.hpp"
 
@@ -46,10 +48,13 @@ UniqueInterpolatedPtr concatenate(
 /// \param[in] traj Input trajectory
 /// \param[in] referenceState Reference state
 /// \param[in] timeStep Time step in finding the closest state
+/// \param[out] distance Distance to the closest state
 /// \return The time of the closest state on the input trajectory
 double findTimeOfClosestStateOnTrajectory(
     const Trajectory& traj,
-    const Eigen::VectorXd& referenceState,
+    const ::dart::dynamics::MetaSkeletonPtr& metaSkeleton,
+    const statespace::StateSpace::State* referenceState,
+    double& distance,
     double timeStep = 0.01);
 
 /// Retrieves part of a given spline trajectory

src/constraint/dart/CollisionFree.cpp

@@ -35,15 +35,22 @@ statespace::ConstStateSpacePtr CollisionFree::getStateSpace() const
 //==============================================================================
 bool CollisionFree::isSatisfied(
     const aikido::statespace::StateSpace::State* _state,
-    TestableOutcome* outcome) const
+    __attribute__((unused)) TestableOutcome* outcome) const
 {
+  // CollisionFreeOutcome* collisionFreeOutcome
+  //     = dynamic_cast_or_throw<CollisionFreeOutcome>(outcome);
+
+  // if (collisionFreeOutcome)
+  // {
+  //   collisionFreeOutcome->clear();
+  // }
+  // else
+  // {
+  //   collisionFreeOutcome = new CollisionFreeOutcome();
+  // }
+  auto _outcome = this->createOutcome();
   auto collisionFreeOutcome
-      = dynamic_cast_or_throw<CollisionFreeOutcome>(outcome);
-
-  if (collisionFreeOutcome)
-  {
-    collisionFreeOutcome->clear();
-  }
+      = dynamic_cast<CollisionFreeOutcome*>(_outcome.get());
 
   auto skelStatePtr = static_cast<const aikido::statespace::dart::
                                       MetaSkeletonStateSpace::State*>(_state);
@@ -65,6 +72,7 @@ bool CollisionFree::isSatisfied(
       {
         collisionFreeOutcome->mPairwiseContacts = collisionResult.getContacts();
       }
+      std::cout << collisionFreeOutcome->toString() << std::endl;
       return false;
     }
   }
@@ -79,6 +87,7 @@ bool CollisionFree::isSatisfied(
       {
         collisionFreeOutcome->mSelfContacts = collisionResult.getContacts();
       }
+      std::cout << collisionFreeOutcome->toString() << std::endl;
       return false;
     }
   }

src/constraint/dart/InverseKinematicsSampleable.cpp

@@ -220,7 +220,8 @@ bool IkSampleGenerator::sample(statespace::StateSpace::State* _state)
 
     mInverseKinematics->getTarget()->setTransform(poseState.getIsometry());
 
-    // Run the IK solver. If an exact solution is computed, apply it to the skeleton.
+// Run the IK solver. If an exact solution is computed, apply it to the
+// skeleton.
 #if DART_VERSION_AT_LEAST(6, 8, 0)
     if (mInverseKinematics->solveAndApply(true))
 #else

src/constraint/dart/TSR.cpp

@@ -5,8 +5,8 @@
 #include <vector>
 #include <boost/format.hpp>
 #include <dart/common/Console.hpp>
-#include "aikido/common/memory.hpp"
 #include <dart/math/Geometry.hpp>
+#include "aikido/common/memory.hpp"
 
 #undef dtwarn
 #define dtwarn (::dart::common::colorErr("Warning", __FILE__, __LINE__, 33))

src/control/KinematicSimulationTrajectoryExecutor.cpp

@@ -139,6 +139,7 @@ void KinematicSimulationTrajectoryExecutor::step(
   // Check if trajectory has completed.
   if (executionTime >= mTraj->getEndTime())
   {
+    std::cout << "Trajectory completed" << std::endl;
     mTraj.reset();
     mStateSpace.reset();
     mMetaSkeleton.reset();

src/control/ros/Conversions.cpp

@@ -8,12 +8,14 @@
 #include "aikido/control/ros/Conversions.hpp"
 #include "aikido/statespace/dart/RnJoint.hpp"
 #include "aikido/statespace/dart/SO2Joint.hpp"
+#include "aikido/trajectory/Interpolated.hpp"
 
 using aikido::statespace::CartesianProduct;
 using aikido::statespace::dart::MetaSkeletonStateSpace;
 using SplineTrajectory = aikido::trajectory::Spline;
 using aikido::statespace::dart::R1Joint;
 using aikido::statespace::dart::SO2Joint;
+using aikido::statespace::InterpolatorPtr;
 
 namespace aikido {
 namespace control {
@@ -96,13 +98,20 @@ void extractJointTrajectoryPoint(
 /// Extract a state on the trajectory given a timepoint.
 /// \param[in] space MetaSkeletonStateSpace of the trajectory.
 /// \param[in] trajectory Trajectory to extract point from.
+/// \param[in] interpolator Interpolator used to maintain continuity in
+/// extraction.
 /// \param[in] timeFromStart Timepoint to extract trajectory point at.
-/// \param[out] waypoint The extracted trajectory point.
+/// \param[in] waypoint The extracted trajectory point.
+/// \param[in] previousPoint Previously extracted trajectory point to
+/// ensure continuity in representation when extracting multiple points.
+/// Set to zero vector if not required.
 void extractTrajectoryPoint(
     const std::shared_ptr<const MetaSkeletonStateSpace>& space,
     const aikido::trajectory::ConstTrajectoryPtr& trajectory,
+    const aikido::statespace::InterpolatorPtr& interpolator,
     double timeFromStart,
-    trajectory_msgs::JointTrajectoryPoint& waypoint);
+    trajectory_msgs::JointTrajectoryPoint& waypoint,
+    Eigen::VectorXd& previousPoint);
 
 //==============================================================================
 void reorder(
@@ -255,8 +264,10 @@ void extractJointTrajectoryPoint(
 void extractTrajectoryPoint(
     const std::shared_ptr<const MetaSkeletonStateSpace>& space,
     const aikido::trajectory::ConstTrajectoryPtr& trajectory,
+    const aikido::statespace::InterpolatorPtr& interpolator,
     double timeFromStart,
-    trajectory_msgs::JointTrajectoryPoint& waypoint)
+    trajectory_msgs::JointTrajectoryPoint& waypoint,
+    Eigen::VectorXd& previousPoint)
 {
   const auto numDerivatives = std::min<int>(trajectory->getNumDerivatives(), 1);
   const auto timeAbsolute = trajectory->getStartTime() + timeFromStart;
@@ -265,8 +276,29 @@ void extractTrajectoryPoint(
 
   Eigen::VectorXd tangentVector;
   auto state = space->createState();
+
+  auto prevState = space->createState();
+  space->convertPositionsToState(previousPoint, prevState);
+
   trajectory->evaluate(timeAbsolute, state);
-  space->logMap(state, tangentVector);
+  space->convertStateToPositions(state, tangentVector);
+
+  auto geodesicInterpolator
+      = std::dynamic_pointer_cast<aikido::statespace::GeodesicInterpolator>(
+          interpolator);
+  if (!geodesicInterpolator)
+  {
+    throw std::invalid_argument(
+        "The interpolator of trajectory should be a GeodesicInterpolator");
+  }
+
+  auto diff = geodesicInterpolator->getTangentVector(prevState, state);
+  tangentVector = previousPoint + diff;
+
+  for (int i = 0; i < tangentVector.size(); ++i)
+  {
+    previousPoint(i) = tangentVector(i);
+  }
 
   assert(tangentVector.size() == numDof);
 
@@ -583,14 +615,25 @@ trajectory_msgs::JointTrajectory toRosJointTrajectory(
     jointTrajectory.joint_names.emplace_back(jointDofName);
   }
 
+  Eigen::VectorXd previousPoint(Eigen::VectorXd::Zero(space->getDimension()));
+
   // Evaluate trajectory at each timestep and insert it into jointTrajectory
   jointTrajectory.points.reserve(numWaypoints);
-  for (const auto& timeFromStart : timeSequence)
-  {
-    trajectory_msgs::JointTrajectoryPoint waypoint;
 
-    extractTrajectoryPoint(space, trajectory, timeFromStart, waypoint);
+  // Create the geodesic interpolator used to extract trajectory points.
+  auto interpolator
+      = std::make_shared<aikido::statespace::GeodesicInterpolator>(space);
 
+  for (std::size_t i = 0; i < numWaypoints; ++i)
+  {
+    trajectory_msgs::JointTrajectoryPoint waypoint;
+    extractTrajectoryPoint(
+        space,
+        trajectory,
+        interpolator,
+        timeSequence[i],
+        waypoint,
+        previousPoint);
     jointTrajectory.points.emplace_back(waypoint);
   }
 

src/control/ros/RosTrajectoryExecutor.cpp

@@ -150,6 +150,7 @@ std::future<void> RosTrajectoryExecutor::execute(
         goal,
         boost::bind(&RosTrajectoryExecutor::transitionCallback, this, _1));
 
+    std::cout << "Returning future" << std::endl;
     return mPromise->get_future();
   }
 }

src/planner/SnapConfigurationToConfigurationPlanner.cpp

@@ -2,6 +2,7 @@
 
 #include "aikido/common/VanDerCorput.hpp"
 #include "aikido/constraint/Testable.hpp"
+#include "aikido/constraint/dart/CollisionFreeOutcome.hpp"
 #include "aikido/statespace/StateSpace.hpp"
 
 namespace aikido {
@@ -37,6 +38,7 @@ trajectory::TrajectoryPtr SnapConfigurationToConfigurationPlanner::plan(
   auto constraint = problem.getConstraint();
 
   aikido::common::VanDerCorput vdc{1, true, true, 0.02}; // TODO junk resolution
+
   for (const auto alpha : vdc)
   {
     mInterpolator->interpolate(startState, goalState, alpha, testState);

src/planner/kunzretimer/KunzRetimer.cpp

@@ -1,8 +1,8 @@
 #include "aikido/planner/kunzretimer/KunzRetimer.hpp"
 
-#include "aikido/common/memory.hpp"
 #include "aikido/common/Spline.hpp"
 #include "aikido/common/StepSequence.hpp"
+#include "aikido/common/memory.hpp"
 #include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp"
 #include "aikido/trajectory/Interpolated.hpp"
 #include "aikido/trajectory/Spline.hpp"

src/planner/parabolic/ParabolicSmoother.cpp

@@ -2,8 +2,8 @@
 
 #include <cassert>
 #include <set>
-#include "aikido/common/memory.hpp"
 #include "aikido/common/Spline.hpp"
+#include "aikido/common/memory.hpp"
 #include "aikido/planner/parabolic/ParabolicTimer.hpp"
 #include "DynamicPath.h"
 #include "HauserParabolicSmootherHelpers.hpp"

src/planner/parabolic/ParabolicTimer.cpp

@@ -2,8 +2,8 @@
 
 #include <cassert>
 #include <set>
-#include "aikido/common/memory.hpp"
 #include "aikido/common/Spline.hpp"
+#include "aikido/common/memory.hpp"
 #include "aikido/trajectory/Interpolated.hpp"
 #include "DynamicPath.h"
 #include "ParabolicUtil.hpp"

src/planner/parabolic/ParabolicUtil.cpp

@@ -3,9 +3,9 @@
 #include <cassert>
 #include <set>
 
-#include "aikido/common/memory.hpp"
 #include <dart/dart.hpp>
 #include "aikido/common/Spline.hpp"
+#include "aikido/common/memory.hpp"
 #include "aikido/statespace/GeodesicInterpolator.hpp"
 #include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp"
 #include "aikido/trajectory/Interpolated.hpp"
@@ -101,8 +101,9 @@ std::unique_ptr<aikido::trajectory::Spline> convertToSpline(
   Eigen::VectorXd positionPrev, velocityPrev;
   evaluateAtTime(_inputPath, timePrev, positionPrev, velocityPrev);
 
-  auto _outputTrajectory = ::aikido::common::make_unique<aikido::trajectory::Spline>(
-      _stateSpace, timePrev + _startTime);
+  auto _outputTrajectory
+      = ::aikido::common::make_unique<aikido::trajectory::Spline>(
+          _stateSpace, timePrev + _startTime);
   auto segmentStartState = _stateSpace->createState();
 
   for (const auto timeCurr : transitionTimes)

src/planner/vectorfield/detail/VectorFieldIntegrator.cpp

@@ -1,8 +1,8 @@
 #include "VectorFieldIntegrator.hpp"
 #include <exception>
 #include <string>
-#include <aikido/common/memory.hpp>
 #include <aikido/common/Spline.hpp>
+#include <aikido/common/memory.hpp>
 #include <aikido/planner/vectorfield/VectorFieldUtil.hpp>
 #include <aikido/statespace/GeodesicInterpolator.hpp>
 #include "VectorFieldPlannerExceptions.hpp"

src/robot/util.cpp

@@ -2,10 +2,10 @@
 
 #include <algorithm>
 #include <dart/common/Console.hpp>
-#include "aikido/common/memory.hpp"
 #include <dart/common/Timer.hpp>
 #include <ompl/geometric/planners/rrt/RRTConnect.h>
 #include "aikido/common/RNG.hpp"
+#include "aikido/common/memory.hpp"
 #include "aikido/constraint/CyclicSampleable.hpp"
 #include "aikido/constraint/FiniteSampleable.hpp"
 #include "aikido/constraint/NewtonsMethodProjectable.hpp"

src/rviz/InteractiveMarkerViewer.cpp

@@ -1,7 +1,7 @@
 #include <aikido/rviz/InteractiveMarkerViewer.hpp>
 
-#include "aikido/common/memory.hpp"
 #include <dart/dart.hpp>
+#include "aikido/common/memory.hpp"
 #include <aikido/rviz/FrameMarker.hpp>
 #include <aikido/rviz/SkeletonMarker.hpp>
 #include <aikido/rviz/TrajectoryMarker.hpp>

src/rviz/ShapeFrameMarker.cpp

@@ -39,7 +39,7 @@ ShapeFrameMarker::ShapeFrameMarker(
   , mForceUpdate(true)
   , mVersion()
   , mShowVisual(true)
-  , mShowCollision(false)
+  , mShowCollision(true)
 {
   using std::placeholders::_1;
   using std::placeholders::_2;

src/trajectory/BSpline.cpp

@@ -2,8 +2,8 @@
 
 #include <sstream>
 #include <Eigen/Core>
-#include "aikido/common/memory.hpp"
 #include "aikido/common/StepSequence.hpp"
+#include "aikido/common/memory.hpp"
 
 namespace aikido {
 namespace trajectory {

src/trajectory/CMakeLists.txt

@@ -9,6 +9,7 @@ add_library("${PROJECT_NAME}_trajectory" SHARED ${sources})
 target_link_libraries("${PROJECT_NAME}_trajectory"
   PUBLIC
     "${PROJECT_NAME}_common"
+    "${PROJECT_NAME}_distance"
     "${PROJECT_NAME}_statespace"
 )
 target_compile_options("${PROJECT_NAME}_trajectory"
@@ -17,6 +18,6 @@ target_compile_options("${PROJECT_NAME}_trajectory"
 
 add_component(${PROJECT_NAME} trajectory)
 add_component_targets(${PROJECT_NAME} trajectory "${PROJECT_NAME}_trajectory")
-add_component_dependencies(${PROJECT_NAME} trajectory common statespace)
+add_component_dependencies(${PROJECT_NAME} trajectory common statespace distance)
 
 clang_format_add_sources(${sources})