invisible pose, chill pose, and feeding neighbor work

Author: amalnanavati

config/gen2_feeding_demo.yaml

@@ -56,7 +56,7 @@ wheelchair:
 feedingDemo:
   heightAbovePlate: 0.15
   heightAboveFood: 0.08
-  heightIntoFood: 0.085
+  heightIntoFood: 0.105
   moveOutofFood: 0.025
   #distanceToPerson: 0.25
   distanceToPerson: 0.05

config/named_configs_gen2_6dof.yaml

@@ -4,10 +4,16 @@ arm:
   # hardcoded pose in front of person
   # Wheelchair
   in_front_person_pose: [-2.30252, 4.23221, 3.84109, -4.65546, 3.94225, 4.26543]
+  # side_in_front_person_pose: [-0.28053, 4.316922, 4.187446, 0.562417, -1.9308152, -2.541829]
+  side_in_front_person_pose: [2.74709, 2.01400, 1.85257, -2.83523, -1.61925, -2.67325]
   # Tripod
   # moveIFOPose: [-1.81753, 4.32404, 4.295815, 3.12878, 1.89724, -0.61526]
   # Participant
   # moveIFOPose: [-2.30293, 4.04904, 3.63059, 1.62787, -2.34089, -2.01773]
   # Participant Tripod
   # moveIFOPose: [-1.81752, 4.60286, 4.64300, -3.05122, 1.89743, -0.61493]
   home_config: [-2.11666, 3.34967, 2.04129, -2.30031, -2.34026, 2.9545]
+  # kinova_invisible_pose: [-1.56836, 2.61908, 0.46859, -1.60935, 0.09055, 1.74078] # Collides with the table
+  invisible_pose: [-1.56836, 2.61908, 0.55755, -1.60935, 0.09055, 1.74078]
+  # feed_other_person: [-0.189133, 3.220472, 1.560045, -1.817854, 0.732292, 1.276713]
+  feed_other_person: [-0.378204, 3.410839, 1.590085, -2.009985, 0.857700, 1.464658]

include/feeding/util.hpp

@@ -61,7 +61,7 @@ void dumpSplinePhasePlot(const aikido::trajectory::Spline &spline,
 /// param[in] food_only If true, only food choices are valid
 /// param[in]] nodeHandle Ros Node to set food name for detection.
 std::string getUserFoodInput(bool food_only, ros::NodeHandle &nodeHandle,
-                             bool useAlexa = true, double timeout = 10);
+                             bool useAlexa = true, double timeout = 13);
 
 int getUserInputWithOptions(const std::vector<std::string> &optionPrompts,
                             const std::string &prompt);

scripts/demo.cpp

@@ -43,8 +43,28 @@ void demo(
     if (feedingDemo.getFTThresholdHelper())
         feedingDemo.getFTThresholdHelper()->setThresholds(STANDARD_FT_THRESHOLD);
 
+    const std::shared_ptr<::ada::Ada> &ada = feedingDemo.getAda();
+    int ifo_pose_i = getUserInputWithOptions(std::vector<std::string>{"(1) no","(2) yes"}, "Start in invisible pose?");
+    if  (ifo_pose_i == 2) {
+      // NOTE: This is dangerous since we aren't using a constraint! It is done
+      // to account for the strange orientation the head is sometimes placed in,
+      // after perceiveFace,which messes up collision checking. 
+      auto trajectory = ada->getArm()->planToConfiguration(
+        ada->getArm()->getNamedConfiguration("invisible_pose"));/*,
+        ada->getArm()->getWorldCollisionConstraint());*/
+      bool success = true;
+      try {
+        auto future = ada->getArm()->executeTrajectory(
+            trajectory); // check velocity limits are set in FeedingDemo
+        future.get();
+      } catch (const std::exception &e) {
+        dtwarn << "Exception in trajectoryExecution: " << e.what() << std::endl;
+        success = false;
+      }
+    }
+
     if (useSound)
-      talk("What food would you like?", false);
+      talk("What food would you like?", true);
 
     auto foodName = getUserFoodInput(false, nodeHandle, feedingDemo.mUseAlexa);// "cantaloupe";//
     if (foodName == std::string("quit")) {