HACK BODY NODE FALLS OFF

personalrobotics · Jun 7, 2018 · 40b1207 · 40b1207
1 parent 714c26f
commit 40b1207
Show file tree

Hide file tree

Showing 2 changed files with 47 additions and 3 deletions.
diff --git 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
@@ -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;
 }