fix simpleSmoother; kris hauser's smoother

icaros-usc · Sep 22, 2019 · b42800b · b42800b
1 parent 310dc92
commit b42800b
Show file tree

Hide file tree

Showing 39 changed files with 5,598 additions and 171 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -52,15 +52,15 @@ find_package(aikido 0.3.0 REQUIRED
 include_directories(${aikido_INCLUDE_DIRS})
 find_package(libada REQUIRED)
 include_directories(${libada_INCLUDE_DIRS})
-find_package(pr_assets REQUIRED)
-include_directories(${pr_assets_INCLUDE_DIRS})
 find_package(pr_tsr REQUIRED)
 include_directories(${pr_tsr_INCLUDE_DIRS})
 find_package(Boost COMPONENTS program_options REQUIRED)
 include_directories(${Boost_INCLUDE_DIRS})
 
 include_directories(include)
 
+add_subdirectory("src/external/hauser_parabolic_smoother")
+
 add_executable(wecook
         src/main.cpp
         src/TaskManager.cpp
@@ -121,18 +121,42 @@ add_executable(wecook
         src/TSRIKNode.cpp
         include/wecook/TaskGraph.h
         src/TaskGraph.cpp
-        include/wecook/ActionNode.h include/wecook/PrimitiveTaskGraph.h include/wecook/PrimitiveActionNode.h include/wecook/PrimitiveGraspNode.h include/wecook/Human.h include/wecook/Robot.h include/wecook/PrimitiveEngageNode.h include/wecook/PrimitiveActuateNode.h include/wecook/PrimitivePlaceNode.h include/wecook/RelativeIKMotionNode.h src/RelativeIKMotionNode.cpp src/PrimitiveTaskGraph.cpp src/PrimitiveActionNode.cpp include/wecook/PrimitiveActionExecutor.h src/PrimitiveActionExecutor.cpp src/PrimitiveGraspNode.cpp src/PrimitiveEngageNode.cpp src/PrimitivePlaceNode.cpp src/PrimitiveActuateNode.cpp src/Robot.cpp src/Human.cpp include/wecook/ObjectMgr.h src/ObjectMgr.cpp include/wecook/TaskExecutorThread.h src/TaskExecutorThread.cpp)
+        include/wecook/ActionNode.h
+        include/wecook/PrimitiveTaskGraph.h
+        include/wecook/PrimitiveActionNode.h
+        include/wecook/PrimitiveGraspNode.h
+        include/wecook/Human.h
+        include/wecook/Robot.h
+        include/wecook/PrimitiveEngageNode.h
+        include/wecook/PrimitiveActuateNode.h
+        include/wecook/PrimitivePlaceNode.h
+        include/wecook/RelativeIKMotionNode.h
+        src/RelativeIKMotionNode.cpp
+        src/PrimitiveTaskGraph.cpp
+        src/PrimitiveActionNode.cpp
+        include/wecook/PrimitiveActionExecutor.h
+        src/PrimitiveActionExecutor.cpp
+        src/PrimitiveGraspNode.cpp
+        src/PrimitiveEngageNode.cpp
+        src/PrimitivePlaceNode.cpp
+        src/PrimitiveActuateNode.cpp
+        src/Robot.cpp src/Human.cpp
+        include/wecook/ObjectMgr.h
+        src/ObjectMgr.cpp
+        include/wecook/TaskExecutorThread.h
+        src/TaskExecutorThread.cpp
+        include/wecook/planner/SmootherHelpers.h
+        src/planner/SmootherHelpers.cpp src/planner/SmootherUtil.h src/planner/SmootherUtil.cpp src/planner/SimpleDynamicPath.h src/planner/SimpleDynamicPath.cpp include/wecook/MetaActuateInfo.h)
 add_dependencies(wecook wecook_generate_messages_cpp)
 target_link_libraries(wecook
         ${DART_LIBRARIES}
         ${aikido_LIBRARIES}
         ${Boost_LIBRARIES}
-        libada)
+        libada
+        ${PROJECT_NAME}_external_hauserparabolicsmoother)
 
 install(TARGETS wecook RUNTIME DESTINATION bin)
 
-#catkin_install_python(PROGRAMS scripts/ex_task1.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
-
 #include(ClangFormat)
 #clang_format_setup(VERSION 3.8)
 #

diff --git a/include/wecook/Action.h b/include/wecook/Action.h
@@ -16,16 +16,29 @@ class Action {
          const std::vector<std::string> &location,
          const std::vector<std::string> &ingredients,
          const std::string &verb,
-         const std::string &tool) : m_pids(pids), m_ingredients(ingredients), m_location(location), m_tool(tool), m_verb(verb) {
+         const std::string &tool)
+      : m_pids(pids), m_ingredients(ingredients), m_locations(location), m_tool(tool), m_verb(verb) {
 
   }
 
+  Action() {
+
+  }
+
+  Action(const Action &other) {
+    m_pids = other.get_pids();
+    m_ingredients = other.get_ingredients();
+    m_locations = other.get_locations();
+    m_tool = other.get_tool();
+    m_verb = other.get_verb();
+  }
+
   std::string get_verb() const {
     return m_verb;
   }
 
-  std::vector<std::string> get_location() const {
-    return m_location;
+  std::vector<std::string> get_locations() const {
+    return m_locations;
   }
 
   std::string get_tool() const {
@@ -42,7 +55,7 @@ class Action {
 
  private:
   std::vector<std::string> m_pids;
-  std::vector<std::string> m_location;
+  std::vector<std::string> m_locations;
   std::string m_tool;
   std::vector<std::string> m_ingredients;
   std::string m_verb;

diff --git a/include/wecook/ActionNode.h b/include/wecook/ActionNode.h
@@ -24,6 +24,10 @@ class ActionNode {
     return m_children;
   }
 
+  inline std::vector<ActionNode *> getFathers() {
+    return m_fathers;
+  }
+
   inline void addChild(ActionNode *actionNode) {
     m_children.emplace_back(actionNode);
   }
@@ -52,12 +56,14 @@ class ActionNode {
   std::map<std::string, std::vector<std::shared_ptr<MotionNode>>> m_motionSeqMap;
 
   PrimitiveTaskGraph m_primitiveTaskGraph;
+  bool m_ifExecuted = false;
  private:
   Action m_action;
   bool m_ifHead;
   std::vector<ActionNode *> m_children;
   std::vector<ActionNode *> m_fathers;
   std::vector<std::string> m_pids;
+
 };
 
 }

diff --git a/include/wecook/ContainingMap.h b/include/wecook/ContainingMap.h
@@ -47,6 +47,16 @@ class ContainingMap {
     return nullptr;
   }
 
+  inline std::vector<dart::dynamics::BodyNode *> getContainedBodyNodes(const std::string &containerName) {
+    std::vector<dart::dynamics::BodyNode *> ret;
+    for (const auto &container : m_containers) {
+      if (container.getContainerName() == containerName) {
+        ret.emplace_back(container.getContainedBodyNode());
+      }
+    }
+    return ret;
+  }
+
   inline bool hasTuple(const std::string &containerName, const std::string &containedName) {
     for (const auto &container : m_containers) {
       if (container.getContainerName() == containerName && container.getContainedName() == containedName) {

diff --git a/include/wecook/MetaActuateInfo.h b/include/wecook/MetaActuateInfo.h
@@ -0,0 +1,30 @@
+//
+// Created by hejia on 9/19/19.
+//
+
+#ifndef WECOOK_METAACTUATEINFO_H
+#define WECOOK_METAACTUATEINFO_H
+
+#include "Action.h"
+
+namespace wecook {
+class MetaActuateInfo {
+ public:
+  MetaActuateInfo(const Action &action) : m_action(action) {
+
+  }
+
+  MetaActuateInfo(const MetaActuateInfo& other) {
+    m_action = other.getAction();
+  }
+
+  Action getAction() const {
+    return m_action;
+  }
+
+ private:
+  Action m_action;
+};
+}
+
+#endif //WECOOK_METAACTUATEINFO_H
diff --git a/include/wecook/ObjectMgr.h b/include/wecook/ObjectMgr.h
@@ -70,7 +70,8 @@ class ObjectMgr {
   }
 
   dart::collision::CollisionGroupPtr createCollisionGroupExceptFoodAndMovingObj(const std::string &toMove,
-                                                                                dart::collision::FCLCollisionDetectorPtr &collisionDetector);
+                                                                                dart::collision::FCLCollisionDetectorPtr &collisionDetector,
+                                                                                dart::dynamics::BodyNode *blackNode);
 
  private:
   std::map<std::string, InternalObject> m_objects;

diff --git a/include/wecook/PrimitiveActionNode.h b/include/wecook/PrimitiveActionNode.h
@@ -98,6 +98,12 @@ class PrimitiveActionNode {
 
   void removeFather(std::shared_ptr<PrimitiveActionNode> &father);
 
+  double m_timeStep = 0.02;
+
+  void setTimeStep(double timeStep) {
+    m_timeStep = timeStep;
+  }
+
  protected:
   bool m_ifTail;
   bool m_ifHead;

diff --git a/include/wecook/PrimitiveActuateNode.h b/include/wecook/PrimitiveActuateNode.h
@@ -10,6 +10,7 @@
 
 #include "PrimitiveActionNode.h"
 #include "ContainingMap.h"
+#include "MetaActuateInfo.h"
 
 namespace wecook {
 
@@ -20,14 +21,18 @@ class PrimitiveActuateNode : public PrimitiveActionNode {
                        const std::string &motionType,
                        const std::string &grabbingObj,
                        const std::string &placingObj,
+                       const MetaActuateInfo &metaActuateInfo,
                        bool ifHead = false,
                        bool ifTail = false)
       : PrimitiveActionNode(pid,
                             "predefined",
                             grabbingObj,
                             placingObj,
                             ifHead,
-                            ifTail), m_motionType(motionType), m_manipulatedObj(manipulatedObj) {
+                            ifTail),
+        m_motionType(motionType),
+        m_manipulatedObj(manipulatedObj),
+        m_metaActuateInfo(metaActuateInfo) {
 
   }
 
@@ -36,6 +41,7 @@ class PrimitiveActuateNode : public PrimitiveActionNode {
                std::shared_ptr<ContainingMap> &containingMap);
 
  private:
+  MetaActuateInfo m_metaActuateInfo;
   std::string m_motionType;
   std::string m_manipulatedObj;
 };

diff --git a/include/wecook/PrimitivePlaceNode.h b/include/wecook/PrimitivePlaceNode.h
@@ -18,12 +18,14 @@ class PrimitivePlaceNode : public PrimitiveActionNode {
                      const std::string &pid,
                      const std::string &grabbingObj,
                      const std::string &placingObj,
-                     bool ifHead = false,
-                     bool ifTail = false)
+                     bool ifHead,
+                     bool ifTail,
+                     bool ifDebug = false)
       : PrimitiveActionNode(pid, "place", grabbingObj, placingObj, ifHead, ifTail),
         m_refObject(refObject),
         m_targetPose(targetPose),
-        m_toPlace(toPlace) {
+        m_toPlace(toPlace),
+        m_ifDebug(ifDebug) {
 
   }
 
@@ -32,6 +34,7 @@ class PrimitivePlaceNode : public PrimitiveActionNode {
                std::shared_ptr<ContainingMap> &containingMap);
 
  private:
+  bool m_ifDebug;
   std::string m_toPlace;
   aikido::constraint::dart::TSRPtr m_targetPose;
   std::string m_refObject;

diff --git a/include/wecook/TSRMotionNode.h b/include/wecook/TSRMotionNode.h
@@ -27,11 +27,16 @@ class TSRMotionNode : public MotionNode {
 
   void plan(const std::shared_ptr<ada::Ada> &ada);
 
+  void setTimeStep(double timeStep) {
+    m_retimeTimeStep = timeStep;
+  }
+
  private:
   aikido::constraint::dart::CollisionFreePtr m_collisionFree = nullptr;
   dart::dynamics::BodyNode *m_bn = nullptr;
   aikido::constraint::dart::TSRPtr m_goalTSR;
   bool m_debug;
+  double m_retimeTimeStep = 0.02;
 };
 
 }

diff --git a/include/wecook/TaskGraph.h b/include/wecook/TaskGraph.h
@@ -13,8 +13,15 @@ namespace wecook {
 class TaskGraph {
  public:
   TaskGraph(const std::vector<Action> &actionSeq) {
+    int i = 0;
     for (auto &action : actionSeq) {
-      addNode(action);
+      i += 1;
+      if (i == 6) {
+        addNode(action, true);
+      } else {
+        addNode(action);
+      }
+
     }
   }
 
@@ -24,7 +31,7 @@ class TaskGraph {
     }
   }
 
-  void addNode(const Action& action);
+  void addNode(const Action& action, bool ifEnd=false);
 
   void addArc();
 

diff --git a/include/wecook/planner/SmootherHelpers.h b/include/wecook/planner/SmootherHelpers.h
@@ -0,0 +1,77 @@
+//
+// Created by hejia on 9/16/19.
+//
+
+#ifndef WECOOK_SMOOTHERHELPERS_H
+#define WECOOK_SMOOTHERHELPERS_H
+
+#include <libada/Ada.hpp>
+#include <dart/dynamics/BodyNode.hpp>
+#include <aikido/trajectory/Trajectory.hpp>
+#include <aikido/constraint/Testable.hpp>
+#include <aikido/trajectory/Spline.hpp>
+#include <aikido/trajectory/Interpolated.hpp>
+#include <aikido/planner/parabolic/ParabolicTimer.hpp>
+
+namespace wecook {
+namespace planner {
+/*
+ * Simple short cutting
+ */
+aikido::trajectory::InterpolatedPtr simpleSmoothPath(const std::shared_ptr<ada::Ada> &ada,
+                                                     const aikido::statespace::dart::MetaSkeletonStateSpacePtr &metaStateSpace,
+                                                     const dart::dynamics::MetaSkeletonPtr &metaSkeletonPtr,
+                                                     const aikido::trajectory::Trajectory *path,
+                                                     const aikido::constraint::TestablePtr &constraint);
+
+aikido::trajectory::InterpolatedPtr simpleDoShortcut(const aikido::statespace::dart::MetaSkeletonStateSpacePtr &metaStateSpace,
+                                                     const dart::dynamics::MetaSkeletonPtr &metaSkeletonPtr,
+                                                     const aikido::trajectory::Interpolated &inputInterpolated,
+                                                     aikido::common::RNG &rng,
+                                                     const aikido::constraint::TestablePtr &collisionTestable,
+                                                     const Eigen::VectorXd &maxVelocity,
+                                                     const Eigen::VectorXd &maxAcceleration,
+                                                     double timeLimit = 6.0,
+                                                     double checkResolution = 5e-4,
+                                                     double tolerance = 1e-3);
+
+/*
+ * Smoothing with Kris Hauser's parabolic smoother
+ */
+aikido::trajectory::UniqueSplinePtr hauserSmoothPath(const std::shared_ptr<ada::Ada> &ada,
+                                                     const aikido::statespace::dart::MetaSkeletonStateSpacePtr &metaStateSpace,
+                                                     const dart::dynamics::MetaSkeletonPtr &metaSkeletonPtr,
+                                                     const aikido::trajectory::Trajectory *path,
+                                                     const aikido::constraint::TestablePtr &constraint);
+
+aikido::trajectory::UniqueSplinePtr hauserDoShortcut(const aikido::statespace::dart::MetaSkeletonStateSpacePtr &metaStateSpace,
+                                                     const dart::dynamics::MetaSkeletonPtr &metaSkeletonPtr,
+                                                     const aikido::trajectory::Interpolated &inputInterpolated,
+                                                     aikido::common::RNG &rng,
+                                                     const aikido::constraint::TestablePtr &collisionTestable,
+                                                     const Eigen::VectorXd &maxVelocity,
+                                                     const Eigen::VectorXd &maxAcceleration,
+                                                     double timeLimit = 6.0,
+                                                     double checkResolution = 5e-4,
+                                                     double tolerance = 1e-3);
+
+aikido::trajectory::InterpolatedPtr hauserSmoothPathInterpolated(const std::shared_ptr<ada::Ada> &ada,
+                                                                 const aikido::statespace::dart::MetaSkeletonStateSpacePtr &metaStateSpace,
+                                                                 const dart::dynamics::MetaSkeletonPtr &metaSkeletonPtr,
+                                                                 const aikido::trajectory::Trajectory *path,
+                                                                 const aikido::constraint::TestablePtr &constraint);
+
+aikido::trajectory::InterpolatedPtr hauserDoShortcutInterpolated(const aikido::statespace::dart::MetaSkeletonStateSpacePtr &metaStateSpace,
+                                                                 const dart::dynamics::MetaSkeletonPtr &metaSkeletonPtr,
+                                                                 const aikido::trajectory::Interpolated &inputInterpolated,
+                                                                 aikido::common::RNG &rng,
+                                                                 const aikido::constraint::TestablePtr &collisionTestable,
+                                                                 const Eigen::VectorXd &maxVelocity,
+                                                                 const Eigen::VectorXd &maxAcceleration,
+                                                                 double timeLimit = 8.0,
+                                                                 double checkResolution = 5e-4,
+                                                                 double tolerance = 1e-3);
+}
+}
+
+#endif //WECOOK_SMOOTHERHELPERS_H
diff --git a/package.xml b/package.xml
@@ -12,7 +12,6 @@
     <depend>libada</depend>
     <depend>tf_conversions</depend>
     <depend>ada_description</depend>
-    <depend>pr_assets</depend>
     <depend>pr_tsr</depend>
     <depend>cv_bridge</depend>
     <depend>image_transport</depend>