Add PostureManager #7

orikuma · 2023-09-25T05:51:08Z

Problem:
When I want to play multi-contact motion planned by a multi-contact motion planner, resulting motion of MultiContactController is different from the planned motion because the reference joint angle is always the same in QP solved in MultiContactController.

Solution:
To specify target postures in multi-contact locomotion, I added PostureManager.
It loads target postures from nominalPostureList in MultiContactController.yaml and set them to PostureTask in QP according to the specified time.
The entry in the yaml file should be as follows.

  MCC::ConfigMotion_:
    base: MCC::ConfigMotion
    configs:
      baseTime: Relative
      nominalPostureList:
        - time: 0.4
          target:
            JOINT0: [0.05]
            JOINT1: [0.01]

…sk according to reference configurations

…stures

…ng controllers

…nState

mmurooka

Thank you for the PR! Sorry for the detailed (and in some cases personal preference) remarks.

mmurooka · 2024-04-11T03:01:34Z

include/MultiContactController/PostureManager.h

+  /** \brief Remove entries from the logger. */
+  virtual void removeFromLogger(mc_rtc::Logger & logger);
+
+  /** \brief Append a nominal centroidal pose


Comment is not valid.

Fixed in 3e96498

mmurooka · 2024-04-11T03:01:51Z

include/MultiContactController/PostureManager.h

+  /** \brief Append a nominal centroidal pose
+      \param t time
+      \param PostureMap map from joint names to joint angle
+      \return whether nominalCentroidalPose is appended


Comment is not valid.

Fixed in 3e96498

mmurooka · 2024-04-11T03:14:29Z

include/MultiContactController/PostureManager.h

+
+      This method should be called once when controller is reset.
+   */
+  virtual void reset(const PostureManager::PostureMap & initialPosture);


initialPosture = {}とデフォルト値を指定して，引数無しのresetメソッドは消去することってできるんでしたでしょうか？

確かにデフォルト値の指定ができたので 3e96498 で修正しました

mmurooka · 2024-04-11T03:15:25Z

src/PostureManager.cpp

+void PostureManager::update()
+{
+  // Set data
+  PostureManager::PostureMap ref = getNominalPosture(ctl().t());


Is PostureManager:: necessary for PostureMap here?

ref -> nominalPosture

Fixed in 3e96498

mmurooka · 2024-04-11T03:18:30Z

src/PostureManager.cpp

+PostureManager::PostureMap PostureManager::getNominalPosture(double t) const
+{
+  // if nominalPostureList_ is empty or specified past time, return empty map
+  PostureManager::PostureMap postures;


postures -> nominalPosture

Fixed in 3e96498

mmurooka · 2024-04-11T03:19:08Z

src/PostureManager.cpp

+  return postures;
+}
+
+bool PostureManager::appendNominalPosture(double t, const PostureManager::PostureMap & postures)


postures -> nominalPosture (same for header)

Fixed in 3e96498

mmurooka · 2024-04-11T03:20:54Z

src/states/ConfigMotionState.cpp

+      {
+        time += baseTime;
+      }
+      PostureManager::PostureMap refPosture = nominalPostureConfig("target");


refPosture -> nominalPosture

Fixed in 3e96498

mmurooka · 2024-04-11T03:23:26Z

src/states/InitialState.cpp

+    if(config_.has("configs") && config_("configs").has("nominalPosture"))
+    {
+      mc_rtc::Configuration nominalPostureConfig = config_("configs")("nominalPosture");
+      PostureManager::PostureMap initPosture = nominalPostureConfig("target");


initPosture -> initialPosture

Fixed in 3e96498

mmurooka · 2024-04-11T03:26:20Z

include/MultiContactController/PostureManager.h

+
+  /** \brief Append a nominal centroidal pose
+      \param t time
+      \param PostureMap map from joint names to joint angle


Please write [rad] or [deg]

Fixed in 3e96498

mmurooka · 2024-04-11T03:29:14Z

src/PostureManager.cpp

+
+PostureManager::PostureMap PostureManager::getNominalPosture(double t) const
+{
+  // if nominalPostureList_ is empty or specified past time, return empty map


if -> If

Fixed in 3e96498

mmurooka · 2024-04-11T03:35:03Z

include/MultiContactController/PostureManager.h

+  */
+  virtual void stop();
+
+  // TODO: implement GUI functions


以下のように現在指定している関節数がGUIで表示できたら良いかと思いますがいかがでしょうか．

Number of specified joints: 6

21e4efa でRvizに現在nominalPostureとして指定されている関節数を表示するようにしました

mmurooka · 2024-04-11T07:14:49Z

src/states/ConfigMotionState.cpp

@@ -89,6 +105,11 @@ void ConfigMotionState::start(mc_control::fsm::Controller & _ctl)
    }
  }

+  if(config_.has("configs") && config_("configs").has("exitWhenPostureManagerFinished"))


Sorry, but after reading the following page, I thought that the original expression was safer and needless to change, since it allowed us to check corectness of the type.
https://jrl-umi3218.github.io/mc_rtc/tutorials/usage/mc_rtc_configuration.html

~~Could this be replaced by the following?~~

if(config_.has("configs")) { config_("configs")("exitWhenPostureManagerFinished", exitWhenPostureManagerFinished_); }

orikuma added 10 commits September 19, 2023 15:19

[WIP] add PostureManager, which update reference posture in PostureTa…

6a47f24

…sk according to reference configurations

[WIP] use PostureManager in MultiContactController

1a7eb93

Configuration in PostureManager is not mandatory

f503af7

Preserve map from joint names to joint angles instead of vector of po…

362a85d

…stures

Register logger for PostureManager

e8d322c

Load nominal posture trajectory from configurations

e19c408

Reset PostureManager in InitialState

63493b2

Clear nominalPostureList_ in PostureManager::reset

c3a7817

Show joint name for nominalPosture in mc_rtc log

ca55e66

Apply .clang-format-fix.sh

0283198

mmurooka self-requested a review September 25, 2023 07:53

orikuma mentioned this pull request Oct 18, 2023

Overwrite nominal centroidal pose in initial state #8

Closed

orikuma added 4 commits October 19, 2023 15:53

Set initial posture by nominalPosture in MCC::Initial

f1c716e

Do not reset shared_ptr for PostureManger to prevent SEGV when changi…

53a8c66

…ng controllers

Merge remote-tracking branch 'origin/master' into add-posture-manager

a86b9c0

Add option to wait for finishing nominal postures to exit ConfigMotio…

e762b60

…nState

mmurooka approved these changes Apr 11, 2024

View reviewed changes

mmurooka reviewed Apr 11, 2024

View reviewed changes

orikuma added a commit to orikuma/MultiContactController that referenced this pull request Apr 11, 2024

Fix PR comment in isri-aist#7

3e96498

mmurooka reviewed Apr 11, 2024

View reviewed changes

orikuma mentioned this pull request Apr 12, 2024

Put PRs for multi-contact locomotion together for easy merge #16

Merged

mmurooka closed this in #16 Apr 13, 2024

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add PostureManager #7

Add PostureManager #7

orikuma commented Sep 25, 2023 •

edited

Loading

mmurooka left a comment

mmurooka Apr 11, 2024

orikuma Apr 12, 2024

mmurooka Apr 11, 2024

orikuma Apr 12, 2024

mmurooka Apr 11, 2024

orikuma Apr 12, 2024

mmurooka Apr 11, 2024

orikuma Apr 12, 2024

mmurooka Apr 11, 2024

orikuma Apr 12, 2024

mmurooka Apr 11, 2024

orikuma Apr 12, 2024 •

edited

Loading

mmurooka Apr 11, 2024

orikuma Apr 12, 2024

mmurooka Apr 11, 2024

orikuma Apr 12, 2024

mmurooka Apr 11, 2024

orikuma Apr 12, 2024

mmurooka Apr 11, 2024

orikuma Apr 12, 2024

mmurooka Apr 11, 2024 •

edited

Loading

orikuma Apr 12, 2024

mmurooka Apr 11, 2024 •

edited

Loading

Add PostureManager #7

Add PostureManager #7

Conversation

orikuma commented Sep 25, 2023 • edited Loading

mmurooka left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

orikuma Apr 12, 2024 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

mmurooka Apr 11, 2024 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

mmurooka Apr 11, 2024 • edited Loading

Choose a reason for hiding this comment

orikuma commented Sep 25, 2023 •

edited

Loading

orikuma Apr 12, 2024 •

edited

Loading

mmurooka Apr 11, 2024 •

edited

Loading

mmurooka Apr 11, 2024 •

edited

Loading