mj-kdl-wrapper  0.1.0
MuJoCo + KDL bridge for robot kinematics and dynamics
Loading...
Searching...
No Matches
Scene Building

Build and modify MuJoCo scenes from MJCF specifications. More...

Functions

bool mj_kdl::save_model_xml (const mjModel *model, const char *path)
 
bool mj_kdl::attach_to_spec (mjSpec *robot_spec, const AttachmentSpec *a)
 
bool mj_kdl::build_scene (mjModel **out_model, mjData **out_data, const SceneSpec *spec)
 
void mj_kdl::destroy_scene (mjModel *model, mjData *data)
 
bool mj_kdl::scene_add_object (mjModel **model, mjData **data, SceneSpec *spec, const SceneObject &obj)
 
bool mj_kdl::scene_remove_object (mjModel **model, mjData **data, SceneSpec *spec, const std::string &name)
 

Detailed Description

Build and modify MuJoCo scenes from MJCF specifications.

The primary entry point is build_scene(). Use scene_add_object() and scene_remove_object() to dynamically modify a running scene.

Function Documentation

◆ attach_to_spec()

bool mj_kdl::attach_to_spec ( mjSpec *  robot_spec,
const AttachmentSpec a 
)

Apply one attachment to an arm spec using the MuJoCo spec API (mjs_attach). Parses a->mjcf_path, attaches its first root body under a->attach_to with the given pos/euler offset, prefixes all element names with a->prefix, and registers contact exclusions via mjs_addExclude. Can be called repeatedly to build a chain: each subsequent a->attach_to may reference any body added by prior calls.

Parameters
[in,out]robot_specAccumulated robot spec to attach into.
[in]aAttachment; a->mjcf_path must not be null.
Returns
true on success.

◆ build_scene()

bool mj_kdl::build_scene ( mjModel **  out_model,
mjData **  out_data,
const SceneSpec spec 
)

Build a MuJoCo scene from one or more robots using the MuJoCo spec API. This is the primary scene-building function.

For each RobotSpec: mj_parseXML loads the root MJCF, then attach_to_spec() applies each entry in RobotSpec::attachments in order (mount, sensor, gripper, etc.), and mjs_attach places the complete robot spec at the given position. A single mj_compile produces the final model – no intermediate XML files are written.

Parameters
[out]out_modelNewly allocated MuJoCo model; caller frees via destroy_scene().
[out]out_dataNewly allocated MuJoCo data; caller frees via destroy_scene().
[in]specScene description: robots (with attachment chains), table, objects, timestep, gravity, floor, skybox.
Returns
true on success.

Referenced by main(), DualArmTest::SetUp(), InitTest::SetUp(), MjcfLoadTest::SetUp(), MjcfGripperTest::SetUp(), MjcfPickTest::SetUp(), MjcfPosCtrlTest::SetUp(), MjcfTrqCtrlTest::SetUp(), MjcfVelCtrlTest::SetUp(), and TableSceneTest::SetUp().

◆ destroy_scene()

void mj_kdl::destroy_scene ( mjModel *  model,
mjData *  data 
)

Free a model/data pair allocated by any scene-building function.

Parameters
[in]modelModel to free (may be null).
[in]dataData to free (may be null).

Referenced by main(), DualArmTest::TearDown(), InitTest::TearDown(), MjcfLoadTest::TearDown(), MjcfGripperTest::TearDown(), MjcfPickTest::TearDown(), MjcfPosCtrlTest::TearDown(), MjcfTrqCtrlTest::TearDown(), MjcfVelCtrlTest::TearDown(), and TableSceneTest::TearDown().

◆ save_model_xml()

bool mj_kdl::save_model_xml ( const mjModel *  model,
const char *  path 
)

Save the compiled model to an MJCF XML file for later reloading via build_scene(). Must be called with the model returned by the most recent build_scene() call - MuJoCo only retains the last compiled model's XML internally. Typical use: build a combined scene (dual-arm, arm+gripper, ...) once, save it, then reload via build_scene() in subsequent runs to skip all build steps.

Parameters
modelModel to save; must be the most recently compiled model.
pathOutput path for the MJCF XML file.
Returns
true on success.

◆ scene_add_object()

bool mj_kdl::scene_add_object ( mjModel **  model,
mjData **  data,
SceneSpec spec,
const SceneObject obj 
)

Add an object to the scene by appending it to spec->objects and rebuilding the model. The old model/data are freed; new ones replace them. Any Robot handles sharing the old model/data become stale - call init_robot() again on the new model/data after this call.

Parameters
[in,out]modelCurrent model pointer; updated to new model on success.
[in,out]dataCurrent data pointer; updated to new data on success.
[in,out]specScene spec; obj is appended to spec->objects.
[in]objObject to add.
Returns
true on success; model/data and spec->objects unchanged on failure.

Referenced by TEST_F().

◆ scene_remove_object()

bool mj_kdl::scene_remove_object ( mjModel **  model,
mjData **  data,
SceneSpec spec,
const std::string &  name 
)

Remove a named object from the scene by erasing it from spec->objects and rebuilding the model. The old model/data are freed; new ones replace them. Any Robot handles sharing the old model/data become stale - call init_robot() again on the new model/data after this call.

Parameters
[in,out]modelCurrent model pointer; updated to new model on success.
[in,out]dataCurrent data pointer; updated to new data on success.
[in,out]specScene spec; named object removed from spec->objects.
[in]nameName of the object to remove.
Returns
true on success; false if name not found or rebuild fails.

Referenced by TEST_F().