add planWithEndEffectorTwist

CMakeLists.txt

@@ -26,7 +26,8 @@ endif()
 include(ExternalProject)
 
 find_package(DART 6.6.2 REQUIRED
-  OPTIONAL_COMPONENTS utils
+  COMPONENTS utils utils-urdf optimizer-nlopt
+
 )
 
 find_package(aikido 0.3.0 REQUIRED
@@ -169,7 +170,7 @@ install(FILES "package.xml"
 #
 
 include(ClangFormat)
-clang_format_setup(VERSION 3.8)
+clang_format_setup(VERSION 3.9)
 
 if(CLANG_FORMAT_EXECUTABLE)
 

include/libada/Ada.hpp

@@ -66,7 +66,8 @@ class Ada final : public aikido::robot::Robot
 
   const aikido::robot::util::VectorFieldPlannerParameters vfParams
       = aikido::robot::util::VectorFieldPlannerParameters(
-          0.2, 0.03, 0.03, 0.001, 1e-3, 1e-3, 1.0, 0.2, 0.01);
+          // 0.2, 0.03, 0.03, 0.001, 1e-3, 1e-3, 1.0, 0.2, 0.01);
+          0.2, 0.001, 0.001, 0.001, 1e-3, 1e-3, 1.0, 0.2, 0.01);
 
   /// Construct Ada metaskeleton using a URI.
   /// \param[in] env World (either for planning, post-processing, or executing).
@@ -291,6 +292,18 @@ class Ada final : public aikido::robot::Robot
       double angularTolerance,
       const std::vector<double>& velocityLimits = std::vector<double>());
 
+  /// Moves the end effector with a certain twist. ? within a durations?
+  /// Throws a runtime_error if no trajectory could be found.
+  /// \return True if the trajectory was completed successfully.
+  bool moveArmWithEndEffectorTwist(
+      const Eigen::Vector6d& twists,
+      double durations,
+      const aikido::constraint::dart::CollisionFreePtr& collisionFree,
+      double timelimit,
+      double positionTolerance,
+      double angularTolerance,
+      const std::vector<double>& velocityLimits);
+
   /// Moves the robot to a configuration.
   /// Throws a runtime_error if no trajectory could be found.
   /// \return True if the trajectory was completed successfully.
@@ -327,13 +340,59 @@ class Ada final : public aikido::robot::Robot
       double timelimit,
       double positionTolerance,
       double angularTolerance);
+// designate a startState
+  aikido::trajectory::TrajectoryPtr planArmToEndEffectorOffset(
+      const Eigen::Vector3d& direction,
+      double length,
+      State* startState,
+      const aikido::constraint::dart::CollisionFreePtr& collisionFree,
+      double timelimit,
+      double positionTolerance,
+      double angularTolerance);
+
+//===========================================
+  aikido::trajectory::TrajectoryPtr planToEndEffectorPose(
+    const Eigen::Isometry3d& goalPose,
+    double conversionRatioInGeodesicDistance,
+    // State* startState,
+    const aikido::constraint::dart::CollisionFreePtr& collisionFree,
+    double timelimit,
+    double poseErrorTolerance);
+
+
+  aikido::trajectory::TrajectoryPtr planWithEndEffectorTwist(
+    const Eigen::Vector6d& twists,
+    double durations,
+    const aikido::constraint::dart::CollisionFreePtr& collisionFree,
+    double timelimit,
+    double positionTolerance,
+    double angularTolerance);
+
+// designate a startState
+  aikido::trajectory::TrajectoryPtr planWithEndEffectorTwist(
+    const Eigen::Vector6d& twists,
+    double durations,
+    State* startState,
+    const aikido::constraint::dart::CollisionFreePtr& collisionFree,
+    double timelimit,
+    double positionTolerance,
+    double angularTolerance);
 
   /// Opens Ada's hand
   void openHand();
 
   /// Closes Ada's hand
   void closeHand();
 
+  /// Switches between controllers.
+  /// \param[in] startControllers Controllers to start.
+  /// \param[in] stopControllesr Controllers to stop.
+  bool switchControllers(
+      const std::vector<std::string>& startControllers,
+      const std::vector<std::string>& stopControllers);
+    // Trajectory executor.
+  std::shared_ptr<aikido::control::TrajectoryExecutor> mTrajectoryExecutor;
+
 private:
   // Named Configurations are read from a YAML file
   using ConfigurationMap = std::unordered_map<std::string, Eigen::VectorXd>;
@@ -363,13 +422,6 @@ class Ada final : public aikido::robot::Robot
   std::shared_ptr<aikido::control::TrajectoryExecutor>
   createTrajectoryExecutor();
 
-  /// Switches between controllers.
-  /// \param[in] startControllers Controllers to start.
-  /// \param[in] stopControllesr Controllers to stop.
-  bool switchControllers(
-      const std::vector<std::string>& startControllers,
-      const std::vector<std::string>& stopControllers);
-
   /// Plans the end effector to a TSR.
   /// Throws a runtime_error if no trajectory could be found.
   /// \return trajectory if the planning is successful.
@@ -384,7 +436,7 @@ class Ada final : public aikido::robot::Robot
   const bool mSimulation;
 
   // Names of the trajectory executors
-  const std::string mArmTrajectoryExecutorName;
+  std::string mArmTrajectoryExecutorName;
   const std::string mHandTrajectoryExecutorName = "j2n6s200_hand_controller";
 
   // Collision resolution.
@@ -424,9 +476,6 @@ class Ada final : public aikido::robot::Robot
   // Thread for updating joint state from the Ros joint state client.
   std::unique_ptr<aikido::common::ExecutorThread> mJointStateThread;
 
-  // Trajectory executor.
-  std::shared_ptr<aikido::control::TrajectoryExecutor> mTrajectoryExecutor;
-
   // Name of the first link of the arm in the URDF
   std::string mArmBaseName;
 

include/libada/AdaHand.hpp

@@ -127,12 +127,7 @@ class AdaHand : public aikido::robot::Hand
   ///
   /// Maps an end-effector transform name (string) to a transform.
   using EndEffectorTransformMap = std::
-      unordered_map<std::string,
-                    Eigen::Isometry3d,
-                    std::hash<std::string>,
-                    std::equal_to<std::string>,
-                    Eigen::aligned_allocator<std::pair<const std::string,
-                                                       Eigen::Isometry3d>>>;
+      unordered_map<std::string, Eigen::Isometry3d, std::hash<std::string>, std::equal_to<std::string>, Eigen::aligned_allocator<std::pair<const std::string, Eigen::Isometry3d>>>;
 
   /// Create controllers in real or simulation.
   ///

include/libada/AdaHandKinematicSimulationPositionCommandExecutor.hpp

@@ -81,9 +81,9 @@ class AdaHandKinematicSimulationPositionCommandExecutor
       = std::array<std::size_t, kNumPositionExecutors>{{1, 1}};
 
   /// Executor for proximal and distal joints
-  std::array<AdaFingerKinematicSimulationPositionCommandExecutorPtr,
-             kNumPositionExecutors>
-      mPositionCommandExecutors;
+  std::
+      array<AdaFingerKinematicSimulationPositionCommandExecutorPtr, kNumPositionExecutors>
+          mPositionCommandExecutors;
 
   /// Duration to wait for futures from executors
   constexpr static auto kWaitPeriod = std::chrono::milliseconds(0);

include/libada/util.hpp

@@ -12,6 +12,8 @@
 namespace ada {
 namespace util {
 
+void waitForAnyKey();
+
 /// Displays a message and waits for the user to press the enter key.
 /// Terminates if 'n' is pressed.
 /// \param[in] msg The message to display.

src/Ada.cpp

@@ -295,7 +295,7 @@ std::unique_ptr<aikido::trajectory::Spline> Ada::retimePathWithKunz(
 //==============================================================================
 std::future<void> Ada::executeTrajectory(const TrajectoryPtr& trajectory) const
 {
-  return mRobot->executeTrajectory(trajectory);
+  return mTrajectoryExecutor->execute(trajectory);
 }
 
 //==============================================================================
@@ -342,6 +342,7 @@ void Ada::step(const std::chrono::system_clock::time_point& timepoint)
   std::lock_guard<std::mutex> lock(mRobotSkeleton->getMutex());
   mRobot->step(timepoint);
   mArm->step(timepoint);
+  mTrajectoryExecutor->step(timepoint);
 
   if (!mSimulation)
   {
@@ -648,7 +649,12 @@ bool Ada::switchControllers(
       = controller_manager_msgs::SwitchControllerRequest::STRICT;
 
   if (mControllerServiceClient->call(srv) && srv.response.ok)
+  {
+    mArmTrajectoryExecutorName = startControllers[0];
+    mTrajectoryExecutor = createTrajectoryExecutor();
+    ROS_INFO_STREAM("Controllers switched");
     return true;
+  }
   else
     throw std::runtime_error("SwitchController failed.");
 }
@@ -695,7 +701,10 @@ bool Ada::moveArmToTSR(
       = planArmToTSR(tsr, collisionFree, timelimit, maxNumTrials, ranker);
 
   if (!trajectory)
+  {
+    dtwarn << "Failed to plan to tsr" << std::endl;
     return false;
+  }
 
   return moveArmOnTrajectory(
       trajectory, collisionFree, postprocessType, velocityLimits);
@@ -725,6 +734,30 @@ bool Ada::moveArmToEndEffectorOffset(
   return moveArmOnTrajectory(traj, collisionFree, KUNZ, velocityLimits);
 }
 
+//==============================================================================
+bool Ada::moveArmWithEndEffectorTwist(
+    const Eigen::Vector6d& twists,
+    double durations,
+    const aikido::constraint::dart::CollisionFreePtr& collisionFree,
+    double timelimit,
+    double positionTolerance,
+    double angularTolerance,
+    const std::vector<double>& velocityLimits)
+{
+  auto traj = planWithEndEffectorTwist(
+    twists,
+    durations,
+    collisionFree,
+    timelimit,
+    positionTolerance,
+    angularTolerance);
+
+  if (!traj)
+    return false;
+
+  return moveArmOnTrajectory(traj, collisionFree, KUNZ, velocityLimits);
+}
+
 //==============================================================================
 aikido::trajectory::TrajectoryPtr Ada::planArmToEndEffectorOffset(
     const Eigen::Vector3d& direction,
@@ -747,6 +780,105 @@ aikido::trajectory::TrajectoryPtr Ada::planArmToEndEffectorOffset(
 
   return trajectory;
 }
+//========================================================================
+/*
+  designate a startState
+*/
+aikido::trajectory::TrajectoryPtr Ada::planArmToEndEffectorOffset(
+    const Eigen::Vector3d& direction,
+    double length,
+    State* startState,
+    const aikido::constraint::dart::CollisionFreePtr& collisionFree,
+    double timelimit,
+    double positionTolerance,
+    double angularTolerance)
+{
+  auto trajectory = mArm->planToEndEffectorOffset(
+      mArmSpace,
+      startState,
+      mArm->getMetaSkeleton(),
+      mHand->getEndEffectorBodyNode(),
+      collisionFree,
+      direction,
+      length,
+      timelimit,
+      positionTolerance,
+      angularTolerance);
+
+  return trajectory;
+}
+
+//========================================================================
+aikido::trajectory::TrajectoryPtr Ada::planToEndEffectorPose(
+    const Eigen::Isometry3d& goalPose,
+    double conversionRatioInGeodesicDistance,
+    // State* startState,
+    const aikido::constraint::dart::CollisionFreePtr& collisionFree,
+    double timelimit,
+    double poseErrorTolerance)
+{
+  auto trajectory = mArm->planToEndEffectorPose(
+    mArmSpace,
+    // State* startState,
+    mArm->getMetaSkeleton(),
+    mHand->getEndEffectorBodyNode(),
+    collisionFree,
+    goalPose,
+    conversionRatioInGeodesicDistance,
+    timelimit,
+    poseErrorTolerance);
+
+  return trajectory;
+}
+
+aikido::trajectory::TrajectoryPtr Ada::planWithEndEffectorTwist(
+    const Eigen::Vector6d& twists,
+    double durations,
+    const aikido::constraint::dart::CollisionFreePtr& collisionFree,
+    double timelimit,
+    double positionTolerance,
+    double angularTolerance)
+{
+  auto trajectory = mArm->planWithEndEffectorTwist(
+      mArmSpace,
+      mArm->getMetaSkeleton(),
+      mHand->getEndEffectorBodyNode(),
+      twists,
+      durations,
+      collisionFree,
+      timelimit,
+      positionTolerance,
+      angularTolerance);
+
+  return trajectory;
+}
+
+//==============================================================================
+// designate a startState
+
+aikido::trajectory::TrajectoryPtr Ada::planWithEndEffectorTwist(
+    const Eigen::Vector6d& twists,
+    double durations,
+    State* startState,
+    const aikido::constraint::dart::CollisionFreePtr& collisionFree,
+    double timelimit,
+    double positionTolerance,
+    double angularTolerance)
+{
+   auto trajectory = mArm->planWithEndEffectorTwist(
+      mArmSpace,
+      startState,
+      mArm->getMetaSkeleton(),
+      mHand->getEndEffectorBodyNode(),
+      twists,
+      durations,
+      collisionFree,
+      timelimit,
+      positionTolerance,
+      angularTolerance);
+
+  return trajectory;
+}
 
 //==============================================================================
 bool Ada::moveArmToConfiguration(
@@ -772,9 +904,7 @@ bool Ada::moveArmOnTrajectory(
     std::vector<double> smoothVelocityLimits)
 {
   if (!trajectory)
-  {
-    throw std::runtime_error("Trajectory execution failed: Empty trajectory.");
-  }
+    return false;
 
   std::vector<aikido::constraint::ConstTestablePtr> constraints;
 

src/AdaHandKinematicSimulationPositionCommandExecutor.cpp

@@ -8,14 +8,12 @@ constexpr std::chrono::milliseconds
     AdaHandKinematicSimulationPositionCommandExecutor::kWaitPeriod;
 constexpr int
     AdaHandKinematicSimulationPositionCommandExecutor::kNumPositionExecutors;
-constexpr std::array<std::size_t,
-                     AdaHandKinematicSimulationPositionCommandExecutor::
-                         kNumPositionExecutors>
-    AdaHandKinematicSimulationPositionCommandExecutor::kPrimalDofs;
-constexpr std::array<std::size_t,
-                     AdaHandKinematicSimulationPositionCommandExecutor::
-                         kNumPositionExecutors>
-    AdaHandKinematicSimulationPositionCommandExecutor::kDistalDofs;
+constexpr std::
+    array<std::size_t, AdaHandKinematicSimulationPositionCommandExecutor::kNumPositionExecutors>
+        AdaHandKinematicSimulationPositionCommandExecutor::kPrimalDofs;
+constexpr std::
+    array<std::size_t, AdaHandKinematicSimulationPositionCommandExecutor::kNumPositionExecutors>
+        AdaHandKinematicSimulationPositionCommandExecutor::kDistalDofs;
 
 //==============================================================================
 AdaHandKinematicSimulationPositionCommandExecutor::

src/util.cpp

@@ -12,6 +12,11 @@
 namespace ada {
 namespace util {
 
+//==============================================================================
+ void waitForAnyKey() {
+  std::cout << "Press Enter to Continue... \n";
+  std::cin.ignore();
+ }
 //==============================================================================
 void waitForUser(const std::string& msg, const std::shared_ptr<Ada>& ada)
 {