Merge pull request #27 from manumerous/feature/add-mujoco-sim

Add robot runtime and mujoco sim

Author: manumerous

.github/workflows/build_test.yml

@@ -64,4 +64,5 @@ jobs:
 
       - name: Build All
         run: |
+          export PARALLEL_JOBS=1
           make build-all

Makefile

@@ -48,8 +48,10 @@ NMPC_PACKAGES := $(call find_ros2_packages,$(current_path)/humanoid_nmpc)
 
 ROBOT_MODEL_PACKAGES := $(call find_ros2_packages,$(current_path)/robot_models)
 
+RUNTIME_PACKAGES := $(call find_ros2_packages,$(current_path)/robot_runtime)
+
 # Unified package list
-PACKAGES ?= $(NMPC_PACKAGES) $(ROBOT_MODEL_PACKAGES)
+PACKAGES ?= $(NMPC_PACKAGES) $(ROBOT_MODEL_PACKAGES) $(RUNTIME_PACKAGES)
 
 ############################################################
 # Customizable Configuration - User can override these
@@ -179,23 +181,24 @@ launch-g1-dummy-sim:
 	source install/setup.bash && \
 	ros2 launch g1_centroidal_mpc dummy_sim.launch.py 
 
-launch-wb-g1-dummy-sim:
+launch-g1-sim:
 	cd ${build_dir} && \
 	source ${ros_source_file} && \
 	source install/setup.bash && \
-	ros2 launch g1_wb_mpc wb_dummy_sim.launch.py 
+	ros2 launch g1_centroidal_mpc mujoco_sim.launch.py 
 
-launch-neo-dummy-sim:
+
+launch-wb-g1-dummy-sim:
 	cd ${build_dir} && \
 	source ${ros_source_file} && \
 	source install/setup.bash && \
-	ros2 launch neo_centroidal_mpc dummy_sim.launch.py 
+	ros2 launch g1_wb_mpc dummy_sim.launch.py 
 
-launch-wb-neo-dummy-sim:
+launch-wb-g1-sim:
 	cd ${build_dir} && \
 	source ${ros_source_file} && \
 	source install/setup.bash && \
-	ros2 launch neo_wb_mpc wb_dummy_sim.launch.py 
+	ros2 launch g1_wb_mpc mujoco_sim.launch.py 
 
 run-ocs2-tests:
 	echo "make sure you call 'make build-relwithdebinfo' to build the tests before running them." && \

dependencies.txt

@@ -7,6 +7,11 @@ libboost-all-dev
 libboost-filesystem-dev
 libboost-log-dev
 libglfw3-dev
+liburdfdom-dev
+libabsl-dev
+libxinerama-dev
+libxcursor-dev
+libxi-dev
 ninja-build
 ccache
 python3-pygame

humanoid_nmpc/humanoid_centroidal_mpc/CMakeLists.txt

@@ -19,6 +19,7 @@ find_package(ocs2_mpc REQUIRED)
 find_package(ocs2_sqp REQUIRED)
 find_package(ocs2_pinocchio_interface REQUIRED)
 find_package(humanoid_common_mpc REQUIRED)
+find_package(robot_model REQUIRED)
 
 find_package(GTest REQUIRED)
 
@@ -38,7 +39,7 @@ find_package(Eigen3 3.3 REQUIRED NO_MODULE)
 ## Build ##
 ###########
 
-set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD 20)
 
 set(FLAGS ${OCS2_CXX_FLAGS} -Wno-invalid-partial-specialization)
 
@@ -64,6 +65,7 @@ set(dependencies
   ocs2_sqp
   ocs2_pinocchio_interface
   humanoid_common_mpc
+  robot_model
 )
 
 link_directories(
@@ -86,6 +88,7 @@ add_library(${PROJECT_NAME}
   src/initialization/CentroidalWeightCompInitializer.cpp
   src/CentroidalMpcInterface.cpp
   src/command/CentroidalMpcTargetTrajectoriesCalculator.cpp
+  src/mrt/CentroidalMpcMrtJointController.cpp
 )
 ament_target_dependencies(${PROJECT_NAME}
   ${dependencies}
@@ -111,34 +114,6 @@ target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS})
 if(BUILD_TESTING)
   find_package(ament_cmake_gtest REQUIRED)
 
-# Disable for now until best testing strategy is found. 
-
-  # ament_add_gtest(test_dynamics_helper_functions test/testDynamicsHelperFunctions.cpp)
-  # target_link_libraries(test_dynamics_helper_functions 
-  #   ${rclcpp_LIBRARIES} 
-  #   ${rmw_connext_cpp_LIBRARIES} 
-  #   ${std_interfaces}
-  #   ${PROJECT_NAME}
-  # )
-
-  # ament_add_gtest(test_pinocchio_frame_conversions test/testPinocchioFrameConversions.cpp)
-  # target_link_libraries(test_pinocchio_frame_conversions 
-  #   ${rclcpp_LIBRARIES} 
-  #   ${rmw_connext_cpp_LIBRARIES} 
-  #   ${std_interfaces}
-  #   ${PROJECT_NAME}
-  # )
-
-# This test is diabled for now. A smarter test structure working with different active joints is needed. 
-
-# ament_add_gtest(test_centroidal_model_conversions test/testCentroidalConversions.cpp)
-#   target_link_libraries(test_centroidal_model_conversions
-#     ${rclcpp_LIBRARIES}
-#     ${rmw_connext_cpp_LIBRARIES}
-#     ${std_interfaces}
-#     ${PROJECT_NAME}
-#   )
-
 endif()
 
 #############

humanoid_nmpc/humanoid_centroidal_mpc/include/humanoid_centroidal_mpc/CentroidalMpcInterface.h

@@ -60,11 +60,7 @@ class CentroidalMpcInterface final : public RobotInterface {
    * @param [in] urdfFile: The absolute path to the URDF file for the robot.
    * @param [in] referenceFile: The absolute path to the reference configuration file.
    */
-  CentroidalMpcInterface(const std::string& taskFile,
-                         const std::string& urdfFile,
-                         const std::string& referenceFile,
-                         const std::string& gaitFile,
-                         bool setupOCP = true);
+  CentroidalMpcInterface(const std::string& taskFile, const std::string& urdfFile, const std::string& referenceFile, bool setupOCP = true);
 
   ~CentroidalMpcInterface() override = default;
 

humanoid_nmpc/humanoid_centroidal_mpc/include/humanoid_centroidal_mpc/common/CentroidalMpcRobotModel.h

@@ -233,6 +233,8 @@ class CentroidalMpcRobotModel : public MpcRobotModelBase<SCALAR_T> {
     return state.head(6);
   };
 
+  const CentroidalModelInfoTpl<SCALAR_T>& getCentroidalModelInfo() const { return centroidalModelInfo_; }
+
  private:
   CentroidalMpcRobotModel(const CentroidalMpcRobotModel& rhs)
       : MpcRobotModelBase<SCALAR_T>(rhs),

humanoid_nmpc/humanoid_centroidal_mpc/include/humanoid_centroidal_mpc/mrt/CentroidalMpcMrtJointController.h

@@ -0,0 +1,112 @@
+/******************************************************************************
+Copyright (c) 2025, Manuel Yves Galliker. All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+******************************************************************************/
+
+#include <ocs2_mpc/MPC_MRT_Interface.h>
+
+#include "humanoid_centroidal_mpc/CentroidalMpcInterface.h"
+
+#include <ocs2_ros2_interfaces/mrt/DummyObserver.h>
+#include <robot_model/ControllerBase.h>
+#include "humanoid_common_mpc/reference_manager/ProceduralMpcMotionManager.h"
+#include "robot_model/RobotDescription.h"
+
+namespace ocs2::humanoid {
+
+class CentroidalMpcMrtJointController final : public ::robot::model::ControlBase {
+ public:
+  /**
+   * Constructor.
+   *
+   * @param [in] mpc: The underlying MPC class to be used.
+   * @param [in] topicPrefix: The robot's name.
+   * @param [in] mpcDesiredFrequency: The max frequency to run the mpc at.
+   */
+  CentroidalMpcMrtJointController(const ::robot::model::RobotDescription& robotDescription,
+                                  const ModelSettings& modelSettings,
+                                  const CentroidalMpcRobotModel<scalar_t>& mpcRobotModel,
+                                  MPC_BASE& mpc,
+                                  PinocchioInterface pinocchioInterface,
+                                  scalar_t mpcDesiredFrequency = -1,
+                                  std::shared_ptr<DummyObserver> rVizVisualizerPtr = nullptr);
+
+  /**
+   * Destructor.
+   */
+  ~CentroidalMpcMrtJointController();
+
+  bool ready() const { return mcpMrtInterface_.initialPolicyReceived(); }
+
+  /**
+   * Handles the low level controller loop that updates the mpc observation, reads out the latest policy and sets the joint control action.
+   */
+
+  void computeJointControlAction(scalar_t time,
+                                 const ::robot::model::RobotState& robotState,
+                                 ::robot::model::RobotJointAction& robotJointAction) override;
+
+  void startMpcThread(const ::robot::model::RobotState& initRobotState);
+
+ private:
+  /**
+   * Handles the MPC solver thread.
+   */
+  void solverWorker();
+
+  /**
+   * Method to convert the latest observation msg to a stable desired trajectory (current position, zero velocity and
+   * acceleration)
+   *
+   * @param [in] msg: The observation message.
+   */
+  TargetTrajectories currentObservationToResetTrajectory(const SystemObservation& currentMpcObservation);
+
+  void updateMpcState(vector_t& mpcState, const ::robot::model::RobotState& robotState);
+  void updateMpcObservation(ocs2::SystemObservation& mpcObservation, const ::robot::model::RobotState& robotState);
+
+  MPC_MRT_Interface mcpMrtInterface_;
+
+  PinocchioInterface pinocchioInterface_;
+  ocs2::SystemObservation currentMpcObservation_;
+  std::unique_ptr<CentroidalMpcRobotModel<scalar_t>> mpcRobotModelPtr_;
+  std::vector<size_t> mpcJointIndices_;
+  std::vector<size_t> otherJointIndices_;
+
+  size_t mpcDeltaTMicroSeconds_;
+  bool realtime_;  // True if MPC is to be run as fast as possible
+
+  std::atomic_bool terminateThread_{false};
+  std::jthread solver_worker_;
+
+  std::shared_ptr<DummyObserver> visualizerPtr_;
+
+  vector_t inverse_dynamics_kp_;
+  vector_t inverse_dynamics_kd_;
+};
+
+}  // namespace ocs2::humanoid

humanoid_nmpc/humanoid_centroidal_mpc/package.xml

@@ -20,6 +20,7 @@
   <depend>ocs2_pinocchio_interface</depend>
   <depend>ocs2_ros2_interfaces</depend>
   <depend>pinocchio</depend>
+  <depend>robot_model</depend>
 
   <depend>humanoid_common_mpc</depend>
 

humanoid_nmpc/humanoid_centroidal_mpc/src/CentroidalMpcInterface.cpp

@@ -69,8 +69,10 @@ namespace ocs2::humanoid {
 /******************************************************************************************************/
 /******************************************************************************************************/
 
-CentroidalMpcInterface::CentroidalMpcInterface(
-    const std::string& taskFile, const std::string& urdfFile, const std::string& referenceFile, const std::string& gaitFile, bool setupOCP)
+CentroidalMpcInterface::CentroidalMpcInterface(const std::string& taskFile,
+                                               const std::string& urdfFile,
+                                               const std::string& referenceFile,
+                                               bool setupOCP)
     : taskFile_(taskFile),
       urdfFile_(urdfFile),
       referenceFile_(referenceFile),

humanoid_nmpc/humanoid_centroidal_mpc/src/command/CentroidalMpcTargetTrajectoriesCalculator.cpp

@@ -106,7 +106,7 @@ TargetTrajectories CentroidalMpcTargetTrajectoriesCalculator::commandedVelocityT
 
   vector6_t targetMomentum;
 
-  const Eigen::Matrix<scalar_t, 6, 6> Ab = A.template leftCols<6>();
+  const Eigen::Matrix<scalar_t, 6, 6> Ab = A.leftCols<6>();
   const Eigen::Matrix<scalar_t, 6, 6> Ab_inv = computeFloatingBaseCentroidalMomentumMatrixInverse(Ab);
 
   // This did not lead to meaningful commands around the z axis. Needs more investigations.