From 236dcb4ffd57a534adc1869a120472fb88fe26bc Mon Sep 17 00:00:00 2001 From: Nimrod Gileadi Date: Mon, 4 Dec 2023 08:30:41 -0800 Subject: [PATCH 1/8] Fix a bug with PID actuators, when plugin definition order differs from actuator order. PiperOrigin-RevId: 587729879 Change-Id: If71c7e374f36ef08c5147e7316066d6f5365925f --- plugin/actuator/pid.cc | 8 ++++---- plugin/actuator/pid.h | 2 +- test/plugin/actuator/pid_test.cc | 12 ++++++++---- 3 files changed, 13 insertions(+), 9 deletions(-) diff --git a/plugin/actuator/pid.cc b/plugin/actuator/pid.cc index 54260fe0..62fba647 100644 --- a/plugin/actuator/pid.cc +++ b/plugin/actuator/pid.cc @@ -160,7 +160,7 @@ mjtNum Pid::GetCtrl(const mjModel* m, const mjData* d, const State& state, } void Pid::ActDot(const mjModel* m, mjData* d, int instance) const { - State state = GetState(m, d, instance); + State state = GetState(m, d, actuator_idx_); mjtNum ctrl = GetCtrl(m, d, state, /*actearly=*/false); mjtNum error = ctrl - d->actuator_length[actuator_idx_]; @@ -180,7 +180,7 @@ void Pid::ActDot(const mjModel* m, mjData* d, int instance) const { } void Pid::Compute(const mjModel* m, mjData* d, int instance) { - State state = GetState(m, d, instance); + State state = GetState(m, d, actuator_idx_); mjtNum ctrl = GetCtrl(m, d, state, m->actuator_actearly[actuator_idx_]); mjtNum error = ctrl - d->actuator_length[actuator_idx_]; @@ -217,9 +217,9 @@ int Pid::ActDim(const mjModel* m, int instance, int actuator_id) { return (i_gain ? 1 : 0) + (HasSlew(m, instance) ? 1 : 0); } -Pid::State Pid::GetState(const mjModel* m, mjData* d, int instance) const { +Pid::State Pid::GetState(const mjModel* m, mjData* d, int actuator_idx) const { State state; - int state_idx = m->actuator_actadr[instance]; + int state_idx = m->actuator_actadr[actuator_idx]; if (config_.i_gain) { state.integral = d->act[state_idx++]; } diff --git a/plugin/actuator/pid.h b/plugin/actuator/pid.h index 1e2c5f24..185e07f3 100644 --- a/plugin/actuator/pid.h +++ b/plugin/actuator/pid.h @@ -84,7 +84,7 @@ class Pid { mjtNum integral = 0; }; // Reads data from d->act and returns it as a State struct. - State GetState(const mjModel* m, mjData* d, int instance) const; + State GetState(const mjModel* m, mjData* d, int actuator_idx) const; // Returns the PID setpoint, which is normally d->ctrl, but can be d->act for // actuators with dyntype != none. diff --git a/test/plugin/actuator/pid_test.cc b/test/plugin/actuator/pid_test.cc index 5e13c439..06938d2f 100644 --- a/test/plugin/actuator/pid_test.cc +++ b/test/plugin/actuator/pid_test.cc @@ -322,10 +322,6 @@ TEST_F(PidTest, ITerm) { - - - - @@ -337,6 +333,14 @@ TEST_F(PidTest, ITerm) { + + + + + From c062a8db19b645e15cc4522779471e5c89b1581d Mon Sep 17 00:00:00 2001 From: Nimrod Gileadi Date: Mon, 4 Dec 2023 10:07:20 -0800 Subject: [PATCH 2/8] Allow multiple actuators to use the same PID plugin. Maintain separate state for each actuator in the Plugin object. PiperOrigin-RevId: 587760569 Change-Id: I633a1b94f0dde9377120145066a26aef6a4033b8 --- plugin/actuator/pid.cc | 110 +++++++++++++++---------------- plugin/actuator/pid.h | 13 ++-- test/plugin/actuator/pid_test.cc | 61 +++++++++++++++++ 3 files changed, 122 insertions(+), 62 deletions(-) diff --git a/plugin/actuator/pid.cc b/plugin/actuator/pid.cc index 62fba647..d99ceb15 100644 --- a/plugin/actuator/pid.cc +++ b/plugin/actuator/pid.cc @@ -107,46 +107,39 @@ std::unique_ptr Pid::Create(const mjModel* m, int instance) { return nullptr; } - int actuator_idx = -1; + std::vector actuators; for (int i = 0; i < m->nu; i++) { if (m->actuator_plugin[i] == instance) { - if (actuator_idx != -1) { - mju_warning("multiple actuators found for plugin instance %d", - instance); - return nullptr; - } - actuator_idx = i; + actuators.push_back(i); } } - if (actuator_idx == -1) { + if (actuators.empty()) { mju_warning("actuator not found for plugin instance %d", instance); return nullptr; } - return std::unique_ptr(new Pid(config, actuator_idx)); + return std::unique_ptr(new Pid(config, std::move(actuators))); } -void Pid::Reset(mjtNum* plugin_state) { - integral_ = 0.0; - previous_ctrl_ = 0.0; -} +void Pid::Reset(mjtNum* plugin_state) {} -mjtNum Pid::GetCtrl(const mjModel* m, const mjData* d, const State& state, +mjtNum Pid::GetCtrl(const mjModel* m, const mjData* d, int actuator_idx, + const State& state, bool actearly) const { mjtNum ctrl = 0; - if (m->actuator_dyntype[actuator_idx_] == mjDYN_NONE) { - ctrl = d->ctrl[actuator_idx_]; + if (m->actuator_dyntype[actuator_idx] == mjDYN_NONE) { + ctrl = d->ctrl[actuator_idx]; // clamp ctrl - if (m->actuator_ctrllimited[actuator_idx_]) { - ctrl = mju_clip(ctrl, m->actuator_ctrlrange[2 * actuator_idx_], - m->actuator_ctrlrange[2 * actuator_idx_ + 1]); + if (m->actuator_ctrllimited[actuator_idx]) { + ctrl = mju_clip(ctrl, m->actuator_ctrlrange[2 * actuator_idx], + m->actuator_ctrlrange[2 * actuator_idx + 1]); } } else { // Use of act instead of ctrl, to create integrated-velocity controllers or // to filter the controls. - int actadr = m->actuator_actadr[actuator_idx_] + - m->actuator_actnum[actuator_idx_] - 1; + int actadr = m->actuator_actadr[actuator_idx] + + m->actuator_actnum[actuator_idx] - 1; if (actearly) { - ctrl = NextActivation(m, d, actuator_idx_, actadr, d->act_dot[actadr]); + ctrl = NextActivation(m, d, actuator_idx, actadr, d->act_dot[actadr]); } else { ctrl = d->act[actadr]; } @@ -160,48 +153,55 @@ mjtNum Pid::GetCtrl(const mjModel* m, const mjData* d, const State& state, } void Pid::ActDot(const mjModel* m, mjData* d, int instance) const { - State state = GetState(m, d, actuator_idx_); - mjtNum ctrl = GetCtrl(m, d, state, /*actearly=*/false); - mjtNum error = ctrl - d->actuator_length[actuator_idx_]; + for (int actuator_idx : actuators_) { + State state = GetState(m, d, actuator_idx); + mjtNum ctrl = GetCtrl(m, d, actuator_idx, state, /*actearly=*/false); + mjtNum error = ctrl - d->actuator_length[actuator_idx]; - int state_idx = m->actuator_actadr[actuator_idx_]; - if (config_.i_gain) { - mjtNum integral = state.integral + error * m->opt.timestep; - if (config_.i_max.has_value()) { - integral = mju_clip(integral, -*config_.i_max, *config_.i_max); + int state_idx = m->actuator_actadr[actuator_idx]; + if (config_.i_gain) { + mjtNum integral = state.integral + error * m->opt.timestep; + if (config_.i_max.has_value()) { + integral = mju_clip(integral, -*config_.i_max, *config_.i_max); + } + d->act_dot[state_idx] = (integral - d->act[state_idx]) / m->opt.timestep; + ++state_idx; + } + if (config_.slew_max.has_value()) { + d->act_dot[state_idx] = (ctrl - d->act[state_idx]) / m->opt.timestep; + ++state_idx; } - d->act_dot[state_idx] = (integral - d->act[state_idx]) / m->opt.timestep; - ++state_idx; - } - if (config_.slew_max.has_value()) { - d->act_dot[state_idx] = (ctrl - d->act[state_idx]) / m->opt.timestep; - ++state_idx; } } void Pid::Compute(const mjModel* m, mjData* d, int instance) { - State state = GetState(m, d, actuator_idx_); - mjtNum ctrl = GetCtrl(m, d, state, m->actuator_actearly[actuator_idx_]); + for (int i = 0; i < actuators_.size(); i++) { + int actuator_idx = actuators_[i]; + State state = GetState(m, d, actuator_idx); + mjtNum ctrl = + GetCtrl(m, d, actuator_idx, state, m->actuator_actearly[actuator_idx]); - mjtNum error = ctrl - d->actuator_length[actuator_idx_]; + mjtNum error = ctrl - d->actuator_length[actuator_idx]; - mjtNum ctrl_dot = m->actuator_dyntype[actuator_idx_] == mjDYN_NONE - ? 0 - : d->act_dot[m->actuator_actadr[actuator_idx_] + - m->actuator_actnum[actuator_idx_] - 1]; - mjtNum error_dot = ctrl_dot - d->actuator_velocity[actuator_idx_]; + mjtNum ctrl_dot = m->actuator_dyntype[actuator_idx] == mjDYN_NONE + ? 0 + : d->act_dot[m->actuator_actadr[actuator_idx] + + m->actuator_actnum[actuator_idx] - 1]; + mjtNum error_dot = ctrl_dot - d->actuator_velocity[actuator_idx]; - if (config_.i_gain) { - integral_ = state.integral + error * m->opt.timestep; - if (config_.i_max.has_value()) { - integral_ = mju_clip(integral_, -*config_.i_max, *config_.i_max); + mjtNum integral = 0; + if (config_.i_gain) { + integral = state.integral + error * m->opt.timestep; + if (config_.i_max.has_value()) { + integral = + mju_clip(integral, -*config_.i_max, *config_.i_max); + } } - } - d->actuator_force[actuator_idx_] = config_.p_gain * error + - config_.d_gain * error_dot + - config_.i_gain * integral_; - previous_ctrl_ = ctrl; + d->actuator_force[actuator_idx] = config_.p_gain * error + + config_.d_gain * error_dot + + config_.i_gain * integral; + } } void Pid::Advance(const mjModel* m, mjData* d, int instance) const { @@ -279,7 +279,7 @@ void Pid::RegisterPlugin() { mjp_registerPlugin(&plugin); } -Pid::Pid(PidConfig config, int actuator_idx) - : config_(std::move(config)), actuator_idx_(actuator_idx) {} +Pid::Pid(PidConfig config, std::vector actuators) + : config_(std::move(config)), actuators_(std::move(actuators)) {} } // namespace mujoco::plugin::actuator diff --git a/plugin/actuator/pid.h b/plugin/actuator/pid.h index 185e07f3..87a1014e 100644 --- a/plugin/actuator/pid.h +++ b/plugin/actuator/pid.h @@ -17,6 +17,7 @@ #include #include +#include #include #include @@ -73,7 +74,7 @@ class Pid { static void RegisterPlugin(); private: - Pid(PidConfig config, int actuator_idx); + Pid(PidConfig config, std::vector actuators); struct State { mjtNum previous_ctrl = 0; @@ -88,14 +89,12 @@ class Pid { // Returns the PID setpoint, which is normally d->ctrl, but can be d->act for // actuators with dyntype != none. - mjtNum GetCtrl(const mjModel* m, const mjData* d, const State& state, - bool actearly) const; + mjtNum GetCtrl(const mjModel* m, const mjData* d, int actuator_idx, + const State& state, bool actearly) const; PidConfig config_; - int actuator_idx_ = 0; - - mjtNum integral_ = 0.0; - mjtNum previous_ctrl_ = 0.0; + // set of actuator IDs controlled by this plugin instance. + std::vector actuators_; }; } // namespace mujoco::plugin::actuator diff --git a/test/plugin/actuator/pid_test.cc b/test/plugin/actuator/pid_test.cc index 06938d2f..1eec513d 100644 --- a/test/plugin/actuator/pid_test.cc +++ b/test/plugin/actuator/pid_test.cc @@ -562,6 +562,67 @@ TEST_F(PidTest, CopyData) { EXPECT_EQ(d1->qpos[1], d2->qpos[1]); } +TEST_F(PidTest, MultipleActuatorsSamePlugin) { + constexpr absl::string_view kModelXml = R"( + + + + + + + + + + + + )"; + + char error[1024] = {0}; + mjModel* m = LoadModelFromString(kModelXml, error, sizeof(error)); + ASSERT_THAT(m, NotNull()) << error; + absl::Cleanup m_deleter = [m] { mj_deleteModel(m); }; + + // having a slew rate means that there should be one extra state variable + // for the plugin. + EXPECT_EQ(m->actuator_actnum[0], 1); + EXPECT_EQ(m->actuator_actnum[1], 1); + + mjData* d = mj_makeData(m); + absl::Cleanup d_deleter = [d] { mj_deleteData(d); }; + + // Set different ctrls for the two actuators, and check that they're + // independent. + d->ctrl[0] = 1.0; + d->ctrl[1] = -1.0; + + for (int i = 0; i < 2; i++) { + mj_step(m, d); + + EXPECT_EQ(d->actuator_force[0], -d->actuator_force[1]) + << "actuator_force mismatch at step " << i; + EXPECT_EQ(d->qfrc_actuator[0], -d->qfrc_actuator[1]) + << "qfrc_actuator mismatch at step " << i; + EXPECT_EQ(d->qpos[0], -d->qpos[1]) << "qpos mismatch at step " << i; + } +} + TEST_F(PidTest, InvalidClamp) { constexpr absl::string_view kModelXml = R"( From 8c7c39a211e2cc4c6ff693d577b1ee18afaea481 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Tue, 5 Dec 2023 03:55:58 -0800 Subject: [PATCH 3/8] Lock the sim mutex in PhysicsLoop and PhysicsThread. This prevents an invalid memory access when the stack is used in engine_vis. Fixes #1219. PiperOrigin-RevId: 588017796 Change-Id: I1ee18c5aa5e96ccaeebb0a467cd384355882b80b --- simulate/main.cc | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) diff --git a/simulate/main.cc b/simulate/main.cc index 57763646..f23a0e30 100644 --- a/simulate/main.cc +++ b/simulate/main.cc @@ -267,6 +267,9 @@ void PhysicsLoop(mj::Simulate& sim) { if (dnew) { sim.Load(mnew, dnew, sim.dropfilename); + // lock the sim mutex + const std::unique_lock lock(sim.mtx); + mj_deleteData(d); mj_deleteModel(m); @@ -292,6 +295,9 @@ void PhysicsLoop(mj::Simulate& sim) { if (dnew) { sim.Load(mnew, dnew, sim.filename); + // lock the sim mutex + const std::unique_lock lock(sim.mtx); + mj_deleteData(d); mj_deleteModel(m); @@ -421,9 +427,18 @@ void PhysicsThread(mj::Simulate* sim, const char* filename) { if (filename != nullptr) { sim->LoadMessage(filename); m = LoadModel(filename, *sim); - if (m) d = mj_makeData(m); + if (m) { + // lock the sim mutex + const std::unique_lock lock(sim->mtx); + + d = mj_makeData(m); + } if (d) { sim->Load(m, d, filename); + + // lock the sim mutex + const std::unique_lock lock(sim->mtx); + mj_forward(m, d); // allocate ctrlnoise From 3c05f9fafcf670ead3e1e2ae3d00a4f43d1d684f Mon Sep 17 00:00:00 2001 From: Nimrod Gileadi Date: Tue, 5 Dec 2023 09:38:47 -0800 Subject: [PATCH 4/8] Add a note to future change log about releasing 10.16 macOS wheels. This change relates to #1213, and should fix it when we push the next release. PiperOrigin-RevId: 588099974 Change-Id: Ic1c4164f8a6400e92b5f509c3e03ff9fbc1c6033 --- doc/changelog.rst | 3 +++ 1 file changed, 3 insertions(+) diff --git a/doc/changelog.rst b/doc/changelog.rst index bf1cbe34..826c1a1d 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -37,6 +37,9 @@ Bug fixes Before this fix such actuators could lead to non-conservation of momentum. - Fix bug that prevented using flex with the :ref:`passive viewer`. - Fix bug that prevented the use of elasticity plugins in combination with pinned flex vertices. +- Release Python wheels targeting macOS 10.16 to support x86_64 systems where SYSTEM_VERSION_COMPAT is set. The minimum + supported version is still 11.0, but we release these wheels to fix compatibility for those users. See + :github:issue:`1213`. Version 3.0.1 (November 15, 2023) --------------------------------- From 5e9906be8422f6d928f0bb8d7cbef82145537aab Mon Sep 17 00:00:00 2001 From: Nimrod Gileadi Date: Tue, 5 Dec 2023 10:18:53 -0800 Subject: [PATCH 5/8] s/paramters/parameters/ PiperOrigin-RevId: 588113325 Change-Id: I1b7e754c747baedb8d2b154c3dc395a7062e8fc5 --- src/engine/engine_io.c | 2 +- src/engine/engine_io.h | 2 +- src/engine/engine_util_misc.c | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index 48a1bd8c..32994e9c 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -94,7 +94,7 @@ void mj_defaultLROpt(mjLROpt* opt) { //------------------------------- mjOption --------------------------------------------------------- -// set default solver paramters +// set default solver parameters void mj_defaultSolRefImp(mjtNum* solref, mjtNum* solimp) { if (solref) { solref[0] = 0.02; // timeconst diff --git a/src/engine/engine_io.h b/src/engine/engine_io.h index 6983990d..4f8b530c 100644 --- a/src/engine/engine_io.h +++ b/src/engine/engine_io.h @@ -35,7 +35,7 @@ extern "C" { // Set default options for length range computation. MJAPI void mj_defaultLROpt(mjLROpt* opt); -// set default solver paramters +// set default solver parameters MJAPI void mj_defaultSolRefImp(mjtNum* solref, mjtNum* solimp); // set options to default values diff --git a/src/engine/engine_util_misc.c b/src/engine/engine_util_misc.c index e1e126d7..30eb0d2a 100644 --- a/src/engine/engine_util_misc.c +++ b/src/engine/engine_util_misc.c @@ -163,7 +163,7 @@ static mjtNum wrap_circle(mjtNum* pnt, const mjtNum* d, const mjtNum* sd, mjtNum // input: pair of 2D points in d[4], radius // output: pair of 2D points in pnt[4]; return 0 if wrap, -1 if no wrap static mjtNum wrap_inside(mjtNum* pnt, const mjtNum* d, mjtNum rad) { - // algorithm paramters + // algorithm parameters const int maxiter = 20; const mjtNum zinit = 1 - 1e-7; const mjtNum tolerance = 1e-6; From 899ba4b7ec4583236d7edfd58bc169428a4400c3 Mon Sep 17 00:00:00 2001 From: Kevin Zakka Date: Tue, 5 Dec 2023 11:01:35 -0800 Subject: [PATCH 6/8] Add missing period in documentation. PiperOrigin-RevId: 588127219 Change-Id: Ib39808f303cebb6c619d1119024e9a1352326473 --- doc/XMLreference.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 50307135..fa963647 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -4620,7 +4620,7 @@ joint types (slide and hinge) can be used. :at:`polycoef`: :at-val:`real(5), "0 1 0 0 0"` Coefficients a0 ... a4 of the quartic polynomial. If the two joint values are y and x, and their reference positions (corresponding to the joint values in the initial model configuration) are y0 and x0, the constraint is: - y-y0 = a0 + a1*(x-x0) + a2*(x-x0)^2 + a3*(x-x0)^3 + a4*(x-x0)^4 + y-y0 = a0 + a1*(x-x0) + a2*(x-x0)^2 + a3*(x-x0)^3 + a4*(x-x0)^4. Omitting the second joint is equivalent to setting x = x0, in which case the constraint is y = y0 + a0. From b3ccf67ebf016ecee4a46a980da1ccd84867b11e Mon Sep 17 00:00:00 2001 From: Erik Frey Date: Tue, 5 Dec 2023 23:07:00 -0800 Subject: [PATCH 7/8] Cleanup MJX tests and migrate them to put_model/put_data. PiperOrigin-RevId: 588304729 Change-Id: I58075383e6eed64ae00cea305a568eba6465b0b0 --- mjx/mujoco/mjx/__init__.py | 8 + mjx/mujoco/mjx/_src/collision_driver_test.py | 36 ++- mjx/mujoco/mjx/_src/constraint_test.py | 177 +++++--------- mjx/mujoco/mjx/_src/device.py | 9 + mjx/mujoco/mjx/_src/device_test.py | 10 +- mjx/mujoco/mjx/_src/forward.py | 24 +- mjx/mujoco/mjx/_src/forward_test.py | 145 ++++++----- mjx/mujoco/mjx/_src/io.py | 3 +- mjx/mujoco/mjx/_src/passive.py | 2 +- mjx/mujoco/mjx/_src/passive_test.py | 121 ++++----- mjx/mujoco/mjx/_src/scan_test.py | 2 +- mjx/mujoco/mjx/_src/smooth.py | 22 +- mjx/mujoco/mjx/_src/smooth_test.py | 231 +++++++----------- mjx/mujoco/mjx/_src/solver_test.py | 137 ++++------- mjx/mujoco/mjx/_src/support_test.py | 10 +- mjx/mujoco/mjx/_src/test_util.py | 4 +- .../integration_test/collision_driver_test.py | 4 +- .../mjx/integration_test/forward_test.py | 7 +- .../mjx/integration_test/smooth_test.py | 4 +- mjx/mujoco/mjx/test_data/ant.xml | 82 ------- mjx/mujoco/mjx/test_data/constraints.xml | 53 ++++ mjx/mujoco/mjx/test_data/equality.xml | 71 ------ mjx/mujoco/mjx/test_data/humanoid.xml | 109 --------- mjx/mujoco/mjx/test_data/pendula.xml | 43 ++-- 24 files changed, 446 insertions(+), 868 deletions(-) delete mode 100644 mjx/mujoco/mjx/test_data/ant.xml create mode 100644 mjx/mujoco/mjx/test_data/constraints.xml delete mode 100644 mjx/mujoco/mjx/test_data/equality.xml delete mode 100644 mjx/mujoco/mjx/test_data/humanoid.xml diff --git a/mjx/mujoco/mjx/__init__.py b/mjx/mujoco/mjx/__init__.py index 1c8d0661..d8e3f437 100644 --- a/mjx/mujoco/mjx/__init__.py +++ b/mjx/mujoco/mjx/__init__.py @@ -16,10 +16,17 @@ # pylint:disable=g-importing-member from mujoco.mjx._src.collision_driver import collision +from mujoco.mjx._src.constraint import count_constraints from mujoco.mjx._src.constraint import make_constraint from mujoco.mjx._src.device import device_get_into from mujoco.mjx._src.device import device_put +from mujoco.mjx._src.forward import euler from mujoco.mjx._src.forward import forward +from mujoco.mjx._src.forward import fwd_acceleration +from mujoco.mjx._src.forward import fwd_actuation +from mujoco.mjx._src.forward import fwd_position +from mujoco.mjx._src.forward import fwd_velocity +from mujoco.mjx._src.forward import rungekutta4 from mujoco.mjx._src.forward import step from mujoco.mjx._src.io import get_data from mujoco.mjx._src.io import make_data @@ -34,4 +41,5 @@ from mujoco.mjx._src.smooth import kinematics from mujoco.mjx._src.smooth import mul_m from mujoco.mjx._src.smooth import rne from mujoco.mjx._src.smooth import transmission +from mujoco.mjx._src.solver import solve from mujoco.mjx._src.types import * diff --git a/mjx/mujoco/mjx/_src/collision_driver_test.py b/mjx/mujoco/mjx/_src/collision_driver_test.py index afb7bd94..5b58f7f8 100644 --- a/mjx/mujoco/mjx/_src/collision_driver_test.py +++ b/mjx/mujoco/mjx/_src/collision_driver_test.py @@ -52,9 +52,9 @@ def _collide( mjcf: str, assets: Optional[Dict[str, str]] = None ) -> Tuple[mujoco.MjModel, mujoco.MjData, Model, Data]: m = mujoco.MjModel.from_xml_string(mjcf, assets or {}) - mx = mjx.device_put(m) + mx = mjx.put_model(m) d = mujoco.MjData(m) - dx = mjx.device_put(d) + dx = mjx.put_data(m, d) mujoco.mj_step(m, d) collision_jit_fn = jax.jit(mjx.collision) @@ -418,9 +418,9 @@ class BodyPairFilterTest(absltest.TestCase): def test_filter_parent_child(self): """Tests that parent-child collisions get filtered.""" m = mujoco.MjModel.from_xml_string(self._PARENT_CHILD) - mx = mjx.device_put(m) + mx = mjx.put_model(m) d = mujoco.MjData(m) - dx = mjx.device_put(d) + dx = mjx.put_data(m, d) mujoco.mj_step(m, d) collision_jit_fn = jax.jit(mjx.collision) @@ -435,9 +435,9 @@ class BodyPairFilterTest(absltest.TestCase): """Tests that filterparent flag disables parent-child filtering.""" m = mujoco.MjModel.from_xml_string(self._PARENT_CHILD) m.opt.disableflags |= mujoco.mjtDisableBit.mjDSBL_FILTERPARENT - mx = mjx.device_put(m) + mx = mjx.put_model(m) d = mujoco.MjData(m) - dx = mjx.device_put(d) + dx = mjx.put_data(m, d) mujoco.mj_step(m, d) collision_jit_fn = jax.jit(mjx.collision) @@ -454,22 +454,14 @@ class NconTest(parameterized.TestCase): """Tests ncon.""" def test_ncon(self): - m = test_util.load_test_file('ant.xml') - d = mujoco.MjData(m) - d.qpos[2] = 0.0 - - mx = mjx.device_put(m) - ncon = collision_driver.ncon(mx) - self.assertEqual(ncon, 4) + m = test_util.load_test_file('constraints.xml') + ncon = collision_driver.ncon(m) + self.assertEqual(ncon, 16) def test_disable_contact(self): - m = test_util.load_test_file('ant.xml') - d = mujoco.MjData(m) - d.qpos[2] = 0.0 - - m.opt.disableflags = m.opt.disableflags | DisableBit.CONTACT - mx = mjx.device_put(m) - ncon = collision_driver.ncon(mx) + m = test_util.load_test_file('constraints.xml') + m.opt.disableflags |= DisableBit.CONTACT + ncon = collision_driver.ncon(m) self.assertEqual(ncon, 0) @@ -500,12 +492,12 @@ class TopKContactTest(absltest.TestCase): def test_top_k_contacts(self): m = mujoco.MjModel.from_xml_string(self._CAPSULES) - mx_top_k = mjx.device_put(m) + mx_top_k = mjx.put_model(m) mx_all = mx_top_k.replace( nnumeric=0, name_numericadr=np.array([]), numeric_data=np.array([]) ) d = mujoco.MjData(m) - dx = mjx.device_put(d) + dx = mjx.put_data(m, d) collision_jit_fn = jax.jit(mjx.collision) kinematics_jit_fn = jax.jit(mjx.kinematics) diff --git a/mjx/mujoco/mjx/_src/constraint_test.py b/mjx/mujoco/mjx/_src/constraint_test.py index 0510e1df..a2bd8cc9 100644 --- a/mjx/mujoco/mjx/_src/constraint_test.py +++ b/mjx/mujoco/mjx/_src/constraint_test.py @@ -15,159 +15,92 @@ """Tests for constraint functions.""" from absl.testing import absltest -from absl.testing import parameterized -import jax from jax import numpy as jp import mujoco from mujoco import mjx from mujoco.mjx._src import constraint from mujoco.mjx._src import test_util -# pylint: disable=g-importing-member -from mujoco.mjx._src.types import DisableBit -from mujoco.mjx._src.types import SolverType -# pylint: enable=g-importing-member import numpy as np -def _assert_eq(a, b, name, step, fname, atol=5e-3, rtol=5e-3): - err_msg = f'mismatch: {name} at step {step} in {fname}' - np.testing.assert_allclose(a, b, err_msg=err_msg, atol=atol, rtol=rtol) +# tolerance for difference between MuJoCo and MJX constraint calculations, +# mostly due to float precision +_TOLERANCE = 5e-5 -class ConstraintTest(parameterized.TestCase): +def _assert_eq(a, b, name): + tol = _TOLERANCE * 10 # avoid test noise + err_msg = f'mismatch: {name}' + np.testing.assert_allclose(a, b, err_msg=err_msg, atol=tol, rtol=tol) - @parameterized.parameters(enumerate(test_util.TEST_FILES)) - def test_constraints(self, seed, fname): + +def _assert_attr_eq(a, b, attr): + _assert_eq(getattr(a, attr), getattr(b, attr), attr) + + +class ConstraintTest(absltest.TestCase): + + def test_constraints(self): """Test constraints.""" - np.random.seed(seed) - - # exclude convex.xml since convex contacts are not exactly equivalent - if fname == 'convex.xml': - return - - m = test_util.load_test_file(fname) + m = test_util.load_test_file('constraints.xml') d = mujoco.MjData(m) - mx = mjx.device_put(m) - dx = mjx.make_data(mx) + mujoco.mj_step(m, d, 100) # at 100 steps mix of active/inactive constraints + mujoco.mj_forward(m, d) + mx = mjx.put_model(m) + dx = mjx.put_data(m, d) - forward_jit_fn = jax.jit(mjx.forward) - - # give the system a little kick to ensure we have non-identity rotations - d.qvel = np.random.random(m.nv) - for i in range(100): - dx = dx.replace(qpos=jax.device_put(d.qpos), qvel=jax.device_put(d.qvel)) - mujoco.mj_step(m, d) - dx = forward_jit_fn(mx, dx) - - nnz_filter = dx.efc_J.any(axis=1) - - mj_efc_j = d.efc_J.reshape((-1, m.nv)) - mjx_efc_j = dx.efc_J[nnz_filter] - _assert_eq(mj_efc_j, mjx_efc_j, 'efc_J', i, fname) - - mjx_efc_d = dx.efc_D[nnz_filter] - _assert_eq(d.efc_D, mjx_efc_d, 'efc_D', i, fname) - - mjx_efc_aref = dx.efc_aref[nnz_filter] - _assert_eq(d.efc_aref, mjx_efc_aref, 'efc_aref', i, fname) - - mjx_efc_frictionloss = dx.efc_frictionloss[nnz_filter] - _assert_eq( - d.efc_frictionloss, - mjx_efc_frictionloss, - 'efc_frictionloss', - i, - fname, - ) - - _JNT_RANGE = """ - - - - - - - - - - - - - """ - - def test_jnt_range(self): - """Tests that mixed joint ranges are respected.""" - # TODO(robotics-simulation): also test ball - m = mujoco.MjModel.from_xml_string(self._JNT_RANGE) - m.opt.solver = SolverType.CG.value - d = mujoco.MjData(m) - d.qpos = np.array([2.0, 15.0]) - - mx = mjx.device_put(m) - dx = mjx.device_put(d) - efc = jax.jit(constraint._instantiate_limit_slide_hinge)(mx, dx) - - # first joint is outside the joint range - np.testing.assert_array_almost_equal(efc.J[0, 0], -1.0) - - # second joint has no range, so only one efc row - self.assertEqual(efc.J.shape[0], 1) + dx = mjx.make_constraint(mx, dx) + nnz = dx.efc_J.any(axis=1) + _assert_eq(d.efc_J, dx.efc_J[nnz].reshape(-1), 'efc_J') + _assert_eq(d.efc_D, dx.efc_D[nnz], 'efc_D') + _assert_eq(d.efc_aref, dx.efc_aref[nnz], 'efc_aref') + _assert_eq(d.efc_frictionloss, dx.efc_frictionloss[nnz], 'efc_frictionloss') def test_disable_refsafe(self): - m = test_util.load_test_file('ant.xml') + m = test_util.load_test_file('constraints.xml') timeconst = m.opt.timestep / 4.0 # timeconst < 2 * timestep solimp = jp.array([timeconst, 1.0]) solref = jp.array([0.8, 0.99, 0.001, 0.2, 2]) pos = jp.ones(3) - m.opt.disableflags = m.opt.disableflags | DisableBit.REFSAFE + m.opt.disableflags = m.opt.disableflags | mjx.DisableBit.REFSAFE mx = mjx.device_put(m) k, *_ = constraint._kbi(mx, solimp, solref, pos) self.assertEqual(k, 1 / (0.99**2 * timeconst**2)) - m.opt.disableflags = m.opt.disableflags & ~DisableBit.REFSAFE - mx = mjx.device_put(m) - k, *_ = constraint._kbi(mx, solimp, solref, pos) - self.assertEqual(k, 1 / (0.99**2 * (2 * m.opt.timestep) ** 2)) - - def test_disableconstraint(self): - m = test_util.load_test_file('ant.xml') - d = mujoco.MjData(m) - - m.opt.disableflags = m.opt.disableflags | DisableBit.CONSTRAINT - mx, dx = mjx.device_put(m), mjx.device_put(d) - dx = constraint.make_constraint(mx, dx) + def test_disable_constraint(self): + m = test_util.load_test_file('constraints.xml') + m.opt.disableflags = m.opt.disableflags | mjx.DisableBit.CONSTRAINT + ne, nf, nl, nc = mjx.count_constraints(m) + self.assertEqual(ne, 0) + self.assertEqual(nf, 0) + self.assertEqual(nl, 0) + self.assertEqual(nc, 0) + dx = constraint.make_constraint(mjx.put_model(m), mjx.make_data(m)) self.assertEqual(dx.efc_J.shape[0], 0) def test_disable_equality(self): - m = test_util.load_test_file('equality.xml') - d = mujoco.MjData(m) - - m.opt.disableflags = m.opt.disableflags | DisableBit.EQUALITY - mx, dx = mjx.device_put(m), mjx.device_put(d) - dx = constraint.make_constraint(mx, dx) - self.assertEqual(dx.efc_J.shape[0], 0) + m = test_util.load_test_file('constraints.xml') + m.opt.disableflags = m.opt.disableflags | mjx.DisableBit.EQUALITY + ne, nf, nl, nc = mjx.count_constraints(m) + self.assertEqual(ne, 0) + self.assertEqual(nf, 0) + self.assertEqual(nl, 2) + self.assertEqual(nc, 64) + dx = constraint.make_constraint(mjx.put_model(m), mjx.make_data(m)) + self.assertEqual(dx.efc_J.shape[0], 66) # only joint range, contact def test_disable_contact(self): - m = test_util.load_test_file('ant.xml') - d = mujoco.MjData(m) - d.qpos[2] = 0.0 - mujoco.mj_forward(m, d) - - m.opt.disableflags = m.opt.disableflags & ~DisableBit.CONTACT - mx, dx = mjx.device_put(m), mjx.device_put(d) - dx = dx.tree_replace( - {'contact.frame': dx.contact.frame.reshape((-1, 3, 3))} - ) - efc = constraint._instantiate_contact(mx, dx) - self.assertIsNotNone(efc) - - m.opt.disableflags = m.opt.disableflags | DisableBit.CONTACT - mx, dx = mjx.device_put(m), mjx.device_put(d) - efc = constraint._instantiate_contact(mx, dx) - self.assertIsNone(efc) + m = test_util.load_test_file('constraints.xml') + m.opt.disableflags = m.opt.disableflags | mjx.DisableBit.CONTACT + ne, nf, nl, nc = mjx.count_constraints(m) + self.assertEqual(ne, 10) + self.assertEqual(nf, 0) + self.assertEqual(nl, 2) + self.assertEqual(nc, 0) + dx = constraint.make_constraint(mjx.put_model(m), mjx.make_data(m)) + self.assertEqual(dx.efc_J.shape[0], 12) # only joint range, limit if __name__ == '__main__': diff --git a/mjx/mujoco/mjx/_src/device.py b/mjx/mujoco/mjx/_src/device.py index 327b89de..2819d9ea 100644 --- a/mjx/mujoco/mjx/_src/device.py +++ b/mjx/mujoco/mjx/_src/device.py @@ -184,6 +184,11 @@ def device_put(value): Returns: on-device MJX struct reflecting the input value """ + warnings.warn( + 'device_put is deprecated, use put_model and put_data instead', + category=DeprecationWarning, + ) + clz = _TYPE_MAP.get(type(value)) if clz is None: raise NotImplementedError(f'{type(value)} is not supported for device_put.') @@ -242,6 +247,10 @@ def device_get_into(result, value): Raises: RuntimeError: if result length doesn't match data batch size """ + warnings.warn( + 'device_get_into is deprecated, use get_data instead', + category=DeprecationWarning, + ) value = jax.device_get(value) diff --git a/mjx/mujoco/mjx/_src/device_test.py b/mjx/mujoco/mjx/_src/device_test.py index e6eb7562..9f518d91 100644 --- a/mjx/mujoco/mjx/_src/device_test.py +++ b/mjx/mujoco/mjx/_src/device_test.py @@ -130,31 +130,31 @@ class ValidateInputTest(absltest.TestCase): mjx.device_put(m) def test_trn(self): - m = test_util.load_test_file('ant.xml') + m = test_util.load_test_file('pendula.xml') m.actuator_trntype[0] = mujoco.mjtTrn.mjTRN_SITE with self.assertRaises(NotImplementedError): mjx.device_put(m) def test_dyn(self): - m = test_util.load_test_file('ant.xml') + m = test_util.load_test_file('pendula.xml') m.actuator_dyntype[0] = mujoco.mjtDyn.mjDYN_MUSCLE with self.assertRaises(NotImplementedError): mjx.device_put(m) def test_gain(self): - m = test_util.load_test_file('ant.xml') + m = test_util.load_test_file('pendula.xml') m.actuator_gaintype[0] = mujoco.mjtGain.mjGAIN_MUSCLE with self.assertRaises(NotImplementedError): mjx.device_put(m) def test_bias(self): - m = test_util.load_test_file('ant.xml') + m = test_util.load_test_file('pendula.xml') m.actuator_gaintype[0] = mujoco.mjtGain.mjGAIN_MUSCLE with self.assertRaises(NotImplementedError): mjx.device_put(m) def test_condim(self): - m = test_util.load_test_file('ant.xml') + m = test_util.load_test_file('constraints.xml') for i in [1, 4, 6]: m.geom_condim[0] = i with self.assertRaises(NotImplementedError): diff --git a/mjx/mujoco/mjx/_src/forward.py b/mjx/mujoco/mjx/_src/forward.py index 06bec894..827c8f31 100644 --- a/mjx/mujoco/mjx/_src/forward.py +++ b/mjx/mujoco/mjx/_src/forward.py @@ -60,7 +60,7 @@ def named_scope(fn, name: str = ''): @named_scope -def _position(m: Model, d: Data) -> Data: +def fwd_position(m: Model, d: Data) -> Data: """Position-dependent computations.""" # TODO(robotics-simulation): tendon d = smooth.kinematics(m, d) @@ -74,7 +74,7 @@ def _position(m: Model, d: Data) -> Data: @named_scope -def _velocity(m: Model, d: Data) -> Data: +def fwd_velocity(m: Model, d: Data) -> Data: """Velocity-dependent computations.""" d = d.replace(actuator_velocity=d.actuator_moment @ d.qvel) d = smooth.com_vel(m, d) @@ -84,7 +84,7 @@ def _velocity(m: Model, d: Data) -> Data: @named_scope -def _actuation(m: Model, d: Data) -> Data: +def fwd_actuation(m: Model, d: Data) -> Data: """Actuation-dependent computations.""" if not m.nu or m.opt.disableflags & DisableBit.ACTUATION: return d.replace( @@ -190,7 +190,7 @@ def _actuation(m: Model, d: Data) -> Data: @named_scope -def _acceleration(m: Model, d: Data) -> Data: +def fwd_acceleration(m: Model, d: Data) -> Data: """Add up all non-constraint forces, compute qacc_smooth.""" qfrc_applied = d.qfrc_applied + support.xfrc_accumulate(m, d) qfrc_smooth = d.qfrc_passive - d.qfrc_bias + d.qfrc_actuator + qfrc_applied @@ -263,7 +263,7 @@ def _advance( @named_scope -def _euler(m: Model, d: Data) -> Data: +def euler(m: Model, d: Data) -> Data: """Euler integrator, semi-implicit in velocity.""" # integrate damping implicitly qacc = d.qacc @@ -277,7 +277,7 @@ def _euler(m: Model, d: Data) -> Data: @named_scope -def _rungekutta4(m: Model, d: Data) -> Data: +def rungekutta4(m: Model, d: Data) -> Data: """Runge-Kutta explicit order 4 integrator.""" d_t0 = d # pylint: disable=invalid-name @@ -323,10 +323,10 @@ def _rungekutta4(m: Model, d: Data) -> Data: @named_scope def forward(m: Model, d: Data) -> Data: """Forward dynamics.""" - d = _position(m, d) - d = _velocity(m, d) - d = _actuation(m, d) - d = _acceleration(m, d) + d = fwd_position(m, d) + d = fwd_velocity(m, d) + d = fwd_actuation(m, d) + d = fwd_acceleration(m, d) if d.efc_J.size == 0: d = d.replace(qacc=d.qacc_smooth) @@ -343,9 +343,9 @@ def step(m: Model, d: Data) -> Data: d = forward(m, d) if m.opt.integrator == IntegratorType.EULER: - d = _euler(m, d) + d = euler(m, d) elif m.opt.integrator == IntegratorType.RK4: - d = _rungekutta4(m, d) + d = rungekutta4(m, d) else: raise NotImplementedError(f'integrator {m.opt.integrator} not implemented.') diff --git a/mjx/mujoco/mjx/_src/forward_test.py b/mjx/mujoco/mjx/_src/forward_test.py index 40da667c..fbe28c9d 100644 --- a/mjx/mujoco/mjx/_src/forward_test.py +++ b/mjx/mujoco/mjx/_src/forward_test.py @@ -15,77 +15,75 @@ """Tests for forward functions.""" from absl.testing import absltest -from absl.testing import parameterized import jax -from jax import numpy as jp import mujoco from mujoco import mjx -from mujoco.mjx._src import forward from mujoco.mjx._src import test_util -# pylint: disable=g-importing-member -from mujoco.mjx._src.types import DisableBit -# pylint: enable=g-importing-member import numpy as np -def _assert_attr_eq(a, b, attr, step, fname, atol=1e-3, rtol=1e-3): - err_msg = f'mismatch: {attr} at step {step} in {fname}' - a, b = getattr(a, attr), getattr(b, attr) - np.testing.assert_allclose(a, b, err_msg=err_msg, atol=atol, rtol=rtol) +# tolerance for difference between MuJoCo and MJX forward calculations - mostly +# due to float precision +_TOLERANCE = 1e-5 -class ForwardTest(parameterized.TestCase): +def _assert_eq(a, b, name): + tol = _TOLERANCE * 10 # avoid test noise + err_msg = f'mismatch: {name}' + np.testing.assert_allclose(a, b, err_msg=err_msg, atol=tol, rtol=tol) - @parameterized.parameters( - filter(lambda s: s not in ('equality.xml',), test_util.TEST_FILES) - ) - def test_forward(self, fname): - """Test mujoco mj forward function matches mujoco_mjx forward function.""" - np.random.seed(test_util.TEST_FILES.index(fname)) - m = test_util.load_test_file(fname) +def _assert_attr_eq(a, b, attr): + _assert_eq(getattr(a, attr), getattr(b, attr), attr) + + +class ForwardTest(absltest.TestCase): + + def test_forward(self): + m = test_util.load_test_file('constraints.xml') d = mujoco.MjData(m) - mx = mjx.device_put(m) - dx = mjx.make_data(mx) - forward_jit_fn = jax.jit(mjx.forward) + # apply some control and xfrc input + d.ctrl = np.array([-18, 0.59, 0.47]) + d.xfrc_applied[0, 2] = 0.1 # torque + d.xfrc_applied[1, 4] = 0.3 # linear force + mujoco.mj_step(m, d, 100) # get some dynamics going + mujoco.mj_forward(m, d) - # give the system a little kick to ensure we have non-identity rotations - d.qvel = np.random.random(m.nv) * 0.05 - for i in range(100): - qpos, qvel = d.qpos.copy(), d.qvel.copy() - mujoco.mj_step(m, d) - dx = forward_jit_fn(mx, dx.replace(qpos=qpos, qvel=qvel)) + mx = mjx.put_model(m) - _assert_attr_eq(d, dx, 'qfrc_smooth', i, fname) - _assert_attr_eq(d, dx, 'qacc_smooth', i, fname) + # fwd_actuation + dx = jax.jit(mjx.fwd_actuation)(mx, mjx.put_data(m, d)) + _assert_attr_eq(d, dx, 'act_dot') + _assert_attr_eq(d, dx, 'qfrc_actuator') - @parameterized.parameters( - filter(lambda s: s not in ('equality.xml',), test_util.TEST_FILES) - ) - def test_step(self, fname): - """Test mujoco mj step matches mujoco_mjx step.""" - np.random.seed(test_util.TEST_FILES.index(fname)) - m = test_util.load_test_file(fname) - step_jit_fn = jax.jit(forward.step) + # fwd_accleration (fwd_position and fwd_velocity already tested elsewhere) + dx = jax.jit(mjx.fwd_acceleration)(mx, mjx.put_data(m, d)) + _assert_attr_eq(d, dx, 'qfrc_smooth') + _assert_attr_eq(d, dx, 'qacc_smooth') - mx = mjx.device_put(m) + # euler + dx = jax.jit(mjx.euler)(mx, mjx.put_data(m, d)) + mujoco.mj_Euler(m, d) + _assert_attr_eq(d, dx, 'act') + _assert_attr_eq(d, dx, 'qpos') + _assert_attr_eq(d, dx, 'time') + + def test_step(self): + m = test_util.load_test_file('constraints.xml') d = mujoco.MjData(m) - # give the system a little kick to ensure we have non-identity rotations - d.qvel = np.random.normal(m.nv) * 0.05 - for i in range(100): - # in order to avoid re-jitting, reuse the same mj_data shape - qpos, qvel = d.qpos, d.qvel - d = mujoco.MjData(m) - d.qpos, d.qvel = qpos, qvel - dx = mjx.device_put(d) + # apply some control and xfrc input + d.ctrl = np.array([-18, 0.59, 0.47]) + d.xfrc_applied[0, 2] = 0.1 # torque + d.xfrc_applied[1, 4] = 0.3 # linear force + mujoco.mj_step(m, d, 100) # get some dynamics going - mujoco.mj_step(m, d) - dx = step_jit_fn(mx, dx) - - _assert_attr_eq(d, dx, 'qvel', i, fname, atol=1e-2) - _assert_attr_eq(d, dx, 'qpos', i, fname, atol=1e-2) - _assert_attr_eq(d, dx, 'act', i, fname) - _assert_attr_eq(d, dx, 'time', i, fname) + mx = mjx.put_model(m) + dx = jax.jit(mjx.step)(mx, mjx.put_data(m, d)) + mujoco.mj_step(m, d) + _assert_attr_eq(d, dx, 'act') + _assert_attr_eq(d, dx, 'time') + _assert_attr_eq(d, dx, 'qvel') + _assert_attr_eq(d, dx, 'qpos') def test_rk4(self): m = mujoco.MjModel.from_xml_string(""" @@ -94,7 +92,6 @@ class ForwardTest(parameterized.TestCase): - @@ -107,39 +104,33 @@ class ForwardTest(parameterized.TestCase): """) - step_jit_fn = jax.jit(forward.step) - mx = mjx.device_put(m) d = mujoco.MjData(m) # give the system a little kick to ensure we have non-identity rotations - d.qvel = np.random.normal(m.nv) * 0.05 - for i in range(100): - # in order to avoid re-jitting, reuse the same mj_data shape - qpos, qvel = d.qpos, d.qvel - d = mujoco.MjData(m) - d.qpos, d.qvel = qpos, qvel - dx = mjx.device_put(d) + d.qvel = np.array([0.2, -0.1]) + mujoco.mj_step(m, d, 10) # let dynamics get state significantly non-zero + mujoco.mj_forward(m, d) - mujoco.mj_step(m, d) - dx = step_jit_fn(mx, dx) + mx = mjx.put_model(m) + dx = jax.jit(mjx.rungekutta4)(mx, mjx.put_data(m, d)) + mujoco.mj_RungeKutta(m, d, 4) - _assert_attr_eq(d, dx, 'qvel', i, 'test_rk4', atol=1e-2) - _assert_attr_eq(d, dx, 'qpos', i, 'test_rk4', atol=1e-2) - _assert_attr_eq(d, dx, 'act', i, 'test_rk4') - _assert_attr_eq(d, dx, 'time', i, 'test_rk4') + _assert_attr_eq(d, dx, 'qvel') + _assert_attr_eq(d, dx, 'qpos') + _assert_attr_eq(d, dx, 'act') + _assert_attr_eq(d, dx, 'time') def test_disable_eulerdamp(self): - m = test_util.load_test_file('ant.xml') - m.opt.disableflags = m.opt.disableflags | DisableBit.EULERDAMP + m = test_util.load_test_file('pendula.xml') + self.assertTrue((m.dof_damping > 0).any()) + m.opt.disableflags = m.opt.disableflags | mjx.DisableBit.EULERDAMP d = mujoco.MjData(m) - mx = mjx.device_put(m) - self.assertTrue((mx.dof_damping > 0).any()) - dx = mjx.device_put(d) - dx = jax.jit(forward.forward)(mx, dx) + d.qvel[:] = 1.0 + d.qacc[:] = 1.0 + mx = mjx.put_model(m) + dx = jax.jit(mjx.euler)(mx, mjx.put_data(m, d)) - dx = dx.replace(qvel=jp.ones_like(dx.qvel), qacc=jp.ones_like(dx.qacc)) - dx = jax.jit(forward._euler)(mx, dx) np.testing.assert_allclose(dx.qvel, 1 + m.opt.timestep) diff --git a/mjx/mujoco/mjx/_src/io.py b/mjx/mujoco/mjx/_src/io.py index 060ffa50..9658ad53 100644 --- a/mjx/mujoco/mjx/_src/io.py +++ b/mjx/mujoco/mjx/_src/io.py @@ -77,6 +77,7 @@ def _put_statistic(s: mujoco.MjStatistic, device=None) -> types.Statistic: def put_model(m: mujoco.MjModel, device=None) -> types.Model: """Puts mujoco.MjModel onto a device, resulting in mjx.Model.""" + if m.ntendon: raise NotImplementedError('tendons are not supported') @@ -150,7 +151,7 @@ def make_data(m: Union[types.Model, mujoco.MjModel]) -> types.Data: d = types.Data( solver_niter=jp.array(0, dtype=jp.int32), time=jp.array(0.0), - qpos=m.qpos0, + qpos=jp.array(m.qpos0), qvel=zero_nv, act=zero_na, qacc_warmstart=zero_nv, diff --git a/mjx/mujoco/mjx/_src/passive.py b/mjx/mujoco/mjx/_src/passive.py index 9b0b41f0..268de126 100644 --- a/mjx/mujoco/mjx/_src/passive.py +++ b/mjx/mujoco/mjx/_src/passive.py @@ -76,7 +76,7 @@ def _inertia_box_fluid_model( def passive(m: Model, d: Data) -> Data: """Adds all passive forces.""" if m.opt.disableflags & DisableBit.PASSIVE: - return d + return d.replace(qfrc_passive=jp.zeros(m.nv)) # joint-level springs def fn(jnt_typs, stiffness, qpos_spring, qpos): diff --git a/mjx/mujoco/mjx/_src/passive_test.py b/mjx/mujoco/mjx/_src/passive_test.py index 4a5026c8..6ff5a596 100644 --- a/mjx/mujoco/mjx/_src/passive_test.py +++ b/mjx/mujoco/mjx/_src/passive_test.py @@ -14,100 +14,65 @@ # ============================================================================== """Tests passive forces.""" -import itertools - from absl.testing import absltest -from absl.testing import parameterized -from etils import epath import jax -import jax.numpy as jp import mujoco from mujoco import mjx +from mujoco.mjx._src import test_util import numpy as np - -def _assert_attr_eq(a, b, attr, step, fname, atol=1e-4, rtol=1e-4): - err_msg = f'mismatch: {attr} at step {step} in {fname}' - a, b = getattr(a, attr), getattr(b, attr) - np.testing.assert_allclose(a, b, err_msg=err_msg, atol=atol, rtol=rtol) +# tolerance for difference between MuJoCo and MJX passive calculations - mostly +# due to float precision +_TOLERANCE = 1e-7 -class PassiveTest(parameterized.TestCase): +def _assert_eq(a, b, name): + tol = _TOLERANCE * 10 # avoid test noise + err_msg = f'mismatch: {name}' + np.testing.assert_allclose(a, b, err_msg=err_msg, atol=tol, rtol=tol) - @parameterized.parameters(enumerate(('ant.xml', 'pendula.xml'))) - def test_stiffness_damping(self, seed, fname): - """Tests stiffness and damping on Ant.""" - np.random.seed(seed) - path = epath.resource_path('mujoco.mjx') / 'test_data' - path /= fname - m = mujoco.MjModel.from_xml_string(path.read_text()) - # set stiffness/damping - m.jnt_stiffness = np.random.uniform(size=m.njnt) - m.dof_damping = np.random.uniform(size=m.nv) +def _assert_attr_eq(a, b, attr): + _assert_eq(getattr(a, attr), getattr(b, attr), attr) + + +class PassiveTest(absltest.TestCase): + + def test_passive(self): + m = test_util.load_test_file('pendula.xml') d = mujoco.MjData(m) - d.qvel = np.random.random(m.nv) # random kick + # give the system a little kick to ensure we have non-identity rotations + d.ctrl = np.array([0.1, -0.1, 0.2, 0.3, -0.4]) + mujoco.mj_step(m, d, 10) # let dynamics get state significantly non-zero + mujoco.mj_forward(m, d) + mx = mjx.put_model(m) - mx = mjx.device_put(m) - dx = mjx.make_data(mx) + dx = jax.jit(mjx.passive)(mx, mjx.put_data(m, d)) + _assert_attr_eq(d, dx, 'qfrc_passive') - passive_jit_fn = jax.jit(mjx.passive) + # test with fluid forces + m.opt.density = 0.01 + mujoco.mj_forward(m, d) + mx = mjx.put_model(m) + dx = jax.jit(mjx.passive)(mx, mjx.put_data(m, d)) + _assert_attr_eq(d, dx, 'qfrc_passive') - for i in range(100): - qpos, qvel = d.qpos.copy(), d.qvel.copy() - mujoco.mj_step(m, d) - dx = passive_jit_fn(mx, dx.replace(qpos=qpos, qvel=qvel)) - _assert_attr_eq(d, dx, 'qfrc_passive', i, fname) + m.opt.viscosity = 0.02 + mujoco.mj_forward(m, d) + mx = mjx.put_model(m) + dx = jax.jit(mjx.passive)(mx, mjx.put_data(m, d)) + _assert_attr_eq(d, dx, 'qfrc_passive') - @parameterized.parameters( - itertools.product(range(3), ('pendula.xml',)) - ) - def test_fluid(self, seed, fname): - np.random.seed(seed) - path = epath.resource_path('mujoco.mjx') / 'test_data' - path /= fname - m = mujoco.MjModel.from_xml_string(path.read_text()) + m.opt.wind = np.array([0.03, 0.04, 0.05]) + mujoco.mj_forward(m, d) + mx = mjx.put_model(m) + dx = jax.jit(mjx.passive)(mx, mjx.put_data(m, d)) + _assert_attr_eq(d, dx, 'qfrc_passive') - # set density/viscosity/wind - m.opt.density = np.random.uniform() - m.opt.viscosity = np.random.uniform() - m.opt.wind = np.random.uniform() - - passive_jit_fn = jax.jit(mjx.passive) - - mx = mjx.device_put(m) - d = mujoco.MjData(m) - d.qvel = np.random.random(m.nv) # random kick - - for i in range(100): - mujoco.mj_step(m, d) - dx = mjx.device_put(d) - mujoco.mj_passive(m, d) - dx = passive_jit_fn(mx, dx) - _assert_attr_eq(d, dx, 'qfrc_passive', i, fname) - - def test_disable_passive(self): - m = mujoco.MjModel.from_xml_string(""" - - - - - - - - - - """) - mx = mjx.device_put(m) - d = mujoco.MjData(m) - dx = mjx.device_put(d) - dx = dx.replace(qvel=jp.ones(mx.nv)) - - passive_jit_fn = jax.jit(mjx.passive) - dx = passive_jit_fn(mx, dx) - np.testing.assert_equal(dx.qfrc_passive, np.zeros(mx.nv)) + # test disable passive + mx = mx.tree_replace({'opt.disableflags': mjx.DisableBit.PASSIVE}) + dx = jax.jit(mjx.passive)(mx, mjx.put_data(m, d)) + np.testing.assert_allclose(dx.qfrc_passive, 0) if __name__ == '__main__': diff --git a/mjx/mujoco/mjx/_src/scan_test.py b/mjx/mujoco/mjx/_src/scan_test.py index fe4448ca..456067d6 100644 --- a/mjx/mujoco/mjx/_src/scan_test.py +++ b/mjx/mujoco/mjx/_src/scan_test.py @@ -193,7 +193,7 @@ class ScanTest(absltest.TestCase): """ - def testscan_actuators(self): + def test_scan_actuators(self): """Tests scanning over actuators.""" m = mujoco.MjModel.from_xml_string(self._MULTI_ACT_XML) m = mjx.device_put(m) diff --git a/mjx/mujoco/mjx/_src/smooth.py b/mjx/mujoco/mjx/_src/smooth.py index 624291d8..ba470bed 100644 --- a/mjx/mujoco/mjx/_src/smooth.py +++ b/mjx/mujoco/mjx/_src/smooth.py @@ -435,40 +435,38 @@ def transmission(m: Model, d: Data) -> Data: if not m.nu: return d - def fn(gear, jnt_typ, m_i, m_j, qpos): + def fn(gear, jnt_typ, m_j, qpos): # handles joint transmissions only if jnt_typ == JointType.FREE: length = jp.zeros(1) moment = gear - m_i = jp.repeat(m_i, 6) m_j = m_j + jp.arange(6) elif jnt_typ == JointType.BALL: - axis, _ = math.quat_to_axis_angle(qpos) - length = jp.dot(axis, gear[:3])[None] + axis, angle = math.quat_to_axis_angle(qpos) + length = jp.dot(axis * angle, gear[:3])[None] moment = gear[:3] - m_i = jp.repeat(m_i, 3) m_j = m_j + jp.arange(3) elif jnt_typ in (JointType.SLIDE, JointType.HINGE): length = qpos * gear[0] moment = gear[:1] - m_i, m_j = m_i[None], m_j[None] + m_j = m_j[None] else: raise RuntimeError(f'unrecognized joint type: {jnt_typ}') - return length, moment, m_i, m_j + moment = jp.zeros((m.nv,)).at[m_j].set(moment) + return length, moment - length, m_val, m_i, m_j = scan.flat( + length, moment = scan.flat( m, fn, - 'ujujq', - 'uvvv', + 'ujjq', + 'uuuu', m.actuator_gear, m.jnt_type, - jp.arange(m.nu), jp.array(m.jnt_dofadr), d.qpos, group_by='u', ) - moment = jp.zeros((m.nu, m.nv)).at[m_i, m_j].set(m_val) length = length.reshape((m.nu,)) + moment = moment.reshape((m.nu, m.nv)) d = d.replace(actuator_length=length, actuator_moment=moment) return d diff --git a/mjx/mujoco/mjx/_src/smooth_test.py b/mjx/mujoco/mjx/_src/smooth_test.py index bd9348bb..203cb654 100644 --- a/mjx/mujoco/mjx/_src/smooth_test.py +++ b/mjx/mujoco/mjx/_src/smooth_test.py @@ -15,122 +15,107 @@ """Tests for smooth dynamics functions.""" from absl.testing import absltest -from absl.testing import parameterized import jax from jax import numpy as jp import mujoco from mujoco import mjx from mujoco.mjx._src import test_util -# pylint: disable=g-importing-member -from mujoco.mjx._src.types import DisableBit -# pylint: enable=g-importing-member import numpy as np - -def _assert_eq(a, b, name, step, fname, atol=5e-4, rtol=5e-4): - err_msg = f'mismatch: {name} at step {step} in {fname}' - np.testing.assert_allclose(a, b, err_msg=err_msg, atol=atol, rtol=rtol) +# tolerance for difference between MuJoCo and MJX smooth calculations - mostly +# due to float precision +_TOLERANCE = 5e-5 -def _assert_attr_eq(a, b, attr, step, fname, atol=5e-4, rtol=5e-4): - err_msg = f'mismatch: {attr} at step {step} in {fname}' - a, b = getattr(a, attr), getattr(b, attr) - np.testing.assert_allclose(a, b, err_msg=err_msg, atol=atol, rtol=rtol) +def _assert_eq(a, b, name): + tol = _TOLERANCE * 10 # avoid test noise + err_msg = f'mismatch: {name}' + np.testing.assert_allclose(a, b, err_msg=err_msg, atol=tol, rtol=tol) -class SmoothTest(parameterized.TestCase): +def _assert_attr_eq(a, b, attr): + _assert_eq(getattr(a, attr), getattr(b, attr), attr) - @parameterized.parameters(enumerate(test_util.TEST_FILES)) - def test_smooth(self, seed, fname): - """Tests mujoco mj smooth functions match mujoco_mjx smooth functions.""" - if fname in ('convex.xml', 'equality.xml'): - return - np.random.seed(seed) +class SmoothTest(absltest.TestCase): - m = test_util.load_test_file(fname) + def setUp(self): + super().setUp() + # although we already have generous padding of thresholds, it doesn't hurt + # to also fix the seed to reduce test flakiness + np.random.seed(0) + + def test_smooth(self): + """Tests MJX smooth functions match MuJoCo smooth functions.""" + + m = test_util.load_test_file('pendula.xml') d = mujoco.MjData(m) - - kinematics_jit_fn = jax.jit(mjx.kinematics) - com_pos_jit_fn = jax.jit(mjx.com_pos) - crb_jit_fn = jax.jit(mjx.crb) - factor_m_fn = jax.jit(mjx.factor_m) - com_vel_jit_fn = jax.jit(mjx.com_vel) - rne_jit_fn = jax.jit(mjx.rne) - mul_m_jit_fn = jax.jit(mjx.mul_m) - transmission_jit_fn = jax.jit(mjx.transmission) - - mx = mjx.device_put(m) - dx = mjx.make_data(mx) - # give the system a little kick to ensure we have non-identity rotations d.qvel = np.random.random(m.nv) - for i in range(100): - qpos, qvel = d.qpos.copy(), d.qvel.copy() - mujoco.mj_step(m, d) + mujoco.mj_step(m, d, 10) # let dynamics get state significantly non-zero + mujoco.mj_forward(m, d) + mx = mjx.put_model(m) - # kinematics - dx = kinematics_jit_fn(mx, dx.replace(qpos=qpos, qvel=qvel)) - _assert_attr_eq(d, dx, 'xanchor', i, fname) - _assert_attr_eq(d, dx, 'xaxis', i, fname) - _assert_attr_eq(d, dx, 'xpos', i, fname) - _assert_attr_eq(d, dx, 'xquat', i, fname) - _assert_eq(d.xmat.reshape((-1, 3, 3)), dx.xmat, 'xmat', i, fname) - _assert_attr_eq(d, dx, 'xipos', i, fname) - _assert_eq(d.ximat.reshape((-1, 3, 3)), dx.ximat, 'ximat', i, fname) - _assert_attr_eq(d, dx, 'geom_xpos', i, fname) - _assert_eq( - d.geom_xmat.reshape((-1, 3, 3)), - dx.geom_xmat, - 'geom_xmat', - i, - fname, - ) + # kinematics + dx = jax.jit(mjx.kinematics)(mx, mjx.put_data(m, d)) + _assert_attr_eq(d, dx, 'xanchor') + _assert_attr_eq(d, dx, 'xaxis') + _assert_attr_eq(d, dx, 'xpos') + _assert_attr_eq(d, dx, 'xquat') + _assert_eq(d.xmat.reshape((-1, 3, 3)), dx.xmat, 'xmat') + _assert_attr_eq(d, dx, 'xipos') + _assert_eq(d.ximat.reshape((-1, 3, 3)), dx.ximat, 'ximat') + _assert_attr_eq(d, dx, 'geom_xpos') + _assert_eq(d.geom_xmat.reshape((-1, 3, 3)), dx.geom_xmat, 'geom_xmat') + _assert_attr_eq(d, dx, 'site_xpos') + _assert_eq(d.site_xmat.reshape((-1, 3, 3)), dx.site_xmat, 'site_xmat') + # com_pos + dx = jax.jit(mjx.com_pos)(mx, mjx.put_data(m, d)) + _assert_attr_eq(d, dx, 'subtree_com') + _assert_attr_eq(d, dx, 'cinert') + _assert_attr_eq(d, dx, 'cdof') + # crb + dx = jax.jit(mjx.crb)(mx, mjx.put_data(m, d)) + _assert_attr_eq(d, dx, 'crb') + _assert_attr_eq(d, dx, 'qM') + # factor_m + dx = mjx.put_data(m, d) + dx = jax.jit(mjx.factor_m)(mx, dx, dx.qM) + _assert_attr_eq(d, dx, 'qLD') + _assert_attr_eq(d, dx, 'qLDiagInv') + # com_vel + dx = jax.jit(mjx.com_vel)(mx, mjx.put_data(m, d)) + _assert_attr_eq(d, dx, 'cvel') + _assert_attr_eq(d, dx, 'cdof_dot') + # rne + dx = jax.jit(mjx.rne)(mx, mjx.put_data(m, d)) + _assert_attr_eq(d, dx, 'qfrc_bias') + # transmission + dx = jax.jit(mjx.transmission)(mx, mjx.put_data(m, d)) + _assert_attr_eq(d, dx, 'actuator_length') + _assert_attr_eq(d, dx, 'actuator_moment') - # com_pos - dx = com_pos_jit_fn(mx, dx) - _assert_attr_eq(d, dx, 'subtree_com', i, fname) - _assert_attr_eq(d, dx, 'cinert', i, fname) - _assert_attr_eq(d, dx, 'cdof', i, fname) + def test_mul_m(self): + m = test_util.load_test_file('pendula.xml') + d = mujoco.MjData(m) + # give the system a little kick to ensure we have non-identity rotations + d.qvel = np.random.random(m.nv) + mujoco.mj_step(m, d, 10) # let dynamics get state significantly non-zero + mujoco.mj_forward(m, d) + mx = mjx.put_model(m) + dx = mjx.put_data(m, d) + vec = np.random.random(m.nv) + mjx_vec = jax.jit(mjx.mul_m)(mx, dx, jp.array(vec)) + mj_vec = np.zeros(m.nv) + mujoco.mj_mulM(m, d, mj_vec, vec) + _assert_eq(mj_vec, mjx_vec, 'mul_m') - # crb - dx = crb_jit_fn(mx, dx) - _assert_attr_eq(d, dx, 'crb', i, fname) - _assert_attr_eq(d, dx, 'qM', i, fname) - - # factor_m - dx = factor_m_fn(mx, dx, dx.qM) - _assert_attr_eq(d, dx, 'qLD', i, fname, atol=1e-3) - _assert_attr_eq(d, dx, 'qLDiagInv', i, fname, atol=1e-3) - - # com_vel - dx = com_vel_jit_fn(mx, dx) - _assert_attr_eq(d, dx, 'cvel', i, fname) - _assert_attr_eq(d, dx, 'cdof_dot', i, fname) - - # rne - dx = rne_jit_fn(mx, dx) - _assert_attr_eq(d, dx, 'qfrc_bias', i, fname) - - # mul_m (auxilliary function, not part of smooth step) - vec = np.random.random(m.nv) - mjx_vec = mul_m_jit_fn(mx, dx, jp.array(vec)) - mj_vec = np.zeros(m.nv) - mujoco.mj_mulM(m, d, mj_vec, vec) - _assert_eq(mj_vec, mjx_vec, 'mul_m', i, fname) - - # transmission - dx = transmission_jit_fn(mx, dx) - _assert_attr_eq(d, dx, 'actuator_length', i, fname) - _assert_attr_eq(d, dx, 'actuator_moment', i, fname) - - -class DisableGravityTest(absltest.TestCase): - - def test_disabled(self): + def test_disable_gravity(self): m = mujoco.MjModel.from_xml_string(""" - @@ -139,63 +124,13 @@ class DisableGravityTest(absltest.TestCase): """) - mx = mjx.device_put(m) d = mujoco.MjData(m) - dx = mjx.device_put(d) - - # test with gravity - step_jit_fn = jax.jit(mjx.step) - dx = step_jit_fn(mx, dx) - np.testing.assert_array_almost_equal( - dx.qpos, np.array([0.0, 0.0, -9.81e-4, 1.0, 0.0, 0.0, 0.0]), decimal=7 - ) - - # test with gravity disabled - mx = mx.tree_replace( - {'opt.disableflags': mx.opt.disableflags | DisableBit.GRAVITY} - ) - dx = mjx.device_put(d) - step_jit_fn = jax.jit(mjx.step) - dx = step_jit_fn(mx, dx) - np.testing.assert_equal( - dx.qpos, np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0]) - ) - - -class SiteTest(absltest.TestCase): - - def test_site(self): - """Tests that site positions and orientations match MuJoCo.""" - m = mujoco.MjModel.from_xml_string(""" - - - - - - - - - - - - - - - - """) - d = mujoco.MjData(m) - - mx = mjx.device_put(m) - dx = mjx.device_put(d) - mujoco.mj_forward(m, d) - dx = mjx.forward(mx, dx) - - np.testing.assert_array_almost_equal(dx.site_xpos, d.site_xpos) - np.testing.assert_array_almost_equal( - dx.site_xmat, d.site_xmat.reshape((-1, 3, 3)) - ) + mx = mjx.put_model(m) + dx = mjx.put_data(m, d) + dx = jax.jit(mjx.rne)(mx, dx) + np.testing.assert_allclose(dx.qfrc_bias, 0) if __name__ == '__main__': absltest.main() diff --git a/mjx/mujoco/mjx/_src/solver_test.py b/mjx/mujoco/mjx/_src/solver_test.py index 6a4f7792..d1677ff9 100644 --- a/mjx/mujoco/mjx/_src/solver_test.py +++ b/mjx/mujoco/mjx/_src/solver_test.py @@ -12,119 +12,64 @@ # See the License for the specific language governing permissions and # limitations under the License. # ============================================================================== -"""Tests for forward functions.""" +"""Tests for constraint functions.""" from absl.testing import absltest -from absl.testing import parameterized -from etils import epath import jax import mujoco from mujoco import mjx +from mujoco.mjx._src import test_util import numpy as np -def _assert_attr_eq(a, b, attr, step, fname, atol=1e-2, rtol=1e-2): - err_msg = f'mismatch: {attr} at step {step} in {fname}' - a, b = getattr(a, attr), getattr(b, attr) - np.testing.assert_allclose(a, b, err_msg=err_msg, atol=atol, rtol=rtol) +# tolerance for difference between MuJoCo and MJX constraint calculations, +# mostly due to float precision +_TOLERANCE = 5e-5 -class Solver64Test(parameterized.TestCase): - """Tests solvers at 64 bit precision.""" +def _assert_eq(a, b, name, tol=_TOLERANCE): + tol = tol * 10 # avoid test noise + err_msg = f'mismatch: {name}' + np.testing.assert_allclose(a, b, err_msg=err_msg, atol=tol, rtol=tol) - def setUp(self): - super().setUp() - jax.config.update('jax_enable_x64', True) - def tearDown(self): - super().tearDown() - jax.config.update('jax_enable_x64', False) +def _assert_attr_eq(a, b, attr): + _assert_eq(getattr(a, attr), getattr(b, attr), attr) - @parameterized.parameters(enumerate(('ant.xml', 'humanoid.xml'))) - def test_cg(self, seed, fname): - """Test mjx cg solver matches mujoco cg solver at 64 bit precision.""" - f = epath.resource_path('mujoco.mjx') / 'test_data' / fname - m = mujoco.MjModel.from_xml_string(f.read_text()) + +class SolverTest(absltest.TestCase): + + def test_solver(self): + """Test solver.""" + m = test_util.load_test_file('constraints.xml') d = mujoco.MjData(m) - mx = mjx.device_put(m) + mujoco.mj_step(m, d, 100) # at 100 steps mix of active/inactive constraints + mujoco.mj_forward(m, d) + mx = mjx.put_model(m) - jax.config.update('jax_enable_x64', True) - forward_jit_fn = jax.jit(mjx.forward) + dx = jax.jit(mjx.solve)(mx, mjx.put_data(m, d)) + _assert_attr_eq(d, dx, 'qacc_warmstart') + _assert_attr_eq(d, dx, 'qacc') + _assert_attr_eq(d, dx, 'qfrc_constraint') + nnz = dx.efc_J.any(axis=1) + _assert_eq(d.efc_force, dx.efc_force[nnz], 'efc_force') - # give the system a little kick to ensure we have non-identity rotations - np.random.seed(seed) - d.qvel = 0.01 * np.random.random(m.nv) - - for i in range(100): - # in order to avoid re-jitting, reuse the same mj_data shape - save = d.qpos, d.qvel, d.time, d.qacc_warmstart, d.qacc_smooth - d = mujoco.MjData(m) - d.qpos, d.qvel, d.time, d.qacc_warmstart, d.qacc_smooth = save - dx = mjx.device_put(d) - - mujoco.mj_step(m, d) - dx = forward_jit_fn(mx, dx) - - # at 64 bits the solutions returned by the two solvers are quite close - self.assertLessEqual(dx.solver_niter[0], d.solver_niter[0]) - _assert_attr_eq(d, dx, 'qfrc_constraint', i, fname) - _assert_attr_eq(d, dx, 'qacc', i, fname) - - -class SolverTest(parameterized.TestCase): - - @parameterized.parameters(enumerate(('ant.xml', 'humanoid.xml'))) - def test_cg(self, seed, fname): - """Test mjx cg solver is close to mj at 32 bit precision. - - Args: - seed: int - fname: file to test - - At lower float resolution there's wiggle room in valid forces that satisfy - constraints. So instead let's mainly validate that mjx is finding solutions - with as good cost as mujoco, even if the resulting forces/accelerations - are not quite the same. - """ - f = epath.resource_path('mujoco.mjx') / 'test_data' / fname - m = mujoco.MjModel.from_xml_string(f.read_text()) - d = mujoco.MjData(m) - mx = mjx.device_put(m) - - forward_jit_fn = jax.jit(mjx.forward) - - # give the system a little kick to ensure we have non-identity rotations - np.random.seed(seed) - d.qvel = 0.01 * np.random.random(m.nv) - - for i in range(100): - # in order to avoid re-jitting, reuse the same mj_data shape - save = d.qpos, d.qvel, d.time, d.qacc_warmstart, d.qacc_smooth - d = mujoco.MjData(m) - d.qpos, d.qvel, d.time, d.qacc_warmstart, d.qacc_smooth = save - dx = mjx.device_put(d) - - mujoco.mj_step(m, d) - dx = forward_jit_fn(mx, dx) - - def cost(qacc): - jaref = np.zeros(d.nefc) - mujoco.mj_mulJacVec(m, d, jaref, qacc) - jaref -= d.efc_aref - cost = np.array([0.0]) - mujoco.mj_constraintUpdate(m, d, jaref, cost, 0) - return cost[0] - - cost_mj, cost_mjx = cost(d.qacc), cost(dx.qacc) - - self.assertLessEqual( - cost_mjx, - cost_mj * 1.01, - msg=f'mismatch: {fname} at step {i}, cost too high', - ) - _assert_attr_eq(d, dx, 'qfrc_constraint', i, fname, atol=1e-1, rtol=1e-1) - _assert_attr_eq(d, dx, 'qacc', i, fname, atol=1e-1, rtol=1e-1) + # also test normal CG + m.opt.solver = mujoco.mjtSolver.mjSOL_CG + mujoco.mj_forward(m, d) + dx = jax.jit(mjx.solve)(mx, mjx.put_data(m, d)) + _assert_attr_eq(d, dx, 'qacc_warmstart') + _assert_attr_eq(d, dx, 'qacc') + _assert_attr_eq(d, dx, 'qfrc_constraint') + _assert_eq(d.efc_force, dx.efc_force[nnz], 'efc_force') + # without warmstart, the solution is not as close + m.opt.solver = mujoco.mjtSolver.mjSOL_NEWTON + m.opt.disableflags |= mujoco.mjtDisableBit.mjDSBL_WARMSTART + mujoco.mj_forward(m, d) + mx = mjx.put_model(m) + dx = jax.jit(mjx.solve)(mx, mjx.put_data(m, d)) + _assert_eq(d.efc_force, dx.efc_force[nnz], 'efc_force', tol=2e-2) if __name__ == '__main__': absltest.main() diff --git a/mjx/mujoco/mjx/_src/support_test.py b/mjx/mujoco/mjx/_src/support_test.py index fe88fc84..fb3a5389 100644 --- a/mjx/mujoco/mjx/_src/support_test.py +++ b/mjx/mujoco/mjx/_src/support_test.py @@ -34,8 +34,8 @@ class SupportTest(parameterized.TestCase): m = test_util.load_test_file(fname) d = mujoco.MjData(m) mujoco.mj_step(m, d) - mx = mjx.device_put(m) - dx = mjx.device_put(d) + mx = mjx.put_model(m) + dx = mjx.put_data(m, d) point = np.random.randn(3) body = np.random.choice(m.nbody) jacp, jacr = jax.jit(support.jac)(mx, dx, point, body) @@ -49,11 +49,11 @@ class SupportTest(parameterized.TestCase): """Tests that xfrc_accumulate ouput matches mj_xfrcAccumulate.""" np.random.seed(0) - m = test_util.load_test_file('ant.xml') + m = test_util.load_test_file('pendula.xml') d = mujoco.MjData(m) mujoco.mj_step(m, d) - mx = mjx.device_put(m) - dx = mjx.device_put(d) + mx = mjx.put_model(m) + dx = mjx.put_data(m, d) self.assertFalse((dx.xipos == 0.0).all()) xfrc = np.random.rand(*dx.xfrc_applied.shape) diff --git a/mjx/mujoco/mjx/_src/test_util.py b/mjx/mujoco/mjx/_src/test_util.py index 8d765644..870621f7 100644 --- a/mjx/mujoco/mjx/_src/test_util.py +++ b/mjx/mujoco/mjx/_src/test_util.py @@ -23,10 +23,8 @@ import mujoco import numpy as np TEST_FILES: List[str] = [ - 'ant.xml', + 'constraints.xml', 'convex.xml', - 'equality.xml', - 'humanoid.xml', 'pendula.xml', ] diff --git a/mjx/mujoco/mjx/integration_test/collision_driver_test.py b/mjx/mujoco/mjx/integration_test/collision_driver_test.py index 1e28b053..a9891656 100644 --- a/mjx/mujoco/mjx/integration_test/collision_driver_test.py +++ b/mjx/mujoco/mjx/integration_test/collision_driver_test.py @@ -58,9 +58,9 @@ class CollisionDriverIntegrationTest(parameterized.TestCase): ) m = mujoco.MjModel.from_xml_string(mjcf) - mx = mjx.device_put(m) + mx = mjx.put_model(m) d = mujoco.MjData(m) - dx = mjx.device_put(d) + dx = mjx.put_data(m, d) mujoco.mj_step(m, d) collision_jit_fn = jax.jit(mjx.collision) diff --git a/mjx/mujoco/mjx/integration_test/forward_test.py b/mjx/mujoco/mjx/integration_test/forward_test.py index 6fa913c4..67f20371 100644 --- a/mjx/mujoco/mjx/integration_test/forward_test.py +++ b/mjx/mujoco/mjx/integration_test/forward_test.py @@ -19,7 +19,6 @@ from absl.testing import parameterized import jax import mujoco from mujoco import mjx -from mujoco.mjx._src import forward from mujoco.mjx._src import test_util import numpy as np @@ -46,7 +45,7 @@ class ActuationIntegrationTest(parameterized.TestCase): enable_contact=False, ) m = mujoco.MjModel.from_xml_string(mjcf) - actuation_jit_fn = jax.jit(forward._actuation) + actuation_jit_fn = jax.jit(mjx.fwd_actuation) # init d = mujoco.MjData(m) @@ -57,8 +56,8 @@ class ActuationIntegrationTest(parameterized.TestCase): mujoco.mj_fwdVelocity(m, d) # put on device - mx = mjx.device_put(m) - dx = mjx.device_put(d) + mx = mjx.put_model(m) + dx = mjx.put_data(m, d) mujoco.mj_fwdActuation(m, d) dx = actuation_jit_fn(mx, dx) diff --git a/mjx/mujoco/mjx/integration_test/smooth_test.py b/mjx/mujoco/mjx/integration_test/smooth_test.py index c924e005..d032be79 100644 --- a/mjx/mujoco/mjx/integration_test/smooth_test.py +++ b/mjx/mujoco/mjx/integration_test/smooth_test.py @@ -60,8 +60,8 @@ class TransmissionIntegrationTest(parameterized.TestCase): d.qvel = np.random.random(m.nv) # put on device - mx = mjx.device_put(m) - dx = mjx.device_put(d) + mx = mjx.put_model(m) + dx = mjx.put_data(m, d) mujoco.mj_transmission(m, d) dx = transmission_jit_fn(mx, dx) diff --git a/mjx/mujoco/mjx/test_data/ant.xml b/mjx/mujoco/mjx/test_data/ant.xml deleted file mode 100644 index 7417c3ae..00000000 --- a/mjx/mujoco/mjx/test_data/ant.xml +++ /dev/null @@ -1,82 +0,0 @@ - - - diff --git a/mjx/mujoco/mjx/test_data/constraints.xml b/mjx/mujoco/mjx/test_data/constraints.xml new file mode 100644 index 00000000..52eb95d8 --- /dev/null +++ b/mjx/mujoco/mjx/test_data/constraints.xml @@ -0,0 +1,53 @@ + + + diff --git a/mjx/mujoco/mjx/test_data/equality.xml b/mjx/mujoco/mjx/test_data/equality.xml deleted file mode 100644 index e5c9d184..00000000 --- a/mjx/mujoco/mjx/test_data/equality.xml +++ /dev/null @@ -1,71 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/mjx/mujoco/mjx/test_data/humanoid.xml b/mjx/mujoco/mjx/test_data/humanoid.xml deleted file mode 100644 index 2d7158ee..00000000 --- a/mjx/mujoco/mjx/test_data/humanoid.xml +++ /dev/null @@ -1,109 +0,0 @@ - - - - - - - - diff --git a/mjx/mujoco/mjx/test_data/pendula.xml b/mjx/mujoco/mjx/test_data/pendula.xml index 2363a3f8..0dc476ab 100644 --- a/mjx/mujoco/mjx/test_data/pendula.xml +++ b/mjx/mujoco/mjx/test_data/pendula.xml @@ -18,6 +18,8 @@ + + @@ -26,45 +28,49 @@ - + + - + + - + + - - + + - + - + - + - + + @@ -72,14 +78,14 @@ - + - + - + @@ -89,14 +95,21 @@ - + - + - + + + + + + + + From 762371c3e4c5ed7204d2533411956c34b498baf5 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Wed, 6 Dec 2023 04:27:34 -0800 Subject: [PATCH 8/8] Add `kv` damping attribute to `position` and `intvelocity` actuator shortcuts. PiperOrigin-RevId: 588377097 Change-Id: I8ded2ff982e52ed8673019979e29b706ef65c743 --- doc/XMLreference.rst | 39 ++- doc/XMLschema.rst | 8 +- doc/changelog.rst | 4 + src/user/user_objects.cc | 2 +- src/xml/xml_native_reader.cc | 51 ++-- test/engine/testdata/actuation/refsite.xml | 16 +- test/user/user_objects_test.cc | 4 +- test/xml/xml_native_reader_test.cc | 268 ++++++++++++++------- 8 files changed, 250 insertions(+), 142 deletions(-) diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index fa963647..2f423abf 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -5317,13 +5317,13 @@ This element does not have custom attributes. It only has common attributes, whi This element creates a position servo. The underlying :el:`general` attributes are set as follows: -========= ======= ========= ======= +========= ======= ========= ========= Attribute Setting Attribute Setting -========= ======= ========= ======= +========= ======= ========= ========= dyntype none dynprm 1 0 0 gaintype fixed gainprm kp 0 0 -biastype affine biasprm 0 -kp 0 -========= ======= ========= ======= +biastype affine biasprm 0 -kp -kv +========= ======= ========= ========= This element has one custom attribute in addition to the common attributes: @@ -5377,6 +5377,11 @@ This element has one custom attribute in addition to the common attributes: :at:`kp`: :at-val:`real, "1"` Position feedback gain. +.. _actuator-position-kv: + +:at:`kv`: :at-val:`real, "0"` + Damping applied by the actuator. + When using this attribute, it is recommended to use the implicitfast or implicit :ref:`integrators`. .. _actuator-velocity: @@ -5385,7 +5390,9 @@ This element has one custom attribute in addition to the common attributes: This element creates a velocity servo. Note that in order create a PD controller, one has to define two actuators: a position servo and a velocity servo. This is because MuJoCo actuators are SISO while a PD controller takes two control -inputs (reference position and reference velocity). The underlying :el:`general` attributes are set as follows: +inputs (reference position and reference velocity). +When using this actuator, it is recommended to use the implicitfast or implicit :ref:`integrators`. +The underlying :el:`general` attributes are set as follows: ========= ======= ========= ======= Attribute Setting Attribute Setting @@ -5456,14 +5463,14 @@ This element creates an integrated-velocity servo. For more information, see the :ref:`Activation clamping ` section of the Modeling chapter. The underlying :el:`general` attributes are set as follows: -========== =========== ========= ======= +========== =========== ========= ========= Attribute Setting Attribute Setting -========== =========== ========= ======= +========== =========== ========= ========= dyntype integrator dynprm 1 0 0 gaintype fixed gainprm kp 0 0 -biastype affine biasprm 0 -kp 0 +biastype affine biasprm 0 -kp -kv actlimited true -========== =========== ========= ======= +========== =========== ========= ========= This element has one custom attribute in addition to the common attributes: @@ -5518,6 +5525,11 @@ This element has one custom attribute in addition to the common attributes: :at:`kp`: :at-val:`real, "1"` Position feedback gain. +.. _actuator-intvelocity-kv: + +:at:`kv`: :at-val:`real, "0"` + Damping applied by the actuator. + When using this attribute, it is recommended to use the implicitfast or implicit :ref:`integrators`. .. _actuator-damper: @@ -5525,8 +5537,9 @@ This element has one custom attribute in addition to the common attributes: ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ This element is an active damper which produces a force proportional to both velocity and control: ``F = - kv * velocity -* control``, where ``kv`` must be nonnegative. :at:`ctrlrange` is required and must also be nonnegative. The underlying -:el:`general` attributes are set as follows: +* control``, where ``kv`` must be nonnegative. :at:`ctrlrange` is required and must also be nonnegative. +When using this actuator, it is recommended to use the implicitfast or implicit :ref:`integrators`. +The underlying :el:`general` attributes are set as follows: =========== ======= ========= ======= Attribute Setting Attribute Setting @@ -7680,6 +7693,8 @@ slidersite, cranksite. .. _default-position-kp: +.. _default-position-kv: + :el-prefix:`default/` |-| **position** (?) ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ @@ -7736,6 +7751,8 @@ tendon, slidersite, cranksite. .. _default-intvelocity-kp: +.. _default-intvelocity-kv: + :el-prefix:`default/` |-| **intvelocity** (?) ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index a8d0d0b3..702adeb2 100644 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -778,7 +778,7 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | | :ref:`jointinparent` | :ref:`tendon` | :ref:`slidersite` | :ref:`cranksite` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`site` | :ref:`refsite` | :ref:`kp` | | | +| | | | :ref:`site` | :ref:`refsite` | :ref:`kp` | :ref:`kv` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_| actuator |br| |_| |L| | | .. table:: | @@ -810,6 +810,8 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | | :ref:`cranksite` | :ref:`site` | :ref:`refsite` | :ref:`kp` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`kv` | | | | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_| actuator |br| |_| |L| | | .. table:: | | :ref:`damper | \* | :class: mjcf-attributes | @@ -1440,7 +1442,7 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | | :ref:`gear` | :ref:`cranklength` | :ref:`user` | :ref:`group` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`kp` | | | | | +| | | | :ref:`kp` | :ref:`kv` | | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_| default |br| |_| |L| | | .. table:: | @@ -1462,7 +1464,7 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | | :ref:`actrange` | :ref:`gear` | :ref:`cranklength` | :ref:`user` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`group` | :ref:`kp` | | | | +| | | | :ref:`group` | :ref:`kp` | :ref:`kv` | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_| default |br| |_| |L| | | .. table:: | diff --git a/doc/changelog.rst b/doc/changelog.rst index 826c1a1d..56904ed8 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -12,6 +12,10 @@ General robust for very small or large geom sizes. - Added :ref:`frame` to MJCF, a :ref:`meta-element` which defines a pure coordinate transformation on its direct children, without requiring a :ref:`body`. +- Added the :at:`kv` attribute to the :ref:`position` and :ref:`intvelocity` + actuators, for specifying actuator-applied damping. This can be used to implement a PD controller with 0 reference + velocity. When using this attribute, it is recommended to use the implicitfast or implicit + :ref:`integrators`. Plugins ^^^^^^^ diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 3d7ef0b8..009ecebc 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -3876,7 +3876,7 @@ void mjCActuator::Compile(void) { throw mjCError(this, "invalid control range for actuator '%s' (id = %d)", name.c_str(), id); } if (actrange[0]>=actrange[1] && actlimited) { - throw mjCError(this, "invalid activation range for actuator '%s' (id = %d)", name.c_str(), id); + throw mjCError(this, "invalid actrange for actuator '%s' (id = %d)", name.c_str(), id); } if (actlimited && dyntype == mjDYN_NONE) { throw mjCError(this, "actrange specified but dyntype is 'none' in actuator '%s' (id = %d)", diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 4121b2b9..57671db9 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -166,16 +166,16 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = { "dyntype", "gaintype", "biastype", "dynprm", "gainprm", "biasprm", "actearly"}, {"motor", "?", "8", "ctrllimited", "forcelimited", "ctrlrange", "forcerange", "gear", "cranklength", "user", "group"}, - {"position", "?", "9", "ctrllimited", "forcelimited", "ctrlrange", "forcerange", + {"position", "?", "10", "ctrllimited", "forcelimited", "ctrlrange", "forcerange", "gear", "cranklength", "user", "group", - "kp"}, + "kp", "kv"}, {"velocity", "?", "9", "ctrllimited", "forcelimited", "ctrlrange", "forcerange", "gear", "cranklength", "user", "group", "kv"}, - {"intvelocity", "?", "10", "ctrllimited", "forcelimited", + {"intvelocity", "?", "11", "ctrllimited", "forcelimited", "ctrlrange", "forcerange", "actrange", "gear", "cranklength", "user", "group", - "kp"}, + "kp", "kv"}, {"damper", "?", "8", "forcelimited", "ctrlrange", "forcerange", "gear", "cranklength", "user", "group", "kv"}, @@ -379,22 +379,22 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = { "ctrllimited", "forcelimited", "ctrlrange", "forcerange", "lengthrange", "gear", "cranklength", "user", "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite"}, - {"position", "*", "19", "name", "class", "group", + {"position", "*", "20", "name", "class", "group", "ctrllimited", "forcelimited", "ctrlrange", "forcerange", "lengthrange", "gear", "cranklength", "user", "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite", - "kp"}, + "kp", "kv"}, {"velocity", "*", "19", "name", "class", "group", "ctrllimited", "forcelimited", "ctrlrange", "forcerange", "lengthrange", "gear", "cranklength", "user", "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite", "kv"}, - {"intvelocity", "*", "20", "name", "class", "group", + {"intvelocity", "*", "21", "name", "class", "group", "ctrllimited", "forcelimited", "ctrlrange", "forcerange", "actrange", "lengthrange", "gear", "cranklength", "user", "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite", - "kp"}, + "kp", "kv"}, {"damper", "*", "18", "name", "class", "group", "forcelimited", "ctrlrange", "forcerange", "lengthrange", "gear", "cranklength", "user", @@ -1895,19 +1895,26 @@ void mjXReader::OneActuator(XMLElement* elem, mjCActuator* pact) { pact->biastype = mjBIAS_NONE; } - // position servo - else if (type=="position") { - // clear bias - mjuu_zerovec(pact->biasprm, mjNBIAS); - + // position or integrated velocity servo + else if (type=="position" || type=="intvelocity") { // explicit attributes ReadAttr(elem, "kp", 1, pact->gainprm, text); pact->biasprm[1] = -pact->gainprm[0]; + if (ReadAttr(elem, "kv", 1, pact->biasprm + 2, text)) { + if (pact->biasprm[2] < 0) + throw mjXError(elem, "kv cannot be negative"); + pact->biasprm[2] *= -1; + } + // implied parameters - pact->dyntype = mjDYN_NONE; pact->gaintype = mjGAIN_FIXED; pact->biastype = mjBIAS_AFFINE; + + if (type=="intvelocity") { + pact->dyntype = mjDYN_INTEGRATOR; + pact->actlimited = 1; + } } // velocity servo @@ -1925,22 +1932,6 @@ void mjXReader::OneActuator(XMLElement* elem, mjCActuator* pact) { pact->biastype = mjBIAS_AFFINE; } - // integrated velocity - else if (type=="intvelocity") { - // clear bias - mjuu_zerovec(pact->biasprm, mjNBIAS); - - // explicit attributes - ReadAttr(elem, "kp", 1, pact->gainprm, text); - - // implied parameters - pact->dyntype = mjDYN_INTEGRATOR; - pact->gaintype = mjGAIN_FIXED; - pact->biastype = mjBIAS_AFFINE; - pact->actlimited = 1; - pact->biasprm[1] = -pact->gainprm[0]; - } - // damper else if (type=="damper") { // clear gain diff --git a/test/engine/testdata/actuation/refsite.xml b/test/engine/testdata/actuation/refsite.xml index e4b0a2fa..eda8e284 100644 --- a/test/engine/testdata/actuation/refsite.xml +++ b/test/engine/testdata/actuation/refsite.xml @@ -13,7 +13,7 @@ - + - +