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.