Large update: Re-implement the controller to specialize in arm retarg…

…eting and add a calibration function.

etc/HumanRetargetingController.in.yaml

@@ -3,6 +3,8 @@
 Managed: false
 # If true and the FSM is self-managed, transitions should be triggered
 StepByStep: true
+# When true reset the posture tasks to the current posture before transitioning to the next state
+ResetPostures: false
 # Change idle behaviour, if true the state is kept until transition,
 # otherwise the FSM holds the last state until transition
 IdleKeepState: true

include/HumanRetargetingController/ArmRetargetingManager.h

@@ -0,0 +1,248 @@
+#pragma once
+
+#include <optional>
+
+#include <TrajColl/CubicInterpolator.h>
+
+#include <mc_rtc/gui/StateBuilder.h>
+#include <mc_rtc/log/Logger.h>
+
+#include <HumanRetargetingController/ArmSide.h>
+
+namespace mc_tasks
+{
+struct TransformTask;
+} // namespace mc_tasks
+
+namespace HRC
+{
+class HumanRetargetingController;
+class RosPoseManager;
+
+/** \brief Retargeting manager.
+
+    Retargeting manager manages the retargeting task.
+*/
+class ArmRetargetingManager
+{
+  // Allow access to protected members
+  friend class RetargetingManagerSet;
+
+public:
+  /** \brief Configuration. */
+  struct Configuration
+  {
+    //! Arm side
+    ArmSide armSide;
+
+    //! Topic name of human elbow pose
+    std::string humanElbowPoseTopicName;
+
+    //! Topic name of human wrist pose
+    std::string humanWristPoseTopicName;
+
+    //! Body part of elbow retargeting task
+    std::string elbowBodyPart;
+
+    //! Body part of wrist retargeting task
+    std::string wristBodyPart;
+
+    //! Retargeting task stiffness
+    Eigen::Vector6d stiffness = Eigen::Vector6d::Constant(10);
+
+    //! mc_rtc configuration of calibration result
+    mc_rtc::Configuration calibResultConfig;
+
+    //! Robot postures for calibration
+    std::map<std::string, std::map<std::string, double>> robotCalibPostures;
+
+    /** \brief Load mc_rtc configuration.
+        \param mcRtcConfig mc_rtc configuration
+    */
+    void load(const mc_rtc::Configuration & mcRtcConfig);
+  };
+
+  /** \brief Calibration result. */
+  struct CalibResult
+  {
+    //! Whether initialized or not
+    bool isInitialized;
+
+    //! Transformation from human base (i.e., waist) to shoulder
+    sva::PTransformd humanTransFromBaseToShoulder;
+
+    //! Transformation from robot base to shoulder
+    sva::PTransformd robotTransFromBaseToShoulder;
+
+    //! Rotational transformation from human elbow to robot elbow
+    sva::PTransformd elbowRotTransFromHumanToRobot;
+
+    //! Rotational transformation from human wrist to robot wrist
+    sva::PTransformd wristRotTransFromHumanToRobot;
+
+    /** \brief Scale of elbow position relative to shoulder
+
+        Set to a value greater than 1 if the robot's scale is greater than the human's.
+    */
+    double elbowScale;
+
+    /** \brief Scale of wrist position relative to shoulder
+
+        Set to a value greater than 1 if the robot's scale is greater than the human's.
+    */
+    double wristScale;
+
+    /** \brief Constructor. */
+    CalibResult();
+
+    /** \brief Reset. */
+    void reset();
+
+    /** \brief Load mc_rtc configuration.
+        \param mcRtcConfig mc_rtc configuration
+    */
+    void load(const mc_rtc::Configuration & mcRtcConfig);
+
+    /** \brief Dump in YAML format. */
+    void dump() const;
+  };
+
+  /** \brief Calibration source.
+
+      The format is {axis, {elbow pose, wrist pose}}.
+      The axis is one of "X", "Y", or "Z".
+      The elbow pose and wrist pose are expressed relative to the base pose.
+   */
+  using CalibSource = std::unordered_map<std::string, std::array<sva::PTransformd, 2>>;
+
+public:
+  /** \brief Constructor.
+      \param ctlPtr pointer to controller
+      \param mcRtcConfig mc_rtc configuration
+  */
+  ArmRetargetingManager(HumanRetargetingController * ctlPtr, const mc_rtc::Configuration & mcRtcConfig = {});
+
+  /** \brief Reset.
+
+      This method should be called once when controller is reset.
+  */
+  void reset();
+
+  /** \brief Update.
+
+      This method should be called once every control cycle.
+  */
+  void update();
+
+  /** \brief Stop.
+
+      This method should be called once when stopping the controller.
+  */
+  void stop();
+
+  /** \brief Const accessor to the configuration. */
+  inline const Configuration & config() const noexcept
+  {
+    return config_;
+  }
+
+  /** \brief Add entries to the GUI. */
+  void addToGUI(mc_rtc::gui::StateBuilder & gui);
+
+  /** \brief Remove entries from the GUI. */
+  void removeFromGUI(mc_rtc::gui::StateBuilder & gui);
+
+  /** \brief Add entries to the logger. */
+  void addToLogger(mc_rtc::Logger & logger);
+
+  /** \brief Remove entries from the logger. */
+  void removeFromLogger(mc_rtc::Logger & logger);
+
+protected:
+  /** \brief Const accessor to the controller. */
+  inline const HumanRetargetingController & ctl() const
+  {
+    return *ctlPtr_;
+  }
+
+  /** \brief Accessor to the controller. */
+  inline HumanRetargetingController & ctl()
+  {
+    return *ctlPtr_;
+  }
+
+  /** \brief Enable retargeting. */
+  void enable();
+
+  /** \brief Disable retargeting. */
+  void disable();
+
+  /** \brief Accessor to the ROS pose manager for human waist pose. */
+  const std::shared_ptr<RosPoseManager> & humanWaistPoseManager() const;
+
+  /** \brief Update target of retargeting task. */
+  void updateTaskTarget(const std::shared_ptr<mc_tasks::TransformTask> & task, const sva::PTransformd & pose);
+
+  /** \brief Set human data as calibration source. */
+  void setHumanCalibSource(const std::string & axis);
+
+  /** \brief Set robot data as calibration source. */
+  void setRobotCalibSource(const std::string & axis);
+
+  /** \brief Clear robot for calibration. */
+  void clearCalibRobot();
+
+  /** \brief Update calibration. */
+  void updateCalib();
+
+  /** \brief Calculate shoulder pose for calibration. */
+  sva::PTransformd calcShoulderPoseForCalib(const CalibSource & calibSource) const;
+
+  /** \brief Calculate lengths of elbow and wrist from shoulder for calibration. */
+  std::array<double, 2> calcLimbLengthsForCalib(const CalibSource & calibSource,
+                                                const sva::PTransformd & shoulderPose) const;
+
+protected:
+  //! Configuration
+  Configuration config_;
+
+  //! Pointer to controller
+  HumanRetargetingController * ctlPtr_ = nullptr;
+
+  //! ROS pose manager for human elbow pose
+  std::shared_ptr<RosPoseManager> humanElbowPoseManager_;
+
+  //! ROS pose manager for human wrist pose
+  std::shared_ptr<RosPoseManager> humanWristPoseManager_;
+
+  //! Target pose of robot shoulder
+  std::optional<sva::PTransformd> robotShoulderPose_ = std::nullopt;
+
+  //! Target pose of robot elbow
+  std::optional<sva::PTransformd> robotElbowPose_ = std::nullopt;
+
+  //! Target pose of robot wrist
+  std::optional<sva::PTransformd> robotWristPose_ = std::nullopt;
+
+  //! Retargeting task of elbow
+  std::shared_ptr<mc_tasks::TransformTask> elbowTask_;
+
+  //! Retargeting task of wrist
+  std::shared_ptr<mc_tasks::TransformTask> wristTask_;
+
+  //! Function to interpolate task stiffness
+  std::shared_ptr<TrajColl::CubicInterpolator<double>> stiffnessRatioFunc_;
+
+  //! Human data as calibration source
+  CalibSource humanCalibSource_;
+
+  //! Robot data as calibration source
+  CalibSource robotCalibSource_;
+
+  //! Calibration result
+  CalibResult calibResult_;
+
+  //! Robot for calibration
+  std::shared_ptr<mc_rbdyn::Robots> calibRobots_;
+};
+} // namespace HRC

include/HumanRetargetingController/ArmSide.h

@@ -0,0 +1,40 @@
+#pragma once
+
+#include <set>
+
+namespace HRC
+{
+/** \brief Left/right side of the arm. */
+enum class ArmSide
+{
+  //! Left arm
+  Left = 0,
+
+  //! Right arm
+  Right
+};
+
+namespace ArmSides
+{
+//! Both arms
+const std::set<ArmSide> Both = {ArmSide::Left, ArmSide::Right};
+} // namespace ArmSides
+
+/** \brief Convert string to arm side. */
+ArmSide strToArmSide(const std::string & armSideStr);
+
+/** \brief Get the opposite arm side. */
+ArmSide opposite(const ArmSide & armSide);
+
+/** \brief Get the sign of arm side.
+
+    Positive for left arm side, negative for right arm side.
+*/
+int sign(const ArmSide & armSide);
+} // namespace HRC
+
+namespace std
+{
+/** \brief Convert armSide to string. */
+std::string to_string(const HRC::ArmSide & armSide);
+} // namespace std

include/HumanRetargetingController/HumanRetargetingController.h

@@ -18,7 +18,7 @@ struct HumanRetargetingController : public BWC::BaselineWalkingController
   /** \brief Constructor.
       \param rm robot module
       \param dt control timestep
-      \param _config controller configuration
+      \param config controller configuration
       \param allowEmptyManager whether to allow the managers to be empty (assuming initialized in the parent class)
    */
   HumanRetargetingController(mc_rbdyn::RobotModulePtr rm,
@@ -48,7 +48,7 @@ struct HumanRetargetingController : public BWC::BaselineWalkingController
   //! Retargeting tasks
   std::unordered_map<std::string, std::shared_ptr<mc_tasks::TransformTask>> retargetingTasks_;
 
-  //! Manipulation manager
+  //! Retargeting manager set
   std::shared_ptr<RetargetingManagerSet> retargetingManagerSet_;
 };
 } // namespace HRC