From 40b120754b0c1c7a6c6bc24139428afbd2a8dec4 Mon Sep 17 00:00:00 2001
From: herb_admin <herb.mugface@gmail.com>
Date: Thu, 7 Jun 2018 15:59:46 -0700
Subject: [PATCH] HACK BODY NODE FALLS OFF

---
 .../planner/parabolic/ParabolicSmoother.hpp   | 16 ++++++++-
 src/planner/parabolic/ParabolicSmoother.cpp   | 34 +++++++++++++++++--
 2 files changed, 47 insertions(+), 3 deletions(-)

diff --git a/include/aikido/planner/parabolic/ParabolicSmoother.hpp b/include/aikido/planner/parabolic/ParabolicSmoother.hpp
index b8d08ee90b..96b3d4b6b2 100644
--- a/include/aikido/planner/parabolic/ParabolicSmoother.hpp
+++ b/include/aikido/planner/parabolic/ParabolicSmoother.hpp
@@ -6,6 +6,9 @@
 #include "aikido/trajectory/Interpolated.hpp"
 #include "aikido/trajectory/Spline.hpp"
 
+// HACK!
+#include <dart/dart.hpp>
+
 namespace aikido {
 namespace planner {
 namespace parabolic {
@@ -181,13 +184,24 @@ class ParabolicSmoother : public aikido::planner::TrajectoryPostProcessor
       const aikido::common::RNG& _rng,
       const aikido::constraint::TestablePtr& _collisionTestable) override;
 
+  // HACK HACK HACK.
+  std::unique_ptr<aikido::trajectory::Spline> postprocess(
+      const aikido::trajectory::Spline& _inputTraj,
+      const aikido::common::RNG& _rng,
+      const aikido::constraint::TestablePtr& _collisionTestable,
+      const dart::dynamics::BodyNodePtr& armEnd,
+      const dart::dynamics::BodyNodePtr& hand);
+
+
 private:
   /// Common logic to do shortcutting and/or blending on the input trajectory
   /// as dictated by mEnableShortcut and mEnableBlend.
   std::unique_ptr<aikido::trajectory::Spline> handleShortcutOrBlend(
       const aikido::trajectory::Spline& _inputTraj,
       const aikido::common::RNG& _rng,
-      const aikido::constraint::TestablePtr& _collisionTestable);
+      const aikido::constraint::TestablePtr& _collisionTestable,
+      const dart::dynamics::BodyNodePtr& armEnd = nullptr,
+      const dart::dynamics::BodyNodePtr& hand = nullptr);
 
   /// Set to the value of \c _feasibilityCheckResolution.
   double mFeasibilityCheckResolution;
diff --git a/src/planner/parabolic/ParabolicSmoother.cpp b/src/planner/parabolic/ParabolicSmoother.cpp
index c233c82279..4599eb3f4c 100644
--- a/src/planner/parabolic/ParabolicSmoother.cpp
+++ b/src/planner/parabolic/ParabolicSmoother.cpp
@@ -173,13 +173,36 @@ std::unique_ptr<aikido::trajectory::Spline> ParabolicSmoother::postprocess(
   return timedTrajectory;
 }
 
+
+//==============================================================================
+std::unique_ptr<aikido::trajectory::Spline> ParabolicSmoother::postprocess(
+    const aikido::trajectory::Spline& _inputTraj,
+    const aikido::common::RNG& _rng,
+    const aikido::constraint::TestablePtr& _collisionTestable,
+    const dart::dynamics::BodyNodePtr& armEnd,
+    const dart::dynamics::BodyNodePtr& hand
+) {
+  // Get timed trajectory for arm
+  auto timedTrajectory = computeParabolicTiming(
+      _inputTraj, mVelocityLimits, mAccelerationLimits);
+
+  auto shortcutOrBlendTrajectory
+      = handleShortcutOrBlend(*timedTrajectory, _rng, _collisionTestable);
+  if (shortcutOrBlendTrajectory)
+    return shortcutOrBlendTrajectory;
+
+  return timedTrajectory;
+}
+
 //==============================================================================
 std::unique_ptr<aikido::trajectory::Spline>
 ParabolicSmoother::handleShortcutOrBlend(
     const aikido::trajectory::Spline& _inputTraj,
     const aikido::common::RNG& _rng,
-    const aikido::constraint::TestablePtr& _collisionTestable)
-{
+    const aikido::constraint::TestablePtr& _collisionTestable,
+    const dart::dynamics::BodyNodePtr& armEnd,
+    const dart::dynamics::BodyNodePtr& hand
+) {
   if (!_collisionTestable)
     throw std::invalid_argument(
         "_collisionTestable passed to ParabolicSmoother is nullptr.");
@@ -223,6 +246,13 @@ ParabolicSmoother::handleShortcutOrBlend(
         mFeasibilityApproxTolerance);
   }
 
+
+  auto endDirection = armEnd->getWorldTransform().linear().col(2).normalized();
+  auto handDirection = hand->getWorldTransform().linear().col(2).normalized();
+
+  if (!endDirection.isApprox(handDirection))
+    std::cout << "HAND FELL OFF!" << std::endl; 
+  
   return nullptr;
 }