Skip to content

Commit f375319

Browse files
manumerousmanumerous
committed
Add nmpc and examples
1 parent d8ad175 commit f375319

File tree

311 files changed

+26634
-1
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

311 files changed

+26634
-1
lines changed

‎.gitmodules

+18
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,18 @@
1+
[submodule "lib/catkin"]
2+
path = lib/catkin
3+
url = https://github.com/ros/catkin.git
4+
[submodule "lib/magic_enum"]
5+
path = lib/magic_enum
6+
url = https://github.com/Neargye/magic_enum.git
7+
[submodule "lib/cmake_modules"]
8+
path = lib/cmake_modules
9+
url = https://github.com/ros/cmake_modules.git
10+
[submodule "lib/pinocchio"]
11+
path = lib/pinocchio
12+
url = https://github.com/manumerous/pinocchio.git
13+
[submodule "lib/mujoco"]
14+
path = lib/mujoco
15+
url = https://github.com/google-deepmind/mujoco.git
16+
[submodule "lib/ocs2_ros2"]
17+
path = lib/ocs2_ros2
18+
url = https://github.com/manumerous/ocs2_ros2.git

‎LICENCE.txt

+29
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,29 @@
1+
BSD 3-Clause License
2+
3+
Copyright (c) 2024, 1X Technologies.
4+
All rights reserved.
5+
6+
Redistribution and use in source and binary forms, with or without
7+
modification, are permitted provided that the following conditions are met:
8+
9+
* Redistributions of source code must retain the above copyright notice, this
10+
list of conditions and the following disclaimer.
11+
12+
* Redistributions in binary form must reproduce the above copyright notice,
13+
this list of conditions and the following disclaimer in the documentation
14+
and/or other materials provided with the distribution.
15+
16+
* Neither the name of the copyright holder nor the names of its
17+
contributors may be used to endorse or promote products derived from
18+
this software without specific prior written permission.
19+
20+
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
21+
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
22+
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
23+
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
24+
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
25+
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
26+
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
27+
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
28+
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
29+
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

‎Makefile

+207
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,207 @@
1+
SHELL := /bin/bash
2+
3+
############################################################
4+
# Standard Configuration
5+
############################################################
6+
mkfile_path := $(abspath $(lastword $(MAKEFILE_LIST)))
7+
current_path := $(dir $(mkfile_path))
8+
current_dir := $(notdir $(patsubst %/,%,$(dir $(mkfile_path))))
9+
build_dir ?= $(abspath $(lastword $(MAKEFILE_LIST))/../../..)
10+
11+
CCACHE_DIR := $(build_dir)/.ccache
12+
13+
ros_source_file := /bin/ros_setup.sh
14+
15+
ifeq ("$(wildcard /opt/ros/jazzy/setup.bash)","")
16+
ifeq ("$(wildcard $(ros_source_file))","")
17+
ros_source_file := /opt/ros/humble/setup.bash
18+
endif
19+
else
20+
ifeq ("$(wildcard $(ros_source_file))","")
21+
ros_source_file := /opt/ros/jazzy/setup.bash
22+
endif
23+
endif
24+
25+
LINKER_FLAGS = "$(shell python3-config --ldflags --embed)"
26+
27+
# Find ROS2 packages in a given directory, two levels deep, and return only the package name
28+
define find_ros2_packages
29+
$(shell \
30+
for dir in $$(find $(1) -mindepth 1 -maxdepth 2 -type d); do \
31+
if [ -f "$$dir/CMakeLists.txt" ] && [ -f "$$dir/package.xml" ]; then \
32+
basename $$dir; \
33+
fi; \
34+
done)
35+
endef
36+
37+
define find_ros2_python_packages
38+
$(shell \
39+
for dir in $$(find $(1) -mindepth 1 -maxdepth 2 -type d); do \
40+
if [ -f "$$dir/setup.py" ] && [ -f "$$dir/package.xml" ] && [[ "$$(basename $$dir)" == *_py ]]; then \
41+
basename $$dir; \
42+
fi; \
43+
done)
44+
endef
45+
46+
# Individual package lists from specific subdirectories
47+
NMPC_PACKAGES := $(call find_ros2_packages,$(current_path)/humanoid_nmpc)
48+
49+
ROBOT_MODEL_PACKAGES := $(call find_ros2_packages,$(current_path)/robot_models)
50+
51+
# Unified package list
52+
PACKAGES ?= $(NMPC_PACKAGES) $(ROBOT_MODEL_PACKAGES)
53+
54+
############################################################
55+
# Customizable Configuration - User can override these
56+
############################################################
57+
BUILD_TYPE ?= Release
58+
BUILD_TESTING ?= ON
59+
BUILD_WITH_NINJA ?= ON
60+
PARALLEL_JOBS ?= 6
61+
PARALLEL_COLCON_JOBS ?= 4
62+
CPP_VERSION ?= -std=c++17
63+
64+
############################################################
65+
# Set flags based on configuration
66+
############################################################
67+
68+
COMMON_CMAKE_ARGS ?= \
69+
-DCMAKE_EXPORT_COMPILE_COMMANDS=ON \
70+
-DCMAKE_BUILD_TYPE=$(BUILD_TYPE) \
71+
-DBUILD_TESTING=$(BUILD_TESTING) \
72+
-DCMAKE_SHARED_LINKER_FLAGS=$(LINKER_FLAGS) \
73+
-DBUILD_HOST_ONEX_OS=$(BUILD_HOST_ONEX_OS) \
74+
-DCMAKE_CXX_FLAGS=$(CPP_VERSION)
75+
76+
# Conditionally add flags specific for the Ninja build system
77+
ifeq ($(BUILD_WITH_NINJA), ON)
78+
BUILD_SYSTEM=Ninja
79+
EVENT_HANDLERS=--event-handlers=console_cohesion+
80+
# Include ccache specific flags for Ninja builds
81+
COMMON_CMAKE_ARGS += \
82+
-G${BUILD_SYSTEM} \
83+
-DCMAKE_C_COMPILER_LAUNCHER=ccache \
84+
-DCMAKE_CXX_COMPILER_LAUNCHER=ccache
85+
else
86+
BUILD_SYSTEM="Unix Makefiles"
87+
# Just specify the generator for non-Ninja builds
88+
COMMON_CMAKE_ARGS += \
89+
-G${BUILD_SYSTEM}
90+
endif
91+
92+
COMMON_COLCON_BUILD_FLAGS ?= \
93+
--parallel-workers=${PARALLEL_COLCON_JOBS} \
94+
${EVENT_HANDLERS} \
95+
--symlink-install \
96+
--build-base $(build_dir)/build \
97+
--install-base $(build_dir)/install
98+
99+
############################################################
100+
# Define build and test targets
101+
############################################################
102+
define default-build-package
103+
cd ${build_dir} && \
104+
export MAKEFLAGS="-j ${PARALLEL_JOBS} -d" && \
105+
source ${ros_source_file} && \
106+
colcon build ${COMMON_COLCON_BUILD_FLAGS} --packages-up-to $(1) \
107+
--cmake-args ${COMMON_CMAKE_ARGS} $(EXTRA_CMAKE_ARGS) && \
108+
source $(build_dir)/install/setup.bash
109+
endef
110+
111+
define default-build-python-package
112+
cd ${build_dir} && \
113+
source ${ros_source_file} && \
114+
colcon build ${COMMON_COLCON_BUILD_FLAGS} --packages-up-to $(1)
115+
endef
116+
117+
define default-test-package
118+
cd ${build_dir} && \
119+
source ${ros_source_file} && \
120+
source $(build_dir)/install/setup.bash && \
121+
colcon test --packages-select $(1) --event-handlers console_direct+ --return-code-on-test-failure
122+
endef
123+
124+
############################################################
125+
# Command Line Interface
126+
############################################################
127+
.PHONY: build-all build-debug build-release build-relwithdebinfo build \
128+
test-all test $(addprefix build-,$(PACKAGES)) $(addprefix test-,$(PACKAGES))
129+
130+
build-all: $(addprefix build-,$(PACKAGES))
131+
132+
$(addprefix build-,$(PACKAGES)):
133+
$(call default-build-package,$(patsubst build-%,%,$@))
134+
135+
build:
136+
@$(if $(PKG),$(call default-build-package,$(PKG)),@echo "Please specify a package to build by setting the PKG variable. Example: make build PKG=package_name")
137+
138+
build-debug:
139+
@$(MAKE) BUILD_TYPE=Debug $(if $(PKG),build PKG=$(PKG),build-all)
140+
141+
build-release:
142+
@$(MAKE) BUILD_TYPE=Release BUILD_TESTING=OFF $(if $(PKG),build PKG=$(PKG),build-all)
143+
144+
build-relwithdebinfo:
145+
@$(MAKE) BUILD_TYPE=RelWithDebInfo $(if $(PKG),build PKG=$(PKG),build-all)
146+
147+
148+
test-all: $(addprefix test-,$(PACKAGES))
149+
150+
$(addprefix test-,$(PACKAGES)):
151+
$(call default-test-package,$(patsubst test-%,%,$@))
152+
153+
test:
154+
@$(if $(PKG),$(call default-test-package,$(PKG)),@echo "Please specify a package to test by setting the PKG variable. Example: make test PKG=package_name")
155+
156+
echo-packages:
157+
@echo "Packages to be built: $(PACKAGES)"
158+
159+
update-submodules:
160+
git submodule update --init --recursive
161+
162+
git-lfs:
163+
git lfs install && git lfs pull
164+
165+
clean-ws:
166+
cd ${build_dir} && \
167+
rm -rf build install log .ccache
168+
169+
clean-cppad:
170+
cd ${build_dir} && \
171+
rm -rf cppad_code_gen
172+
173+
format:
174+
lib/halodi-ros2-code-quality/Tools/fix_code_style.sh robot_models humanoid_nmpc
175+
176+
validate-format:
177+
lib/halodi-ros2-code-quality/Tools/check_code_style.sh robot_models humanoid_nmpc
178+
179+
180+
launch-g1-dummy-sim:
181+
cd ${build_dir} && \
182+
source ${ros_source_file} && \
183+
source install/setup.bash && \
184+
ros2 launch g1_centroidal_mpc dummy_sim.launch.py
185+
186+
test-pinocchio-model:
187+
cd ${build_dir} && \
188+
source ${ros_source_file} && \
189+
source install/setup.bash && \
190+
ros2 run humanoid_centroidal_mpc test_pinocchio_model
191+
192+
run-ocs2-tests:
193+
echo "make sure you call 'make build-relwithdebinfo' to build the tests before running them." && \
194+
cd ${build_dir} && \
195+
source ${ros_source_file} && \
196+
source install/setup.bash && \
197+
colcon test --event-handlers console_direct+ --return-code-on-test-failure --packages-select ocs2_robotic_assets ocs2_thirdparty \
198+
ocs2_robotic_assets ocs2_ros2_msgs ocs2_core ocs2_oc ocs2_qp_solver ocs2_mpc ocs2_robotic_tools ocs2_ddp ocs2_ros2_interfaces ocs2_sqp ocs2_pinocchio_interface ocs2_centroidal_model
199+
200+
run-mpc-tests:
201+
echo "make sure you call 'make build-relwithdebinfo' to build the tests before running them." && \
202+
cd ${build_dir} && \
203+
source ${ros_source_file} && \
204+
source install/setup.bash && \
205+
colcon test --event-handlers console_direct+ --return-code-on-test-failure --packages-select humanoid_common_mpc \
206+
humanoid_common_mpc_ros2 humanoid_centroidal_mpc humanoid_centroidal_mpc_ros2 humanoid_wb_mpc
207+

‎README.md

+76-1
Original file line numberDiff line numberDiff line change
@@ -1 +1,76 @@
1-
#OCS2 Humanoid MPC
1+
# OCS2 Humanoid MPC
2+
3+
This repository contains a Nonlinear Model Predictove Controller (NMPC) for humanoid loco-manipulation control. It contains a library of
4+
functionality and extends OCS2 to enable motion planning for humanoids.
5+
6+
It contains off the shelf functionality to runt he following MPC configurations.
7+
8+
### Centroidal MPC
9+
The centroidal MPC optimizes over the whole-body kinematics and the center off mass dynamics, with a choice to either use a single rigid
10+
body model or the full centroidal dynamics. It's functionality is contained in `humanoid_centroidal_mpc`.
11+
12+
### Whole-Body Dynamics MPC
13+
The whole-body dynamics MPC optimized over the contact forces and joint accelerations with the option to compute the joint torques for
14+
each step planned accross the horizon. It's functionality is contained in `humanoid_wb_mpc`.
15+
16+
### Robot Examples
17+
18+
The project supports the following robot examples:
19+
20+
- Unitree G1
21+
- 1X Neo (Comming soon)
22+
23+
## Get Started
24+
25+
### Setup Workspace
26+
27+
Create a colcon workspace and clone the repository into the src folder:
28+
29+
```bash
30+
mkdir -p humanoid_mpc_ws/src && cd humanoid_mpc_ws/src
31+
git clone https://github.com/manumerous/ocs2-humanoid-mpc.git
32+
```
33+
34+
Then initialize all submodules using:
35+
36+
```bash
37+
cd ocs2-humanoid-mpc
38+
git submodule update --init --recursive
39+
```
40+
41+
### Install Dependencies
42+
43+
Install all aptitude dependencies using:
44+
45+
```bash
46+
xargs -a dependencies.txt sudo apt install
47+
```
48+
49+
Then install ros2 jazzy as specified in
50+
the [installation guide](https://docs.ros.org/en/jazzy/Installation/Ubuntu-Install-Debs.html).
51+
52+
As a final step:
53+
54+
```bash
55+
sudo apt install ros-jazzy-ament-cmake-clang-format ros-jazzy-joint-state-publisher-gui ros-jazzy-xacro ros-jazzy-mcap-vendor ros-jazzy-interactive-markers
56+
```
57+
58+
### Building the MPC
59+
60+
```bash
61+
make build-all
62+
```
63+
64+
## Running the examples
65+
66+
On the top level folder run:
67+
68+
```make launch-g1-dummy-sim```
69+
70+
A window with Rviz will appear, On the first run the auto differentiation libraries will be generated. This might take up to 5-10 min depending on your system. Once done the robot appears and you can control it via the gamepad.
71+
72+
## Acknowledgements
73+
74+
This work was developed by the company [1X Technologies](https://www.1x.tech/). I would like to thank everyone that contributed in code, discussion or suggestions to the success of this project.
75+
76+
In particular I would like to acknowledge the support from Michael Purcell, Jesper Smith, Simon Zimmermann, Joel Filho, Varit (Ohm) Vichathorn, Armin Nurkanovic, Charles Khazoom, Farbod Farshidian, Eric Jang, Bernt Børnich and everyone at 1X Technologies.

‎dependencies.txt

+13
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,13 @@
1+
clang-format
2+
make
3+
build-essential
4+
locales
5+
libeigen3-dev
6+
libglpk-dev
7+
libboost-all-dev
8+
libboost-filesystem-dev
9+
libboost-log-dev
10+
libglfw3-dev
11+
ninja-build
12+
ccache
13+
python3-pygame

0 commit comments

Comments
 (0)