Skip to content

Commit fcbe4f7

Browse files
azeeyAddisu Z. Taddeseiche033
authored
mujoco: Initial implementation (#811)
Initial implementation of the Mujoco plugin targetting Mujoco 3.5. Enough of the features are implemented to enable testing with gz-sim --------- Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org> Signed-off-by: Addisu Z. Taddese <addisuzt@intrinsic.ai> Co-authored-by: Addisu Z. Taddese <addisu@openrobotics.org> Co-authored-by: Ian Chen <iche@google.com>
1 parent 67811c2 commit fcbe4f7

21 files changed

Lines changed: 2158 additions & 2 deletions

‎CMakeLists.txt‎

Lines changed: 12 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -35,6 +35,10 @@ endif()
3535
#============================================================================
3636
message(STATUS "\n\n-- ====== Finding Dependencies ======")
3737

38+
#--------------------------------------
39+
# TODO(azeey) Remove this and depend on a system package before release
40+
add_subdirectory(third_party/mujoco_vendor)
41+
3842
#--------------------------------------
3943
# Find gz-common
4044
gz_find_package(gz-common
@@ -87,6 +91,13 @@ gz_find_package(GzBullet
8791
PKGCONFIG bullet
8892
PKGCONFIG_VER_COMPARISON >=)
8993

94+
#--------------------------------------
95+
# Find mujoco for the mujoco plugin wrapper
96+
# gz_find_package(mujoco
97+
# REQUIRED_BY mujoco
98+
# PKGCONFIG_IGNORE # TODO(azeey) Revisit this
99+
# CONFIG)
100+
90101
message(STATUS "-------------------------------------------\n")
91102

92103
# Plugin install dirs
@@ -101,7 +112,7 @@ set(GZ_PHYSICS_ENGINE_INSTALL_DIR
101112
# Configure the build
102113
#============================================================================
103114
gz_configure_build(QUIT_IF_BUILD_ERRORS
104-
COMPONENTS sdf heightmap mesh dartsim tpe bullet bullet-featherstone)
115+
COMPONENTS sdf heightmap mesh dartsim tpe bullet bullet-featherstone mujoco)
105116

106117

107118
#============================================================================

‎include/gz/physics/detail/EntityStorage.hh‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -45,7 +45,7 @@ namespace detail
4545
/// parent ID to a vector of object IDs. This is used to determine the index of
4646
/// an entity within the container. However, if EntityStorage objects are
4747
/// hierarchically stored in `*Info` structs, the indexInContainerToID will only
48-
/// have one entry.
48+
/// have one entry. See the Mujoco plugin implementation for reference.
4949
template <typename Value1, typename Key2 = Value1>
5050
struct EntityStorage
5151
{

‎mujoco/CMakeLists.txt‎

Lines changed: 46 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,46 @@
1+
gz_get_libsources_and_unittests(sources test_sources)
2+
3+
gz_add_component(mujoco-plugin
4+
DEPENDS_ON_COMPONENTS sdf mesh
5+
SOURCES ${sources}
6+
GET_TARGET_NAME plugin_target)
7+
8+
target_link_libraries(${plugin_target}
9+
PUBLIC
10+
${PROJECT_LIBRARY_TARGET_NAME}-sdf
11+
${PROJECT_LIBRARY_TARGET_NAME}-heightmap
12+
${PROJECT_LIBRARY_TARGET_NAME}-mesh
13+
gz-common::gz-common
14+
PRIVATE
15+
mujoco::mujoco
16+
)
17+
18+
# Note that plugins are currently being installed in 2 places:
19+
# /lib and the engine-plugins dir
20+
install(TARGETS ${plugin_target}
21+
DESTINATION ${GZ_PHYSICS_ENGINE_RELATIVE_INSTALL_DIR})
22+
23+
# Testing
24+
gz_build_tests(
25+
TYPE UNIT_mujoco
26+
SOURCES ${test_sources}
27+
LIB_DEPS
28+
${plugin_target}
29+
gz-physics-test
30+
${PROJECT_LIBRARY_TARGET_NAME}-sdf
31+
${PROJECT_LIBRARY_TARGET_NAME}-heightmap
32+
${PROJECT_LIBRARY_TARGET_NAME}-mesh
33+
TEST_LIST tests
34+
ENVIRONMENT
35+
GZ_PHYSICS_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX}
36+
INCLUDE_DIRS
37+
${CMAKE_CURRENT_SOURCE_DIR}/src
38+
)
39+
40+
foreach(test ${tests})
41+
target_compile_definitions(${test} PRIVATE
42+
"mujoco_plugin_LIB=\"$<TARGET_FILE:${plugin_target}>\"")
43+
44+
# Helps when we want to build a single test after making changes to plugin_target
45+
add_dependencies(${test} ${plugin_target})
46+
endforeach()

‎mujoco/README.md‎

Lines changed: 248 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,248 @@
1+
# MuJoCo Physics Plugin
2+
3+
## Physics features list
4+
5+
These are the specific physics API features implemented.
6+
7+
### EntityManagementFeatureList
8+
9+
```c++
10+
11+
struct EntityManagementFeatureList : FeatureList<
12+
GetEngineInfo,
13+
GetWorldFromEngine,
14+
GetLinkFromModel,
15+
GetModelFromWorld,
16+
GetShapeFromLink,
17+
RemoveEntities,
18+
ConstructEmptyWorldFeature
19+
// ConstructEmptyModelFeature,
20+
// ConstructEmptyLinkFeature
21+
// CollisionFilterMaskFeature,
22+
// WorldModelFeature
23+
> { };
24+
25+
```
26+
27+
TODO:
28+
29+
- [ ] (S) Get entities by name
30+
- [ ] (S) Get an entities index
31+
- [ ] (M) Remove models
32+
33+
### FreeGroupFeatureList
34+
35+
```c++
36+
struct FreeGroupFeatureList : gz::physics::FeatureList<
37+
FindFreeGroupFeature,
38+
SetFreeGroupWorldPose,
39+
SetFreeGroupWorldVelocity
40+
> { };
41+
```
42+
43+
TODO:
44+
45+
- [ ] (S) Implement checks to see if a given model has a freegroup in `FindFreeGroupFeature`.
46+
- [x] (S) Set linear and angular velocities on a freegroup.
47+
- [x] (S) Set pose on a freegroup
48+
49+
### KinematicsFeatureList
50+
51+
```c++
52+
struct KinematicsFeatureList : FeatureList<
53+
LinkFrameSemantics,
54+
ShapeFrameSemantics,
55+
JointFrameSemantics,
56+
FreeGroupFrameSemantics
57+
> { };
58+
```
59+
60+
TODO:
61+
62+
- [ ] (S) Compute frame data for shapes
63+
- [ ] (S) Compute frame data for joints
64+
- I'm not sure if this is actually needed by gz-sim.
65+
66+
### SDFFeatureList
67+
68+
```c++
69+
struct SDFFeatureList : FeatureList<
70+
sdf::ConstructSdfWorld,
71+
sdf::ConstructSdfModel
72+
> { };
73+
```
74+
75+
TODO:
76+
77+
- [ ] (S) Resolve SDFormat frames before instead of using raw poses
78+
- [ ] (S) Apply poses of collisions
79+
- [ ] (M) Support nested models
80+
- [ ] (M) Support joints
81+
- [ ] (S) ConstructSdfCollisions
82+
83+
### SimulationFeatureList
84+
85+
```c++
86+
struct SimulationFeatureList : gz::physics::FeatureList<
87+
ForwardStep
88+
> { };
89+
```
90+
91+
TODO:
92+
93+
- [ ] Report only links with changed poses instead of all the links.
94+
95+
## Features to Implement for a fully viable engine, but with some missing features
96+
97+
```c++
98+
99+
struct JointFeatureList : FeatureList<
100+
GetBasicJointState,
101+
SetBasicJointState,
102+
GetBasicJointProperties,
103+
104+
SetJointVelocityCommandFeature,
105+
SetJointPositionLimitsFeature,
106+
SetJointVelocityLimitsFeature,
107+
SetJointEffortLimitsFeature,
108+
109+
SetJointTransformFromParentFeature,
110+
AttachFixedJointFeature,
111+
DetachJointFeature,
112+
113+
GetRevoluteJointProperties,
114+
GetPrismaticJointProperties,
115+
FixedJointCast,
116+
GetJointTransmittedWrench,
117+
> { };
118+
```
119+
120+
```c++
121+
struct KinematicLinkFeatureList : FeatureList<
122+
KinematicLink
123+
> { };
124+
```
125+
126+
```c++
127+
struct LinkFeatureList : FeatureList<
128+
AddLinkExternalForceTorque
129+
> { };
130+
```
131+
132+
133+
```c++
134+
135+
struct SimulationFeatureList : gz::physics::FeatureList<
136+
ForwardStep,
137+
GetContactsFromLastStepFeature
138+
> { };
139+
```
140+
141+
- The GetContactsFromLastStepFeature feature is missing
142+
143+
```
144+
145+
struct WorldFeatureList : FeatureList<
146+
Gravity,
147+
> { };
148+
```
149+
150+
151+
152+
## Notes
153+
154+
The Physics system in Gazebo requires the following features at a minimum:
155+
156+
```c++
157+
/// \brief This is the minimum set of features that any physics engine must
158+
/// implement to be supported by this system.
159+
/// New features can't be added to this list in minor / patch releases, in
160+
/// order to maintain backwards compatibility with downstream physics plugins.
161+
public: struct MinimumFeatureList : physics::FeatureList<
162+
physics::FindFreeGroupFeature,
163+
physics::SetFreeGroupWorldPose,
164+
physics::FreeGroupFrameSemantics,
165+
physics::LinkFrameSemantics,
166+
physics::ForwardStep,
167+
physics::RemoveModelFromWorld,
168+
physics::sdf::ConstructSdfModel,
169+
physics::sdf::ConstructSdfWorld,
170+
physics::GetLinkFromModel,
171+
physics::GetShapeFromLink
172+
>{};
173+
```
174+
175+
In addition to these, the following is a minimal list of features should be
176+
implemented to evaluate MuJoCo in gz-sim.
177+
178+
```c++
179+
/// Feature list to handle joints.
180+
struct JointFeatureList : physics::FeatureList<
181+
MinimumFeatureList,
182+
physics::GetJointFromModel,
183+
physics::GetBasicJointProperties,
184+
physics::GetBasicJointState,
185+
physics::SetBasicJointState>{};
186+
```
187+
188+
```c++
189+
190+
// Joint transmitted wrench
191+
/// \brief Feature list for getting joint transmitted wrenches.
192+
public: struct JointGetTransmittedWrenchFeatureList : physics::FeatureList<
193+
physics::GetJointTransmittedWrench>{};
194+
```
195+
196+
```c++
197+
// Joint velocity command
198+
/// \brief Feature list for set joint velocity command.
199+
public: struct JointVelocityCommandFeatureList : physics::FeatureList<
200+
physics::SetJointVelocityCommandFeature>{};
201+
```
202+
203+
Not sure if this is needed for meshes.
204+
205+
```c++
206+
/// \brief Feature list to handle collisions.
207+
public: struct CollisionFeatureList : physics::FeatureList<
208+
MinimumFeatureList,
209+
physics::sdf::ConstructSdfCollision>{};
210+
// Meshes
211+
/// \brief Feature list for meshes.
212+
/// Include MinimumFeatureList so created collision can be automatically
213+
/// up-cast.
214+
public: struct MeshFeatureList : physics::FeatureList<
215+
CollisionFeatureList,
216+
physics::mesh::AttachMeshShapeFeature>{};
217+
```
218+
219+
```c++
220+
// Nested Models
221+
/// \brief Feature list to construct nested models
222+
public: struct NestedModelFeatureList : physics::FeatureList<
223+
MinimumFeatureList,
224+
physics::sdf::ConstructSdfNestedModel>{};
225+
```
226+
227+
1. Critical gz-sim Requirements (Highest Priority)
228+
- Set Model Pose: The `SetFreeGroupWorldPose` function is not implemented.
229+
- Remove Models: The `RemoveModelFromWorld` function is not implemented.
230+
231+
2. Major Missing Features
232+
- Joints: The entire `JointFeatureList` is missing, which prevents all joint functionality (creation, state access, commanding, feedback).
233+
- Contact Reporting: `GetContactsFromLastStepFeature` is not implemented, so contact sensor data is unavailable.
234+
- External Forces: `AddLinkExternalForceTorque` is not implemented.
235+
- World Properties: The `WorldFeatureList` (including Gravity) is not implemented.
236+
237+
3. Incomplete Features & Specific Runtime Construction Limitations
238+
- Whole Model Runtime Construction: This is supported. New, complete models can be added to the world after startup.
239+
- Piecemeal Runtime Construction: The ability to add individual components to existing models at runtime is missing. This includes:
240+
- `AttachMeshShapeFeature` (adding a mesh to an existing link).
241+
- `ConstructSdfCollision` (adding a collision to an existing link).
242+
- SDF & Model Loading Details:
243+
- SDF frames are not fully resolved (uses `RawPose`).
244+
- Support for nested models is incomplete.
245+
- Heightmap and Polyline geometries are not supported.
246+
- Entity Management: Functions to find entities by name or get their index are stubbed.
247+
- Kinematics: Calculating frame data for shapes and joints is a TODO.
248+
- Simulation Step: Pose updates are inefficient (all links are reported as changed).

‎mujoco/src/Base.cc‎

Lines changed: 53 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,53 @@
1+
/*
2+
* Copyright (C) 2025 Open Source Robotics Foundation
3+
*
4+
* Licensed under the Apache License, Version 2.0 (the "License");
5+
* you may not use this file except in compliance with the License.
6+
* You may obtain a copy of the License at
7+
*
8+
* http://www.apache.org/licenses/LICENSE-2.0
9+
*
10+
* Unless required by applicable law or agreed to in writing, software
11+
* distributed under the License is distributed on an "AS IS" BASIS,
12+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
* See the License for the specific language governing permissions and
14+
* limitations under the License.
15+
*
16+
*/
17+
18+
#include "Base.hh"
19+
20+
#include <gz/physics/Implements.hh>
21+
22+
namespace gz
23+
{
24+
namespace physics
25+
{
26+
namespace mujoco
27+
{
28+
29+
bool Base::RecompileSpec(WorldInfo &_worldInfo) const
30+
{
31+
if (!_worldInfo.specDirty)
32+
return true;
33+
34+
int rc = mj_recompile(_worldInfo.mjSpecObj, nullptr, _worldInfo.mjModelObj,
35+
_worldInfo.mjDataObj);
36+
_worldInfo.specDirty = false;
37+
38+
if (rc != 0) {
39+
std::cerr << "Error compiling:" << mjs_getError(_worldInfo.mjSpecObj)
40+
<< "\n";
41+
return false;
42+
}
43+
// TODO(azeey): Saving the resulting MJCF is useful for debugging, but should
44+
// be removed once the plugin is finalized mj_saveXML(_worldInfo.mjSpecObj,
45+
// "/tmp/mujoco_model.xml", nullptr, 0);
46+
47+
mj_forward(_worldInfo.mjModelObj, _worldInfo.mjDataObj);
48+
return true;
49+
}
50+
51+
} // namespace mujoco
52+
} // namespace physics
53+
} // namespace gz

0 commit comments

Comments
 (0)