Build Errors

[willthibault]

Hello @IoannisDadiotis

I am trying to build this package, but am experiencing build errors related to the following missing file:

#include <base_estimation/ContactsWrench.h>

I am currently building with the ioannis_centauro branch of base_estimation along with your fork of OCS2 (the robotx_school2024 branch as it is more up to date with the main OCS2). Currently, I can't find that header file in the base_estimation repo suggested, is it up to date?

Also, I noticed that in using the more recent version of your OCS2 fork that the following changes needed to be made. Here they are in case you choose to update to it:

diff --git a/gazebo_ocs2_ros_interfaces/include/gazebo_ocs2_ros_interfaces/common/RosMsgConversions.h b/gazebo_ocs2_ros_interfaces/include/gazebo_ocs2_ros_interfaces/common/RosMsgConversions.h
index 7de6e7b..b23b8ea 100644
--- a/gazebo_ocs2_ros_interfaces/include/gazebo_ocs2_ros_interfaces/common/RosMsgConversions.h
+++ b/gazebo_ocs2_ros_interfaces/include/gazebo_ocs2_ros_interfaces/common/RosMsgConversions.h
@@ -33,7 +33,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <ocs2_core/reference/ModeSchedule.h>
 #include <ocs2_core/reference/TargetTrajectories.h>
 #include <ocs2_mpc/SystemObservation.h>
-#include <ocs2_oc/oc_solver/PerformanceIndex.h>
+#include <ocs2_oc/oc_solver/oc_data/PerformanceIndex.h>

 // MPC messages
 #include <ocs2_msgs/mode_schedule.h>
diff --git a/ocs2_centauro/include/ocs2_centauro/LeggedRobotInterface.h b/ocs2_centauro/include/ocs2_centauro/LeggedRobotInterface.h
index 5d4c6c5..44f6b70 100644
--- a/ocs2_centauro/include/ocs2_centauro/LeggedRobotInterface.h
+++ b/ocs2_centauro/include/ocs2_centauro/LeggedRobotInterface.h
@@ -39,7 +39,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <ocs2_pinocchio_interface/PinocchioInterface.h>
 #include <ocs2_robotic_tools/common/RobotInterface.h>
 #include <ocs2_robotic_tools/end_effector/EndEffectorKinematics.h>
-#include <ocs2_sqp/MultipleShootingSettings.h>
+#include <ocs2_sqp/SqpSettings.h>

 #include "ocs2_centauro/common/ModelSettings.h"
 #include "ocs2_centauro/initialization/LeggedRobotInitializer.h"
@@ -77,7 +77,7 @@ class LeggedRobotInterface : public RobotInterface {
   const ddp::Settings& ddpSettings() const { return ddpSettings_; }
   const mpc::Settings& mpcSettings() const { return mpcSettings_; }
   const rollout::Settings& rolloutSettings() const { return rolloutSettings_; }
-  const multiple_shooting::Settings& sqpSettings() { return sqpSettings_; }
+  const sqp::Settings& sqpSettings() { return sqpSettings_; }

   const vector_t& getInitialState() const { return initialState_; }
   void setInitialState(const vector_t& initialState) { initialState_ = initialState; }
@@ -123,7 +123,7 @@ class LeggedRobotInterface : public RobotInterface {
   ModelSettings modelSettings_;
   ddp::Settings ddpSettings_;
   mpc::Settings mpcSettings_;
-  multiple_shooting::Settings sqpSettings_;
+  sqp::Settings sqpSettings_;

   std::unique_ptr<PinocchioInterface> pinocchioInterfacePtr_;
   CentroidalModelInfo centroidalModelInfo_;
diff --git a/ocs2_centauro/src/LeggedRobotInterface.cpp b/ocs2_centauro/src/LeggedRobotInterface.cpp
index 91eaa83..a952b18 100644
--- a/ocs2_centauro/src/LeggedRobotInterface.cpp
+++ b/ocs2_centauro/src/LeggedRobotInterface.cpp
@@ -112,7 +112,7 @@ LeggedRobotInterface::LeggedRobotInterface(const std::string& taskFile, const st
   ddpSettings_ = ddp::loadSettings(taskFile, "ddp", verbose);
   mpcSettings_ = mpc::loadSettings(taskFile, "mpc", verbose);
   rolloutSettings_ = rollout::loadSettings(taskFile, "rollout", verbose);
-  sqpSettings_ = multiple_shooting::loadSettings(taskFile, "multiple_shooting", verbose);
+  sqpSettings_ = sqp::loadSettings(taskFile, "multiple_shooting", verbose);

   // OptimalConrolProblem
   setupOptimalConrolProblem(taskFile, urdfFile, referenceFile, verbose);

This work looks great and I look forward to your reply. Thanks!

[IoannisDadiotis]

Hi @willthibault

Which package is giving you the header file error? It may be that I missed base_estimation as a dependency of gazebo_ocs2_ros_interfaces and ocs2_centauro. Can you try pulling the latest wb_mpc_centauro and base_estimation (branches main and ioannis_centauro) and build again?

Regarding my ocs2 fork, branch robotx_school2024 is not suitable for using with this package, it is for a different project. I suggest that you use the humanoids2023 branch. In case you need to have the latest ocs2 developments the commit 83bfc062fd8e2a895bb05dd0732bbf394e055840 has merged them and so I could checkout a new branch from that commit and review a pull request from your side for wb_mpc_centauro :)

[willthibault]

Hi @IoannisDadiotis,

Thanks for the quick reply!

Those changes you made to the dependencies fixed that missing file issue.

For the latest OCS2, I was in fact using the commit you mentioned above (83bfc062fd8e2a895bb05dd0732bbf394e055840) not the latest of robotx_school2024. I have made a pull request with changes for building with that version of OCS2 and a couple changes to the centauro_ddp.launch file so it runs.

I did notice that I needed a few other packages to get Centauro to visualize properly in Rviz. These packages were:

• iit-dagana-ros-pkg
• realsense_gazebo_description
• realsense_gazebo_plugin
• velodyne_simulator

I'm not sure if these are the same as what you used, but they seemed to work for me.

Now I can get Centauro to show up in Rviz, but shortly after launching the dummy node seems to die after the MPC starts. I get the following error message:

#####################################################
#####################################################
#################  MPC is reset.  ###################
#####################################################
#####################################################
[ INFO] [1717043646.216467276]: MPC node has been reset.
[ INFO] [1717043646.216555248]: resetMpcNodeCustom
WARNING: The solution time window might be shorter than the MPC delay!
[ INFO] [1717043647.501975450]: Initial policy has been received.
### Current time 0
<<< New MPC policy starting at 1e-09
[FATAL] [1717043647.509259830]: ASSERTION FAILED
        file = /opt/ros/noetic/include/ros/publisher.h
        line = 107
        cond = false
        message = 
[FATAL] [1717043647.509288389]: Call to publish() on an invalid Publisher
[FATAL] [1717043647.509297496]: 

[legged_robot_dummy-4] process has died [pid 35036, exit code -5, cmd /home/centauro_ocs2_ws/devel/lib/ocs2_centauro_ros/legged_robot_dummy __name:=legged_robot_dummy __log:=/root/.ros/log/d8c4c9ec-1e3d-11ef-9808-2c6dc10e565f/legged_robot_dummy-4.log].
log file: /root/.ros/log/d8c4c9ec-1e3d-11ef-9808-2c6dc10e565f/legged_robot_dummy-4*.log
WARNING: The solution time window might be shorter than the MPC delay!
[ WARN] [1717043648.034632735]: Interactive marker 'Goal' contains unnormalized quaternions. This warning will only be output once but may be true for others; enable DEBUG messages for ros.rviz.quaternions to see more details.
WARNING: The solution time window might be shorter than the MPC delay!

Do you have any ideas what may have caused this?

Also, I was wondering how I run the simulation in Gazebo. I understand I need xbot for this (so xbot needs to be configured in the launch file and task file), but is there a launch file for starting the Gazebo simulation?

Thanks!

[IoannisDadiotis]

Hi @IoannisDadiotis,

Thanks for the quick reply!

Those changes you made to the dependencies fixed that missing file issue.

For the latest OCS2, I was in fact using the commit you mentioned above (83bfc062fd8e2a895bb05dd0732bbf394e055840) not the latest of robotx_school2024. I have made a pull request with changes for building with that version of OCS2 and a couple changes to the centauro_ddp.launch file so it runs.

Great, thanks. I will review when I find some time. I did notice that I needed a few other packages to get Centauro to visualize properly in Rviz. These packages were:

• iit-dagana-ros-pkg
• realsense_gazebo_description
• realsense_gazebo_plugin
• velodyne_simulator

I'm not sure if these are the same as what you used, but they seemed to work for me.

Right, we have few sensors in the urdf so that's why you need these packages. Good to know that you have figured this out.

Now I can get Centauro to show up in Rviz, but shortly after launching the dummy node seems to die after the MPC starts. I get the following error message:

#####################################################
#####################################################
#################  MPC is reset.  ###################
#####################################################
#####################################################
[ INFO] [1717043646.216467276]: MPC node has been reset.
[ INFO] [1717043646.216555248]: resetMpcNodeCustom
WARNING: The solution time window might be shorter than the MPC delay!
[ INFO] [1717043647.501975450]: Initial policy has been received.
### Current time 0
<<< New MPC policy starting at 1e-09
[FATAL] [1717043647.509259830]: ASSERTION FAILED
        file = /opt/ros/noetic/include/ros/publisher.h
        line = 107
        cond = false
        message = 
[FATAL] [1717043647.509288389]: Call to publish() on an invalid Publisher
[FATAL] [1717043647.509297496]: 

[legged_robot_dummy-4] process has died [pid 35036, exit code -5, cmd /home/centauro_ocs2_ws/devel/lib/ocs2_centauro_ros/legged_robot_dummy __name:=legged_robot_dummy __log:=/root/.ros/log/d8c4c9ec-1e3d-11ef-9808-2c6dc10e565f/legged_robot_dummy-4.log].
log file: /root/.ros/log/d8c4c9ec-1e3d-11ef-9808-2c6dc10e565f/legged_robot_dummy-4*.log
WARNING: The solution time window might be shorter than the MPC delay!
[ WARN] [1717043648.034632735]: Interactive marker 'Goal' contains unnormalized quaternions. This warning will only be output once but may be true for others; enable DEBUG messages for ros.rviz.quaternions to see more details.
WARNING: The solution time window might be shorter than the MPC delay!

Do you have any ideas what may have caused this?

Seems like an assertion is failed. When the mpc is run the ros dummy node feeds the mpc node with the needed targets trajectories (state reference or EE pose targets if needed) here. I am not sure but it may be related with that. You can run centauro_ddp.launch with gdb:=true and try to spot the issue from the backtrace.

Also, I was wondering how I run the simulation in Gazebo. I understand I need xbot for this (so xbot needs to be configured in the launch file and task file), but is there a launch file for starting the Gazebo simulation?

For gazebo you need to configure xbot in the task file (xbotcorerunning and xbotcorefeedback variables). The xbot argument in the launch file is not needed (this was for running the mpc as a xbot plugin but i have not maintained this for a while).

For setting up the simulation of centauro you will need xbotcore binaries and iit-centauro-ros-pkg.

[willthibault]

Hi @IoannisDadiotis,

I believe I have tracked down the error mentioned above.

The backtrace from gdb showed the error coming from a publisher in LeggedRobotVisualizer.cpp with the error occurring at line 349. It seems that it is trying to publish to a publisher that does not exist. This is because the size of centroidalModelInfo_.numThreeDofContacts is 6 (4 feet and 2 hands), but only 4 publishers were declared earlier.

I have tested limiting to 4 publishers and adding two more publishers, which both yield similar results. It starts out seemingly stable, then the robot starts to spin and float away in Rviz (see the video below).

https://github.com/ADVRHumanoids/wb_mpc_centauro/assets/18635347/c8f3d778-55ce-42dc-a346-94a23eb3e92e

I get the following error and backtrace from gdb for the MPC node:

centauro_ddp_mpc: /home/centauro_ocs2_ws/src/wb_mpc_centauro/ocs2_centauro/src/foot_planner/CubicSpline.cpp:39: ocs2::legged_robot::CubicSpline::CubicSpline(ocs2::legged_robot::CubicSpline::Node, ocs2::legged_robot::CubicSpline::Node): Assertion `start.time < end.time' failed.

Thread 1 "centauro_ddp_mp" received signal SIGABRT, Aborted.
__GI_raise (sig=sig@entry=6) at ../sysdeps/unix/sysv/linux/raise.c:50
50  ../sysdeps/unix/sysv/linux/raise.c: No such file or directory.

#0  __GI_raise (sig=sig@entry=6) at ../sysdeps/unix/sysv/linux/raise.c:50
#1  0x00007607de683859 in __GI_abort () at abort.c:79
#2  0x00007607de683729 in __assert_fail_base (
    fmt=0x7607de819588 "%s%s%s:%u: %s%sAssertion `%s' failed.\n%n", 
    assertion=0x7607df8510aa "start.time < end.time", 
    file=0x7607df851050 "/home/centauro_ocs2_ws/src/wb_mpc_centauro/ocs2_centauro/src/foot_planner/CubicSpline.cpp", line=39, function=<optimized out>)
    at assert.c:92
#3  0x00007607de694fd6 in __GI___assert_fail (
    assertion=0x7607df8510aa "start.time < end.time", 
    file=0x7607df851050 "/home/centauro_ocs2_ws/src/wb_mpc_centauro/ocs2_centauro/src/foot_planner/CubicSpline.cpp", line=39, 
    function=0x7607df850fd0 "ocs2::legged_robot::CubicSpline::CubicSpline(ocs2::legged_robot::CubicSpline::Node, ocs2::legged_robot::CubicSpline::Node)")
    at assert.c:101
#4  0x00007607df65412f in ocs2::legged_robot::CubicSpline::CubicSpline (
    this=0x7fff26d613c0, start=..., end=...)
    at /home/centauro_ocs2_ws/src/wb_mpc_centauro/ocs2_centauro/src/foot_planner/CubicSpline.cpp:39
#5  0x00007607df661591 in ocs2::legged_robot::ArmSwingTrajectoryPlanner::updateAlongModes (this=0x5f78b9a538f0, initTime=1.0280000000000007, modeSchedule=...)
    at /home/centauro_ocs2_ws/src/wb_mpc_centauro/ocs2_centauro/src/foot_planner/ArmSwingTrajectoryPlanner.cpp:387
#6  0x00007607df6606df in ocs2::legged_robot::ArmSwingTrajectoryPlanner::update
    (this=0x5f78b9a538f0, modeSchedule=..., initTime=1.0280000000000007, 
    currentEePosition=..., currentEeOrientation=..., targetTrajectories=...)
    at /home/centauro_ocs2_ws/src/wb_mpc_centauro/ocs2_centauro/src/foot_planner/ArmSwingTrajectoryPlanner.cpp:306
#7  0x00007607df64e1cb in ocs2::legged_robot::SwitchedModelReferenceManager::modifyReferences (this=0x5f78b9a78030, initTime=1.0280000000000007, 
    finalTime=2.0280000000000005, initState=..., targetTrajectories=..., 
    modeSchedule=...)
    at /home/centauro_ocs2_ws/src/wb_mpc_centauro/ocs2_centauro/src/reference_manager/SwitchedModelReferenceManager.cpp:112
#8  0x00005f78b8314fcd in ocs2::ReferenceManagerDecorator::preSolverRun (
    this=0x5f78b9ffb950, initTime=1.0280000000000007, 
    finalTime=2.0280000000000005, initState=...)
    at /ocs2_catkin_ws/src/ocs2/ocs2_oc/include/ocs2_oc/synchronized_module/ReferenceManagerDecorator.h:52
#9  0x00007607e05cb0d9 in ocs2::SolverBase::preRun (
    this=this@entry=0x5f78ba282a70, 
    initTime=initTime@entry=1.0280000000000007, initState=..., 
    finalTime=finalTime@entry=2.0280000000000005)
    at /usr/include/c++/9/bits/shared_ptr_base.h:1020
#10 0x00007607e05cb541 in ocs2::SolverBase::run (this=0x5f78ba282a70, 
    initTime=1.0280000000000007, initState=..., finalTime=2.0280000000000005)
    at /ocs2_catkin_ws/src/ocs2/ocs2_oc/src/oc_solver/SolverBase.cpp:50
#11 0x00005f78b8312d1d in ocs2::GaussNewtonDDP_MPC::calculateController (
    this=0x7fff26d62840, initTime=1.0280000000000007, initState=..., 
    finalTime=2.0280000000000005)
    at /ocs2_catkin_ws/src/ocs2/ocs2_ddp/include/ocs2_ddp/GaussNewtonDDP_MPC.h:80
#12 0x00007607e0d4635a in ocs2::MPC_BASE::run (this=0x7fff26d62840, 
    currentTime=<optimized out>, currentState=...)
    at /ocs2_catkin_ws/src/ocs2/ocs2_mpc/src/MPC_BASE.cpp:75
#13 0x00007607e0395b4e in ocs2::MPC_ROS_Interface::mpcObservationCallback (
    this=0x7fff26d62b20, msg=...)
    at /home/centauro_ocs2_ws/src/wb_mpc_centauro/gazebo_ocs2_ros_interfaces/src/mpc/MPC_ROS_Interface.cpp:275
#14 0x00007607e03a5e0f in boost::_mfi::mf1<void, ocs2::MPC_ROS_Interface, boost::shared_ptr<ocs2_msgs::mpc_observation_<std::allocator<void> > const> const&>::operator() (this=0x5f78ba6f4f48, p=0x7fff26d62b20, a1=...)
    at /usr/include/boost/bind/mem_fn_template.hpp:165
#15 0x00007607e03a4cc5 in boost::_bi::list2<boost::_bi::value<ocs2::MPC_ROS_Interface*>, boost::arg<1> >::operator()<boost::_mfi::mf1<void, ocs2::MPC_ROS_Interface, boost::shared_ptr<ocs2_msgs::mpc_observation_<std::allocator<void> > const> const&>, boost::_bi::rrlist1<boost::shared_ptr<ocs2_msgs::mpc_observation_<std::allocator<void> > const> const&> > (this=0x5f78ba6f4f58, f=..., a=...)
--Type <RET> for more, q to quit, c to continue without paging--
    at /usr/include/boost/bind/bind.hpp:319
#16 0x00007607e03a3fed in boost::_bi::bind_t<void, boost::_mfi::mf1<void, ocs2::MPC_ROS_Interface, boost::shared_ptr<ocs2_msgs::mpc_observation_<std::allocator<void> > const> const&>, boost::_bi::list2<boost::_bi::value<ocs2::MPC_ROS_Interface*>, boost::arg<1> > >::operator()<boost::shared_ptr<ocs2_msgs::mpc_observation_<std::allocator<void> > const> const&> (this=0x5f78ba6f4f48, a1=...)
    at /usr/include/boost/bind/bind.hpp:1306
#17 0x00007607e03a344c in boost::detail::function::void_function_obj_invoker1<boost::_bi::bind_t<void, boost::_mfi::mf1<void, ocs2::MPC_ROS_Interface, boost::shared_ptr<ocs2_msgs::mpc_observation_<std::allocator<void> > const> const&>, boost::_bi::list2<boost::_bi::value<ocs2::MPC_ROS_Interface*>, boost::arg<1> > >, void, boost::shared_ptr<ocs2_msgs::mpc_observation_<std::allocator<void> > const> const&>::invoke (function_obj_ptr=..., a0=...)
    at /usr/include/boost/function/function_template.hpp:158
#18 0x00007607e0b51932 in boost::function1<void, boost::shared_ptr<ocs2_msgs::mpc_observation_<std::allocator<void> > const> const&>::operator() (
    this=0x5f78ba6f4f40, a0=...)
    at /usr/include/boost/function/function_template.hpp:763
#19 0x00007607e0b50b0c in boost::detail::function::void_function_obj_invoker1<boost::function<void (boost::shared_ptr<ocs2_msgs::mpc_observation_<std::allocator<void> > const> const&)>, void, boost::shared_ptr<ocs2_msgs::mpc_observation_<std::allocator<void> > const> >::invoke(boost::detail::function::function_buffer&, boost::shared_ptr<ocs2_msgs::mpc_observation_<std::allocator<void> > const>) (
    function_obj_ptr=..., a0=...)
    at /usr/include/boost/function/function_template.hpp:158
#20 0x00007607e0b5335b in boost::function1<void, boost::shared_ptr<ocs2_msgs::mpc_observation_<std::allocator<void> > const> >::operator() (
    this=0x5f78ba6f57d8, a0=...)
    at /usr/include/boost/function/function_template.hpp:763
#21 0x00007607e0b52ccd in ros::SubscriptionCallbackHelperT<boost::shared_ptr<ocs2_msgs::mpc_observation_<std::allocator<void> > const> const&, void>::call (
    this=0x5f78ba6f57d0, params=...)
    at /opt/ros/noetic/include/ros/subscription_callback_helper.h:144
#22 0x00007607deb8d139 in ros::SubscriptionQueue::call() ()
   from /opt/ros/noetic/lib/libroscpp.so
#23 0x00007607deb3b172 in ros::CallbackQueue::callOneCB(ros::CallbackQueue::TLS*) () from /opt/ros/noetic/lib/libroscpp.so
#24 0x00007607deb3c883 in ros::CallbackQueue::callAvailable(ros::WallDuration)
    () from /opt/ros/noetic/lib/libroscpp.so
#25 0x00007607e0396534 in ocs2::MPC_ROS_Interface::spin (this=0x7fff26d62b20)
    at /home/centauro_ocs2_ws/src/wb_mpc_centauro/gazebo_ocs2_ros_interfaces/src/mpc/MPC_ROS_Interface.cpp:346
#26 0x00007607e0396c52 in ocs2::MPC_ROS_Interface::launchNodes (
    this=0x7fff26d62b20, nodeHandle=...)
    at /home/centauro_ocs2_ws/src/wb_mpc_centauro/gazebo_ocs2_ros_interfaces/src/mpc/MPC_ROS_Interface.cpp:374
#27 0x00005f78b83110c2 in main (argc=1, argv=0x7fff26d633f8)
    at /home/centauro_ocs2_ws/src/wb_mpc_centauro/ocs2_centauro_ros/src/CentauroDdpMpcNode.cpp:123

Do you have any suggestions for how to resolve this issue seeing as fixing the number of publishers doesn't seem to completely solve the problem?

Also with respect to the Gazebo simulation, I have both of those packages installed but is there a launch file/startup sequence for loading Centauro in Gazebo?

Thanks!

[IoannisDadiotis]

Hi @willthibault , Thanks for finding the publishers' issue. This is only related with visualization of the mpc solution and should not affect the mpc itself.

I cannot reproduce this issue of the mpc node on my machine. Usually such a visual output where the robot state shifts may be an numerical integration issue (since when we run rviz only the state of the robot is received from forward simulating the system). Here are my suggestions to debug this:

• First check the printed time in the dummy node terminal. Is it real-time? If the time printed is too slow then this means you forgot to build the catkin workspace with -DCMAKE_BUILD_TYPE RelWithDebInfo.
• I would try to check where the times start.time and end.time that fail the assertion come from in /home/centauro_ocs2_ws/src/wb_mpc_centauro/ocs2_centauro/src/foot_planner/CubicSpline.cpp. Try to check if this happens for a specific end-effector.
• Try to run the mpc with gazebo in the loop. When we run the mpc with gazebo in a closed-loop the state of the robot is received from gazebo and we no longer forward simulate the system. Thus you will be able to understand if it is a numerical integration issue.

For gazebo simulation you can use the following launch file roslaunch centauro_gazebo centauro_world.launch. If you are able to see the robot then you can run on new terminals xbot with xbot2-core -S and its GUI with xbot2-gui.

[IoannisDadiotis]

Notice that if you are using the full centroidal dynamics in rviz only (no gazebo) you will need to re-tune a bit the control input cost as mentioned here, otherwise the forward simulation is unstable.

[willthibault]

Hi @IoannisDadiotis,

I wanted to follow up on my progress, sorry for the slow reply.

The issue I previously showed with Centauro spinning and floating away was mostly solved by building with -DCMAKE_BUILD_TYPE=RelWithDebInfo. This kept Centauro stable and I could move the en effectors around nicely.

Based on your work with Centauro I started a version intended for humanoids (pretty similar just only 2 legs). My initial conversion can be found here: humanoid_wb_mpc. At the moment, everything builds well and runs, but I encounter similar issues to that of Centauro (though I didn't forget the build flag this time). I was wondering if you have any suggestions for tuning or values that worked well for Centauro in Rviz? I tried increasing the control input cost as suggested, but didn't find that it improved noticeably. This is what I'm getting:

https://github.com/user-attachments/assets/de1c7c92-341c-4513-98cb-146cd0a7de5f

Currently, I am working with REEM-C, but will also add TALOS once the setup is stable and complete. I may use a slightly different setup to integrate Gazebo simulations though I need to spend more time looking into this. I'm happy to share progress with these developments.

Thanks!

[IoannisDadiotis]

Hi, Nice to hear you got this solved.

For your humanoid I see it is moving quite slow so I would check again if the issue is the same, i.e. check that the time printed in the dummy node terminal progresses real-time.

Then for further debugging I would try the following steps:

1. simplify the optimal control problem and keep the bare minimum terms since at the beginning it is good to just make sure the robot stands and then you can start adding additional functionalities. From the .info file you can disable self-collision avoidance, joint pos/vel limits constraints and any arm EE tracking (through soft/hard constraint). You can also disable the cost term based on R_taskspace so that you only have a state and control input terms in the cost function.
2. check for numerical instabilities by enabling the flag checkNumericalStability in the .info file. This will check and report in the mpc terminal if your formulation violates some assumptions that the ddp solver requires. Once you check that your formulation is ok you should set this to false again.
3. make sure the initial state vector corresponds to a statically stable configuration of the robot.
4. If you make sure to check the above then I guess you will have to start tuning the cost function, this is more of a trial and error process. Probably the first thing to try tuning is the matrix related with the control input gain, try increasing & reducing the gains on forces and then joint velocities to see if the performance becomes any better. Then you will have to follow similar procedure for the state-related gain as well.

I hope this helps!

[willthibault]

Hi @IoannisDadiotis,

Thanks for the tips! It seemed like a clean build fixed my issue (catkin clean then rebuild).

I have some follow up questions:

1. for walking, I see that I can adjust the swing_trajectory_config. when addPlanarConstraints is false then a dynamic stepping occurs (see attached video) and I can command the base to a certain position. When setting it to true and changing the "long" parameters the robot will walk forward with better swing steps without a base command but will become unstable. Is there a way to alter the step trajectory and still command a position (is this a tuning issue or maybe I'm not understanding the parameters/the way the foot trajectory is currently implemented)?
2. for moving the arms, I am trying to move the hands to a given position with the interactive markers. Activating the soft constraints to do this, seems to do very little. If I increase the mu values then the robot slides towards the marker (see video) but doesn't really move the arms. When I activate the constraints or planners of arm_swing_trajectory_config the robot immediately becomes unstable. What is the recommended way to move the arms for interactive marks and for arm swing during walking? Also, maybe this is another tuning issue I'm experiencing?
3. can you elaborate on the use of the R_taskspace weights?
4. I imagine the contacts for Centauro don't well represent REEM-C (big rectangular feet), are the friction cone contacts for Centauro essentially points at the wheel contacts?
5. My goal is to achieve a whole body MPC for loco-manipulation, did you have any luck with simultaneous arm manipulation during locomotion?

Here are the videos:

https://github.com/user-attachments/assets/1c6c4554-95e4-4de6-8ea1-f9f42a77951e

https://github.com/user-attachments/assets/87c96c7f-4d95-4977-9989-5f042fc932db

Thanks again!

[willthibault]

Sorry, one last topic I was wondering about:

I under that recompileLibrariesCppAd recompiles the auto diff libraries for the modules that use CppAd. I only need to recompile these when code relating to one of those libraries is updated, right ? Changes like parameters in the task file and non CppAd parts shouldn't require recompilation as I understand. Also, the recompilation of the libraries for Centauro is fairly quick, but I have noticed that for my updated version with REEM-C that it can be very slow. Have you experienced any slow down in the compilation before?

[willthibault]

Just a quick update on the recompilation time mentioned above. The difference is SRBD vs full centroidal dynamics, so slower compile time makes sense.

Also, while SRBD does slightly improve simulation stability, behaviour above is pretty similar.

[IoannisDadiotis]

1. for walking, I see that I can adjust the swing_trajectory_config. when addPlanarConstraints is false then a dynamic stepping occurs (see attached video) and I can command the base to a certain position. When setting it to true and changing the "long" parameters the robot will walk forward with better swing steps without a base command but will become unstable. Is there a way to alter the step trajectory and still command a position (is this a tuning issue or maybe I'm not understanding the parameters/the way the foot trajectory is currently implemented)?

The addPlanarConstraints true actually adds constraint for the x and y components of the swing trajectory so that the robot takes steps of specific length. For the z component the constraint exists in any case. If i remember correct long is the step length in the longitudinal direction. I didn't really use this case and so I am not sure if I ever had a target position for the robot using specific step length. I was usually setting addPlanarConstraints to false so that the solver decides the length of the steps to take.

2. for moving the arms, I am trying to move the hands to a given position with the interactive markers. Activating the soft constraints to do this, seems to do very little. If I increase the mu values then the robot slides towards the marker (see video) but doesn't really move the arms. When I activate the constraints or planners of arm_swing_trajectory_config the robot immediately becomes unstable. What is the recommended way to move the arms for interactive marks and for arm swing during walking? Also, maybe this is another tuning issue I'm experiencing?

It is up to you how do you formulate you optimal control problem. If you use arm_swing_trajectory_config you actually use equality constraints for the trajectory of the arm EEs which is kind of more strict. Soft constraint adds a term in the cost function and, thus is less strict but also depends how stiff you tune it (mu gain). Both of them should work properly.

For the specific issue you are facing, did you try to reduce the elements of the state gain matrix Q that correspond to the arm joint positions? This may help to make the arm joints less "stiff"

3. can you elaborate on the use of the R_taskspace weights?

That is the gain of a cost term that penalizes cartesian velocities of the leg EEs. This improved a lot the swing trajectories during stepping for my case. Notice that the penalization is happening by mapping joint velocities to cartesian ones through the jacobian computed at the state reference (which is set to your initial state unless you change it). If during walking you deviate a lot from the this state the mapping may not be very accurate.

4. I imagine the contacts for Centauro don't well represent REEM-C (big rectangular feet), are the friction cone contacts for Centauro essentially points at the wheel contacts?

Yep, in my case point contacts are quite good, for the humanoid you may need to consider surface contact and external torques at the control input as well. You can try to do this by setting the contactName6DoF here

5. My goal is to achieve a whole body MPC for loco-manipulation, did you have any luck with simultaneous arm manipulation during locomotion?

I guess the MPC itself is quite powerful to step and at the same time track some arm motion at least in simulation. I found it more on the hw side challenging to realize these motions.

[IoannisDadiotis]

SRBD is faster in compiling because it's a more simpler model. You can just use SRBD at the beginning since already with this model you can achieve many things. In my case what i did for the cases I wanted to use centroidal dynamics was to compile them once and then save all the files under the folder auto_generated so that can be used without recompiling. You don't have to recompile if you change gains or params in the taskfile, you have to do this if you change your dynamics model or you change or add any cost/constraint.

[willthibault]

Thanks for the detailed reply, that clears up a lot!

After some tuning I managed to achieve reasonably stable walking with addPlanarConstraints true:

https://github.com/user-attachments/assets/560cbec0-f125-469d-9db3-97a537d07121

With a small change to the SwingTrajectoryPlanner.cpp I was able to get each footstep in front of the other foot:

https://github.com/user-attachments/assets/4c4aa7c0-d053-44c6-80c9-5b94c259e472

I added the following after line 172:

 // Get other leg's stance position
 int CurrentStanceLegId = CurrentSwingLegId == 0 ? 1 : 0;
 scalar_t stanceLegLongPosition = feetLongitTrajectories_[CurrentStanceLegId][startTimeIndex].position(startTime);
 // set tragetEePosition to stance position of other foot
 targetEePosition[CurrentSwingLegId].at(0) = stanceLegLongPosition;

This likely doesn't generalize well though, so I'll probably need to update the planner to be better suited for humanoids.

I'll need to look into the arms motion more, but thanks for the ideas. I will be sure to share an update on that progress.

Thanks again!

[IoannisDadiotis]

Close since the original issue has been solved. Feel free to open a new issue if you encounter further errors.