Implement threading for island constraint solving.
Humanoids22 No threads: Benchmark Execution Time: 9.475905812s Humanoids22 with 10 Threads: Benchmark Execution Time: 4.871352s PiperOrigin-RevId: 571214307 Change-Id: I1f4f2c761b4ae6bc8fac1f28c6c695f6c499f339
This commit is contained in:
Matthew Bennice
committed by
Copybara-Service
Copybara-Service
parent
06670155c4
commit
071af3b015
@@ -3446,6 +3446,15 @@ mju_threadPoolCreate
|
||||
|
||||
Create a thread pool with the specified number of threads running.
|
||||
|
||||
.. _mju_bindThreadPool:
|
||||
|
||||
mju_bindThreadPool
|
||||
~~~~~~~~~~~~~~~~~~
|
||||
|
||||
.. mujoco-include:: mju_bindThreadPool
|
||||
|
||||
Adds a thread pool to :ref:`mjData` and configures it for multi-threaded use.
|
||||
|
||||
.. _mju_threadPoolEnqueue:
|
||||
|
||||
mju_threadPoolEnqueue
|
||||
|
||||
+7
-5
@@ -122,11 +122,13 @@ General
|
||||
attributes are specified. See the following
|
||||
`example model <https://github.com/deepmind/mujoco/blob/main/test/engine/testdata/vis_visualize/frustum.xml>`__.
|
||||
- Note that these attributes only take effect for offline rendering and do not affect interactive visualisation.
|
||||
|
||||
22. Added multi-threaded constraint solving via :ref:`mj_island` and :ref:`mjThreadPool` to :ref:`testspeed<saTestspeed>`
|
||||
exposed via npoolthread flag. The `22 humanoids <https://github.com/deepmind/mujoco/blob/main/model/humanoid/22_humanoids.xml>`__
|
||||
model shows a 3x speedup compared to the single threaded simulation.
|
||||
Python bindings
|
||||
^^^^^^^^^^^^^^^
|
||||
|
||||
22. Fixed `#870 <https://github.com/google-deepmind/mujoco/issues/870>`__ where calling ``update_scene`` with an invalid
|
||||
23. Fixed `#870 <https://github.com/google-deepmind/mujoco/issues/870>`__ where calling ``update_scene`` with an invalid
|
||||
camera name used the default camera.
|
||||
|
||||
Simulate
|
||||
@@ -136,18 +138,18 @@ Simulate
|
||||
:align: right
|
||||
:width: 240px
|
||||
|
||||
23. Added **state history** mechanism to :ref:`simulate<saSimulate>` and the managed
|
||||
24. Added **state history** mechanism to :ref:`simulate<saSimulate>` and the managed
|
||||
:ref:`Python viewer<PyViewerManaged>`. State history can be viewed by scrubbing the History slider and (more
|
||||
precisely) with the left and right arrow keys. See screen capture:
|
||||
|
||||
24. The ``LOADING...`` label is now shown correctly.
|
||||
25. The ``LOADING...`` label is now shown correctly.
|
||||
`Contribution <https://github.com/google-deepmind/mujoco/pull/1070>`__ by
|
||||
`Levi Burner <https://github.com/aftersomemath>`__.
|
||||
|
||||
Bug fixes
|
||||
^^^^^^^^^
|
||||
|
||||
25. Fixed a bug that was causing :ref:`geom margin<body-geom-margin>` to be ignored during the construction of
|
||||
26. Fixed a bug that was causing :ref:`geom margin<body-geom-margin>` to be ignored during the construction of
|
||||
midphase collision trees.
|
||||
|
||||
Version 2.3.7 (July 20, 2023)
|
||||
|
||||
@@ -2613,6 +2613,7 @@ int mjp_resourceProviderCount(void);
|
||||
const mjpResourceProvider* mjp_getResourceProvider(const char* resource_name);
|
||||
const mjpResourceProvider* mjp_getResourceProviderAtSlot(int slot);
|
||||
mjThreadPool* mju_threadPoolCreate(size_t number_of_threads);
|
||||
void mju_bindThreadPool(mjData* d, void* thread_pool);
|
||||
void mju_threadPoolEnqueue(mjThreadPool* thread_pool, mjTask* task);
|
||||
void mju_threadPoolDestroy(mjThreadPool* thread_pool);
|
||||
void mju_defaultTask(mjTask* task);
|
||||
|
||||
@@ -1310,6 +1310,9 @@ MJAPI const mjpResourceProvider* mjp_getResourceProviderAtSlot(int slot);
|
||||
// Create a thread pool with the specified number of threads running.
|
||||
MJAPI mjThreadPool* mju_threadPoolCreate(size_t number_of_threads);
|
||||
|
||||
// Adds a thread pool to mjData and configures it for multi-threaded use.
|
||||
MJAPI void mju_bindThreadPool(mjData* d, void* thread_pool);
|
||||
|
||||
// Enqueue a task in a thread pool.
|
||||
MJAPI void mju_threadPoolEnqueue(mjThreadPool* thread_pool, mjTask* task);
|
||||
|
||||
|
||||
@@ -8271,6 +8271,26 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
|
||||
),
|
||||
doc='Create a thread pool with the specified number of threads running.', # pylint: disable=line-too-long
|
||||
)),
|
||||
('mju_bindThreadPool',
|
||||
FunctionDecl(
|
||||
name='mju_bindThreadPool',
|
||||
return_type=ValueType(name='void'),
|
||||
parameters=(
|
||||
FunctionParameterDecl(
|
||||
name='d',
|
||||
type=PointerType(
|
||||
inner_type=ValueType(name='mjData'),
|
||||
),
|
||||
),
|
||||
FunctionParameterDecl(
|
||||
name='thread_pool',
|
||||
type=PointerType(
|
||||
inner_type=ValueType(name='void'),
|
||||
),
|
||||
),
|
||||
),
|
||||
doc='Adds a thread pool to mjData and configures it for multi-threaded use.', # pylint: disable=line-too-long
|
||||
)),
|
||||
('mju_threadPoolEnqueue',
|
||||
FunctionDecl(
|
||||
name='mju_threadPoolEnqueue',
|
||||
|
||||
+15
-3
@@ -119,12 +119,12 @@ void simulate(int id, int nstep, mjtNum* ctrl) {
|
||||
int main(int argc, char** argv) {
|
||||
|
||||
// print help if arguments are missing
|
||||
if (argc < 2 || argc > 6) {
|
||||
return finish("\n Usage: testspeed modelfile [nstep nthread ctrlnoise profile]\n");
|
||||
if (argc < 2 || argc > 7) {
|
||||
return finish("\n Usage: testspeed modelfile [nstep nthread ctrlnoise profile npoolthread]\n");
|
||||
}
|
||||
|
||||
// read arguments
|
||||
int nstep = 10000, nthread = 0, profile = 1;
|
||||
int nstep = 10000, nthread = 0, profile = 1, npoolthread = 0;
|
||||
// inject small noise by default, to avoid fixed contact state
|
||||
mjtNum ctrlnoise = 0.01;
|
||||
if (argc > 2 && (std::sscanf(argv[2], "%d", &nstep) != 1 || nstep <= 0)) {
|
||||
@@ -139,12 +139,16 @@ int main(int argc, char** argv) {
|
||||
if (argc > 5 && std::sscanf(argv[5], "%d", &profile) != 1) {
|
||||
return finish("Invalid profile argument");
|
||||
}
|
||||
if (argc > 6 && std::sscanf(argv[6], "%d", &npoolthread) != 1) {
|
||||
return finish("Invalid npoolthread argument");
|
||||
}
|
||||
|
||||
// clamp ctrlnoise to [0.0, 1.0]
|
||||
ctrlnoise = mjMAX(0.0, mjMIN(ctrlnoise, 1.0));
|
||||
|
||||
// clamp nthread to [1, maxthread]
|
||||
nthread = mjMAX(1, mjMIN(maxthread, nthread));
|
||||
npoolthread = mjMAX(0, mjMIN(maxthread, npoolthread));
|
||||
|
||||
// get filename, determine file type
|
||||
std::string filename(argv[1]);
|
||||
@@ -169,6 +173,10 @@ int main(int argc, char** argv) {
|
||||
return finish("Could not allocate mjData", m);
|
||||
}
|
||||
|
||||
if (npoolthread > 0) {
|
||||
mjThreadPool* threadpool = mju_threadPoolCreate(npoolthread);
|
||||
mju_bindThreadPool(d[id], threadpool);
|
||||
}
|
||||
// init to keyframe "test" if present
|
||||
if (testkey>=0) {
|
||||
mju_copy(d[id]->qpos, m->key_qpos + testkey*m->nq, m->nq);
|
||||
@@ -254,7 +262,11 @@ int main(int argc, char** argv) {
|
||||
|
||||
// free per-thread data
|
||||
for (int id=0; id<nthread; id++) {
|
||||
mjThreadPool* threadpool = (mjThreadPool*) d[id]->threadpool;
|
||||
mj_deleteData(d[id]);
|
||||
if (threadpool) {
|
||||
mju_threadPoolDestroy(threadpool);
|
||||
}
|
||||
}
|
||||
|
||||
// finalize
|
||||
|
||||
@@ -14,6 +14,7 @@
|
||||
|
||||
#include "engine/engine_collision_driver.h"
|
||||
|
||||
#include <math.h>
|
||||
#include <stddef.h>
|
||||
#include <string.h>
|
||||
|
||||
@@ -61,8 +62,10 @@ static void collideGeoms(const mjModel* m, mjData* d,
|
||||
static inline void resetArena(mjData* d) {
|
||||
d->parena = d->ncon * sizeof(mjContact);
|
||||
#ifdef ADDRESS_SANITIZER
|
||||
ASAN_POISON_MEMORY_REGION(
|
||||
(char*)d->arena + d->parena, d->narena - d->pstack - d->parena);
|
||||
if (!d->threadpool) {
|
||||
ASAN_POISON_MEMORY_REGION(
|
||||
(char*)d->arena + d->parena, d->narena - d->pstack - d->parena);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
@@ -40,6 +40,8 @@
|
||||
#include "engine/engine_util_misc.h"
|
||||
#include "engine/engine_util_solve.h"
|
||||
#include "engine/engine_util_sparse.h"
|
||||
#include "thread/thread_pool.h"
|
||||
#include "thread/thread_task.h"
|
||||
|
||||
|
||||
|
||||
@@ -492,6 +494,52 @@ static void warmstart(const mjModel* m, mjData* d) {
|
||||
|
||||
|
||||
|
||||
// struct encapsulating arguments to thread task
|
||||
struct mjSolIslandArgs_ {
|
||||
const mjModel* m;
|
||||
mjData* d;
|
||||
int island;
|
||||
};
|
||||
typedef struct mjSolIslandArgs_ mjSolIslandArgs;
|
||||
|
||||
// extract arguments, pass to solver
|
||||
void* mj_solCG_island_wrapper(void* args) {
|
||||
mjSolIslandArgs* solargs = (mjSolIslandArgs*) args;
|
||||
mj_solCG_island(solargs->m, solargs->d, solargs->island, solargs->m->opt.iterations);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
// CG solver, multi-threaded over islands
|
||||
void mj_solCG_island_multithreaded(const mjModel* m, mjData* d) {
|
||||
mj_markStack(d);
|
||||
// allocate array of arguments to be passed to threads
|
||||
mjSolIslandArgs* sol_cg_island_args =
|
||||
mj_stackAllocByte(d, sizeof(mjSolIslandArgs) * d->nisland, _Alignof(mjSolIslandArgs));
|
||||
mjTask* tasks = mj_stackAllocByte(d, sizeof(mjTask) * d->nisland, _Alignof(mjTask));
|
||||
|
||||
for (int island = 0; island < d->nisland; ++island) {
|
||||
sol_cg_island_args[island].m = m;
|
||||
sol_cg_island_args[island].d = d;
|
||||
sol_cg_island_args[island].island = island;
|
||||
|
||||
mju_defaultTask(&tasks[island]);
|
||||
tasks[island].func = mj_solCG_island_wrapper;
|
||||
tasks[island].args = &sol_cg_island_args[island];
|
||||
mju_threadPoolEnqueue((mjThreadPool*)d->threadpool, &tasks[island]);
|
||||
}
|
||||
|
||||
for (int island = 0; island < d->nisland; ++island) {
|
||||
mju_taskJoin(&tasks[island]);
|
||||
}
|
||||
|
||||
mj_freeStack(d);
|
||||
}
|
||||
|
||||
|
||||
|
||||
// compute efc_b, efc_force, qfrc_constraint; update qacc
|
||||
void mj_fwdConstraint(const mjModel* m, mjData* d) {
|
||||
TM_START;
|
||||
@@ -524,9 +572,15 @@ void mj_fwdConstraint(const mjModel* m, mjData* d) {
|
||||
|
||||
// run solver over constraint islands
|
||||
if (islands_supported) {
|
||||
// loop over islands
|
||||
for (int island=0; island < nisland; island++) {
|
||||
mj_solCG_island(m, d, island, m->opt.iterations);
|
||||
// no threadpool, loop over islands
|
||||
if (!d->threadpool) {
|
||||
for (int island=0; island < nisland; island++) {
|
||||
mj_solCG_island(m, d, island, m->opt.iterations);
|
||||
}
|
||||
}
|
||||
else {
|
||||
// solve using threads
|
||||
mj_solCG_island_multithreaded(m, d);
|
||||
}
|
||||
d->solver_nisland = nisland;
|
||||
}
|
||||
|
||||
@@ -1470,7 +1470,9 @@ static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) {
|
||||
//------------------------------ clear header
|
||||
|
||||
// clear stack pointer
|
||||
d->pstack = 0;
|
||||
if (!d->threadpool) {
|
||||
d->pstack = 0;
|
||||
}
|
||||
d->pbase = 0;
|
||||
|
||||
// clear arena pointers
|
||||
|
||||
@@ -206,7 +206,10 @@ mjStackInfo* mju_getStackInfoForThread(mjData* d, size_t thread_id) {
|
||||
// align the end of the shard to be mjStackInfo.
|
||||
misalignment = result % alignof(mjStackInfo);
|
||||
result -= misalignment;
|
||||
|
||||
#ifdef ADDRESS_SANITIZER
|
||||
// Ensure StackInfo is always accessible
|
||||
ASAN_UNPOISON_MEMORY_REGION((void*)result, sizeof(mjStackInfo));
|
||||
#endif
|
||||
return (mjStackInfo*) result;
|
||||
}
|
||||
|
||||
@@ -253,7 +256,7 @@ static void ConfigureMultiThreadedStack(mjData* d) {
|
||||
}
|
||||
|
||||
// adds a thread pool to mjData and configures it for multi-threaded use.
|
||||
void mju_bindThreadPool(mjData* d, mjThreadPool* thread_pool) {
|
||||
void mju_bindThreadPool(mjData* d, void* thread_pool) {
|
||||
if (d->threadpool) {
|
||||
mju_error("Thread Pool already bound to mjData");
|
||||
}
|
||||
|
||||
@@ -54,7 +54,7 @@ MJAPI mjThreadPool* mju_threadPoolCreate(size_t number_of_threads);
|
||||
mjStackInfo* mju_getStackInfoForThread(mjData* d, size_t thread_id);
|
||||
|
||||
// Adds a thread pool to mjData and configures it for multi-threaded use.
|
||||
MJAPI void mju_bindThreadPool(mjData* d, mjThreadPool* thread_pool);
|
||||
MJAPI void mju_bindThreadPool(mjData* d, void* thread_pool);
|
||||
|
||||
// Gets the number of running threads in the thread pool.
|
||||
MJAPI size_t mju_threadPoolNumberOfThreads(mjThreadPool* thread_pool);
|
||||
|
||||
@@ -15,14 +15,13 @@
|
||||
#ifndef MUJOCO_SRC_THREAD_THREAD_TASK_H_
|
||||
#define MUJOCO_SRC_THREAD_THREAD_TASK_H_
|
||||
|
||||
#include <atomic>
|
||||
#include <new>
|
||||
#include <type_traits>
|
||||
|
||||
#include <mujoco/mjexport.h>
|
||||
#include <mujoco/mjthread.h>
|
||||
|
||||
#ifdef __cplusplus
|
||||
#include <atomic>
|
||||
#include <new>
|
||||
#include <type_traits>
|
||||
namespace mujoco {
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
@@ -48,6 +48,14 @@ target_link_libraries(
|
||||
benchmark::benchmark
|
||||
)
|
||||
|
||||
mujoco_benchmark_test(thread_performance_test)
|
||||
target_link_libraries(
|
||||
thread_performance_test
|
||||
fixture
|
||||
gmock
|
||||
benchmark::benchmark
|
||||
)
|
||||
|
||||
mujoco_benchmark_test(parse_benchmark_test)
|
||||
target_link_libraries(
|
||||
parse_benchmark_test
|
||||
|
||||
@@ -46,23 +46,21 @@ static void run_step_benchmark(const mjModel* model, benchmark::State& state) {
|
||||
int nsteps = kNumWarmupSteps+kNumBenchmarkSteps;
|
||||
std::vector<mjtNum> ctrl = GetCtrlNoise(model, nsteps);
|
||||
|
||||
// warm-up rollout to get a typcal state
|
||||
// warm-up rollout to get a typical state
|
||||
for (int i=0; i < kNumWarmupSteps; i++) {
|
||||
mju_copy(data->ctrl, ctrl.data()+model->nu*i, model->nu);
|
||||
mj_step(model, data);
|
||||
}
|
||||
|
||||
// save state
|
||||
std::vector<mjtNum> qpos = AsVector(data->qpos, model->nq);
|
||||
std::vector<mjtNum> qvel = AsVector(data->qvel, model->nv);
|
||||
std::vector<mjtNum> act = AsVector(data->act, model->na);
|
||||
std::vector<mjtNum> warmstart = AsVector(data->qacc_warmstart, model->nv);
|
||||
int spec = mjSTATE_INTEGRATION;
|
||||
int size = mj_stateSize(model, spec);
|
||||
std::vector<mjtNum> initial_state(size);
|
||||
mj_getState(model, data, initial_state.data(), spec);
|
||||
|
||||
// reset state, benchmark subsequent kNumBenchmarkSteps steps
|
||||
while (state.KeepRunningBatch(kNumBenchmarkSteps)) {
|
||||
mju_copy(data->qpos, qpos.data(), model->nq);
|
||||
mju_copy(data->qvel, qvel.data(), model->nv);
|
||||
mju_copy(data->act, act.data(), model->na);
|
||||
mju_copy(data->qacc_warmstart, warmstart.data(), model->nv);
|
||||
mj_setState(model, data, initial_state.data(), spec);
|
||||
|
||||
for (int i=kNumWarmupSteps; i < nsteps; i++) {
|
||||
mju_copy(data->ctrl, ctrl.data()+model->nu*i, model->nu);
|
||||
|
||||
+2278
File diff suppressed because it is too large
Load Diff
+913
@@ -0,0 +1,913 @@
|
||||
<!-- Copyright 2023 DeepMind Technologies Limited
|
||||
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
-->
|
||||
|
||||
<mujoco model="Humanoid and 200 objects">
|
||||
<!-- Degree of Freedom: 627
|
||||
Actuators: 21
|
||||
|
||||
Humanoid and 200 free primitive objects.
|
||||
-->
|
||||
<include file="humanoid_body.xml"/>
|
||||
|
||||
<option timestep="0.005"/>
|
||||
|
||||
<size memory="1000M"/>
|
||||
|
||||
<default>
|
||||
<geom solimp=".9 .9 .01"/>
|
||||
<default class="object1">
|
||||
<geom type="capsule" material="object1" size="0.1 0.05"/>
|
||||
</default>
|
||||
<default class="object2">
|
||||
<geom type="ellipsoid" material="object2" size="0.15 0.1 0.07"/>
|
||||
</default>
|
||||
<default class="object3">
|
||||
<geom type="box" material="object3" size="0.15 0.1 0.05"/>
|
||||
</default>
|
||||
<default class="object4">
|
||||
<geom type="cylinder" material="object4" size="0.1 0.05" condim="4" friction="1 .01 .01"/>
|
||||
</default>
|
||||
<default class="object5">
|
||||
<geom type="sphere" material="object5" size="0.1"/>
|
||||
</default>
|
||||
<default class="border">
|
||||
<geom type="capsule" size="0.4" rgba=".4 .4 .4 1"/>
|
||||
</default>
|
||||
<default class="borderpost">
|
||||
<geom type="box" size="0.41 0.41 0.41" rgba=".55 .55 .55 1"/>
|
||||
</default>
|
||||
</default>
|
||||
|
||||
<asset>
|
||||
<texture type="skybox" builtin="gradient" width="512" height="512" rgb1=".4 .6 .8" rgb2="0 0 0"/>
|
||||
<texture name="texgeom" type="cube" builtin="flat" mark="cross" width="128" height="128"
|
||||
rgb1="0.6 0.6 0.6" rgb2="0.6 0.6 0.6" markrgb="1 1 1"/>
|
||||
<texture name="texplane" type="2d" builtin="checker" rgb1=".4 .4 .4" rgb2=".6 .6 .6"
|
||||
width="512" height="512"/>
|
||||
<material name='MatPlane' reflectance='0.3' texture="texplane" texrepeat="1 1" texuniform="true"
|
||||
rgba=".7 .7 .7 1"/>
|
||||
<material name='object1' texture="texgeom" texuniform="true" rgba=".4 .9 .6 1" />
|
||||
<material name='object2' texture="texgeom" texuniform="true" rgba=".4 .6 .9 1" />
|
||||
<material name='object3' texture="texgeom" texuniform="true" rgba=".4 .9 .9 1" />
|
||||
<material name='object4' texture="texgeom" texuniform="true" rgba=".8 .6 .8 1" />
|
||||
<material name='object5' texture="texgeom" texuniform="true" rgba=".9 .1 .1 1" />
|
||||
</asset>
|
||||
|
||||
<visual>
|
||||
<quality shadowsize="4096" offsamples="8"/>
|
||||
<map znear="0.1" force="0.05"/>
|
||||
</visual>
|
||||
|
||||
<statistic extent="4"/>
|
||||
|
||||
<worldbody>
|
||||
<light directional="true" diffuse=".8 .8 .8" pos="0 0 10" dir="0 0 -10"/>
|
||||
<geom name="floor" type="plane" size="3 3 .5" material="MatPlane"/>
|
||||
<geom class="border" fromto="-3 3 0 3 3 0" />
|
||||
<geom class="border" fromto="-3 -3 0 3 -3 0" />
|
||||
<geom class="border" fromto="3 3 0 3 -3 0" />
|
||||
<geom class="border" fromto="-3 3 0 -3 -3 0" />
|
||||
<geom class="borderpost" pos="3 3 0"/>
|
||||
<geom class="borderpost" pos="-3 3 0"/>
|
||||
<geom class="borderpost" pos="3 -3 0"/>
|
||||
<geom class="borderpost" pos="-3 -3 0"/>
|
||||
|
||||
<body pos="-2 -2 1" quat="-0.632456 -0.632456 0.316228 0.316228">
|
||||
<freejoint/>
|
||||
<geom class="object1"/>
|
||||
</body>
|
||||
<body pos="-2 -2 2" quat="-0.5547 -0.5547 0.5547 0.27735">
|
||||
<freejoint/>
|
||||
<geom class="object1"/>
|
||||
</body>
|
||||
<body pos="-2 -2 3" quat="-0.471405 -0.471405 0.707107 0.235702">
|
||||
<freejoint/>
|
||||
<geom class="object1"/>
|
||||
</body>
|
||||
<body pos="-2 -2 4" quat="-0.471405 -0.471405 0.707107 0.235702">
|
||||
<freejoint/>
|
||||
<geom class="object1"/>
|
||||
</body>
|
||||
<body pos="-2 -2 5" quat="-0.471405 -0.471405 0.707107 0.235702">
|
||||
<freejoint/>
|
||||
<geom class="object1"/>
|
||||
</body>
|
||||
<body pos="-2 -2 6" quat="-0.471405 -0.471405 0.707107 0.235702">
|
||||
<freejoint/>
|
||||
<geom class="object1"/>
|
||||
</body>
|
||||
<body pos="-2 -2 7" quat="-0.471405 -0.471405 0.707107 0.235702">
|
||||
<freejoint/>
|
||||
<geom class="object1"/>
|
||||
</body>
|
||||
<body pos="-2 -2 8" quat="-0.471405 -0.471405 0.707107 0.235702">
|
||||
<freejoint/>
|
||||
<geom class="object1"/>
|
||||
</body>
|
||||
|
||||
<body pos="-2 -1 1" quat="-0.755929 -0.377964 0.377964 0.377964">
|
||||
<freejoint/>
|
||||
<geom class="object1"/>
|
||||
</body>
|
||||
<body pos="-2 -1 2" quat="-0.632456 -0.316228 0.632456 0.316228">
|
||||
<freejoint/>
|
||||
<geom class="object1"/>
|
||||
</body>
|
||||
<body pos="-2 -1 3" quat="-0.516398 -0.258199 0.774597 0.258199">
|
||||
<freejoint/>
|
||||
<geom class="object1"/>
|
||||
</body>
|
||||
<body pos="-2 -1 4" quat="-0.516398 -0.258199 0.774597 0.258199">
|
||||
<freejoint/>
|
||||
<geom class="object1"/>
|
||||
</body>
|
||||
<body pos="-2 -1 5" quat="-0.755929 -0.377964 0.377964 0.377964">
|
||||
<freejoint/>
|
||||
<geom class="object1"/>
|
||||
</body>
|
||||
<body pos="-2 -1 6" quat="-0.632456 -0.316228 0.632456 0.316228">
|
||||
<freejoint/>
|
||||
<geom class="object1"/>
|
||||
</body>
|
||||
<body pos="-2 -1 7" quat="-0.516398 -0.258199 0.774597 0.258199">
|
||||
<freejoint/>
|
||||
<geom class="object1"/>
|
||||
</body>
|
||||
<body pos="-2 -1 8" quat="-0.516398 -0.258199 0.774597 0.258199">
|
||||
<freejoint/>
|
||||
<geom class="object1"/>
|
||||
</body>
|
||||
|
||||
<body pos="-2 0 1" quat="-0.816497 0 0.408248 0.408248">
|
||||
<freejoint/>
|
||||
<geom class="object1"/>
|
||||
</body>
|
||||
<body pos="-2 0 2" quat="-0.666667 0 0.666667 0.333333">
|
||||
<freejoint/>
|
||||
<geom class="object1"/>
|
||||
</body>
|
||||
<body pos="-2 0 3" quat="-0.534522 0 0.801784 0.267261">
|
||||
<freejoint/>
|
||||
<geom class="object1"/>
|
||||
</body>
|
||||
<body pos="-2 0 4" quat="-0.534522 0 0.801784 0.267261">
|
||||
<freejoint/>
|
||||
<geom class="object1"/>
|
||||
</body>
|
||||
<body pos="-2 0 5" quat="-0.816497 0 0.408248 0.408248">
|
||||
<freejoint/>
|
||||
<geom class="object1"/>
|
||||
</body>
|
||||
<body pos="-2 0 6" quat="-0.666667 0 0.666667 0.333333">
|
||||
<freejoint/>
|
||||
<geom class="object1"/>
|
||||
</body>
|
||||
<body pos="-2 0 7" quat="-0.534522 0 0.801784 0.267261">
|
||||
<freejoint/>
|
||||
<geom class="object1"/>
|
||||
</body>
|
||||
<body pos="-2 0 8" quat="-0.534522 0 0.801784 0.267261">
|
||||
<freejoint/>
|
||||
<geom class="object1"/>
|
||||
</body>
|
||||
|
||||
<body pos="-2 1 1" quat="-0.755929 0.377964 0.377964 0.377964">
|
||||
<freejoint/>
|
||||
<geom class="object1"/>
|
||||
</body>
|
||||
<body pos="-2 1 2" quat="-0.632456 0.316228 0.632456 0.316228">
|
||||
<freejoint/>
|
||||
<geom class="object1"/>
|
||||
</body>
|
||||
<body pos="-2 1 3" quat="-0.516398 0.258199 0.774597 0.258199">
|
||||
<freejoint/>
|
||||
<geom class="object1"/>
|
||||
</body>
|
||||
<body pos="-2 1 4" quat="-0.516398 0.258199 0.774597 0.258199">
|
||||
<freejoint/>
|
||||
<geom class="object1"/>
|
||||
</body>
|
||||
<body pos="-2 1 5" quat="-0.755929 0.377964 0.377964 0.377964">
|
||||
<freejoint/>
|
||||
<geom class="object1"/>
|
||||
</body>
|
||||
<body pos="-2 1 6" quat="-0.632456 0.316228 0.632456 0.316228">
|
||||
<freejoint/>
|
||||
<geom class="object1"/>
|
||||
</body>
|
||||
<body pos="-2 1 7" quat="-0.516398 0.258199 0.774597 0.258199">
|
||||
<freejoint/>
|
||||
<geom class="object1"/>
|
||||
</body>
|
||||
<body pos="-2 1 8" quat="-0.516398 0.258199 0.774597 0.258199">
|
||||
<freejoint/>
|
||||
<geom class="object1"/>
|
||||
</body>
|
||||
|
||||
<body pos="-2 2 1" quat="-0.632456 0.632456 0.316228 0.316228">
|
||||
<freejoint/>
|
||||
<geom class="object1"/>
|
||||
</body>
|
||||
<body pos="-2 2 2" quat="-0.5547 0.5547 0.5547 0.27735">
|
||||
<freejoint/>
|
||||
<geom class="object1"/>
|
||||
</body>
|
||||
<body pos="-2 2 3" quat="-0.471405 0.471405 0.707107 0.235702">
|
||||
<freejoint/>
|
||||
<geom class="object1"/>
|
||||
</body>
|
||||
<body pos="-2 2 4" quat="-0.471405 0.471405 0.707107 0.235702">
|
||||
<freejoint/>
|
||||
<geom class="object1"/>
|
||||
</body>
|
||||
<body pos="-2 2 5" quat="-0.632456 0.632456 0.316228 0.316228">
|
||||
<freejoint/>
|
||||
<geom class="object1"/>
|
||||
</body>
|
||||
<body pos="-2 2 6" quat="-0.5547 0.5547 0.5547 0.27735">
|
||||
<freejoint/>
|
||||
<geom class="object1"/>
|
||||
</body>
|
||||
<body pos="-2 2 7" quat="-0.471405 0.471405 0.707107 0.235702">
|
||||
<freejoint/>
|
||||
<geom class="object1"/>
|
||||
</body>
|
||||
<body pos="-2 2 8" quat="-0.471405 0.471405 0.707107 0.235702">
|
||||
<freejoint/>
|
||||
<geom class="object1"/>
|
||||
</body>
|
||||
|
||||
<body pos="-1 -2 1" quat="-0.377964 -0.755929 0.377964 0.377964">
|
||||
<freejoint/>
|
||||
<geom class="object2"/>
|
||||
</body>
|
||||
<body pos="-1 -2 2" quat="-0.316228 -0.632456 0.632456 0.316228">
|
||||
<freejoint/>
|
||||
<geom class="object2"/>
|
||||
</body>
|
||||
<body pos="-1 -2 3" quat="-0.258199 -0.516398 0.774597 0.258199">
|
||||
<freejoint/>
|
||||
<geom class="object2"/>
|
||||
</body>
|
||||
<body pos="-1 -2 4" quat="-0.258199 -0.516398 0.774597 0.258199">
|
||||
<freejoint/>
|
||||
<geom class="object2"/>
|
||||
</body>
|
||||
<body pos="-1 -2 5" quat="-0.377964 -0.755929 0.377964 0.377964">
|
||||
<freejoint/>
|
||||
<geom class="object2"/>
|
||||
</body>
|
||||
<body pos="-1 -2 6" quat="-0.316228 -0.632456 0.632456 0.316228">
|
||||
<freejoint/>
|
||||
<geom class="object2"/>
|
||||
</body>
|
||||
<body pos="-1 -2 7" quat="-0.258199 -0.516398 0.774597 0.258199">
|
||||
<freejoint/>
|
||||
<geom class="object2"/>
|
||||
</body>
|
||||
<body pos="-1 -2 8" quat="-0.258199 -0.516398 0.774597 0.258199">
|
||||
<freejoint/>
|
||||
<geom class="object2"/>
|
||||
</body>
|
||||
|
||||
<body pos="-1 -1 1" quat="-0.5 -0.5 0.5 0.5">
|
||||
<freejoint/>
|
||||
<geom class="object2"/>
|
||||
</body>
|
||||
<body pos="-1 -1 2" quat="-0.377964 -0.377964 0.755929 0.377964">
|
||||
<freejoint/>
|
||||
<geom class="object2"/>
|
||||
</body>
|
||||
<body pos="-1 -1 3" quat="-0.288675 -0.288675 0.866025 0.288675">
|
||||
<freejoint/>
|
||||
<geom class="object2"/>
|
||||
</body>
|
||||
<body pos="-1 -1 4" quat="-0.288675 -0.288675 0.866025 0.288675">
|
||||
<freejoint/>
|
||||
<geom class="object2"/>
|
||||
</body>
|
||||
<body pos="-1 -1 5" quat="-0.5 -0.5 0.5 0.5">
|
||||
<freejoint/>
|
||||
<geom class="object2"/>
|
||||
</body>
|
||||
<body pos="-1 -1 6" quat="-0.377964 -0.377964 0.755929 0.377964">
|
||||
<freejoint/>
|
||||
<geom class="object2"/>
|
||||
</body>
|
||||
<body pos="-1 -1 7" quat="-0.288675 -0.288675 0.866025 0.288675">
|
||||
<freejoint/>
|
||||
<geom class="object2"/>
|
||||
</body>
|
||||
<body pos="-1 -1 8" quat="-0.288675 -0.288675 0.866025 0.288675">
|
||||
<freejoint/>
|
||||
<geom class="object2"/>
|
||||
</body>
|
||||
|
||||
<body pos="-1 0 1" quat="-0.57735 0 0.57735 0.57735">
|
||||
<freejoint/>
|
||||
<geom class="object2"/>
|
||||
</body>
|
||||
<body pos="-1 0 2" quat="-0.408248 0 0.816497 0.408248">
|
||||
<freejoint/>
|
||||
<geom class="object2"/>
|
||||
</body>
|
||||
<body pos="-1 0 3" quat="-0.301511 0 0.904534 0.301511">
|
||||
<freejoint/>
|
||||
<geom class="object2"/>
|
||||
</body>
|
||||
<body pos="-1 0 4" quat="-0.301511 0 0.904534 0.301511">
|
||||
<freejoint/>
|
||||
<geom class="object2"/>
|
||||
</body>
|
||||
<body pos="-1 0 5" quat="-0.57735 0 0.57735 0.57735">
|
||||
<freejoint/>
|
||||
<geom class="object2"/>
|
||||
</body>
|
||||
<body pos="-1 0 6" quat="-0.408248 0 0.816497 0.408248">
|
||||
<freejoint/>
|
||||
<geom class="object2"/>
|
||||
</body>
|
||||
<body pos="-1 0 7" quat="-0.301511 0 0.904534 0.301511">
|
||||
<freejoint/>
|
||||
<geom class="object2"/>
|
||||
</body>
|
||||
<body pos="-1 0 8" quat="-0.301511 0 0.904534 0.301511">
|
||||
<freejoint/>
|
||||
<geom class="object2"/>
|
||||
</body>
|
||||
|
||||
<body pos="-1 1 1" quat="-0.5 0.5 0.5 0.5">
|
||||
<freejoint/>
|
||||
<geom class="object2"/>
|
||||
</body>
|
||||
<body pos="-1 1 2" quat="-0.377964 0.377964 0.755929 0.377964">
|
||||
<freejoint/>
|
||||
<geom class="object2"/>
|
||||
</body>
|
||||
<body pos="-1 1 3" quat="-0.288675 0.288675 0.866025 0.288675">
|
||||
<freejoint/>
|
||||
<geom class="object2"/>
|
||||
</body>
|
||||
<body pos="-1 1 4" quat="-0.288675 0.288675 0.866025 0.288675">
|
||||
<freejoint/>
|
||||
<geom class="object2"/>
|
||||
</body>
|
||||
<body pos="-1 1 5" quat="-0.5 0.5 0.5 0.5">
|
||||
<freejoint/>
|
||||
<geom class="object2"/>
|
||||
</body>
|
||||
<body pos="-1 1 6" quat="-0.377964 0.377964 0.755929 0.377964">
|
||||
<freejoint/>
|
||||
<geom class="object2"/>
|
||||
</body>
|
||||
<body pos="-1 1 7" quat="-0.288675 0.288675 0.866025 0.288675">
|
||||
<freejoint/>
|
||||
<geom class="object2"/>
|
||||
</body>
|
||||
<body pos="-1 1 8" quat="-0.288675 0.288675 0.866025 0.288675">
|
||||
<freejoint/>
|
||||
<geom class="object2"/>
|
||||
</body>
|
||||
|
||||
<body pos="-1 2 1" quat="-0.377964 0.755929 0.377964 0.377964">
|
||||
<freejoint/>
|
||||
<geom class="object2"/>
|
||||
</body>
|
||||
<body pos="-1 2 2" quat="-0.316228 0.632456 0.632456 0.316228">
|
||||
<freejoint/>
|
||||
<geom class="object2"/>
|
||||
</body>
|
||||
<body pos="-1 2 3" quat="-0.258199 0.516398 0.774597 0.258199">
|
||||
<freejoint/>
|
||||
<geom class="object2"/>
|
||||
</body>
|
||||
<body pos="-1 2 4" quat="-0.258199 0.516398 0.774597 0.258199">
|
||||
<freejoint/>
|
||||
<geom class="object2"/>
|
||||
</body>
|
||||
<body pos="-1 2 5" quat="-0.377964 0.755929 0.377964 0.377964">
|
||||
<freejoint/>
|
||||
<geom class="object2"/>
|
||||
</body>
|
||||
<body pos="-1 2 6" quat="-0.316228 0.632456 0.632456 0.316228">
|
||||
<freejoint/>
|
||||
<geom class="object2"/>
|
||||
</body>
|
||||
<body pos="-1 2 7" quat="-0.258199 0.516398 0.774597 0.258199">
|
||||
<freejoint/>
|
||||
<geom class="object2"/>
|
||||
</body>
|
||||
<body pos="-1 2 8" quat="-0.258199 0.516398 0.774597 0.258199">
|
||||
<freejoint/>
|
||||
<geom class="object2"/>
|
||||
</body>
|
||||
|
||||
<body pos="0 -2 1" quat="0 -0.816497 0.408248 0.408248">
|
||||
<freejoint/>
|
||||
<geom class="object3"/>
|
||||
</body>
|
||||
<body pos="0 -2 2" quat="0 -0.666667 0.666667 0.333333">
|
||||
<freejoint/>
|
||||
<geom class="object3"/>
|
||||
</body>
|
||||
<body pos="0 -2 3" quat="0 -0.534522 0.801784 0.267261">
|
||||
<freejoint/>
|
||||
<geom class="object3"/>
|
||||
</body>
|
||||
<body pos="0 -2 4" quat="0 -0.534522 0.801784 0.267261">
|
||||
<freejoint/>
|
||||
<geom class="object3"/>
|
||||
</body>
|
||||
<body pos="0 -2 5" quat="0 -0.816497 0.408248 0.408248">
|
||||
<freejoint/>
|
||||
<geom class="object3"/>
|
||||
</body>
|
||||
<body pos="0 -2 6" quat="0 -0.666667 0.666667 0.333333">
|
||||
<freejoint/>
|
||||
<geom class="object3"/>
|
||||
</body>
|
||||
<body pos="0 -2 7" quat="0 -0.534522 0.801784 0.267261">
|
||||
<freejoint/>
|
||||
<geom class="object3"/>
|
||||
</body>
|
||||
<body pos="0 -2 8" quat="0 -0.534522 0.801784 0.267261">
|
||||
<freejoint/>
|
||||
<geom class="object3"/>
|
||||
</body>
|
||||
|
||||
<body pos="0 -1 1" quat="0 -0.57735 0.57735 0.57735">
|
||||
<freejoint/>
|
||||
<geom class="object3"/>
|
||||
</body>
|
||||
<body pos="0 -1 2" quat="0 -0.408248 0.816497 0.408248">
|
||||
<freejoint/>
|
||||
<geom class="object3"/>
|
||||
</body>
|
||||
<body pos="0 -1 3" quat="0 -0.301511 0.904534 0.301511">
|
||||
<freejoint/>
|
||||
<geom class="object3"/>
|
||||
</body>
|
||||
<body pos="0 -1 4" quat="0 -0.301511 0.904534 0.301511">
|
||||
<freejoint/>
|
||||
<geom class="object3"/>
|
||||
</body>
|
||||
<body pos="0 -1 5" quat="0 -0.57735 0.57735 0.57735">
|
||||
<freejoint/>
|
||||
<geom class="object3"/>
|
||||
</body>
|
||||
<body pos="0 -1 6" quat="0 -0.408248 0.816497 0.408248">
|
||||
<freejoint/>
|
||||
<geom class="object3"/>
|
||||
</body>
|
||||
<body pos="0 -1 7" quat="0 -0.301511 0.904534 0.301511">
|
||||
<freejoint/>
|
||||
<geom class="object3"/>
|
||||
</body>
|
||||
<body pos="0 -1 8" quat="0 -0.301511 0.904534 0.301511">
|
||||
<freejoint/>
|
||||
<geom class="object3"/>
|
||||
</body>
|
||||
|
||||
<body pos="0.3 0 1" quat="0 0 0.707107 0.707107">
|
||||
<freejoint/>
|
||||
<geom class="object3"/>
|
||||
</body>
|
||||
<body pos="0 0 2" quat="0 0 0.894427 0.447214">
|
||||
<freejoint/>
|
||||
<geom class="object3"/>
|
||||
</body>
|
||||
<body pos="0 0 3" quat="0 0 0.948683 0.316228">
|
||||
<freejoint/>
|
||||
<geom class="object3"/>
|
||||
</body>
|
||||
<body pos="0 0 4" quat="0 0 0.948683 0.316228">
|
||||
<freejoint/>
|
||||
<geom class="object3"/>
|
||||
</body>
|
||||
<body pos="0.3 0 5" quat="0 0 0.707107 0.707107">
|
||||
<freejoint/>
|
||||
<geom class="object3"/>
|
||||
</body>
|
||||
<body pos="0 0 6" quat="0 0 0.894427 0.447214">
|
||||
<freejoint/>
|
||||
<geom class="object3"/>
|
||||
</body>
|
||||
<body pos="0 0 7" quat="0 0 0.948683 0.316228">
|
||||
<freejoint/>
|
||||
<geom class="object3"/>
|
||||
</body>
|
||||
<body pos="0 0 8" quat="0 0 0.948683 0.316228">
|
||||
<freejoint/>
|
||||
<geom class="object3"/>
|
||||
</body>
|
||||
|
||||
<body pos="0 1 1" quat="0 0.57735 0.57735 0.57735">
|
||||
<freejoint/>
|
||||
<geom class="object3"/>
|
||||
</body>
|
||||
<body pos="0 1 2" quat="0 0.408248 0.816497 0.408248">
|
||||
<freejoint/>
|
||||
<geom class="object3"/>
|
||||
</body>
|
||||
<body pos="0 1 3" quat="0 0.301511 0.904534 0.301511">
|
||||
<freejoint/>
|
||||
<geom class="object3"/>
|
||||
</body>
|
||||
<body pos="0 1 4" quat="0 0.301511 0.904534 0.301511">
|
||||
<freejoint/>
|
||||
<geom class="object3"/>
|
||||
</body>
|
||||
<body pos="0 1 5" quat="0 0.57735 0.57735 0.57735">
|
||||
<freejoint/>
|
||||
<geom class="object3"/>
|
||||
</body>
|
||||
<body pos="0 1 6" quat="0 0.408248 0.816497 0.408248">
|
||||
<freejoint/>
|
||||
<geom class="object3"/>
|
||||
</body>
|
||||
<body pos="0 1 7" quat="0 0.301511 0.904534 0.301511">
|
||||
<freejoint/>
|
||||
<geom class="object3"/>
|
||||
</body>
|
||||
<body pos="0 1 8" quat="0 0.301511 0.904534 0.301511">
|
||||
<freejoint/>
|
||||
<geom class="object3"/>
|
||||
</body>
|
||||
|
||||
<body pos="0 2 1" quat="0 0.816497 0.408248 0.408248">
|
||||
<freejoint/>
|
||||
<geom class="object3"/>
|
||||
</body>
|
||||
<body pos="0 2 2" quat="0 0.666667 0.666667 0.333333">
|
||||
<freejoint/>
|
||||
<geom class="object3"/>
|
||||
</body>
|
||||
<body pos="0 2 3" quat="0 0.534522 0.801784 0.267261">
|
||||
<freejoint/>
|
||||
<geom class="object3"/>
|
||||
</body>
|
||||
<body pos="0 2 4" quat="0 0.534522 0.801784 0.267261">
|
||||
<freejoint/>
|
||||
<geom class="object3"/>
|
||||
</body>
|
||||
<body pos="0 2 5" quat="0 0.816497 0.408248 0.408248">
|
||||
<freejoint/>
|
||||
<geom class="object3"/>
|
||||
</body>
|
||||
<body pos="0 2 6" quat="0 0.666667 0.666667 0.333333">
|
||||
<freejoint/>
|
||||
<geom class="object3"/>
|
||||
</body>
|
||||
<body pos="0 2 7" quat="0 0.534522 0.801784 0.267261">
|
||||
<freejoint/>
|
||||
<geom class="object3"/>
|
||||
</body>
|
||||
<body pos="0 2 8" quat="0 0.534522 0.801784 0.267261">
|
||||
<freejoint/>
|
||||
<geom class="object3"/>
|
||||
</body>
|
||||
|
||||
<body pos="1 -2 1" quat="0.377964 -0.755929 0.377964 0.377964">
|
||||
<freejoint/>
|
||||
<geom class="object4"/>
|
||||
</body>
|
||||
<body pos="1 -2 2" quat="0.316228 -0.632456 0.632456 0.316228">
|
||||
<freejoint/>
|
||||
<geom class="object4"/>
|
||||
</body>
|
||||
<body pos="1 -2 3" quat="0.258199 -0.516398 0.774597 0.258199">
|
||||
<freejoint/>
|
||||
<geom class="object4"/>
|
||||
</body>
|
||||
<body pos="1 -2 4" quat="0.258199 -0.516398 0.774597 0.258199">
|
||||
<freejoint/>
|
||||
<geom class="object4"/>
|
||||
</body>
|
||||
<body pos="1 -2 5" quat="0.377964 -0.755929 0.377964 0.377964">
|
||||
<freejoint/>
|
||||
<geom class="object4"/>
|
||||
</body>
|
||||
<body pos="1 -2 6" quat="0.316228 -0.632456 0.632456 0.316228">
|
||||
<freejoint/>
|
||||
<geom class="object4"/>
|
||||
</body>
|
||||
<body pos="1 -2 7" quat="0.258199 -0.516398 0.774597 0.258199">
|
||||
<freejoint/>
|
||||
<geom class="object4"/>
|
||||
</body>
|
||||
<body pos="1 -2 8" quat="0.258199 -0.516398 0.774597 0.258199">
|
||||
<freejoint/>
|
||||
<geom class="object4"/>
|
||||
</body>
|
||||
|
||||
<body pos="1 -1 1" quat="0.5 -0.5 0.5 0.5">
|
||||
<freejoint/>
|
||||
<geom class="object4"/>
|
||||
</body>
|
||||
<body pos="1 -1 2" quat="0.377964 -0.377964 0.755929 0.377964">
|
||||
<freejoint/>
|
||||
<geom class="object4"/>
|
||||
</body>
|
||||
<body pos="1 -1 3" quat="0.288675 -0.288675 0.866025 0.288675">
|
||||
<freejoint/>
|
||||
<geom class="object4"/>
|
||||
</body>
|
||||
<body pos="1 -1 4" quat="0.288675 -0.288675 0.866025 0.288675">
|
||||
<freejoint/>
|
||||
<geom class="object4"/>
|
||||
</body>
|
||||
<body pos="1 -1 5" quat="0.5 -0.5 0.5 0.5">
|
||||
<freejoint/>
|
||||
<geom class="object4"/>
|
||||
</body>
|
||||
<body pos="1 -1 6" quat="0.377964 -0.377964 0.755929 0.377964">
|
||||
<freejoint/>
|
||||
<geom class="object4"/>
|
||||
</body>
|
||||
<body pos="1 -1 7" quat="0.288675 -0.288675 0.866025 0.288675">
|
||||
<freejoint/>
|
||||
<geom class="object4"/>
|
||||
</body>
|
||||
<body pos="1 -1 8" quat="0.288675 -0.288675 0.866025 0.288675">
|
||||
<freejoint/>
|
||||
<geom class="object4"/>
|
||||
</body>
|
||||
|
||||
<body pos="1 0 1" quat="0.57735 0 0.57735 0.57735">
|
||||
<freejoint/>
|
||||
<geom class="object4"/>
|
||||
</body>
|
||||
<body pos="1 0 2" quat="0.408248 0 0.816497 0.408248">
|
||||
<freejoint/>
|
||||
<geom class="object4"/>
|
||||
</body>
|
||||
<body pos="1 0 3" quat="0.301511 0 0.904534 0.301511">
|
||||
<freejoint/>
|
||||
<geom class="object4"/>
|
||||
</body>
|
||||
<body pos="1 0 4" quat="0.301511 0 0.904534 0.301511">
|
||||
<freejoint/>
|
||||
<geom class="object4"/>
|
||||
</body>
|
||||
<body pos="1 0 5" quat="0.57735 0 0.57735 0.57735">
|
||||
<freejoint/>
|
||||
<geom class="object4"/>
|
||||
</body>
|
||||
<body pos="1 0 6" quat="0.408248 0 0.816497 0.408248">
|
||||
<freejoint/>
|
||||
<geom class="object4"/>
|
||||
</body>
|
||||
<body pos="1 0 7" quat="0.301511 0 0.904534 0.301511">
|
||||
<freejoint/>
|
||||
<geom class="object4"/>
|
||||
</body>
|
||||
<body pos="1 0 8" quat="0.301511 0 0.904534 0.301511">
|
||||
<freejoint/>
|
||||
<geom class="object4"/>
|
||||
</body>
|
||||
|
||||
<body pos="1 1 1" quat="0.5 0.5 0.5 0.5">
|
||||
<freejoint/>
|
||||
<geom class="object4"/>
|
||||
</body>
|
||||
<body pos="1 1 2" quat="0.377964 0.377964 0.755929 0.377964">
|
||||
<freejoint/>
|
||||
<geom class="object4"/>
|
||||
</body>
|
||||
<body pos="1 1 3" quat="0.288675 0.288675 0.866025 0.288675">
|
||||
<freejoint/>
|
||||
<geom class="object4"/>
|
||||
</body>
|
||||
<body pos="1 1 4" quat="0.288675 0.288675 0.866025 0.288675">
|
||||
<freejoint/>
|
||||
<geom class="object4"/>
|
||||
</body>
|
||||
<body pos="1 1 5" quat="0.5 0.5 0.5 0.5">
|
||||
<freejoint/>
|
||||
<geom class="object4"/>
|
||||
</body>
|
||||
<body pos="1 1 6" quat="0.377964 0.377964 0.755929 0.377964">
|
||||
<freejoint/>
|
||||
<geom class="object4"/>
|
||||
</body>
|
||||
<body pos="1 1 7" quat="0.288675 0.288675 0.866025 0.288675">
|
||||
<freejoint/>
|
||||
<geom class="object4"/>
|
||||
</body>
|
||||
<body pos="1 1 8" quat="0.288675 0.288675 0.866025 0.288675">
|
||||
<freejoint/>
|
||||
<geom class="object4"/>
|
||||
</body>
|
||||
|
||||
<body pos="1 2 1" quat="0.377964 0.755929 0.377964 0.377964">
|
||||
<freejoint/>
|
||||
<geom class="object4"/>
|
||||
</body>
|
||||
<body pos="1 2 2" quat="0.316228 0.632456 0.632456 0.316228">
|
||||
<freejoint/>
|
||||
<geom class="object4"/>
|
||||
</body>
|
||||
<body pos="1 2 3" quat="0.258199 0.516398 0.774597 0.258199">
|
||||
<freejoint/>
|
||||
<geom class="object4"/>
|
||||
</body>
|
||||
<body pos="1 2 4" quat="0.258199 0.516398 0.774597 0.258199">
|
||||
<freejoint/>
|
||||
<geom class="object4"/>
|
||||
</body>
|
||||
<body pos="1 2 5" quat="0.377964 0.755929 0.377964 0.377964">
|
||||
<freejoint/>
|
||||
<geom class="object4"/>
|
||||
</body>
|
||||
<body pos="1 2 6" quat="0.316228 0.632456 0.632456 0.316228">
|
||||
<freejoint/>
|
||||
<geom class="object4"/>
|
||||
</body>
|
||||
<body pos="1 2 7" quat="0.258199 0.516398 0.774597 0.258199">
|
||||
<freejoint/>
|
||||
<geom class="object4"/>
|
||||
</body>
|
||||
<body pos="1 2 8" quat="0.258199 0.516398 0.774597 0.258199">
|
||||
<freejoint/>
|
||||
<geom class="object4"/>
|
||||
</body>
|
||||
|
||||
<body pos="2 -2 1" quat="0.632456 -0.632456 0.316228 0.316228">
|
||||
<freejoint/>
|
||||
<geom class="object5"/>
|
||||
</body>
|
||||
<body pos="2 -2 2" quat="0.5547 -0.5547 0.5547 0.27735">
|
||||
<freejoint/>
|
||||
<geom class="object5"/>
|
||||
</body>
|
||||
<body pos="2 -2 3" quat="0.471405 -0.471405 0.707107 0.235702">
|
||||
<freejoint/>
|
||||
<geom class="object5"/>
|
||||
</body>
|
||||
<body pos="2 -2 4" quat="0.471405 -0.471405 0.707107 0.235702">
|
||||
<freejoint/>
|
||||
<geom class="object5"/>
|
||||
</body>
|
||||
<body pos="2 -2 5" quat="0.632456 -0.632456 0.316228 0.316228">
|
||||
<freejoint/>
|
||||
<geom class="object5"/>
|
||||
</body>
|
||||
<body pos="2 -2 6" quat="0.5547 -0.5547 0.5547 0.27735">
|
||||
<freejoint/>
|
||||
<geom class="object5"/>
|
||||
</body>
|
||||
<body pos="2 -2 7" quat="0.471405 -0.471405 0.707107 0.235702">
|
||||
<freejoint/>
|
||||
<geom class="object5"/>
|
||||
</body>
|
||||
<body pos="2 -2 8" quat="0.471405 -0.471405 0.707107 0.235702">
|
||||
<freejoint/>
|
||||
<geom class="object5"/>
|
||||
</body>
|
||||
|
||||
<body pos="2 -1 1" quat="0.755929 -0.377964 0.377964 0.377964">
|
||||
<freejoint/>
|
||||
<geom class="object5"/>
|
||||
</body>
|
||||
<body pos="2 -1 2" quat="0.632456 -0.316228 0.632456 0.316228">
|
||||
<freejoint/>
|
||||
<geom class="object5"/>
|
||||
</body>
|
||||
<body pos="2 -1 3" quat="0.516398 -0.258199 0.774597 0.258199">
|
||||
<freejoint/>
|
||||
<geom class="object5"/>
|
||||
</body>
|
||||
<body pos="2 -1 4" quat="0.516398 -0.258199 0.774597 0.258199">
|
||||
<freejoint/>
|
||||
<geom class="object5"/>
|
||||
</body>
|
||||
<body pos="2 -1 5" quat="0.755929 -0.377964 0.377964 0.377964">
|
||||
<freejoint/>
|
||||
<geom class="object5"/>
|
||||
</body>
|
||||
<body pos="2 -1 6" quat="0.632456 -0.316228 0.632456 0.316228">
|
||||
<freejoint/>
|
||||
<geom class="object5"/>
|
||||
</body>
|
||||
<body pos="2 -1 7" quat="0.516398 -0.258199 0.774597 0.258199">
|
||||
<freejoint/>
|
||||
<geom class="object5"/>
|
||||
</body>
|
||||
<body pos="2 -1 8" quat="0.516398 -0.258199 0.774597 0.258199">
|
||||
<freejoint/>
|
||||
<geom class="object5"/>
|
||||
</body>
|
||||
|
||||
<body pos="2 0 1" quat="0.816497 0 0.408248 0.408248">
|
||||
<freejoint/>
|
||||
<geom class="object5"/>
|
||||
</body>
|
||||
<body pos="2 0 2" quat="0.666667 0 0.666667 0.333333">
|
||||
<freejoint/>
|
||||
<geom class="object5"/>
|
||||
</body>
|
||||
<body pos="2 0 3" quat="0.534522 0 0.801784 0.267261">
|
||||
<freejoint/>
|
||||
<geom class="object5"/>
|
||||
</body>
|
||||
<body pos="2 0 4" quat="0.534522 0 0.801784 0.267261">
|
||||
<freejoint/>
|
||||
<geom class="object5"/>
|
||||
</body>
|
||||
<body pos="2 0 5" quat="0.816497 0 0.408248 0.408248">
|
||||
<freejoint/>
|
||||
<geom class="object5"/>
|
||||
</body>
|
||||
<body pos="2 0 6" quat="0.666667 0 0.666667 0.333333">
|
||||
<freejoint/>
|
||||
<geom class="object5"/>
|
||||
</body>
|
||||
<body pos="2 0 7" quat="0.534522 0 0.801784 0.267261">
|
||||
<freejoint/>
|
||||
<geom class="object5"/>
|
||||
</body>
|
||||
<body pos="2 0 8" quat="0.534522 0 0.801784 0.267261">
|
||||
<freejoint/>
|
||||
<geom class="object5"/>
|
||||
</body>
|
||||
|
||||
<body pos="2 1 1" quat="0.755929 0.377964 0.377964 0.377964">
|
||||
<freejoint/>
|
||||
<geom class="object5"/>
|
||||
</body>
|
||||
<body pos="2 1 2" quat="0.632456 0.316228 0.632456 0.316228">
|
||||
<freejoint/>
|
||||
<geom class="object5"/>
|
||||
</body>
|
||||
<body pos="2 1 3" quat="0.516398 0.258199 0.774597 0.258199">
|
||||
<freejoint/>
|
||||
<geom class="object5"/>
|
||||
</body>
|
||||
<body pos="2 1 4" quat="0.516398 0.258199 0.774597 0.258199">
|
||||
<freejoint/>
|
||||
<geom class="object5"/>
|
||||
</body>
|
||||
<body pos="2 1 5" quat="0.755929 0.377964 0.377964 0.377964">
|
||||
<freejoint/>
|
||||
<geom class="object5"/>
|
||||
</body>
|
||||
<body pos="2 1 6" quat="0.632456 0.316228 0.632456 0.316228">
|
||||
<freejoint/>
|
||||
<geom class="object5"/>
|
||||
</body>
|
||||
<body pos="2 1 7" quat="0.516398 0.258199 0.774597 0.258199">
|
||||
<freejoint/>
|
||||
<geom class="object5"/>
|
||||
</body>
|
||||
<body pos="2 1 8" quat="0.516398 0.258199 0.774597 0.258199">
|
||||
<freejoint/>
|
||||
<geom class="object5"/>
|
||||
</body>
|
||||
|
||||
<body pos="2 2 1" quat="0.632456 0.632456 0.316228 0.316228">
|
||||
<freejoint/>
|
||||
<geom class="object5"/>
|
||||
</body>
|
||||
<body pos="2 2 2" quat="0.5547 0.5547 0.5547 0.27735">
|
||||
<freejoint/>
|
||||
<geom class="object5"/>
|
||||
</body>
|
||||
<body pos="2 2 3" quat="0.471405 0.471405 0.707107 0.235702">
|
||||
<freejoint/>
|
||||
<geom class="object5"/>
|
||||
</body>
|
||||
<body pos="2 2 4" quat="0.471405 0.471405 0.707107 0.235702">
|
||||
<freejoint/>
|
||||
<geom class="object5"/>
|
||||
</body>
|
||||
<body pos="2 2 5" quat="0.632456 0.632456 0.316228 0.316228">
|
||||
<freejoint/>
|
||||
<geom class="object5"/>
|
||||
</body>
|
||||
<body pos="2 2 6" quat="0.5547 0.5547 0.5547 0.27735">
|
||||
<freejoint/>
|
||||
<geom class="object5"/>
|
||||
</body>
|
||||
<body pos="2 2 7" quat="0.471405 0.471405 0.707107 0.235702">
|
||||
<freejoint/>
|
||||
<geom class="object5"/>
|
||||
</body>
|
||||
<body pos="2 2 8" quat="0.471405 0.471405 0.707107 0.235702">
|
||||
<freejoint/>
|
||||
<geom class="object5"/>
|
||||
</body>
|
||||
|
||||
</worldbody>
|
||||
</mujoco>
|
||||
+158
@@ -0,0 +1,158 @@
|
||||
<!-- Copyright 2023 DeepMind Technologies Limited
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
-->
|
||||
|
||||
<mujoco model="Humanoid body">
|
||||
<!-- Degree of Freedom: 27
|
||||
Actuators: 21
|
||||
|
||||
This simplified humanoid model, introduced in [1], is designed for bipedal locomotion
|
||||
behaviours. While several variants of it exist in the wild, this version is based on the model
|
||||
in the DeepMind Control Suite [2], which has fairly realistic actuator gains.
|
||||
[1] Synthesis and Stabilization of Complex Behaviors through Online Trajectory Optimization.
|
||||
https://doi.org/10.1109/IROS.2012.6386025
|
||||
[2] DeepMind Control Suite, Tassa et al. https://arxiv.org/abs/1801.00690
|
||||
-->
|
||||
|
||||
<asset>
|
||||
<texture type="skybox" builtin="gradient" rgb1=".3 .5 .7" rgb2="0 0 0" width="512" height="512"/>
|
||||
<texture name="body" type="cube" builtin="flat" mark="cross" width="128" height="128"
|
||||
rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" markrgb="1 1 1" random="0.01"/>
|
||||
<material name="body" texture="body" texuniform="true" rgba="0.8 0.6 .4 1"/>
|
||||
<texture name="grid" type="2d" builtin="checker" width="512" height="512" rgb1=".1 .2 .3" rgb2=".2 .3 .4"/>
|
||||
<material name="grid" texture="grid" texrepeat="1 1" texuniform="true" reflectance=".2"/>
|
||||
</asset>
|
||||
|
||||
<default>
|
||||
<motor ctrlrange="-1 1" ctrllimited="true"/>
|
||||
<default class="body">
|
||||
<geom type="capsule" condim="1" friction=".7" solimp=".9 .99 .003" solref=".015 1" material="body"/>
|
||||
<joint type="hinge" damping=".2" stiffness="1" armature=".01" limited="true" solimplimit="0 .99 .01"/>
|
||||
<default class="big_joint">
|
||||
<joint damping="5" stiffness="10"/>
|
||||
<default class="big_stiff_joint">
|
||||
<joint stiffness="20"/>
|
||||
</default>
|
||||
</default>
|
||||
</default>
|
||||
</default>
|
||||
|
||||
<visual>
|
||||
<map force="0.1" zfar="30"/>
|
||||
<rgba haze="0.15 0.25 0.35 1"/>
|
||||
<quality shadowsize="4096"/>
|
||||
<global offwidth="800" offheight="800"/>
|
||||
</visual>
|
||||
|
||||
<worldbody>
|
||||
<body name="torso" pos="0 0 1.5" childclass="body">
|
||||
<light name="top" pos="0 0 2" mode="trackcom"/>
|
||||
<camera name="back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
|
||||
<camera name="side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
|
||||
<freejoint name="root"/>
|
||||
<geom name="torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
|
||||
<geom name="upper_waist" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
|
||||
<body name="head" pos="0 0 .19">
|
||||
<geom name="head" type="sphere" size=".09"/>
|
||||
<camera name="egocentric" pos=".09 0 0" xyaxes="0 -1 0 .1 0 1" fovy="80"/>
|
||||
</body>
|
||||
<body name="lower_waist" pos="-.01 0 -.26">
|
||||
<geom name="lower_waist" fromto="0 -.06 0 0 .06 0" size=".06"/>
|
||||
<joint name="abdomen_z" pos="0 0 .065" axis="0 0 1" range="-45 45" class="big_stiff_joint"/>
|
||||
<joint name="abdomen_y" pos="0 0 .065" axis="0 1 0" range="-75 30" class="big_joint"/>
|
||||
<body name="pelvis" pos="0 0 -.165">
|
||||
<joint name="abdomen_x" pos="0 0 .1" axis="1 0 0" range="-35 35" class="big_joint"/>
|
||||
<geom name="butt" fromto="-.02 -.07 0 -.02 .07 0" size=".09"/>
|
||||
<body name="right_thigh" pos="0 -.1 -.04">
|
||||
<joint name="right_hip_x" axis="1 0 0" range="-25 5" class="big_joint"/>
|
||||
<joint name="right_hip_z" axis="0 0 1" range="-60 35" class="big_joint"/>
|
||||
<joint name="right_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
|
||||
<geom name="right_thigh" fromto="0 0 0 0 .01 -.34" size=".06"/>
|
||||
<body name="right_shin" pos="0 .01 -.403">
|
||||
<joint name="right_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
|
||||
<geom name="right_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
|
||||
<body name="right_foot" pos="0 0 -.39">
|
||||
<joint name="right_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
|
||||
<joint name="right_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
|
||||
<geom name="right_right_foot" fromto="-.07 -.02 0 .14 -.04 0" size=".027"/>
|
||||
<geom name="left_right_foot" fromto="-.07 0 0 .14 .02 0" size=".027"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
<body name="left_thigh" pos="0 .1 -.04">
|
||||
<joint name="left_hip_x" axis="-1 0 0" range="-25 5" class="big_joint"/>
|
||||
<joint name="left_hip_z" axis="0 0 -1" range="-60 35" class="big_joint"/>
|
||||
<joint name="left_hip_y" axis="0 1 0" range="-110 20" class="big_stiff_joint"/>
|
||||
<geom name="left_thigh" fromto="0 0 0 0 -.01 -.34" size=".06"/>
|
||||
<body name="left_shin" pos="0 -.01 -.403">
|
||||
<joint name="left_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
|
||||
<geom name="left_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
|
||||
<body name="left_foot" pos="0 0 -.39">
|
||||
<joint name="left_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
|
||||
<joint name="left_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
|
||||
<geom name="left_left_foot" fromto="-.07 .02 0 .14 .04 0" size=".027"/>
|
||||
<geom name="right_left_foot" fromto="-.07 0 0 .14 -.02 0" size=".027"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
<body name="right_upper_arm" pos="0 -.17 .06">
|
||||
<joint name="right_shoulder1" axis="2 1 1" range="-85 60"/>
|
||||
<joint name="right_shoulder2" axis="0 -1 1" range="-85 60"/>
|
||||
<geom name="right_upper_arm" fromto="0 0 0 .16 -.16 -.16" size=".04 .16"/>
|
||||
<body name="right_lower_arm" pos=".18 -.18 -.18">
|
||||
<joint name="right_elbow" axis="0 -1 1" range="-90 50" stiffness="0"/>
|
||||
<geom name="right_lower_arm" fromto=".01 .01 .01 .17 .17 .17" size=".031"/>
|
||||
<body name="right_hand" pos=".18 .18 .18">
|
||||
<geom name="right_hand" type="sphere" size=".04" zaxis="1 1 1"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
<body name="left_upper_arm" pos="0 .17 .06">
|
||||
<joint name="left_shoulder1" axis="2 -1 1" range="-60 85"/>
|
||||
<joint name="left_shoulder2" axis="0 1 1" range="-60 85"/>
|
||||
<geom name="left_upper_arm" fromto="0 0 0 .16 .16 -.16" size=".04 .16"/>
|
||||
<body name="left_lower_arm" pos=".18 .18 -.18">
|
||||
<joint name="left_elbow" axis="0 -1 -1" range="-90 50" stiffness="0"/>
|
||||
<geom name="left_lower_arm" fromto=".01 -.01 .01 .17 -.17 .17" size=".031"/>
|
||||
<body name="left_hand" pos=".18 -.18 .18">
|
||||
<geom name="left_hand" type="sphere" size=".04" zaxis="1 -1 1"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</worldbody>
|
||||
|
||||
<actuator>
|
||||
<motor name="abdomen_y" gear="40" joint="abdomen_y"/>
|
||||
<motor name="abdomen_z" gear="40" joint="abdomen_z"/>
|
||||
<motor name="abdomen_x" gear="40" joint="abdomen_x"/>
|
||||
<motor name="right_hip_x" gear="40" joint="right_hip_x"/>
|
||||
<motor name="right_hip_z" gear="40" joint="right_hip_z"/>
|
||||
<motor name="right_hip_y" gear="120" joint="right_hip_y"/>
|
||||
<motor name="right_knee" gear="80" joint="right_knee"/>
|
||||
<motor name="right_ankle_x" gear="20" joint="right_ankle_x"/>
|
||||
<motor name="right_ankle_y" gear="20" joint="right_ankle_y"/>
|
||||
<motor name="left_hip_x" gear="40" joint="left_hip_x"/>
|
||||
<motor name="left_hip_z" gear="40" joint="left_hip_z"/>
|
||||
<motor name="left_hip_y" gear="120" joint="left_hip_y"/>
|
||||
<motor name="left_knee" gear="80" joint="left_knee"/>
|
||||
<motor name="left_ankle_x" gear="20" joint="left_ankle_x"/>
|
||||
<motor name="left_ankle_y" gear="20" joint="left_ankle_y"/>
|
||||
<motor name="right_shoulder1" gear="20" joint="right_shoulder1"/>
|
||||
<motor name="right_shoulder2" gear="20" joint="right_shoulder2"/>
|
||||
<motor name="right_elbow" gear="40" joint="right_elbow"/>
|
||||
<motor name="left_shoulder1" gear="20" joint="left_shoulder1"/>
|
||||
<motor name="left_shoulder2" gear="20" joint="left_shoulder2"/>
|
||||
<motor name="left_elbow" gear="40" joint="left_elbow"/>
|
||||
</actuator>
|
||||
</mujoco>
|
||||
@@ -0,0 +1,110 @@
|
||||
// Copyright 2021 DeepMind Technologies Limited
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
#include <array>
|
||||
#include <vector>
|
||||
|
||||
#include <benchmark/benchmark.h>
|
||||
#include <mujoco/mjmodel.h>
|
||||
#include <mujoco/mjthread.h>
|
||||
#include <mujoco/mujoco.h>
|
||||
#include "test/fixture.h"
|
||||
|
||||
namespace mujoco {
|
||||
namespace {
|
||||
|
||||
// number of steps to roll out before benchmarking
|
||||
static const int kNumWarmupSteps = 500;
|
||||
|
||||
void BM_StepHumanoid200(benchmark::State& state) {
|
||||
auto model_path = GetTestDataFilePath("benchmark/testdata/humanoid200.xml");
|
||||
std::array<char, 1024> error;
|
||||
mjModel* model =
|
||||
mj_loadXML(model_path.c_str(), nullptr, error.data(), error.size());
|
||||
|
||||
model->opt.solver = mjSOL_CG; // use CG solver
|
||||
model->opt.enableflags |= mjENBL_ISLAND; // enable islands
|
||||
|
||||
mjData* data = mj_makeData(model);
|
||||
mjThreadPool* threadpool = mju_threadPoolCreate(10);
|
||||
mju_bindThreadPool(data, threadpool);
|
||||
|
||||
// warm-up rollout to get a steady state
|
||||
for (int i = 0; i < kNumWarmupSteps; i++) {
|
||||
mj_step(model, data);
|
||||
}
|
||||
|
||||
// save the initial state and step
|
||||
int spec = mjSTATE_INTEGRATION;
|
||||
int size = mj_stateSize(model, spec);
|
||||
std::vector<mjtNum> initial_state(size);
|
||||
mj_getState(model, data, initial_state.data(), spec);
|
||||
|
||||
// note: this tests resetting and stepping
|
||||
for (int i = 0; i < 10; ++i) {
|
||||
// reset to the saved state, step again, get the resulting state
|
||||
mj_setState(model, data, initial_state.data(), spec);
|
||||
for (int i = 0; i < kNumWarmupSteps; i++) {
|
||||
mj_step(model, data);
|
||||
}
|
||||
}
|
||||
|
||||
state.SetItemsProcessed(state.iterations());
|
||||
mj_deleteData(data);
|
||||
mju_threadPoolDestroy(threadpool);
|
||||
}
|
||||
|
||||
void BM_Step22Humanoids(benchmark::State& state) {
|
||||
auto model_path = GetTestDataFilePath("benchmark/testdata/22_humanoids.xml");
|
||||
std::array<char, 1024> error;
|
||||
mjModel* model =
|
||||
mj_loadXML(model_path.c_str(), nullptr, error.data(), error.size());
|
||||
model->opt.solver = mjSOL_CG; // use CG solver
|
||||
model->opt.enableflags |= mjENBL_ISLAND; // enable islands
|
||||
|
||||
mjData* data = mj_makeData(model);
|
||||
mjThreadPool* threadpool = mju_threadPoolCreate(10);
|
||||
mju_bindThreadPool(data, threadpool);
|
||||
|
||||
// warm-up rollout to get a steady state
|
||||
for (int i = 0; i < kNumWarmupSteps; i++) {
|
||||
mj_step(model, data);
|
||||
}
|
||||
|
||||
// save the initial state and step
|
||||
int spec = mjSTATE_INTEGRATION;
|
||||
int size = mj_stateSize(model, spec);
|
||||
std::vector<mjtNum> initial_state(size);
|
||||
mj_getState(model, data, initial_state.data(), spec);
|
||||
|
||||
std::vector<mjtNum> ctrl = GetCtrlNoise(model, kNumWarmupSteps);
|
||||
|
||||
// note: this tests resetting and stepping
|
||||
for (int i = 0; i < 10; ++i) {
|
||||
// reset to the saved state, step again, get the resulting state
|
||||
mj_setState(model, data, initial_state.data(), spec);
|
||||
for (int i = 0; i < kNumWarmupSteps; i++) {
|
||||
mju_copy(data->ctrl, ctrl.data()+model->nu*i, model->nu);
|
||||
mj_step(model, data);
|
||||
}
|
||||
}
|
||||
|
||||
state.SetItemsProcessed(state.iterations());
|
||||
mj_deleteData(data);
|
||||
mju_threadPoolDestroy(threadpool);
|
||||
}
|
||||
|
||||
BENCHMARK(BM_StepHumanoid200);
|
||||
BENCHMARK(BM_Step22Humanoids);
|
||||
} // namespace
|
||||
} // namespace mujoco
|
||||
@@ -14,15 +14,20 @@
|
||||
|
||||
// Tests for engine/engine_island.c.
|
||||
|
||||
#include <array>
|
||||
#include <cstring>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include <gmock/gmock.h>
|
||||
#include <gtest/gtest.h>
|
||||
#include <mujoco/mjmodel.h>
|
||||
#include <mujoco/mjthread.h>
|
||||
#include <mujoco/mjxmacro.h>
|
||||
#include <mujoco/mujoco.h>
|
||||
#include "src/engine/engine_island.h"
|
||||
#include "src/engine/engine_util_sparse.h"
|
||||
#include "src/thread/thread_pool.h"
|
||||
#include "test/fixture.h"
|
||||
|
||||
namespace mujoco {
|
||||
@@ -382,5 +387,61 @@ TEST_F(IslandTest, IslandEfcElliptic) {
|
||||
mj_deleteModel(model);
|
||||
}
|
||||
|
||||
TEST_F(IslandTest, IslandSingleAndMultiThreadedMatch) {
|
||||
auto model_path = GetTestDataFilePath("benchmark/testdata/22_humanoids.xml");
|
||||
std::array<char, 1024> error;
|
||||
mjModel* model =
|
||||
mj_loadXML(model_path.c_str(), nullptr, error.data(), error.size());
|
||||
model->opt.solver = mjSOL_CG; // use CG solver
|
||||
model->opt.enableflags |= mjENBL_ISLAND; // enable islands
|
||||
|
||||
mjModel* model_threaded =
|
||||
mj_loadXML(model_path.c_str(), nullptr, error.data(), error.size());
|
||||
model_threaded->opt.solver = mjSOL_CG; // use CG solver
|
||||
model_threaded->opt.enableflags |= mjENBL_ISLAND; // enable islands
|
||||
|
||||
mjData* data = mj_makeData(model);
|
||||
mjData* data_threaded = mj_makeData(model_threaded);
|
||||
|
||||
// warm-up rollout to get a steady state
|
||||
static constexpr int kNumWarmupSteps = 100;
|
||||
for (int i = 0; i < kNumWarmupSteps; i++) {
|
||||
mj_step(model, data);
|
||||
}
|
||||
|
||||
// sync the models and data
|
||||
int spec = mjSTATE_INTEGRATION;
|
||||
int size = mj_stateSize(model, spec);
|
||||
std::vector<mjtNum> initial_state(size);
|
||||
mj_getState(model, data, initial_state.data(), spec);
|
||||
mj_setState(model_threaded, data_threaded, initial_state.data(), spec);
|
||||
|
||||
// bind a threadpool to the data_threaded
|
||||
mjThreadPool* threadpool = mju_threadPoolCreate(10);
|
||||
mju_bindThreadPool(data_threaded, threadpool);
|
||||
|
||||
for (int i = 0; i < 10; ++i) {
|
||||
mj_step(model, data);
|
||||
mj_step(model_threaded, data_threaded);
|
||||
}
|
||||
|
||||
// compare the mjData's.
|
||||
{
|
||||
MJDATA_POINTERS_PREAMBLE((model))
|
||||
#define X(type, name, nr, nc) \
|
||||
EXPECT_EQ(std::memcmp(data->name, data_threaded->name, \
|
||||
sizeof(type)*(model->nr)*(nc)), \
|
||||
0) << "mjData::" #name " differs";
|
||||
MJDATA_POINTERS
|
||||
#undef X
|
||||
}
|
||||
|
||||
mj_deleteData(data);
|
||||
mj_deleteModel(model);
|
||||
mj_deleteData(data_threaded);
|
||||
mj_deleteModel(model_threaded);
|
||||
mju_threadPoolDestroy(threadpool);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
} // namespace mujoco
|
||||
|
||||
@@ -7088,5 +7088,8 @@ public static unsafe extern void mjd_subQuat(double* qa, double* qb, double* Da,
|
||||
|
||||
[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
|
||||
public static unsafe extern void mjd_quatIntegrate(double* vel, double scale, double* Dquat, double* Dvel, double* Dscale);
|
||||
|
||||
[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
|
||||
public static unsafe extern void mju_bindThreadPool(mjData_* d, void* thread_pool);
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user