From f15b6e5e219b901a50d123f8a44fa247e7df7ff8 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Mon, 17 Jun 2024 03:06:22 -0700 Subject: [PATCH 01/52] Set orientation type in composite box. Fixes #1732. PiperOrigin-RevId: 643937520 Change-Id: I9ba9d9a8feae6815df35c5d21b0314165713c9d6 --- src/user/user_composite.cc | 1 + 1 file changed, 1 insertion(+) diff --git a/src/user/user_composite.cc b/src/user/user_composite.cc index 3ff3c280..ae1fe677 100644 --- a/src/user/user_composite.cc +++ b/src/user/user_composite.cc @@ -1156,6 +1156,7 @@ bool mjCComposite::MakeBox(mjCModel* model, mjsBody* body, char* error, int erro BoxProject(b->pos); // reorient body + b->alt.type = mjORIENTATION_ZAXIS; mjuu_copyvec(b->alt.zaxis, b->pos, 3); mjuu_normvec(b->alt.zaxis, 3); From c8cc2d51b0b9437aaabc85d399eb3b86791756bc Mon Sep 17 00:00:00 2001 From: Google DeepMind Date: Mon, 17 Jun 2024 06:11:17 -0700 Subject: [PATCH 02/52] Allow the viewer handle to return the mjModel/mjData objects from the simulation. PiperOrigin-RevId: 643982026 Change-Id: Ida5ccb851c46f7352e724798738006be6badad0a --- python/mujoco/simulate.cc | 5 +++++ python/mujoco/viewer.py | 14 ++++++++++++++ 2 files changed, 19 insertions(+) diff --git a/python/mujoco/simulate.cc b/python/mujoco/simulate.cc index 35532daa..0ddd5982 100644 --- a/python/mujoco/simulate.cc +++ b/python/mujoco/simulate.cc @@ -123,6 +123,9 @@ class SimulateWrapper { mujoco::Simulate* simulate() { return simulate_; } + py::object GetModel() const { return m_; } + py::object GetData() const { return d_; } + private: mujoco::Simulate* simulate_; std::atomic_int destroyed_ = 0; @@ -222,6 +225,8 @@ PYBIND11_MODULE(_simulate, pymodule) { .def("lock", GetIfNotNull(&mujoco::Simulate::mtx), py::call_guard(), py::return_value_policy::reference_internal) + .def_property_readonly("m", &SimulateWrapper::GetModel) + .def_property_readonly("d", &SimulateWrapper::GetData) .def_property_readonly("ctrl_noise_std", GetIfNotNull(&mujoco::Simulate::ctrl_noise_std), py::call_guard()) diff --git a/python/mujoco/viewer.py b/python/mujoco/viewer.py index 4bed131b..28c55e44 100644 --- a/python/mujoco/viewer.py +++ b/python/mujoco/viewer.py @@ -94,6 +94,20 @@ class Handle: def user_scn(self): return self._user_scn + @property + def m(self): + sim = self._sim() + if sim is not None: + return sim.m + return None + + @property + def d(self): + sim = self._sim() + if sim is not None: + return sim.d + return None + def close(self): sim = self._sim() if sim is not None: From 8e2f830fd2d079d4391541bcbb4971d12e164e88 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Tue, 18 Jun 2024 09:11:39 -0700 Subject: [PATCH 03/52] Fix type errors in compiler and tests. In preparation for a float32 build of MuJoCo. PiperOrigin-RevId: 644407674 Change-Id: I60069f0f865ef89fbf613f5a2ca0c336cc63461e --- plugin/sdf/bolt.cc | 2 +- plugin/sdf/bowl.cc | 2 +- plugin/sdf/gear.cc | 2 +- plugin/sdf/nut.cc | 2 +- plugin/sdf/sdflib.cc | 2 +- plugin/sdf/torus.cc | 2 +- src/user/user_composite.cc | 32 +-- src/user/user_composite.h | 4 +- src/user/user_flexcomp.cc | 29 +-- src/user/user_flexcomp.h | 2 +- src/user/user_mesh.cc | 128 +++++----- src/user/user_model.cc | 389 ++++++++++++++--------------- src/user/user_objects.cc | 152 ++++++----- src/user/user_objects.h | 22 +- src/user/user_util.cc | 345 ++++++++++++++++++++----- src/user/user_util.h | 42 ++-- src/xml/xml_native_reader.cc | 2 +- test/fixture.cc | 27 +- test/user/user_mesh_test.cc | 23 +- test/user/user_model_test.cc | 2 +- test/xml/xml_native_writer_test.cc | 10 +- 21 files changed, 715 insertions(+), 506 deletions(-) diff --git a/plugin/sdf/bolt.cc b/plugin/sdf/bolt.cc index 4030a164..4f052c06 100644 --- a/plugin/sdf/bolt.cc +++ b/plugin/sdf/bolt.cc @@ -148,7 +148,7 @@ void Bolt::RegisterPlugin() { delete reinterpret_cast(d->plugin_data[instance]); d->plugin_data[instance] = 0; }; - plugin.reset = +[](const mjModel* m, double* plugin_state, void* plugin_data, + plugin.reset = +[](const mjModel* m, mjtNum* plugin_state, void* plugin_data, int instance) { auto sdf = reinterpret_cast(plugin_data); sdf->Reset(); diff --git a/plugin/sdf/bowl.cc b/plugin/sdf/bowl.cc index 55b6515f..2ebbc5ac 100644 --- a/plugin/sdf/bowl.cc +++ b/plugin/sdf/bowl.cc @@ -146,7 +146,7 @@ void Bowl::RegisterPlugin() { delete reinterpret_cast(d->plugin_data[instance]); d->plugin_data[instance] = 0; }; - plugin.reset = +[](const mjModel* m, double* plugin_state, void* plugin_data, + plugin.reset = +[](const mjModel* m, mjtNum* plugin_state, void* plugin_data, int instance) { auto sdf = reinterpret_cast(plugin_data); sdf->Reset(); diff --git a/plugin/sdf/gear.cc b/plugin/sdf/gear.cc index d7b49da3..54288c21 100644 --- a/plugin/sdf/gear.cc +++ b/plugin/sdf/gear.cc @@ -232,7 +232,7 @@ void Gear::RegisterPlugin() { delete reinterpret_cast(d->plugin_data[instance]); d->plugin_data[instance] = 0; }; - plugin.reset = +[](const mjModel* m, double* plugin_state, void* plugin_data, + plugin.reset = +[](const mjModel* m, mjtNum* plugin_state, void* plugin_data, int instance) { auto sdf = reinterpret_cast(plugin_data); sdf->Reset(); diff --git a/plugin/sdf/nut.cc b/plugin/sdf/nut.cc index 448fc4a8..117df406 100644 --- a/plugin/sdf/nut.cc +++ b/plugin/sdf/nut.cc @@ -148,7 +148,7 @@ void Nut::RegisterPlugin() { delete reinterpret_cast(d->plugin_data[instance]); d->plugin_data[instance] = 0; }; - plugin.reset = +[](const mjModel* m, double* plugin_state, void* plugin_data, + plugin.reset = +[](const mjModel* m, mjtNum* plugin_state, void* plugin_data, int instance) { auto sdf = reinterpret_cast(plugin_data); sdf->Reset(); diff --git a/plugin/sdf/sdflib.cc b/plugin/sdf/sdflib.cc index 3985ca5b..1032dbc8 100644 --- a/plugin/sdf/sdflib.cc +++ b/plugin/sdf/sdflib.cc @@ -180,7 +180,7 @@ void SdfLib::RegisterPlugin() { delete reinterpret_cast(d->plugin_data[instance]); d->plugin_data[instance] = 0; }; - plugin.reset = +[](const mjModel* m, double* plugin_state, void* plugin_data, + plugin.reset = +[](const mjModel* m, mjtNum* plugin_state, void* plugin_data, int instance) { auto sdf = reinterpret_cast(plugin_data); sdf->Reset(); diff --git a/plugin/sdf/torus.cc b/plugin/sdf/torus.cc index a3c6195c..d48feb51 100644 --- a/plugin/sdf/torus.cc +++ b/plugin/sdf/torus.cc @@ -95,7 +95,7 @@ void Torus::RegisterPlugin() { delete reinterpret_cast(d->plugin_data[instance]); d->plugin_data[instance] = 0; }; - plugin.reset = +[](const mjModel* m, double* plugin_state, void* plugin_data, + plugin.reset = +[](const mjModel* m, mjtNum* plugin_state, void* plugin_data, int instance) { // do nothing }; diff --git a/src/user/user_composite.cc b/src/user/user_composite.cc index ae1fe677..891ce675 100644 --- a/src/user/user_composite.cc +++ b/src/user/user_composite.cc @@ -31,7 +31,6 @@ #include #include "cc/array_safety.h" #include "engine/engine_io.h" -#include "engine/engine_util_blas.h" #include "engine/engine_util_errmem.h" #include "engine/engine_util_misc.h" #include "user/user_model.h" @@ -428,12 +427,12 @@ bool mjCComposite::MakeParticle(mjCModel* model, mjsBody* body, char* error, int } // compute volume - std::vector volume(uservert.size()/3); - mjtNum t = 1; + std::vector volume(uservert.size()/3); + double thickness = 1; if (dim == 2 && plugin.active) { try { mjCPlugin* pplugin = static_cast(plugin.instance); - t = std::stod(pplugin->config_attribs["thickness"], nullptr); + thickness = std::stod(pplugin->config_attribs["thickness"], nullptr); } catch (const std::invalid_argument& e) { return comperr(error, "Invalid thickness attribute", error_sz); } @@ -441,9 +440,9 @@ bool mjCComposite::MakeParticle(mjCModel* model, mjsBody* body, char* error, int if (!userface.empty()) { face = mjXUtil::String2Vector(userface); for (int j=0; jquat, this_quat, 4); } else { mjuu_setvec(body->pos, length_prev, 0, 0); - mjtNum negquat[4] = {prev_quat[0], -prev_quat[1], -prev_quat[2], -prev_quat[3]}; + double negquat[4] = {prev_quat[0], -prev_quat[1], -prev_quat[2], -prev_quat[3]}; mjuu_mulquat(dquat, negquat, this_quat); mjuu_copyvec(body->quat, dquat, 4); } @@ -1091,7 +1091,7 @@ void mjCComposite::BoxProject(double* pos) { // cylinder else if (type==mjCOMPTYPE_CYLINDER) { - double L0 = mju_max(mju_abs(pos[0]), mju_abs(pos[1])); + double L0 = std::max(std::abs(pos[0]), std::abs(pos[1])); mjuu_normvec(pos, 2); pos[0] *= size[0]*L0; pos[1] *= size[1]*L0; diff --git a/src/user/user_composite.h b/src/user/user_composite.h index 88bccedc..aa16f136 100644 --- a/src/user/user_composite.h +++ b/src/user/user_composite.h @@ -107,7 +107,7 @@ class mjCComposite { std::string initial; // root boundary type std::vector uservert; // user-specified vertex positions std::string userface; // connectivity - mjtNum size[3]; // rope size (meaning depends on the shape) + double size[3]; // rope size (meaning depends on the shape) mjtCompShape curve[3]; // geometric shape // body names used in the skin @@ -137,7 +137,7 @@ class mjCComposite { private: mjsBody* AddRopeBody(mjCModel* model, mjsBody* body, int ix, int ix1); - mjsBody* AddCableBody(mjCModel* model, mjsBody* body, int ix, mjtNum normal[3], mjtNum prev_quat[4]); + mjsBody* AddCableBody(mjCModel* model, mjsBody* body, int ix, double normal[3], double prev_quat[4]); // temporary skin vectors void CopyIntoSkin(mjsSkin* skin); diff --git a/src/user/user_flexcomp.cc b/src/user/user_flexcomp.cc index 663c8b6e..c9ed36fc 100644 --- a/src/user/user_flexcomp.cc +++ b/src/user/user_flexcomp.cc @@ -14,6 +14,7 @@ #include #include +#include #include #include #include @@ -29,10 +30,7 @@ #include "cc/array_safety.h" #include "engine/engine_crossplatform.h" #include "engine/engine_resource.h" -#include "engine/engine_util_blas.h" #include "engine/engine_util_errmem.h" -#include "engine/engine_util_misc.h" -#include "engine/engine_util_spatial.h" #include "user/user_flexcomp.h" #include #include "user/user_model.h" @@ -250,12 +248,9 @@ bool mjCFlexcomp::Make(mjSpec* spec, mjsBody* body, char* error, int error_sz) { } // apply pose transform to points - mjtNum posn[3], quatn[4]; - mju_d2n(posn, pos, 3); - mju_d2n(quatn, quat, 4); for (int i=0; i < npnt; i++) { - mjtNum newp[3], oldp[3] = {point[3*i], point[3*i+1], point[3*i+2]}; - mju_trnVecPose(newp, posn, quatn, oldp); + double newp[3], oldp[3] = {point[3*i], point[3*i+1], point[3*i+2]}; + mjuu_trnVecPose(newp, pos, quat, oldp); point[3*i] = newp[0]; point[3*i+1] = newp[1]; point[3*i+2] = newp[2]; @@ -570,7 +565,7 @@ bool mjCFlexcomp::MakeGrid(char* error, int error_sz) { int quad2tri[2][3] = {{0, 1, 2}, {0, 2, 3}}; // add point - mjtNum pos[2] = {spacing[0]*(ix - 0.5*(count[0]-1)), + double pos[2] = {spacing[0]*(ix - 0.5*(count[0]-1)), spacing[1]*(iy - 0.5*(count[1]-1))}; point.push_back(pos[0]); point.push_back(pos[1]); @@ -578,8 +573,8 @@ bool mjCFlexcomp::MakeGrid(char* error, int error_sz) { // add texture coordinates, if not specified explicitly if (!hastex) { - texcoord.push_back(ix/(mjtNum)mjMAX(count[0]-1, 1)); - texcoord.push_back(iy/(mjtNum)mjMAX(count[1]-1, 1)); + texcoord.push_back(ix/(double)std::max(count[0]-1, 1)); + texcoord.push_back(iy/(double)std::max(count[1]-1, 1)); } // flip triangles if radial projection is requested @@ -713,7 +708,7 @@ void mjCFlexcomp::BoxProject(double* pos, int ix, int iy, int iz) { // cylinder else if (type==mjFCOMPTYPE_CYLINDER) { - double L0 = mjMAX(mju_abs(pos[0]), mju_abs(pos[1])); + double L0 = std::max(std::abs(pos[0]), std::abs(pos[1])); mjuu_normvec(pos, 2); pos[0] *= size[0]*L0; pos[1] *= size[1]*L0; @@ -749,8 +744,8 @@ bool mjCFlexcomp::MakeSquare(char* error, int error_sz) { }; for (int i=0; i < point.size()/3; i++) { - mjtNum* pos = point.data() + i*3; - double L0 = mjMAX(mju_abs(pos[0]), mju_abs(pos[1])); + double* pos = point.data() + i*3; + double L0 = std::max(std::abs(pos[0]), std::abs(pos[1])); mjuu_normvec(pos, 2); pos[0] *= size[0]*L0; pos[1] *= size[1]*L0; @@ -952,10 +947,10 @@ bool mjCFlexcomp::MakeMesh(mjCModel* model, char* error, int error_sz) { // copy faces element = mesh.face_; - // copy vertices, convert from float to mjtNum - point = vector (mesh.nvert()*3); + // copy vertices, convert from float to double + point = vector (mesh.nvert()*3); for (int i=0; i < mesh.nvert()*3; i++) { - point[i] = (mjtNum) mesh.vert_[i]; + point[i] = (double) mesh.vert_[i]; } return true; diff --git a/src/user/user_flexcomp.h b/src/user/user_flexcomp.h index a22780aa..d090022b 100644 --- a/src/user/user_flexcomp.h +++ b/src/user/user_flexcomp.h @@ -89,7 +89,7 @@ class mjCFlexcomp { // set by user or computed internally bool rigid; // all vertices are in parent body (all pinned) bool centered; // all vertex coordinates are (0,0,0) (nothing pinned) - std::vector point; // flex bodies/vertices + std::vector point; // flex bodies/vertices std::vector pinned; // is point pinned (true: no new body) std::vector used; // is point used by any element (false: skip) std::vector element; // flex elements diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index 3d09d181..e83660e3 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -57,11 +57,7 @@ #include "engine/engine_io.h" #include "engine/engine_plugin.h" #include "engine/engine_resource.h" -#include "engine/engine_util_blas.h" #include "engine/engine_util_errmem.h" -#include "engine/engine_util_misc.h" -#include "engine/engine_util_solve.h" -#include "engine/engine_util_spatial.h" #include "user/user_cache.h" #include "user/user_model.h" #include "user/user_objects.h" @@ -77,7 +73,7 @@ using std::string; using std::vector; // compute triangle area, surface normal, center -static mjtNum _triangle(mjtNum* normal, mjtNum* center, +static double _triangle(double* normal, double* center, const float* v1, const float* v2, const float* v3) { // center if (center) { @@ -89,10 +85,10 @@ static mjtNum _triangle(mjtNum* normal, mjtNum* center, // normal = (v2-v1) cross (v3-v1) double b[3] = { v2[0]-v1[0], v2[1]-v1[1], v2[2]-v1[2] }; double c[3] = { v3[0]-v1[0], v3[1]-v1[1], v3[2]-v1[2] }; - mju_cross(normal, b, c); + mjuu_crossvec(normal, b, c); // get length - double len = mju_norm3(normal); + double len = sqrt(mjuu_dot3(normal, normal)); // ignore small faces if (lensdf_aabb(aabb, attributes.data()); mjtNum total = aabb[3] + aabb[4] + aabb[5]; - const mjtNum n = 300; + const double n = 300; int nx, ny, nz; nx = floor(n / total * aabb[3]) + 1; ny = floor(n / total * aabb[4]) + 1; @@ -456,7 +452,7 @@ void mjCMesh::Compile(const mjVFS* vfs) { int v0 = face_[3*i+0]; int v1 = face_[3*i+1]; int v2 = face_[3*i+2]; - mjtNum normal[3]; + double normal[3]; float* vtx = vert_.data(); if (_triangle(normal, nullptr, vtx+3*v0, vtx+3*v1, vtx+3*v2)>sqrt(mjMINVAL)) { halfedge_.push_back(std::pair(v0, v1)); @@ -552,7 +548,7 @@ void mjCMesh::SetBoundingVolume(int faceid) { node->contype = 1; node->pos = center_ + 3*faceid; node->quat = NULL; - mjtNum face_aamm[6] = {1E+10, 1E+10, 1E+10, -1E+10, -1E+10, -1E+10}; + double face_aamm[6] = {1E+10, 1E+10, 1E+10, -1E+10, -1E+10, -1E+10}; for (int j=0; j<3; j++) { int vertid = face_[3*faceid+j]; face_aamm[0] = mjMIN(face_aamm[0], vert_[3*vertid+0]); @@ -1263,15 +1259,15 @@ void mjCMesh::ApplyTransformations() { // rotate if (refquat[0]!=1 || refquat[1]!=0 || refquat[2]!=0 || refquat[3]!=0) { // prepare rotation - mjtNum quat[4] = {refquat[0], refquat[1], refquat[2], refquat[3]}; - mjtNum mat[9]; - mju_normalize4(quat); - mju_quat2Mat(mat, quat); + double quat[4] = {refquat[0], refquat[1], refquat[2], refquat[3]}; + double mat[9]; + mjuu_normvec(quat, 4); + mjuu_quat2mat(mat, quat); // process vertices for (int i=0; i < nvert(); i++) { - mjtNum p1[3], p0[3] = {vert_[3*i], vert_[3*i+1], vert_[3*i+2]}; - mju_mulMatTVec3(p1, mat, p0); + double p1[3], p0[3] = {vert_[3*i], vert_[3*i+1], vert_[3*i+2]}; + mjuu_mulvecmatT(p1, p0, mat); vert_[3*i] = (float) p1[0]; vert_[3*i+1] = (float) p1[1]; vert_[3*i+2] = (float) p1[2]; @@ -1279,8 +1275,8 @@ void mjCMesh::ApplyTransformations() { // process normals for (int i=0; i < nnormal(); i++) { - mjtNum n1[3], n0[3] = {normal_[3*i], normal_[3*i+1], normal_[3*i+2]}; - mju_mulMatTVec3(n1, mat, n0); + double n1[3], n0[3] = {normal_[3*i], normal_[3*i+1], normal_[3*i+2]}; + mjuu_mulvecmatT(n1, n0, mat); normal_[3*i] = (float) n1[0]; normal_[3*i+1] = (float) n1[1]; normal_[3*i+2] = (float) n1[2]; @@ -1449,13 +1445,13 @@ void mjCMesh::Process() { inert[5] = -P[5]; // get quaternion and diagonal inertia - mjtNum eigval[3], eigvec[9], quattmp[4]; - mjtNum full[9] = { + double eigval[3], eigvec[9], quattmp[4]; + double full[9] = { inert[0], inert[3], inert[4], inert[3], inert[1], inert[5], inert[4], inert[5], inert[2] }; - mju_eig3(eigval, eigvec, quattmp, full); + mjuu_eig3(eigval, eigvec, quattmp, full); // check eigval - SHOULD NOT OCCUR if (eigval[2]<=0) { @@ -1479,12 +1475,12 @@ void mjCMesh::Process() { // if volume was valid, copy volume quat to shell and stop, // otherwise use shell quat for coordinate transformations if (type==mjINERTIA_SHELL && validvolume_>0) { - mju_copy4(GetQuatPtr(type), GetQuatPtr(mjINERTIA_VOLUME)); + mjuu_copyvec(GetQuatPtr(type), GetQuatPtr(mjINERTIA_VOLUME), 4); continue; } // rotate vertices and normals into axis-aligned frame - mju_copy4(GetQuatPtr(type), quattmp); + mjuu_copyvec(GetQuatPtr(type), quattmp, 4); double neg[4] = {quattmp[0], -quattmp[1], -quattmp[2], -quattmp[3]}; double mat[9]; mjuu_quat2mat(mat, neg); @@ -1798,16 +1794,16 @@ void mjCMesh::MakeNormal(void) { } // get triangle edges - mjtNum vec01[3], vec02[3]; + double vec01[3], vec02[3]; for (int j=0; j<3; j++) { vec01[j] = vert_[3*vertid[1]+j] - vert_[3*vertid[0]+j]; vec02[j] = vert_[3*vertid[2]+j] - vert_[3*vertid[0]+j]; } // compute face normal - mjtNum nrm[3]; - mju_cross(nrm, vec01, vec02); - mjtNum area = mju_normalize3(nrm); + double nrm[3]; + mjuu_crossvec(nrm, vec01, vec02); + double area = mjuu_normvec(nrm, 3); // add normal to each vertex with weight = area for (int j=0; j<3; j++) { @@ -1833,25 +1829,25 @@ void mjCMesh::MakeNormal(void) { } // get triangle edges - mjtNum vec01[3], vec02[3]; + double vec01[3], vec02[3]; for (int j=0; j<3; j++) { vec01[j] = vert_[3*vertid[1]+j] - vert_[3*vertid[0]+j]; vec02[j] = vert_[3*vertid[2]+j] - vert_[3*vertid[0]+j]; } // compute face normal - mjtNum nrm[3]; - mju_cross(nrm, vec01, vec02); - mjtNum area = mju_normalize3(nrm); + double nrm[3]; + mjuu_crossvec(nrm, vec01, vec02); + double area = mjuu_normvec(nrm, 3); // compare to vertex normal, subtract contribution if dot product too small for (int j=0; j<3; j++) { // normalized vertex normal - mjtNum vnrm[3] = {normal_[3*vertid[j]], normal_[3*vertid[j]+1], normal_[3*vertid[j]+2]}; - mju_normalize3(vnrm); + double vnrm[3] = {normal_[3*vertid[j]], normal_[3*vertid[j]+1], normal_[3*vertid[j]+2]}; + mjuu_normvec(vnrm, 3); // dot too small: remove - if (mju_dot3(nrm, vnrm)<0.8) { + if (mjuu_dot3(nrm, vnrm)<0.8) { for (int k=0; k<3; k++) { nremove[3*vertid[j]+k] += nrm[k]*area; } @@ -1901,28 +1897,28 @@ void mjCMesh::MakeCenter(void) { int* vertid = face_.data() + 3*i; // get triangle edges - mjtNum a[3], b[3]; + double a[3], b[3]; for (int j=0; j<3; j++) { a[j] = vert_[3*vertid[0]+j] - vert_[3*vertid[2]+j]; b[j] = vert_[3*vertid[1]+j] - vert_[3*vertid[2]+j]; } // compute face normal - mjtNum nrm[3]; - mju_cross(nrm, a, b); + double nrm[3]; + mjuu_crossvec(nrm, a, b); // compute circumradius - mjtNum norm_a_2 = mju_dot3(a, a); - mjtNum norm_b_2 = mju_dot3(b, b); - mjtNum area = mju_norm3(nrm); + double norm_a_2 = mjuu_dot3(a, a); + double norm_b_2 = mjuu_dot3(b, b); + double area = mjuu_normvec(nrm, 3); // compute circumcenter - mjtNum res[3], vec[3] = { + double res[3], vec[3] = { norm_a_2 * b[0] - norm_b_2 * a[0], norm_a_2 * b[1] - norm_b_2 * a[1], norm_a_2 * b[2] - norm_b_2 * a[2] }; - mju_cross(res, vec, nrm); + mjuu_crossvec(res, vec, nrm); center_[3*i+0] = res[0]/(2*area*area) + vert_[3*vertid[2]+0]; center_[3*i+1] = res[1]/(2*area*area) + vert_[3*vertid[2]+1]; center_[3*i+2] = res[2]/(2*area*area) + vert_[3*vertid[2]+2]; @@ -2200,13 +2196,13 @@ void mjCSkin::Compile(const mjVFS* vfs) { // normalize bindquat for (int i=0; i < nbone; i++) { - mjtNum quat[4] = { - (mjtNum)bindquat_[4*i], - (mjtNum)bindquat_[4*i+1], - (mjtNum)bindquat_[4*i+2], - (mjtNum)bindquat_[4*i+3] + double quat[4] = { + (double)bindquat_[4*i], + (double)bindquat_[4*i+1], + (double)bindquat_[4*i+2], + (double)bindquat_[4*i+3] }; - mju_normalize4(quat); + mjuu_normvec(quat, 4); bindquat_[4*i] = (float) quat[0]; bindquat_[4*i+1] = (float) quat[1]; @@ -2562,17 +2558,17 @@ void mjCFlex::Compile(const mjVFS* vfs) { } // compute global vertex positions - vertxpos = vector (3*nvert); + vertxpos = vector (3*nvert); for (int i=0; i < nvert; i++) { // get body id, set vertxpos = body.xpos0 int b = rigid ? vertbodyid[0] : vertbodyid[i]; - mju_copy3(vertxpos.data()+3*i, model->Bodies()[b]->xpos0); + mjuu_copyvec(vertxpos.data()+3*i, model->Bodies()[b]->xpos0, 3); // add vertex offset within body if not centered if (!centered) { - mjtNum offset[3]; - mju_rotVecQuat(offset, vert_.data()+3*i, model->Bodies()[b]->xquat0); - mju_addTo3(vertxpos.data()+3*i, offset); + double offset[3]; + mjuu_rotVecQuat(offset, vert_.data()+3*i, model->Bodies()[b]->xquat0); + mjuu_addtovec(vertxpos.data()+3*i, offset, 3); } } @@ -2581,18 +2577,18 @@ void mjCFlex::Compile(const mjVFS* vfs) { if (dim==3) { for (int e=0; e0) { + double nrm[3]; + mjuu_crossvec(nrm, v01, v02); + if (mjuu_dot3(nrm, v03)>0) { // flip orientation int tmp = elem_[e*(dim+1)+1]; elem_[e*(dim+1)+1] = elem_[e*(dim+1)+2]; @@ -2672,9 +2668,9 @@ void mjCFlex::CreateBVH(void) { } // compute min and max along each global axis - mjtNum xmin[3], xmax[3]; - mju_copy3(xmin, vertxpos.data() + 3*edata[0]); - mju_copy3(xmax, vertxpos.data() + 3*edata[0]); + double xmin[3], xmax[3]; + mjuu_copyvec(xmin, vertxpos.data() + 3*edata[0], 3); + mjuu_copyvec(xmax, vertxpos.data() + 3*edata[0], 3); for (int i=1; i <= dim; i++) { for (int j=0; j<3; j++) { xmin[j] = mjMIN(xmin[j], vertxpos[3*edata[i]+j]); diff --git a/src/user/user_model.cc b/src/user/user_model.cc index a8a0d77c..b92232e8 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -15,6 +15,7 @@ #include "user/user_model.h" #include +#include #include #include #include @@ -38,7 +39,6 @@ #include "engine/engine_plugin.h" #include "engine/engine_setconst.h" #include "engine/engine_support.h" -#include "engine/engine_util_blas.h" #include "engine/engine_util_errmem.h" #include "engine/engine_util_misc.h" #include "user/user_api.h" @@ -69,15 +69,6 @@ using std::vector; #endif -// copy real-valued vector -template -static void copyvec(T1* dest, T2* src, int n) { - for (int i=0; idof_invweight0[adr+i]; } - inertia = ((mjtNum)ndim) / mju_max(mjMINVAL, inertia); + inertia = ((mjtNum)ndim) / std::max(mjMINVAL, inertia); // compute stiffness and damping (same as solref computation) - mjtNum stiffness = inertia / mju_max(mjMINVAL, timeconst*timeconst*dampratio*dampratio); - mjtNum damping = 2 * inertia / mju_max(mjMINVAL, timeconst); + mjtNum stiffness = inertia / std::max(mjMINVAL, timeconst*timeconst*dampratio*dampratio); + mjtNum damping = 2 * inertia / std::max(mjMINVAL, timeconst); // assign m->jnt_stiffness[n] = stiffness; @@ -1771,14 +1762,14 @@ void mjCModel::CopyTree(mjModel* m) { m->body_dofadr[i] = (pb->dofnum ? dofadr : -1); m->body_geomnum[i] = (int)pb->geoms.size(); m->body_geomadr[i] = (!pb->geoms.empty() ? pb->geoms[0]->id : -1); - copyvec(m->body_pos+3*i, pb->pos, 3); - copyvec(m->body_quat+4*i, pb->quat, 4); - copyvec(m->body_ipos+3*i, pb->ipos, 3); - copyvec(m->body_iquat+4*i, pb->iquat, 4); + mjuu_copyvec(m->body_pos+3*i, pb->pos, 3); + mjuu_copyvec(m->body_quat+4*i, pb->quat, 4); + mjuu_copyvec(m->body_ipos+3*i, pb->ipos, 3); + mjuu_copyvec(m->body_iquat+4*i, pb->iquat, 4); m->body_mass[i] = (mjtNum)pb->mass; - copyvec(m->body_inertia+3*i, pb->inertia, 3); + mjuu_copyvec(m->body_inertia+3*i, pb->inertia, 3); m->body_gravcomp[i] = pb->gravcomp; - copyvec(m->body_user+nuser_body*i, pb->get_userdata().data(), nuser_body); + mjuu_copyvec(m->body_user+nuser_body*i, pb->get_userdata().data(), nuser_body); m->body_contype[i] = pb->contype; m->body_conaffinity[i] = pb->conaffinity; @@ -1854,23 +1845,23 @@ void mjCModel::CopyTree(mjModel* m) { m->jnt_qposadr[jid] = qposadr; m->jnt_dofadr[jid] = dofadr; m->jnt_bodyid[jid] = pj->body->id; - copyvec(m->jnt_pos+3*jid, pj->pos, 3); - copyvec(m->jnt_axis+3*jid, pj->axis, 3); + mjuu_copyvec(m->jnt_pos+3*jid, pj->pos, 3); + mjuu_copyvec(m->jnt_axis+3*jid, pj->axis, 3); m->jnt_stiffness[jid] = (mjtNum)pj->stiffness; - copyvec(m->jnt_range+2*jid, pj->range, 2); - copyvec(m->jnt_actfrcrange+2*jid, pj->actfrcrange, 2); - copyvec(m->jnt_solref+mjNREF*jid, pj->solref_limit, mjNREF); - copyvec(m->jnt_solimp+mjNIMP*jid, pj->solimp_limit, mjNIMP); + mjuu_copyvec(m->jnt_range+2*jid, pj->range, 2); + mjuu_copyvec(m->jnt_actfrcrange+2*jid, pj->actfrcrange, 2); + mjuu_copyvec(m->jnt_solref+mjNREF*jid, pj->solref_limit, mjNREF); + mjuu_copyvec(m->jnt_solimp+mjNIMP*jid, pj->solimp_limit, mjNIMP); m->jnt_margin[jid] = (mjtNum)pj->margin; - copyvec(m->jnt_user+nuser_jnt*jid, pj->get_userdata().data(), nuser_jnt); + mjuu_copyvec(m->jnt_user+nuser_jnt*jid, pj->get_userdata().data(), nuser_jnt); // not simple if: rotation already found, or pos not zero, or mis-aligned axis if (rotfound || !IsNullPose(m->jnt_pos+3*jid, NULL) || ((pj->type==mjJNT_HINGE || pj->type==mjJNT_SLIDE) && - ((mju_abs(pj->axis[0])>mjEPS) + - (mju_abs(pj->axis[1])>mjEPS) + - (mju_abs(pj->axis[2])>mjEPS)) > 1)) { + ((std::abs(pj->axis[0])>mjEPS) + + (std::abs(pj->axis[1])>mjEPS) + + (std::abs(pj->axis[2])>mjEPS)) > 1)) { m->body_simple[i] = 0; } @@ -1882,9 +1873,9 @@ void mjCModel::CopyTree(mjModel* m) { // set qpos0 and qpos_spring, check type switch (pj->type) { case mjJNT_FREE: - copyvec(m->qpos0+qposadr, pb->pos, 3); - copyvec(m->qpos0+qposadr+3, pb->quat, 4); - mju_copy(m->qpos_spring+qposadr, m->qpos0+qposadr, 7); + mjuu_copyvec(m->qpos0+qposadr, pb->pos, 3); + mjuu_copyvec(m->qpos0+qposadr+3, pb->quat, 4); + mjuu_copyvec(m->qpos_spring+qposadr, m->qpos0+qposadr, 7); break; case mjJNT_BALL: @@ -1892,7 +1883,7 @@ void mjCModel::CopyTree(mjModel* m) { m->qpos0[qposadr+1] = 0; m->qpos0[qposadr+2] = 0; m->qpos0[qposadr+3] = 0; - mju_copy4(m->qpos_spring+qposadr, m->qpos0+qposadr); + mjuu_copyvec(m->qpos_spring+qposadr, m->qpos0+qposadr, 4); break; case mjJNT_SLIDE: @@ -1910,8 +1901,8 @@ void mjCModel::CopyTree(mjModel* m) { // set attributes m->dof_bodyid[dofadr] = pb->id; m->dof_jntid[dofadr] = jid; - copyvec(m->dof_solref+mjNREF*dofadr, pj->solref_friction, mjNREF); - copyvec(m->dof_solimp+mjNIMP*dofadr, pj->solimp_friction, mjNIMP); + mjuu_copyvec(m->dof_solref+mjNREF*dofadr, pj->solref_friction, mjNREF); + mjuu_copyvec(m->dof_solimp+mjNIMP*dofadr, pj->solimp_friction, mjNIMP); m->dof_frictionloss[dofadr] = (mjtNum)pj->frictionloss; m->dof_armature[dofadr] = (mjtNum)pj->armature; m->dof_damping[dofadr] = (mjtNum)pj->damping; @@ -1962,19 +1953,19 @@ void mjCModel::CopyTree(mjModel* m) { m->geom_matid[gid] = pg->matid; m->geom_group[gid] = pg->group; m->geom_priority[gid] = pg->priority; - copyvec(m->geom_size+3*gid, pg->size, 3); - copyvec(m->geom_aabb+6*gid, pg->aabb, 6); - copyvec(m->geom_pos+3*gid, pg->pos, 3); - copyvec(m->geom_quat+4*gid, pg->quat, 4); - copyvec(m->geom_friction+3*gid, pg->friction, 3); + mjuu_copyvec(m->geom_size+3*gid, pg->size, 3); + mjuu_copyvec(m->geom_aabb+6*gid, pg->aabb, 6); + mjuu_copyvec(m->geom_pos+3*gid, pg->pos, 3); + mjuu_copyvec(m->geom_quat+4*gid, pg->quat, 4); + mjuu_copyvec(m->geom_friction+3*gid, pg->friction, 3); m->geom_solmix[gid] = (mjtNum)pg->solmix; - copyvec(m->geom_solref+mjNREF*gid, pg->solref, mjNREF); - copyvec(m->geom_solimp+mjNIMP*gid, pg->solimp, mjNIMP); + mjuu_copyvec(m->geom_solref+mjNREF*gid, pg->solref, mjNREF); + mjuu_copyvec(m->geom_solimp+mjNIMP*gid, pg->solimp, mjNIMP); m->geom_margin[gid] = (mjtNum)pg->margin; m->geom_gap[gid] = (mjtNum)pg->gap; - copyvec(m->geom_fluid+mjNFLUID*gid, pg->fluid, mjNFLUID); - copyvec(m->geom_user+nuser_geom*gid, pg->get_userdata().data(), nuser_geom); - copyvec(m->geom_rgba+4*gid, pg->rgba, 4); + mjuu_copyvec(m->geom_fluid+mjNFLUID*gid, pg->fluid, mjNFLUID); + mjuu_copyvec(m->geom_user+nuser_geom*gid, pg->get_userdata().data(), nuser_geom); + mjuu_copyvec(m->geom_rgba+4*gid, pg->rgba, 4); // determine sameframe if (IsNullPose(m->geom_pos+3*gid, m->geom_quat+4*gid)) { @@ -2006,11 +1997,11 @@ void mjCModel::CopyTree(mjModel* m) { m->site_bodyid[sid] = ps->body->id; m->site_matid[sid] = ps->matid; m->site_group[sid] = ps->group; - copyvec(m->site_size+3*sid, ps->size, 3); - copyvec(m->site_pos+3*sid, ps->pos, 3); - copyvec(m->site_quat+4*sid, ps->quat, 4); - copyvec(m->site_user+nuser_site*sid, ps->userdata_.data(), nuser_site); - copyvec(m->site_rgba+4*sid, ps->rgba, 4); + mjuu_copyvec(m->site_size+3*sid, ps->size, 3); + mjuu_copyvec(m->site_pos+3*sid, ps->pos, 3); + mjuu_copyvec(m->site_quat+4*sid, ps->quat, 4); + mjuu_copyvec(m->site_user+nuser_site*sid, ps->userdata_.data(), nuser_site); + mjuu_copyvec(m->site_rgba+4*sid, ps->rgba, 4); // determine sameframe if (IsNullPose(m->site_pos+3*sid, m->site_quat+4*sid)) { @@ -2038,15 +2029,15 @@ void mjCModel::CopyTree(mjModel* m) { m->cam_bodyid[cid] = pc->body->id; m->cam_mode[cid] = pc->mode; m->cam_targetbodyid[cid] = pc->targetbodyid; - copyvec(m->cam_pos+3*cid, pc->pos, 3); - copyvec(m->cam_quat+4*cid, pc->quat, 4); + mjuu_copyvec(m->cam_pos+3*cid, pc->pos, 3); + mjuu_copyvec(m->cam_quat+4*cid, pc->quat, 4); m->cam_orthographic[cid] = pc->orthographic; m->cam_fovy[cid] = (mjtNum)pc->fovy; m->cam_ipd[cid] = (mjtNum)pc->ipd; - copyvec(m->cam_resolution+2*cid, pc->resolution, 2); - copyvec(m->cam_sensorsize+2*cid, pc->sensor_size, 2); - copyvec(m->cam_intrinsic+4*cid, pc->intrinsic, 4); - copyvec(m->cam_user+nuser_cam*cid, pc->get_userdata().data(), nuser_cam); + mjuu_copyvec(m->cam_resolution+2*cid, pc->resolution, 2); + mjuu_copyvec(m->cam_sensorsize+2*cid, pc->sensor_size, 2); + mjuu_copyvec(m->cam_intrinsic+4*cid, pc->intrinsic, 4); + mjuu_copyvec(m->cam_user+nuser_cam*cid, pc->get_userdata().data(), nuser_cam); } // loop over lights for this body @@ -2062,15 +2053,15 @@ void mjCModel::CopyTree(mjModel* m) { m->light_directional[lid] = (mjtByte)pl->directional; m->light_castshadow[lid] = (mjtByte)pl->castshadow; m->light_active[lid] = (mjtByte)pl->active; - copyvec(m->light_pos+3*lid, pl->pos, 3); - copyvec(m->light_dir+3*lid, pl->dir, 3); + mjuu_copyvec(m->light_pos+3*lid, pl->pos, 3); + mjuu_copyvec(m->light_dir+3*lid, pl->dir, 3); m->light_bulbradius[lid] = pl->bulbradius; - copyvec(m->light_attenuation+3*lid, pl->attenuation, 3); + mjuu_copyvec(m->light_attenuation+3*lid, pl->attenuation, 3); m->light_cutoff[lid] = pl->cutoff; m->light_exponent[lid] = pl->exponent; - copyvec(m->light_ambient+3*lid, pl->ambient, 3); - copyvec(m->light_diffuse+3*lid, pl->diffuse, 3); - copyvec(m->light_specular+3*lid, pl->specular, 3); + mjuu_copyvec(m->light_ambient+3*lid, pl->ambient, 3); + mjuu_copyvec(m->light_diffuse+3*lid, pl->diffuse, 3); + mjuu_copyvec(m->light_specular+3*lid, pl->specular, 3); } } @@ -2210,9 +2201,9 @@ void mjCModel::CopyObjects(mjModel* m) { m->mesh_graphadr[i] = (pme->szgraph() ? graph_adr : -1); m->mesh_bvhnum[i] = pme->tree().nbvh; m->mesh_bvhadr[i] = pme->tree().nbvh ? bvh_adr : -1; - copyvec(&m->mesh_scale[3 * i], pme->get_scale(), 3); - copyvec(&m->mesh_pos[3 * i], pme->GetOffsetPosPtr(), 3); - copyvec(&m->mesh_quat[4 * i], pme->GetOffsetQuatPtr(), 4); + mjuu_copyvec(&m->mesh_scale[3 * i], pme->get_scale(), 3); + mjuu_copyvec(&m->mesh_pos[3 * i], pme->GetOffsetPosPtr(), 3); + mjuu_copyvec(&m->mesh_quat[4 * i], pme->GetOffsetQuatPtr(), 4); // copy vertices, normals, faces, texcoords, aux data pme->CopyVert(m->mesh_vert + 3*vert_adr); @@ -2268,13 +2259,13 @@ void mjCModel::CopyObjects(mjModel* m) { m->flex_group[i] = pfl->group; m->flex_priority[i] = pfl->priority; m->flex_solmix[i] = (mjtNum)pfl->solmix; - copyvec(m->flex_solref + mjNREF * i, pfl->solref, mjNREF); - copyvec(m->flex_solimp + mjNIMP * i, pfl->solimp, mjNIMP); + mjuu_copyvec(m->flex_solref + mjNREF * i, pfl->solref, mjNREF); + mjuu_copyvec(m->flex_solimp + mjNIMP * i, pfl->solimp, mjNIMP); m->flex_radius[i] = (mjtNum)pfl->radius; - copyvec(m->flex_friction + 3 * i, pfl->friction, 3); + mjuu_copyvec(m->flex_friction + 3 * i, pfl->friction, 3); m->flex_margin[i] = (mjtNum)pfl->margin; m->flex_gap[i] = (mjtNum)pfl->gap; - copyvec(m->flex_rgba + 4 * i, pfl->rgba, 4); + mjuu_copyvec(m->flex_rgba + 4 * i, pfl->rgba, 4); // set fields: mesh-like m->flex_dim[i] = pfl->dim; @@ -2338,7 +2329,7 @@ void mjCModel::CopyObjects(mjModel* m) { // copy or set vert if (pfl->centered) { - mju_zero(m->flex_vert + 3*vert_adr, 3*pfl->nvert); + mjuu_zerovec(m->flex_vert + 3*vert_adr, 3*pfl->nvert); } else { memcpy(m->flex_vert + 3*vert_adr, pfl->vert_.data(), 3*pfl->nvert*sizeof(mjtNum)); @@ -2393,7 +2384,7 @@ void mjCModel::CopyObjects(mjModel* m) { // set fields m->skin_matid[i] = psk->matid; m->skin_group[i] = psk->group; - copyvec(m->skin_rgba+4*i, psk->rgba, 4); + mjuu_copyvec(m->skin_rgba+4*i, psk->rgba, 4); m->skin_inflate[i] = psk->inflate; m->skin_vertadr[i] = vert_adr; m->skin_vertnum[i] = psk->get_vert().size()/3; @@ -2448,7 +2439,7 @@ void mjCModel::CopyObjects(mjModel* m) { mjCHField* phf = hfields_[i]; // set fields - copyvec(m->hfield_size+4*i, phf->size, 4); + mjuu_copyvec(m->hfield_size+4*i, phf->size, 4); m->hfield_nrow[i] = phf->nrow; m->hfield_ncol[i] = phf->ncol; m->hfield_adr[i] = data_adr; @@ -2487,14 +2478,14 @@ void mjCModel::CopyObjects(mjModel* m) { // set fields m->mat_texid[i] = pmat->texid; m->mat_texuniform[i] = pmat->texuniform; - copyvec(m->mat_texrepeat+2*i, pmat->texrepeat, 2); + mjuu_copyvec(m->mat_texrepeat+2*i, pmat->texrepeat, 2); m->mat_emission[i] = pmat->emission; m->mat_specular[i] = pmat->specular; m->mat_shininess[i] = pmat->shininess; m->mat_reflectance[i] = pmat->reflectance; m->mat_metallic[i] = pmat->metallic; m->mat_roughness[i] = pmat->roughness; - copyvec(m->mat_rgba+4*i, pmat->rgba, 4); + mjuu_copyvec(m->mat_rgba+4*i, pmat->rgba, 4); } // geom pairs to include @@ -2503,12 +2494,12 @@ void mjCModel::CopyObjects(mjModel* m) { m->pair_geom1[i] = pairs_[i]->geom1->id; m->pair_geom2[i] = pairs_[i]->geom2->id; m->pair_signature[i] = pairs_[i]->signature; - copyvec(m->pair_solref+mjNREF*i, pairs_[i]->solref, mjNREF); - copyvec(m->pair_solreffriction+mjNREF*i, pairs_[i]->solreffriction, mjNREF); - copyvec(m->pair_solimp+mjNIMP*i, pairs_[i]->solimp, mjNIMP); + mjuu_copyvec(m->pair_solref+mjNREF*i, pairs_[i]->solref, mjNREF); + mjuu_copyvec(m->pair_solreffriction+mjNREF*i, pairs_[i]->solreffriction, mjNREF); + mjuu_copyvec(m->pair_solimp+mjNIMP*i, pairs_[i]->solimp, mjNIMP); m->pair_margin[i] = (mjtNum)pairs_[i]->margin; m->pair_gap[i] = (mjtNum)pairs_[i]->gap; - copyvec(m->pair_friction+5*i, pairs_[i]->friction, 5); + mjuu_copyvec(m->pair_friction+5*i, pairs_[i]->friction, 5); } // body pairs to exclude @@ -2526,9 +2517,9 @@ void mjCModel::CopyObjects(mjModel* m) { m->eq_obj1id[i] = peq->obj1id; m->eq_obj2id[i] = peq->obj2id; m->eq_active0[i] = peq->active; - copyvec(m->eq_solref+mjNREF*i, peq->solref, mjNREF); - copyvec(m->eq_solimp+mjNIMP*i, peq->solimp, mjNIMP); - copyvec(m->eq_data+mjNEQDATA*i, peq->data, mjNEQDATA); + mjuu_copyvec(m->eq_solref+mjNREF*i, peq->solref, mjNREF); + mjuu_copyvec(m->eq_solimp+mjNIMP*i, peq->solimp, mjNIMP); + mjuu_copyvec(m->eq_data+mjNEQDATA*i, peq->data, mjNEQDATA); } // tendons and wraps @@ -2544,10 +2535,10 @@ void mjCModel::CopyObjects(mjModel* m) { m->tendon_group[i] = pte->group; m->tendon_limited[i] = (mjtByte)pte->is_limited(); m->tendon_width[i] = (mjtNum)pte->width; - copyvec(m->tendon_solref_lim+mjNREF*i, pte->solref_limit, mjNREF); - copyvec(m->tendon_solimp_lim+mjNIMP*i, pte->solimp_limit, mjNIMP); - copyvec(m->tendon_solref_fri+mjNREF*i, pte->solref_friction, mjNREF); - copyvec(m->tendon_solimp_fri+mjNIMP*i, pte->solimp_friction, mjNIMP); + mjuu_copyvec(m->tendon_solref_lim+mjNREF*i, pte->solref_limit, mjNREF); + mjuu_copyvec(m->tendon_solimp_lim+mjNIMP*i, pte->solimp_limit, mjNIMP); + mjuu_copyvec(m->tendon_solref_fri+mjNREF*i, pte->solref_friction, mjNREF); + mjuu_copyvec(m->tendon_solimp_fri+mjNIMP*i, pte->solimp_friction, mjNIMP); m->tendon_range[2*i] = (mjtNum)pte->range[0]; m->tendon_range[2*i+1] = (mjtNum)pte->range[1]; m->tendon_margin[i] = (mjtNum)pte->margin; @@ -2556,8 +2547,8 @@ void mjCModel::CopyObjects(mjModel* m) { m->tendon_frictionloss[i] = (mjtNum)pte->frictionloss; m->tendon_lengthspring[2*i] = (mjtNum)pte->springlength[0]; m->tendon_lengthspring[2*i+1] = (mjtNum)pte->springlength[1]; - copyvec(m->tendon_user+nuser_tendon*i, pte->get_userdata().data(), nuser_tendon); - copyvec(m->tendon_rgba+4*i, pte->rgba, 4); + mjuu_copyvec(m->tendon_user+nuser_tendon*i, pte->get_userdata().data(), nuser_tendon); + mjuu_copyvec(m->tendon_rgba+4*i, pte->rgba, 4); // set wraps for (int j=0; j<(int)pte->path.size(); j++) { @@ -2597,15 +2588,15 @@ void mjCModel::CopyObjects(mjModel* m) { m->actuator_actlimited[i] = (mjtByte)pac->is_actlimited(); m->actuator_actearly[i] = pac->actearly; m->actuator_cranklength[i] = (mjtNum)pac->cranklength; - copyvec(m->actuator_gear + 6*i, pac->gear, 6); - copyvec(m->actuator_dynprm + mjNDYN*i, pac->dynprm, mjNDYN); - copyvec(m->actuator_gainprm + mjNGAIN*i, pac->gainprm, mjNGAIN); - copyvec(m->actuator_biasprm + mjNBIAS*i, pac->biasprm, mjNBIAS); - copyvec(m->actuator_ctrlrange + 2*i, pac->ctrlrange, 2); - copyvec(m->actuator_forcerange + 2*i, pac->forcerange, 2); - copyvec(m->actuator_actrange + 2*i, pac->actrange, 2); - copyvec(m->actuator_lengthrange + 2*i, pac->lengthrange, 2); - copyvec(m->actuator_user+nuser_actuator*i, pac->get_userdata().data(), nuser_actuator); + mjuu_copyvec(m->actuator_gear + 6*i, pac->gear, 6); + mjuu_copyvec(m->actuator_dynprm + mjNDYN*i, pac->dynprm, mjNDYN); + mjuu_copyvec(m->actuator_gainprm + mjNGAIN*i, pac->gainprm, mjNGAIN); + mjuu_copyvec(m->actuator_biasprm + mjNBIAS*i, pac->biasprm, mjNBIAS); + mjuu_copyvec(m->actuator_ctrlrange + 2*i, pac->ctrlrange, 2); + mjuu_copyvec(m->actuator_forcerange + 2*i, pac->forcerange, 2); + mjuu_copyvec(m->actuator_actrange + 2*i, pac->actrange, 2); + mjuu_copyvec(m->actuator_lengthrange + 2*i, pac->lengthrange, 2); + mjuu_copyvec(m->actuator_user+nuser_actuator*i, pac->get_userdata().data(), nuser_actuator); } // sensors @@ -2625,7 +2616,7 @@ void mjCModel::CopyObjects(mjModel* m) { m->sensor_dim[i] = psen->dim; m->sensor_cutoff[i] = (mjtNum)psen->cutoff; m->sensor_noise[i] = (mjtNum)psen->noise; - copyvec(m->sensor_user+nuser_sensor*i, psen->get_userdata().data(), nuser_sensor); + mjuu_copyvec(m->sensor_user+nuser_sensor*i, psen->get_userdata().data(), nuser_sensor); // calculate address and advance m->sensor_adr[i] = adr; @@ -2690,34 +2681,34 @@ void mjCModel::CopyObjects(mjModel* m) { for (int i=0; ikey_time[i] = (mjtNum)keys_[i]->time; - copyvec(m->key_qpos+i*nq, keys_[i]->qpos_.data(), nq); - copyvec(m->key_qvel+i*nv, keys_[i]->qvel_.data(), nv); + mjuu_copyvec(m->key_qpos+i*nq, keys_[i]->qpos_.data(), nq); + mjuu_copyvec(m->key_qvel+i*nv, keys_[i]->qvel_.data(), nv); if (na) { - copyvec(m->key_act+i*na, keys_[i]->act_.data(), na); + mjuu_copyvec(m->key_act+i*na, keys_[i]->act_.data(), na); } if (nmocap) { - copyvec(m->key_mpos + i*3*nmocap, keys_[i]->mpos_.data(), 3*nmocap); - copyvec(m->key_mquat + i*4*nmocap, keys_[i]->mquat_.data(), 4*nmocap); + mjuu_copyvec(m->key_mpos + i*3*nmocap, keys_[i]->mpos_.data(), 3*nmocap); + mjuu_copyvec(m->key_mquat + i*4*nmocap, keys_[i]->mquat_.data(), 4*nmocap); } // normalize quaternions in m->key_qpos for (int j=0; jnjnt; j++) { if (m->jnt_type[j]==mjJNT_BALL || m->jnt_type[j]==mjJNT_FREE) { - mju_normalize4(m->key_qpos+i*nq+m->jnt_qposadr[j]+3*(m->jnt_type[j]==mjJNT_FREE)); + mjuu_normvec(m->key_qpos+i*nq+m->jnt_qposadr[j]+3*(m->jnt_type[j]==mjJNT_FREE), 4); } } // normalize quaternions in m->key_mquat for (int j=0; jkey_mquat+i*4*nmocap+4*j); + mjuu_normvec(m->key_mquat+i*4*nmocap+4*j, 4); } - copyvec(m->key_ctrl+i*nu, keys_[i]->ctrl_.data(), nu); + mjuu_copyvec(m->key_ctrl+i*nu, keys_[i]->ctrl_.data(), nu); } // save qpos0 in user model (to recognize changed key_qpos in write) qpos0.resize(nq); - mju_copy(qpos0.data(), m->qpos0, nq); + mjuu_copyvec(qpos0.data(), m->qpos0, nq); } @@ -2727,17 +2718,17 @@ void mjCModel::SaveState(const mjData* d) { for (auto joint : joints_) { switch (joint->type) { case mjJNT_FREE: - mju_copy(joint->qpos, d->qpos + joint->qposadr_, 7); - mju_copy(joint->qvel, d->qvel + joint->dofadr_, 6); + mjuu_copyvec(joint->qpos, d->qpos + joint->qposadr_, 7); + mjuu_copyvec(joint->qvel, d->qvel + joint->dofadr_, 6); break; case mjJNT_BALL: - mju_copy(joint->qpos, d->qpos + joint->qposadr_, 4); - mju_copy(joint->qvel, d->qvel + joint->dofadr_, 3); + mjuu_copyvec(joint->qpos, d->qpos + joint->qposadr_, 4); + mjuu_copyvec(joint->qvel, d->qvel + joint->dofadr_, 3); break; case mjJNT_HINGE: case mjJNT_SLIDE: - mju_copy(joint->qpos, d->qpos + joint->qposadr_, 1); - mju_copy(joint->qvel, d->qvel + joint->dofadr_, 1); + mjuu_copyvec(joint->qpos, d->qpos + joint->qposadr_, 1); + mjuu_copyvec(joint->qvel, d->qvel + joint->dofadr_, 1); break; } } @@ -2745,7 +2736,7 @@ void mjCModel::SaveState(const mjData* d) { for (auto actuator : actuators_) { if (actuator->actadr_ != -1) { actuator->act.assign(actuator->actnum_, 0); - mju_copy(actuator->act.data(), d->act + actuator->actadr_, actuator->actnum_); + mjuu_copyvec(actuator->act.data(), d->act + actuator->actadr_, actuator->actnum_); } } } @@ -2767,24 +2758,24 @@ void mjCModel::RestoreState(const mjModel* m, mjData** dest) { } switch (joint->type) { case mjJNT_FREE: - mju_copy(d->qpos + joint->qposadr_, joint->qpos, 7); - mju_copy(d->qvel + joint->dofadr_, joint->qvel, 6); + mjuu_copyvec(d->qpos + joint->qposadr_, joint->qpos, 7); + mjuu_copyvec(d->qvel + joint->dofadr_, joint->qvel, 6); break; case mjJNT_BALL: - mju_copy(d->qpos + joint->qposadr_, joint->qpos, 4); - mju_copy(d->qvel + joint->dofadr_, joint->qvel, 3); + mjuu_copyvec(d->qpos + joint->qposadr_, joint->qpos, 4); + mjuu_copyvec(d->qvel + joint->dofadr_, joint->qvel, 3); break; case mjJNT_HINGE: case mjJNT_SLIDE: - mju_copy(d->qpos + joint->qposadr_, joint->qpos, 1); - mju_copy(d->qvel + joint->dofadr_, joint->qvel, 1); + mjuu_copyvec(d->qpos + joint->qposadr_, joint->qpos, 1); + mjuu_copyvec(d->qvel + joint->dofadr_, joint->qvel, 1); break; } } for (auto actuator : actuators_) { if (mjuu_defined(actuator->act[0])) { - mju_copy(d->act + actuator->actadr_, actuator->act.data(), actuator->actnum_); + mjuu_copyvec(d->act + actuator->actadr_, actuator->act.data(), actuator->actnum_); } } } @@ -3566,14 +3557,14 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) { meaninertia_auto = m->stat.meaninertia; meanmass_auto = m->stat.meanmass; meansize_auto = m->stat.meansize; - copyvec(center_auto, m->stat.center, 3); + mjuu_copyvec(center_auto, m->stat.center, 3); // override model statistics if defined by user if (mjuu_defined(stat.extent)) m->stat.extent = (mjtNum)stat.extent; if (mjuu_defined(stat.meaninertia)) m->stat.meaninertia = (mjtNum)stat.meaninertia; if (mjuu_defined(stat.meanmass)) m->stat.meanmass = (mjtNum)stat.meanmass; if (mjuu_defined(stat.meansize)) m->stat.meansize = (mjtNum)stat.meansize; - if (mjuu_defined(stat.center[0])) copyvec(m->stat.center, stat.center, 3); + if (mjuu_defined(stat.center[0])) mjuu_copyvec(m->stat.center, stat.center, 3); // assert that model has valid references const char* validationerr = mj_validateReferences(m); @@ -3652,15 +3643,15 @@ bool mjCModel::CopyBack(const mjModel* m) { if (m->stat.center[0] != center_auto[0] || m->stat.center[1] != center_auto[1] || m->stat.center[2] != center_auto[2]) { - mju_copy3(stat.center, m->stat.center); + mjuu_copyvec(stat.center, m->stat.center, 3); } // qpos0, qpos_spring for (int i=0; itype) { case mjJNT_FREE: - copyvec(bodies_[m->jnt_bodyid[i]]->pos, m->qpos0+m->jnt_qposadr[i], 3); - copyvec(bodies_[m->jnt_bodyid[i]]->quat, m->qpos0+m->jnt_qposadr[i]+3, 4); + mjuu_copyvec(bodies_[m->jnt_bodyid[i]]->pos, m->qpos0+m->jnt_qposadr[i], 3); + mjuu_copyvec(bodies_[m->jnt_bodyid[i]]->quat, m->qpos0+m->jnt_qposadr[i]+3, 4); break; case mjJNT_SLIDE: @@ -3674,22 +3665,22 @@ bool mjCModel::CopyBack(const mjModel* m) { break; } } - mju_copy(qpos0.data(), m->qpos0, m->nq); + mjuu_copyvec(qpos0.data(), m->qpos0, m->nq); // body mjCBody* pb; for (int i=0; ipos, m->body_pos+3*i, 3); - copyvec(pb->quat, m->body_quat+4*i, 4); - copyvec(pb->ipos, m->body_ipos+3*i, 3); - copyvec(pb->iquat, m->body_iquat+4*i, 4); + mjuu_copyvec(pb->pos, m->body_pos+3*i, 3); + mjuu_copyvec(pb->quat, m->body_quat+4*i, 4); + mjuu_copyvec(pb->ipos, m->body_ipos+3*i, 3); + mjuu_copyvec(pb->iquat, m->body_iquat+4*i, 4); pb->mass = (double)m->body_mass[i]; - copyvec(pb->inertia, m->body_inertia+3*i, 3); + mjuu_copyvec(pb->inertia, m->body_inertia+3*i, 3); if (nuser_body) { - copyvec(pb->userdata_.data(), m->body_user + nuser_body*i, nuser_body); + mjuu_copyvec(pb->userdata_.data(), m->body_user + nuser_body*i, nuser_body); } } @@ -3699,22 +3690,22 @@ bool mjCModel::CopyBack(const mjModel* m) { pj = joints_[i]; // joint data - copyvec(pj->pos, m->jnt_pos+3*i, 3); - copyvec(pj->axis, m->jnt_axis+3*i, 3); + mjuu_copyvec(pj->pos, m->jnt_pos+3*i, 3); + mjuu_copyvec(pj->axis, m->jnt_axis+3*i, 3); pj->stiffness = (double)m->jnt_stiffness[i]; - copyvec(pj->range, m->jnt_range+2*i, 2); - copyvec(pj->solref_limit, m->jnt_solref+mjNREF*i, mjNREF); - copyvec(pj->solimp_limit, m->jnt_solimp+mjNIMP*i, mjNIMP); + mjuu_copyvec(pj->range, m->jnt_range+2*i, 2); + mjuu_copyvec(pj->solref_limit, m->jnt_solref+mjNREF*i, mjNREF); + mjuu_copyvec(pj->solimp_limit, m->jnt_solimp+mjNIMP*i, mjNIMP); pj->margin = (double)m->jnt_margin[i]; if (nuser_jnt) { - copyvec(pj->userdata_.data(), m->jnt_user + nuser_jnt*i, nuser_jnt); + mjuu_copyvec(pj->userdata_.data(), m->jnt_user + nuser_jnt*i, nuser_jnt); } // dof data int j = m->jnt_dofadr[i]; - copyvec(pj->solref_friction, m->dof_solref+mjNREF*j, mjNREF); - copyvec(pj->solimp_friction, m->dof_solimp+mjNIMP*j, mjNIMP); + mjuu_copyvec(pj->solref_friction, m->dof_solref+mjNREF*j, mjNREF); + mjuu_copyvec(pj->solimp_friction, m->dof_solimp+mjNIMP*j, mjNIMP); pj->armature = (double)m->dof_armature[j]; pj->damping = (double)m->dof_damping[j]; pj->frictionloss = (double)m->dof_frictionloss[j]; @@ -3725,19 +3716,19 @@ bool mjCModel::CopyBack(const mjModel* m) { for (int i=0; isize, m->geom_size+3*i, 3); - copyvec(pg->pos, m->geom_pos+3*i, 3); - copyvec(pg->quat, m->geom_quat+4*i, 4); - copyvec(pg->friction, m->geom_friction+3*i, 3); - copyvec(pg->solref, m->geom_solref+mjNREF*i, mjNREF); - copyvec(pg->solimp, m->geom_solimp+mjNIMP*i, mjNIMP); - copyvec(pg->rgba, m->geom_rgba+4*i, 4); + mjuu_copyvec(pg->size, m->geom_size+3*i, 3); + mjuu_copyvec(pg->pos, m->geom_pos+3*i, 3); + mjuu_copyvec(pg->quat, m->geom_quat+4*i, 4); + mjuu_copyvec(pg->friction, m->geom_friction+3*i, 3); + mjuu_copyvec(pg->solref, m->geom_solref+mjNREF*i, mjNREF); + mjuu_copyvec(pg->solimp, m->geom_solimp+mjNIMP*i, mjNIMP); + mjuu_copyvec(pg->rgba, m->geom_rgba+4*i, 4); pg->solmix = (double)m->geom_solmix[i]; pg->margin = (double)m->geom_margin[i]; pg->gap = (double)m->geom_gap[i]; if (nuser_geom) { - copyvec(pg->userdata_.data(), m->geom_user + nuser_geom*i, nuser_geom); + mjuu_copyvec(pg->userdata_.data(), m->geom_user + nuser_geom*i, nuser_geom); } } @@ -3745,8 +3736,8 @@ bool mjCModel::CopyBack(const mjModel* m) { mjCMesh* pm; for (int i=0; iGetOffsetPosPtr(), m->mesh_pos+3*i, 3); - copyvec(pm->GetOffsetQuatPtr(), m->mesh_quat+4*i, 4); + mjuu_copyvec(pm->GetOffsetPosPtr(), m->mesh_pos+3*i, 3); + mjuu_copyvec(pm->GetOffsetQuatPtr(), m->mesh_quat+4*i, 4); } // heightfield @@ -3762,84 +3753,84 @@ bool mjCModel::CopyBack(const mjModel* m) { // copy back in reverse row order for (int j=0; jsize, m->site_size + 3 * i, 3); - copyvec(sites_[i]->pos, m->site_pos+3*i, 3); - copyvec(sites_[i]->quat, m->site_quat+4*i, 4); - copyvec(sites_[i]->rgba, m->site_rgba+4*i, 4); + mjuu_copyvec(sites_[i]->size, m->site_size + 3 * i, 3); + mjuu_copyvec(sites_[i]->pos, m->site_pos+3*i, 3); + mjuu_copyvec(sites_[i]->quat, m->site_quat+4*i, 4); + mjuu_copyvec(sites_[i]->rgba, m->site_rgba+4*i, 4); if (nuser_site) { - copyvec(sites_[i]->userdata_.data(), m->site_user + nuser_site*i, nuser_site); + mjuu_copyvec(sites_[i]->userdata_.data(), m->site_user + nuser_site*i, nuser_site); } } // cameras for (int i=0; ipos, m->cam_pos+3*i, 3); - copyvec(cameras_[i]->quat, m->cam_quat+4*i, 4); + mjuu_copyvec(cameras_[i]->pos, m->cam_pos+3*i, 3); + mjuu_copyvec(cameras_[i]->quat, m->cam_quat+4*i, 4); cameras_[i]->fovy = (double)m->cam_fovy[i]; cameras_[i]->ipd = (double)m->cam_ipd[i]; - copyvec(cameras_[i]->resolution, m->cam_resolution+2*i, 2); - copyvec(cameras_[i]->intrinsic, m->cam_intrinsic+4*i, 4); + mjuu_copyvec(cameras_[i]->resolution, m->cam_resolution+2*i, 2); + mjuu_copyvec(cameras_[i]->intrinsic, m->cam_intrinsic+4*i, 4); if (nuser_cam) { - copyvec(cameras_[i]->userdata_.data(), m->cam_user + nuser_cam*i, nuser_cam); + mjuu_copyvec(cameras_[i]->userdata_.data(), m->cam_user + nuser_cam*i, nuser_cam); } } // lights for (int i=0; ipos, m->light_pos+3*i, 3); - copyvec(lights_[i]->dir, m->light_dir+3*i, 3); - copyvec(lights_[i]->attenuation, m->light_attenuation+3*i, 3); + mjuu_copyvec(lights_[i]->pos, m->light_pos+3*i, 3); + mjuu_copyvec(lights_[i]->dir, m->light_dir+3*i, 3); + mjuu_copyvec(lights_[i]->attenuation, m->light_attenuation+3*i, 3); lights_[i]->cutoff = m->light_cutoff[i]; lights_[i]->exponent = m->light_exponent[i]; - copyvec(lights_[i]->ambient, m->light_ambient+3*i, 3); - copyvec(lights_[i]->diffuse, m->light_diffuse+3*i, 3); - copyvec(lights_[i]->specular, m->light_specular+3*i, 3); + mjuu_copyvec(lights_[i]->ambient, m->light_ambient+3*i, 3); + mjuu_copyvec(lights_[i]->diffuse, m->light_diffuse+3*i, 3); + mjuu_copyvec(lights_[i]->specular, m->light_specular+3*i, 3); } // materials for (int i=0; itexrepeat, m->mat_texrepeat+2*i, 2); + mjuu_copyvec(materials_[i]->texrepeat, m->mat_texrepeat+2*i, 2); materials_[i]->emission = m->mat_emission[i]; materials_[i]->specular = m->mat_specular[i]; materials_[i]->shininess = m->mat_shininess[i]; materials_[i]->reflectance = m->mat_reflectance[i]; - copyvec(materials_[i]->rgba, m->mat_rgba+4*i, 4); + mjuu_copyvec(materials_[i]->rgba, m->mat_rgba+4*i, 4); } // pairs for (int i=0; isolref, m->pair_solref+mjNREF*i, mjNREF); - copyvec(pairs_[i]->solreffriction, m->pair_solreffriction+mjNREF*i, mjNREF); - copyvec(pairs_[i]->solimp, m->pair_solimp+mjNIMP*i, mjNIMP); + mjuu_copyvec(pairs_[i]->solref, m->pair_solref+mjNREF*i, mjNREF); + mjuu_copyvec(pairs_[i]->solreffriction, m->pair_solreffriction+mjNREF*i, mjNREF); + mjuu_copyvec(pairs_[i]->solimp, m->pair_solimp+mjNIMP*i, mjNIMP); pairs_[i]->margin = (double)m->pair_margin[i]; pairs_[i]->gap = (double)m->pair_gap[i]; - copyvec(pairs_[i]->friction, m->pair_friction+5*i, 5); + mjuu_copyvec(pairs_[i]->friction, m->pair_friction+5*i, 5); } // equality constraints for (int i=0; idata, m->eq_data+mjNEQDATA*i, mjNEQDATA); - copyvec(equalities_[i]->solref, m->eq_solref+mjNREF*i, mjNREF); - copyvec(equalities_[i]->solimp, m->eq_solimp+mjNIMP*i, mjNIMP); + mjuu_copyvec(equalities_[i]->data, m->eq_data+mjNEQDATA*i, mjNEQDATA); + mjuu_copyvec(equalities_[i]->solref, m->eq_solref+mjNREF*i, mjNREF); + mjuu_copyvec(equalities_[i]->solimp, m->eq_solimp+mjNIMP*i, mjNIMP); } // tendons for (int i=0; irange, m->tendon_range+2*i, 2); - copyvec(tendons_[i]->solref_limit, m->tendon_solref_lim+mjNREF*i, mjNREF); - copyvec(tendons_[i]->solimp_limit, m->tendon_solimp_lim+mjNIMP*i, mjNIMP); - copyvec(tendons_[i]->solref_friction, m->tendon_solref_fri+mjNREF*i, mjNREF); - copyvec(tendons_[i]->solimp_friction, m->tendon_solimp_fri+mjNIMP*i, mjNIMP); - copyvec(tendons_[i]->rgba, m->tendon_rgba+4*i, 4); + mjuu_copyvec(tendons_[i]->range, m->tendon_range+2*i, 2); + mjuu_copyvec(tendons_[i]->solref_limit, m->tendon_solref_lim+mjNREF*i, mjNREF); + mjuu_copyvec(tendons_[i]->solimp_limit, m->tendon_solimp_lim+mjNIMP*i, mjNIMP); + mjuu_copyvec(tendons_[i]->solref_friction, m->tendon_solref_fri+mjNREF*i, mjNREF); + mjuu_copyvec(tendons_[i]->solimp_friction, m->tendon_solimp_fri+mjNIMP*i, mjNIMP); + mjuu_copyvec(tendons_[i]->rgba, m->tendon_rgba+4*i, 4); tendons_[i]->width = (double)m->tendon_width[i]; tendons_[i]->margin = (double)m->tendon_margin[i]; tendons_[i]->stiffness = (double)m->tendon_stiffness[i]; @@ -3847,7 +3838,7 @@ bool mjCModel::CopyBack(const mjModel* m) { tendons_[i]->frictionloss = (double)m->tendon_frictionloss[i]; if (nuser_tendon) { - copyvec(tendons_[i]->userdata_.data(), m->tendon_user + nuser_tendon*i, nuser_tendon); + mjuu_copyvec(tendons_[i]->userdata_.data(), m->tendon_user + nuser_tendon*i, nuser_tendon); } } @@ -3856,18 +3847,18 @@ bool mjCModel::CopyBack(const mjModel* m) { for (int i=0; idynprm, m->actuator_dynprm+i*mjNDYN, mjNDYN); - copyvec(pa->gainprm, m->actuator_gainprm+i*mjNGAIN, mjNGAIN); - copyvec(pa->biasprm, m->actuator_biasprm+i*mjNBIAS, mjNBIAS); - copyvec(pa->ctrlrange, m->actuator_ctrlrange+2*i, 2); - copyvec(pa->forcerange, m->actuator_forcerange+2*i, 2); - copyvec(pa->actrange, m->actuator_actrange+2*i, 2); - copyvec(pa->lengthrange, m->actuator_lengthrange+2*i, 2); - copyvec(pa->gear, m->actuator_gear+6*i, 6); + mjuu_copyvec(pa->dynprm, m->actuator_dynprm+i*mjNDYN, mjNDYN); + mjuu_copyvec(pa->gainprm, m->actuator_gainprm+i*mjNGAIN, mjNGAIN); + mjuu_copyvec(pa->biasprm, m->actuator_biasprm+i*mjNBIAS, mjNBIAS); + mjuu_copyvec(pa->ctrlrange, m->actuator_ctrlrange+2*i, 2); + mjuu_copyvec(pa->forcerange, m->actuator_forcerange+2*i, 2); + mjuu_copyvec(pa->actrange, m->actuator_actrange+2*i, 2); + mjuu_copyvec(pa->lengthrange, m->actuator_lengthrange+2*i, 2); + mjuu_copyvec(pa->gear, m->actuator_gear+6*i, 6); pa->cranklength = (double)m->actuator_cranklength[i]; if (nuser_actuator) { - copyvec(pa->userdata_.data(), m->actuator_user + nuser_actuator*i, nuser_actuator); + mjuu_copyvec(pa->userdata_.data(), m->actuator_user + nuser_actuator*i, nuser_actuator); } } @@ -3877,7 +3868,7 @@ bool mjCModel::CopyBack(const mjModel* m) { sensors_[i]->noise = (double)m->sensor_noise[i]; if (nuser_sensor) { - copyvec(sensors_[i]->userdata_.data(), m->sensor_user + nuser_sensor*i, nuser_sensor); + mjuu_copyvec(sensors_[i]->userdata_.data(), m->sensor_user + nuser_sensor*i, nuser_sensor); } } @@ -3900,17 +3891,17 @@ bool mjCModel::CopyBack(const mjModel* m) { mjCKey* pk = keys_[i]; pk->time = (double)m->key_time[i]; - copyvec(pk->qpos_.data(), m->key_qpos + i*nq, nq); - copyvec(pk->qvel_.data(), m->key_qvel + i*nv, nv); + mjuu_copyvec(pk->qpos_.data(), m->key_qpos + i*nq, nq); + mjuu_copyvec(pk->qvel_.data(), m->key_qvel + i*nv, nv); if (na) { - copyvec(pk->act_.data(), m->key_act + i*na, na); + mjuu_copyvec(pk->act_.data(), m->key_act + i*na, na); } if (nmocap) { - copyvec(pk->mpos_.data(), m->key_mpos + i*3*nmocap, 3*nmocap); - copyvec(pk->mquat_.data(), m->key_mquat + i*4*nmocap, 4*nmocap); + mjuu_copyvec(pk->mpos_.data(), m->key_mpos + i*3*nmocap, 3*nmocap); + mjuu_copyvec(pk->mquat_.data(), m->key_mquat + i*4*nmocap, 4*nmocap); } if (nu) { - copyvec(pk->ctrl_.data(), m->key_ctrl + i*nu, nu); + mjuu_copyvec(pk->ctrl_.data(), m->key_ctrl + i*nu, nu); } } diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 223d951e..bacf8129 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -37,14 +37,7 @@ #include #include "cc/array_safety.h" #include "engine/engine_resource.h" -#include "engine/engine_io.h" #include "engine/engine_passive.h" -#include "engine/engine_plugin.h" -#include "engine/engine_util_blas.h" -#include "engine/engine_util_errmem.h" -#include "engine/engine_util_misc.h" -#include "engine/engine_util_solve.h" -#include "engine/engine_util_spatial.h" #include #include "user/user_api.h" #include "user/user_cache.h" @@ -104,14 +97,14 @@ const char* FullInertia(double quat[4], double inertia[3], const double fulliner return nullptr; } - mjtNum eigval[3], eigvec[9], quattmp[4]; - mjtNum full[9] = { + double eigval[3], eigvec[9], quattmp[4]; + double full[9] = { fullinertia[0], fullinertia[3], fullinertia[4], fullinertia[3], fullinertia[1], fullinertia[5], fullinertia[4], fullinertia[5], fullinertia[2] }; - mju_eig3(eigval, eigvec, quattmp, full); + mjuu_eig3(eigval, eigvec, quattmp, full); // check mimimal eigenvalue if (eigval[2] elements; elements.reserve(bvleaf_.size()); - mjtNum qinv[4] = {iquat_[0], -iquat_[1], -iquat_[2], -iquat_[3]}; + double qinv[4] = {iquat_[0], -iquat_[1], -iquat_[2], -iquat_[3]}; for (int i = 0; i < bvleaf_.size(); i++) { if (bvleaf_[i].conaffinity || bvleaf_[i].contype) { BVElement element; element.e = &bvleaf_[i]; - mjtNum vert[3] = {element.e->pos[0] - ipos_[0], + double vert[3] = {element.e->pos[0] - ipos_[0], element.e->pos[1] - ipos_[1], element.e->pos[2] - ipos_[2]}; - mju_rotVecQuat(element.lpos, vert, qinv); + mjuu_rotVecQuat(element.lpos, vert, qinv); elements.push_back(std::move(element)); } } @@ -448,15 +441,15 @@ int mjCBoundingVolumeHierarchy::MakeBVH( if (nelements == 0) { return -1; } - mjtNum AAMM[6] = {mjMAXVAL, mjMAXVAL, mjMAXVAL, -mjMAXVAL, -mjMAXVAL, -mjMAXVAL}; + double AAMM[6] = {mjMAXVAL, mjMAXVAL, mjMAXVAL, -mjMAXVAL, -mjMAXVAL, -mjMAXVAL}; // inverse transformation - mjtNum qinv[4] = {iquat_[0], -iquat_[1], -iquat_[2], -iquat_[3]}; + double qinv[4] = {iquat_[0], -iquat_[1], -iquat_[2], -iquat_[3]}; // accumulate AAMM over elements for (auto element = elements_begin; element != elements_end; ++element) { // transform element aabb to aamm format - mjtNum aamm[6] = {element->e->aabb[0] - element->e->aabb[3], + double aamm[6] = {element->e->aabb[0] - element->e->aabb[3], element->e->aabb[1] - element->e->aabb[4], element->e->aabb[2] - element->e->aabb[5], element->e->aabb[0] + element->e->aabb[3], @@ -465,32 +458,32 @@ int mjCBoundingVolumeHierarchy::MakeBVH( // update node AAMM for (int v=0; v<8; v++) { - mjtNum vert[3], box[3]; + double vert[3], box[3]; vert[0] = (v&1 ? aamm[3] : aamm[0]); vert[1] = (v&2 ? aamm[4] : aamm[1]); vert[2] = (v&4 ? aamm[5] : aamm[2]); // rotate to the body inertial frame if specified if (element->e->quat) { - mju_rotVecQuat(box, vert, element->e->quat); + mjuu_rotVecQuat(box, vert, element->e->quat); box[0] += element->e->pos[0] - ipos_[0]; box[1] += element->e->pos[1] - ipos_[1]; box[2] += element->e->pos[2] - ipos_[2]; - mju_rotVecQuat(vert, box, qinv); + mjuu_rotVecQuat(vert, box, qinv); } - AAMM[0] = mjMIN(AAMM[0], vert[0]); - AAMM[1] = mjMIN(AAMM[1], vert[1]); - AAMM[2] = mjMIN(AAMM[2], vert[2]); - AAMM[3] = mjMAX(AAMM[3], vert[0]); - AAMM[4] = mjMAX(AAMM[4], vert[1]); - AAMM[5] = mjMAX(AAMM[5], vert[2]); + AAMM[0] = std::min(AAMM[0], vert[0]); + AAMM[1] = std::min(AAMM[1], vert[1]); + AAMM[2] = std::min(AAMM[2], vert[2]); + AAMM[3] = std::max(AAMM[3], vert[0]); + AAMM[4] = std::max(AAMM[4], vert[1]); + AAMM[5] = std::max(AAMM[5], vert[2]); } } // inflate flat AABBs for (int i=0; i<3; i++) { - if (mju_abs(AAMM[i]-AAMM[i+3])= edges[0] + mjEPS) axis = 1; if (edges[2] >= edges[axis] + mjEPS) axis = 2; @@ -1340,7 +1333,7 @@ void mjCBody::GeomFrame(void) { } // check for small mass - if (mass0) { // fix minimum - mass = mju_max(mass, model->boundmass); - inertia[0] = mju_max(inertia[0], model->boundinertia); - inertia[1] = mju_max(inertia[1], model->boundinertia); - inertia[2] = mju_max(inertia[2], model->boundinertia); + mass = std::max(mass, model->boundmass); + inertia[0] = std::max(inertia[0], model->boundinertia); + inertia[1] = std::max(inertia[1], model->boundinertia); + inertia[2] = std::max(inertia[2], model->boundinertia); // check for negative values if (mass<0 || inertia[0]<0 || inertia[1]<0 ||inertia[2]<0) { @@ -1509,7 +1502,7 @@ void mjCBody::Compile(void) { for (int i=0; icontype; conaffinity |= geoms[i]->conaffinity; - margin = mju_max(margin, geoms[i]->margin); + margin = std::max(margin, geoms[i]->margin); } // compute bounding volume hierarchy @@ -1546,9 +1539,9 @@ void mjCBody::Compile(void) { // compute body global pose (no joint transformations in qpos0) if (id>0) { mjCBody* par = model->Bodies()[parentid]; - mju_rotVecQuat(xpos0, pos, par->xquat0); - mju_addTo3(xpos0, par->xpos0); - mju_mulQuat(xquat0, par->xquat0, quat); + mjuu_rotVecQuat(xpos0, pos, par->xquat0); + mjuu_addtovec(xpos0, par->xpos0, 3); + mjuu_mulquat(xquat0, par->xquat0, quat); } // compile all sites @@ -1680,8 +1673,8 @@ void mjCFrame::PointToLocal() { void mjCFrame::CopyFromSpec() { *static_cast(this) = spec; - mju_copy3(pos, spec.pos); - mju_copy4(quat, spec.quat); + mjuu_copyvec(pos, spec.pos, 3); + mjuu_copyvec(quat, spec.quat, 4); } @@ -2147,7 +2140,7 @@ double mjCGeom::GetRBound(void) { case mjGEOM_HFIELD: hsize = hfield->size; return sqrt(hsize[0]*hsize[0] + hsize[1]*hsize[1] + - mjMAX(hsize[2]*hsize[2], hsize[3]*hsize[3])); + std::max(hsize[2]*hsize[2], hsize[3]*hsize[3])); case mjGEOM_SPHERE: return size[0]; @@ -2159,7 +2152,7 @@ double mjCGeom::GetRBound(void) { return sqrt(size[0]*size[0]+size[1]*size[1]); case mjGEOM_ELLIPSOID: - return mju_max(mju_max(size[0], size[1]), size[2]); + return std::max(std::max(size[0], size[1]), size[2]); case mjGEOM_BOX: return sqrt(size[0]*size[0]+size[1]*size[1]+size[2]*size[2]); @@ -2167,9 +2160,9 @@ double mjCGeom::GetRBound(void) { case mjGEOM_MESH: case mjGEOM_SDF: aamm = mesh->aamm(); - haabb[0] = mju_max(fabs(aamm[0]), fabs(aamm[3])); - haabb[1] = mju_max(fabs(aamm[1]), fabs(aamm[4])); - haabb[2] = mju_max(fabs(aamm[2]), fabs(aamm[5])); + haabb[0] = std::max(std::abs(aamm[0]), std::abs(aamm[3])); + haabb[1] = std::max(std::abs(aamm[1]), std::abs(aamm[4])); + haabb[2] = std::max(std::abs(aamm[2]), std::abs(aamm[5])); return sqrt(haabb[0]*haabb[0] + haabb[1]*haabb[1] + haabb[2]*haabb[2]); default: @@ -2270,18 +2263,21 @@ void mjCGeom::SetFluidCoefs(void) { // coefficients of virtual moment of inertia. Note: if (kz-ky) in numerator // is negative, also the denom is negative. Abs both and clip to MINVAL const auto pow2 = [](const double val) { return val * val; }; - const double Ixfac = pow2(dy*dy - dz*dz) * std::fabs(kz - ky) / std::max( - mjMINVAL, std::fabs(2*(dy*dy - dz*dz) + (dy*dy + dz*dz)*(ky - kz))); - const double Iyfac = pow2(dz*dz - dx*dx) * std::fabs(kx - kz) / std::max( - mjMINVAL, std::fabs(2*(dz*dz - dx*dx) + (dz*dz + dx*dx)*(kz - kx))); - const double Izfac = pow2(dx*dx - dy*dy) * std::fabs(ky - kx) / std::max( - mjMINVAL, std::fabs(2*(dx*dx - dy*dy) + (dx*dx + dy*dy)*(kx - ky))); + const double Ixfac = pow2(dy*dy - dz*dz) * std::abs(kz - ky) / std::max( + mjEPS, std::abs(2*(dy*dy - dz*dz) + (dy*dy + dz*dz)*(ky - kz))); + const double Iyfac = pow2(dz*dz - dx*dx) * std::abs(kx - kz) / std::max( + mjEPS, std::abs(2*(dz*dz - dx*dx) + (dz*dz + dx*dx)*(kz - kx))); + const double Izfac = pow2(dx*dx - dy*dy) * std::abs(ky - kx) / std::max( + mjEPS, std::abs(2*(dx*dx - dy*dy) + (dx*dx + dy*dy)*(kx - ky))); - const mjtNum virtual_mass[3] = { - volume * kx / std::max(mjMINVAL, 2-kx), - volume * ky / std::max(mjMINVAL, 2-ky), - volume * kz / std::max(mjMINVAL, 2-kz)}; - const mjtNum virtual_inertia[3] = {volume*Ixfac/5, volume*Iyfac/5, volume*Izfac/5}; + mjtNum virtual_mass[3]; + virtual_mass[0] = volume * kx / std::max(mjEPS, 2-kx); + virtual_mass[1] = volume * ky / std::max(mjEPS, 2-ky); + virtual_mass[2] = volume * kz / std::max(mjEPS, 2-kz); + mjtNum virtual_inertia[3]; + virtual_inertia[0] = volume*Ixfac/5; + virtual_inertia[1] = volume*Iyfac/5; + virtual_inertia[2] = volume*Izfac/5; writeFluidGeomInteraction(fluid, &fluid_ellipsoid, &fluid_coefs[0], &fluid_coefs[1], &fluid_coefs[2], @@ -2477,9 +2473,9 @@ void mjCGeom::Compile(void) { size[2] = 0.25 * hfield->size[2] + 0.5 * hfield->size[3]; } else if (type==mjGEOM_MESH || type==mjGEOM_SDF) { const double* aamm = mesh->aamm(); - size[0] = mju_max(fabs(aamm[0]), fabs(aamm[3])); - size[1] = mju_max(fabs(aamm[1]), fabs(aamm[4])); - size[2] = mju_max(fabs(aamm[2]), fabs(aamm[5])); + size[0] = std::max(std::abs(aamm[0]), std::abs(aamm[3])); + size[1] = std::max(std::abs(aamm[1]), std::abs(aamm[4])); + size[2] = std::max(std::abs(aamm[2]), std::abs(aamm[5])); } for (double s : size) { @@ -2497,7 +2493,7 @@ void mjCGeom::Compile(void) { if (mass==0) { mass_ = 0; density = 0; - } else if (GetVolume()>mjMINVAL) { + } else if (GetVolume()>mjEPS) { mass_ = mass; density = mass / GetVolume(); SetInertia(); @@ -2847,7 +2843,7 @@ void mjCCamera::Compile(void) { intrinsic[3] = principal_pixel[1] / pixel_density[1] + principal_length[1]; // fovy with principal point at (0, 0) - fovy = mju_atan2((float)sensor_size[1]/2, intrinsic[1]) * 360.0 / mjPI; + fovy = std::atan2(sensor_size[1]/2, intrinsic[1]) * 360.0 / mjPI; } else { intrinsic[0] = model->visual.map.znear; intrinsic[1] = model->visual.map.znear; @@ -2942,7 +2938,7 @@ void mjCLight::Compile(void) { } // normalize direction, make sure it is not zero - if (mjuu_normvec(dir, 3)emax) { throw mjCError(this, "invalid data range in hfield '%s'", file_.c_str()); } for (int i=0; imjMINVAL) { + if (emax-emin>mjEPS) { data[i] /= (emax - emin); } } @@ -4122,12 +4118,12 @@ void mjCPair::Compile(void) { // set undefined margin: max if (!mjuu_defined(margin)) { - margin = mjMAX(geom1->margin, geom2->margin); + margin = std::max(geom1->margin, geom2->margin); } // set undefined gap: max if (!mjuu_defined(gap)) { - gap = mjMAX(geom1->gap, geom2->gap); + gap = std::max(geom1->gap, geom2->gap); } // set undefined condim, friction, solref, solimp: different priority @@ -4165,23 +4161,23 @@ void mjCPair::Compile(void) { else { // condim: max if (condim<0) { - condim = mjMAX(geom1->condim, geom2->condim); + condim = std::max(geom1->condim, geom2->condim); } // friction: max if (!mjuu_defined(friction[0])) { - friction[0] = friction[1] = mju_max(geom1->friction[0], geom2->friction[0]); - friction[2] = mju_max(geom1->friction[1], geom2->friction[1]); - friction[3] = friction[4] = mju_max(geom1->friction[2], geom2->friction[2]); + friction[0] = friction[1] = std::max(geom1->friction[0], geom2->friction[0]); + friction[2] = std::max(geom1->friction[1], geom2->friction[1]); + friction[3] = friction[4] = std::max(geom1->friction[2], geom2->friction[2]); } // solver mix factor double mix; - if (geom1->solmix>=mjMINVAL && geom2->solmix>=mjMINVAL) { + if (geom1->solmix>=mjEPS && geom2->solmix>=mjEPS) { mix = geom1->solmix / (geom1->solmix + geom2->solmix); - } else if (geom1->solmixsolmixsolmixsolmixsolmixsolmixsolref[i], geom2->solref[i]); + solref[i] = std::min(geom1->solref[i], geom2->solref[i]); } } } diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 2135421d..6b3cdf4e 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -98,9 +98,9 @@ class mjCBoundingVolume { int contype; // contact type int conaffinity; // contact affinity - const mjtNum* aabb; // axis-aligned bounding box (center, size) - const mjtNum* pos; // position (set by user or Compile1) - const mjtNum* quat; // orientation (set by user or Compile1) + const double* aabb; // axis-aligned bounding box (center, size) + const double* pos; // position (set by user or Compile1) + const double* quat; // orientation (set by user or Compile1) const int* GetId() const { if (id_) return id_; else return &idval_; } void SetId(const int* id) { id_ = id; } @@ -134,7 +134,7 @@ class mjCBoundingVolumeHierarchy : public mjCBoundingVolumeHierarchy_ { // make bounding volume hierarchy void CreateBVH(void); - void Set(mjtNum ipos_element[3], mjtNum iquat_element[4]); + void Set(double ipos_element[3], double iquat_element[4]); void AllocateBoundingVolumes(int nleaf); void RemoveInactiveVolumes(int nmax); mjCBoundingVolume* GetBoundingVolume(int id); @@ -144,7 +144,7 @@ class mjCBoundingVolumeHierarchy : public mjCBoundingVolumeHierarchy_ { struct BVElement { const mjCBoundingVolume* e; // position of the element in the BVH axes - mjtNum lpos[3]; + double lpos[3]; }; struct BVElementCompare { @@ -231,8 +231,8 @@ class mjCBody_ : public mjCBase { int contype; // OR over geom contypes int conaffinity; // OR over geom conaffinities double margin; // MAX over geom margins - mjtNum xpos0[3]; // global position in qpos0 - mjtNum xquat0[4]; // global orientation in qpos0 + double xpos0[3]; // global position in qpos0 + double xquat0[4]; // global orientation in qpos0 // used internally by compiler int lastdof; // id of last dof @@ -664,20 +664,20 @@ class mjCFlex_ : public mjCBase { std::vector shell; // shell fragment vertex ids (dim per fragment) std::vector elemlayer; // element layer (distance from border) std::vector evpair; // element-vertex pairs - std::vector vertxpos; // global vertex positions + std::vector vertxpos; // global vertex positions mjCBoundingVolumeHierarchy tree; // bounding volume hierarchy - std::vector elemaabb_; // element bounding volume + std::vector elemaabb_; // element bounding volume // variable-size data std::vector vertbody_; // vertex body names - std::vector vert_; // vertex positions + std::vector vert_; // vertex positions std::vector elem_; // element vertex ids std::vector texcoord_; // vertex texture coordinates std::string material_; // name of material used for rendering std::string spec_material_; std::vector spec_vertbody_; - std::vector spec_vert_; + std::vector spec_vert_; std::vector spec_elem_; std::vector spec_texcoord_; }; diff --git a/src/user/user_util.cc b/src/user/user_util.cc index a2a91ee9..13dc5610 100644 --- a/src/user/user_util.cc +++ b/src/user/user_util.cc @@ -25,8 +25,6 @@ #include #include #include "engine/engine_crossplatform.h" -#include "engine/engine_util_misc.h" -#include "engine/engine_util_spatial.h" using std::isnan; using std::string; @@ -90,22 +88,13 @@ void mjuu_setvec(double* dest, const double x, const double y) { } -// copy double array -void mjuu_copyvec(double* dest, const double* src, const int n) { +// add to double array +void mjuu_addtovec(double* dest, const double* src, const int n) { for (int i=0; i mjEPS) { + for (int i=0; i mjEPS) { + for (int i=0; i std::abs(D[2]) && std::abs(D[1]) > std::abs(D[5])) { + rk = 0; // row + ck = 1; // column + rotk = 2; // rotation axis + } else if (std::abs(D[2]) > std::abs(D[5])) { + rk = 0; + ck = 2; + rotk = 1; + } else { + rk = 1; + ck = 2; + rotk = 0; + } + + // terminate if max off-diagonal element too small + if (std::abs(D[3*rk+ck]) < kEigEPS) { + break; + } + + // 2x2 symmetric Schur decomposition + tau = (D[4*ck]-D[4*rk])/(2*D[3*rk+ck]); + if (tau >= 0) { + t = 1.0/(tau + sqrt(1 + tau*tau)); + } else { + t = -1.0/(-tau + sqrt(1 + tau*tau)); + } + c = 1.0/sqrt(1 + t*t); + + // terminate if cosine too close to 1 + if (c > 1.0-kEigEPS) { + break; + } + + // express rotation as quaternion + tmp[1] = tmp[2] = tmp[3] = 0; + tmp[rotk+1] = (tau >= 0 ? -sqrt(0.5-0.5*c) : sqrt(0.5-0.5*c)); + if (rotk == 1) { + tmp[rotk+1] = -tmp[rotk+1]; + } + tmp[0] = sqrt(1.0 - tmp[rotk+1]*tmp[rotk+1]); + mjuu_normvec(tmp, 4); + + // accumulate quaternion rotation + mjuu_mulquat(quat, quat, tmp); + mjuu_normvec(quat, 4); + } + + // sort eigenvalues in decreasing order (bubblesort: 0, 1, 0) + for (int j=0; j < 3; j++) { + int j1 = j%2; // lead index + + // only swap if the eigenvalues are different + if (eigval[j1]+kEigEPS < eigval[j1+1]) { + // swap eigenvalues + t = eigval[j1]; + eigval[j1] = eigval[j1+1]; + eigval[j1+1] = t; + + // rotate quaternion + tmp[0] = 0.707106781186548; // cos(pi/4) = sin(pi/4) + tmp[1] = tmp[2] = tmp[3] = 0; + tmp[(j1+2)%3+1] = tmp[0]; + mjuu_mulquat(quat, quat, tmp); + mjuu_normvec(quat, 4); + } + } + + // recompute eigvec + mjuu_quat2mat(eigvec, quat); + + return iter; +} + +// transform vector by pose +void mjuu_trnVecPose(double res[3], const double pos[3], const double quat[4], + const double vec[3]) { + // res = quat*vec + pos + mjuu_rotVecQuat(res, vec, quat); + res[0] += pos[0]; + res[1] += pos[1]; + res[2] += pos[2]; +} + // strip directory from filename string mjuu_strippath(string filename) { // find last pathsymbol diff --git a/src/user/user_util.h b/src/user/user_util.h index 29ff5dd8..62527fcc 100644 --- a/src/user/user_util.h +++ b/src/user/user_util.h @@ -41,15 +41,23 @@ void mjuu_setvec(float* dest, const double x, const double y, const double z); // set 2D vector void mjuu_setvec(double* dest, const double x, const double y); -// copy double array -void mjuu_copyvec(double* dest, const double* src, const int n); +// copy real-valued vector +template +void mjuu_copyvec(T1* dest, const T2* src, int n) { + for (int i=0; i(text); + fcomp.point = String2Vector(text); } if (ReadAttrTxt(elem, "element", text)){ fcomp.element = String2Vector(text); diff --git a/test/fixture.cc b/test/fixture.cc index c3c1df28..6a141ee3 100644 --- a/test/fixture.cc +++ b/test/fixture.cc @@ -214,18 +214,21 @@ mjtNum CompareModel(const mjModel* m1, const mjModel* m2, #undef X if (maxdif > 0) return maxdif; - // compare arrays -#define X(type, name, nr, nc) \ - for (int r = 0; r < m1->nr; r++) { \ - for (int c = 0; c < nc; c++) { \ - dif = Compare(m1->name[r * nc + c], m2->name[r * nc + c]); \ - if (dif > maxdif) { \ - maxdif = dif; \ - field = #name; \ - field += " row: " + std::to_string(r); \ - field += " col: " + std::to_string(c); \ - } \ - } \ + // compare arrays, apart from bvh-related ones, as those are sensitive to + // numerical differences when meshes are perfectly symmetric. +#define X(type, name, nr, nc) \ + if (strncmp(#name, "bvh_", 4)) { \ + for (int r = 0; r < m1->nr; r++) { \ + for (int c = 0; c < nc; c++) { \ + dif = Compare(m1->name[r * nc + c], m2->name[r * nc + c]); \ + if (dif > maxdif) { \ + maxdif = dif; \ + field = #name; \ + field += " row: " + std::to_string(r); \ + field += " col: " + std::to_string(c); \ + } \ + } \ + } \ } // NOLINT MJMODEL_POINTERS #undef X diff --git a/test/user/user_mesh_test.cc b/test/user/user_mesh_test.cc index cf62423d..11fe0e9b 100644 --- a/test/user/user_mesh_test.cc +++ b/test/user/user_mesh_test.cc @@ -16,6 +16,7 @@ #include #include +#include #include #include @@ -127,7 +128,7 @@ TEST_F(MjCMeshTest, LoadMSHWithVFS) { // should fallback to OS filesystem mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); EXPECT_THAT(model, IsNull()); - EXPECT_THAT(error, HasSubstr("resource not found via provider or OS filesystem")); + EXPECT_THAT(error, HasSubstr("resource not found via provider or OS")); } TEST_F(MjCMeshTest, LoadOBJWithVFS) { @@ -152,7 +153,7 @@ TEST_F(MjCMeshTest, LoadOBJWithVFS) { // should fallback to OS filesystem mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); EXPECT_THAT(model, IsNull()); - EXPECT_THAT(error, HasSubstr("resource not found via provider or OS filesystem")); + EXPECT_THAT(error, HasSubstr("resource not found via provider or OS")); } TEST_F(MjCMeshTest, LoadSTLWithVFS) { @@ -177,7 +178,7 @@ TEST_F(MjCMeshTest, LoadSTLWithVFS) { // should fallback to OS filesystem mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); EXPECT_THAT(model, IsNull()); - EXPECT_THAT(error, HasSubstr("resource not found via provider or OS filesystem")); + EXPECT_THAT(error, HasSubstr("resource not found via provider or OS")); } // ------------- test content_type attributes ---------------------------------- @@ -204,7 +205,7 @@ TEST_F(MjCMeshTest, LoadMSHWithContentType) { // should try opening the file (not found obviously) mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); EXPECT_THAT(model, IsNull()); - EXPECT_THAT(error, HasSubstr("resource not found via provider or OS filesystem")); + EXPECT_THAT(error, HasSubstr("resource not found via provider or OS")); } TEST_F(MjCMeshTest, LoadOBJWithContentType) { @@ -229,7 +230,7 @@ TEST_F(MjCMeshTest, LoadOBJWithContentType) { // should try opening the file (not found obviously) mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); EXPECT_THAT(model, IsNull()); - EXPECT_THAT(error, HasSubstr("resource not found via provider or OS filesystem")); + EXPECT_THAT(error, HasSubstr("resource not found via provider or OS")); } TEST_F(MjCMeshTest, LoadSTLWithContentType) { @@ -254,7 +255,7 @@ TEST_F(MjCMeshTest, LoadSTLWithContentType) { // should try opening the file (not found obviously) mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); EXPECT_THAT(model, IsNull()); - EXPECT_THAT(error, HasSubstr("resource not found via provider or OS filesystem")); + EXPECT_THAT(error, HasSubstr("resource not found via provider or OS")); } TEST_F(MjCMeshTest, LoadMSHWithContentTypeError) { @@ -304,7 +305,7 @@ TEST_F(MjCMeshTest, LoadMSHWithContentTypeParam) { // should try opening the file (not found obviously) mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); EXPECT_THAT(model, IsNull()); - EXPECT_THAT(error, HasSubstr("resource not found via provider or OS filesystem")); + EXPECT_THAT(error, HasSubstr("resource not found via provider or OS")); } // ------------- test vertex de-duplication (STL) ------------------------------ @@ -420,7 +421,8 @@ TEST_F(MjCMeshTest, MaxHullVert) { } TEST_F(MjCMeshTest, MaxHullVertDefault) { - const std::string xml_path = GetTestDataFilePath(kTorusDefaultMaxhullVertPath); + const std::string xml_path = + GetTestDataFilePath(kTorusDefaultMaxhullVertPath); std::array error; mjModel* model = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size()); ASSERT_GT(model->ngeom, 0); @@ -465,8 +467,9 @@ TEST_F(MjCMeshTest, SmallInertiaLoads) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << error.data(); mj_deleteModel(model); } diff --git a/test/user/user_model_test.cc b/test/user/user_model_test.cc index 22b46ec0..d067d81a 100644 --- a/test/user/user_model_test.cc +++ b/test/user/user_model_test.cc @@ -283,7 +283,7 @@ TEST_F(FuseStaticTest, FuseStaticEquivalent) { mj_step(m_fuse, d_fuse); mj_step(m_no_fuse, d_no_fuse); - EXPECT_THAT(d_fuse->qvel[0], DoubleNear(d_no_fuse->qvel[0], 1e-17)) + EXPECT_THAT(d_fuse->qvel[0], DoubleNear(d_no_fuse->qvel[0], 2e-17)) << "Velocity should be the same after 1 step"; EXPECT_NE(d_fuse->qvel[0], 0); diff --git a/test/xml/xml_native_writer_test.cc b/test/xml/xml_native_writer_test.cc index bcb8c68b..09176bd5 100644 --- a/test/xml/xml_native_writer_test.cc +++ b/test/xml/xml_native_writer_test.cc @@ -1302,9 +1302,13 @@ TEST_F(XMLWriterTest, WriteReadCompare) { ASSERT_THAT(error.data(), HasSubstr("file")) << error.data() << " from " << xml.c_str(); } else { - // for a particularly difficult example, relax the tolerance - mjtNum tol = - absl::StrContains(p.path().string(), "belt.xml") ? 1e-13 : 0; + mjtNum tol = 0; + + // for particularly sensitive models, relax the tolerance + if (absl::StrContains(p.path().string(), "belt.xml") || + absl::StrContains(p.path().string(), "cable.xml")) { + tol = 1e-13; + } // compare and delete std::string field = ""; From 44417f0059cdc3439e14fbc92d73417609acf125 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Wed, 19 Jun 2024 07:34:32 -0700 Subject: [PATCH 04/52] Update mjx/requirements.txt PiperOrigin-RevId: 644744086 Change-Id: Ie6733ddbd1af9da43785b35c06e465e90267d9a3 --- mjx/requirements.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/mjx/requirements.txt b/mjx/requirements.txt index 2a371aae..49434299 100644 --- a/mjx/requirements.txt +++ b/mjx/requirements.txt @@ -34,7 +34,8 @@ jaxlib==0.4.18; python_version >= '3.9' \ --hash=sha256:116a0d6aedd3e856b52493d7e392fb1b40952b84fb72448fde1c1ab5687db667 \ --hash=sha256:9593ff69f424947567e206f3e356b2a2df55ca68e6d815d5adc6cae308e8f652 \ --hash=sha256:2b17b3f05b3bbf8e0ddb85fba339525ac03bac21c9f26d0f83dcea1b1654353e \ - --hash=sha256:b35ec08984e2aa5e96ba3f3f8b88e90dee0283649e037f213dec8e85638fa17d + --hash=sha256:b35ec08984e2aa5e96ba3f3f8b88e90dee0283649e037f213dec8e85638fa17d \ + --hash=sha256:0bcc4768d29be80d20fd542aafd3a02510a5b1e47c7953beef8b03a9941fa64d pip==23.3.1 \ --hash=sha256:55eb67bb6171d37447e82213be585b75fe2b12b359e993773aca4de9247a052b pytest==7.4.2 \ From 1eb708646d0b9fb09d104959305b205ead9e8bd4 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Wed, 19 Jun 2024 08:40:53 -0700 Subject: [PATCH 05/52] Update the `simulate` tutorial video. The new video is more comprehensive and covers more features of MuJoCo. PiperOrigin-RevId: 644758072 Change-Id: I1f114746833e00cd61b27659c816316c0e72c46c --- README.md | 2 +- doc/changelog.rst | 12 ++++++++++++ doc/programming/samples.rst | 4 ++-- 3 files changed, 15 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index 7ce14c2d..06c5f29d 100644 --- a/README.md +++ b/README.md @@ -41,7 +41,7 @@ release can be found in the [changelog] in the latest branch. There are two easy ways to get started with MuJoCo: 1. **Run `simulate` on your machine.** -[This video](https://www.youtube.com/watch?v=0ORsj_E17B0) shows a screen capture +[This video](https://www.youtube.com/watch?v=P83tKA1iz2Y) shows a screen capture of `simulate`, MuJoCo's native interactive viewer. Follow the steps described in the [Getting Started] section of the documentation to get `simulate` running on your machine. diff --git a/doc/changelog.rst b/doc/changelog.rst index babd164c..ff007373 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -32,6 +32,18 @@ MJX 8. Fixed a bug that resulted in less-optimal linesearch solutions for some difficult constraint settings. 9. Fixed a bug in the Newton solver that sometimes resulted in less-optimal gradients. + +.. youtube:: P83tKA1iz2Y + :align: right + :width: 360px + +Simulate +^^^^^^^^ +10. Added improved tutorial video. +11. Improved the Brownian noise generator. + +|br| |br| |br| |br| + Version 3.1.6 (Jun 3, 2024) --------------------------- diff --git a/doc/programming/samples.rst b/doc/programming/samples.rst index 92987a49..40cbd434 100644 --- a/doc/programming/samples.rst +++ b/doc/programming/samples.rst @@ -84,7 +84,7 @@ data plots. The model file can be specified as a command-line argument, or loade functionality. This code sample uses the native UI to render various controls, and provides an illustration of how the new UI framework is intended to be used. Below is a screen-capture of ``simulate`` in action: -.. youtube:: 0ORsj_E17B0 +.. youtube:: P83tKA1iz2Y :width: 95% :align: center @@ -94,7 +94,7 @@ selected object by holding Ctrl and dragging the mouse. Dragging the mouse alone are keyboard shortcuts for pausing the simulation, resetting, and re-loading the model file. The latter functionality is very useful while editing the model in an XML editor. -The code is quite long yet reasonably commented, so it is best to just read it. Here we provide a high-level overview. +The code is long yet reasonably commented, so it is best to just read it. Here we provide a high-level overview. The ``main()`` function initializes both MuJoCo and GLFW, opens a window, and install GLFW callbacks for mouse and keyboard handling. Note that there is no render callback; GLFW puts the user in charge, instead of running a rendering loop behind the scenes. The main loop handles UI events and rendering. The simulation is handled in a background From 7bd7065e0e2cdb34267cc9d5936e5015d9ef51f8 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Wed, 19 Jun 2024 09:38:02 -0700 Subject: [PATCH 06/52] Renamed mjs_fullInertia to mjuu_fullInertia and moved it to user_util. PiperOrigin-RevId: 644769379 Change-Id: I83511eeb4c0972ce9f46f0e3443d438cccdd4d9f --- doc/APIreference/functions.rst | 9 -------- doc/includes/references.h | 1 - include/mujoco/mujoco.h | 3 --- introspect/functions.py | 31 -------------------------- src/user/user_api.cc | 7 ------ src/user/user_api.h | 3 --- src/user/user_model.cc | 2 +- src/user/user_objects.cc | 40 ++-------------------------------- src/user/user_objects.h | 2 -- src/user/user_util.cc | 33 ++++++++++++++++++++++++++++ src/user/user_util.h | 3 +++ src/xml/xml_urdf.cc | 2 +- 12 files changed, 40 insertions(+), 96 deletions(-) diff --git a/doc/APIreference/functions.rst b/doc/APIreference/functions.rst index 569d3ee6..c55c15ea 100644 --- a/doc/APIreference/functions.rst +++ b/doc/APIreference/functions.rst @@ -4341,15 +4341,6 @@ mjs_resolveOrientation Resolve alternative orientations to quat, return error if any. -.. _mjs_fullInertia: - -mjs_fullInertia -~~~~~~~~~~~~~~~ - -.. mujoco-include:: mjs_fullInertia - -Compute quat and diag inertia from full inertia matrix, return error if any. - .. _ElementInitialization: Element initialization diff --git a/doc/includes/references.h b/doc/includes/references.h index 9bdb0b1f..2e3e3429 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -3559,7 +3559,6 @@ void mjs_setDefault(mjsElement* element, mjsDefault* def); void mjs_setFrame(mjsElement* dest, mjsFrame* frame); const char* mjs_resolveOrientation(double quat[4], mjtByte degree, const char* sequence, const mjsOrientation* orientation); -const char* mjs_fullInertia(double quat[4], double inertia[3], const double fullinertia[6]); void mjs_defaultSpec(mjSpec* spec); void mjs_defaultOrientation(mjsOrientation* orient); void mjs_defaultBody(mjsBody* body); diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index 86d48cd7..95893542 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -1612,9 +1612,6 @@ MJAPI void mjs_setFrame(mjsElement* dest, mjsFrame* frame); MJAPI const char* mjs_resolveOrientation(double quat[4], mjtByte degree, const char* sequence, const mjsOrientation* orientation); -// Compute quat and diag inertia from full inertia matrix, return error if any. -MJAPI const char* mjs_fullInertia(double quat[4], double inertia[3], const double fullinertia[6]); - //---------------------------------- Element initialization --------------------------------------- diff --git a/introspect/functions.py b/introspect/functions.py index 0658ba1d..9733755b 100644 --- a/introspect/functions.py +++ b/introspect/functions.py @@ -10216,37 +10216,6 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), doc='Resolve alternative orientations to quat, return error if any.', )), - ('mjs_fullInertia', - FunctionDecl( - name='mjs_fullInertia', - return_type=PointerType( - inner_type=ValueType(name='char', is_const=True), - ), - parameters=( - FunctionParameterDecl( - name='quat', - type=ArrayType( - inner_type=ValueType(name='double'), - extents=(4,), - ), - ), - FunctionParameterDecl( - name='inertia', - type=ArrayType( - inner_type=ValueType(name='double'), - extents=(3,), - ), - ), - FunctionParameterDecl( - name='fullinertia', - type=ArrayType( - inner_type=ValueType(name='double', is_const=True), - extents=(6,), - ), - ), - ), - doc='Compute quat and diag inertia from full inertia matrix, return error if any.', # pylint: disable=line-too-long - )), ('mjs_defaultSpec', FunctionDecl( name='mjs_defaultSpec', diff --git a/src/user/user_api.cc b/src/user/user_api.cc index 2efa3856..de3e58af 100644 --- a/src/user/user_api.cc +++ b/src/user/user_api.cc @@ -696,13 +696,6 @@ void mjs_setActivePlugins(mjSpec* s, void* activeplugins) { -// compute full inertia -const char* mjs_fullInertia(double quat[4], double inertia[3], const double fullinertia[6]) { - return FullInertia(quat, inertia, fullinertia); -} - - - // -------------------------- GLOBAL ASSET CACHE ------------------------------- void mj_setCacheSize(mjCache cache, std::size_t size) { diff --git a/src/user/user_api.h b/src/user/user_api.h index d7c03f0f..168ae6d2 100644 --- a/src/user/user_api.h +++ b/src/user/user_api.h @@ -273,9 +273,6 @@ MJAPI void mjs_setFrame(mjsElement* dest, mjsFrame* frame); MJAPI const char* mjs_resolveOrientation(double quat[4], mjtByte degree, const char* sequence, const mjsOrientation* orientation); -// Compute quat and diag inertia from full inertia matrix, return error if any. -MJAPI const char* mjs_fullInertia(double quat[4], double inertia[3], const double fullinertia[6]); - //---------------------------------- Initialization ----------------------------------------------- diff --git a/src/user/user_model.cc b/src/user/user_model.cc index b92232e8..7bd1ebbe 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -2921,7 +2921,7 @@ void mjCModel::FuseStatic(void) { // compute principal axes of inertia mjuu_copyvec(par->fullinertia, toti, 6); - const char* err1 = FullInertia(par->iquat, par->inertia, par->fullinertia); + const char* err1 = mjuu_fullInertia(par->iquat, par->inertia, par->fullinertia); if (err1) { throw mjCError(NULL, "error '%s' in fusing static body inertias", err1); } diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index bacf8129..68fb0a92 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -91,42 +91,6 @@ static bool islimited(int limited, const double range[2]) { return false; } -// compute frame quat and diagonal inertia from full inertia matrix, return error if any -const char* FullInertia(double quat[4], double inertia[3], const double fullinertia[6]) { - if (!mjuu_defined(fullinertia[0])) { - return nullptr; - } - - double eigval[3], eigvec[9], quattmp[4]; - double full[9] = { - fullinertia[0], fullinertia[3], fullinertia[4], - fullinertia[3], fullinertia[1], fullinertia[5], - fullinertia[4], fullinertia[5], fullinertia[2] - }; - - mjuu_eig3(eigval, eigvec, quattmp, full); - - // check mimimal eigenvalue - if (eigval[2]fullinertia); + const char* ierr = mjuu_fullInertia(iquat, inertia, this->fullinertia); if (ierr) { throw mjCError(this, "error '%s' in inertia alternative", ierr); } diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 6b3cdf4e..4e1b8a9f 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -86,8 +86,6 @@ const char* ResolveOrientation(double* quat, // set frame quat const char* sequence, // euler sequence format: "xyz" const mjsOrientation& orient); -// compute frame quat and diagonal inertia from full inertia matrix, return error if any -const char* FullInertia(double quat[4], double inertia[3], const double fullinertia[6]); //------------------------- class mjCBoundingVolumeHierarchy --------------------------------------- diff --git a/src/user/user_util.cc b/src/user/user_util.cc index 13dc5610..652ab05b 100644 --- a/src/user/user_util.cc +++ b/src/user/user_util.cc @@ -751,6 +751,39 @@ string mjuu_strippath(string filename) { } +// compute frame quat and diagonal inertia from full inertia matrix, return error if any +const char* mjuu_fullInertia(double quat[4], double inertia[3], const double fullinertia[6]) { + if (!mjuu_defined(fullinertia[0])) { + return nullptr; + } + + double eigval[3], eigvec[9], quattmp[4]; + double full[9] = { + fullinertia[0], fullinertia[3], fullinertia[4], + fullinertia[3], fullinertia[1], fullinertia[5], + fullinertia[4], fullinertia[5], fullinertia[2] + }; + + mjuu_eig3(eigval, eigvec, quattmp, full); + + // check mimimal eigenvalue + if (eigval[2]inertia, pbody->fullinertia); + const char* altres = mjuu_fullInertia(lquat, pbody->inertia, pbody->fullinertia); // inertia are sometimes 0 in URDF files: ignore error in altres, fix later (void) altres; From 3f3b39bbb18a6aa5dd342b29410d4eb9e11541bb Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Wed, 19 Jun 2024 10:51:17 -0700 Subject: [PATCH 07/52] Fix type-related issues in dependent code. Remove hardcoded mjUSEDOUBLE. Add mjUSESINGLE compiler flag. This CL does not change the default build behavior of MuJoCo. To use single-precision floating-point, build MuJoCo with `-DmjUSESINGLE`. PiperOrigin-RevId: 644782648 Change-Id: Ie815df9916798ca8054306437b39a33f84ce9e08 --- doc/APIreference/APIglobals.rst | 8 ++++++++ doc/APIreference/APItypes.rst | 26 +++++++++++++++----------- doc/changelog.rst | 16 +++++++++++----- include/mujoco/mjtnum.h | 6 +----- include/mujoco/mujoco.h | 2 +- sample/testspeed.cc | 22 ++++++++++++---------- simulate/main.cc | 3 ++- simulate/simulate.cc | 4 +++- src/engine/engine_core_constraint.c | 4 ++-- src/engine/engine_support.c | 2 +- src/engine/engine_util_blas.c | 4 ++-- src/engine/engine_util_blas.h | 4 ++-- src/engine/engine_util_sparse_avx.h | 4 ++-- unity/Runtime/Bindings/MjBindings.cs | 1 - 14 files changed, 62 insertions(+), 44 deletions(-) diff --git a/doc/APIreference/APIglobals.rst b/doc/APIreference/APIglobals.rst index f1ad2c5b..2d08ce08 100644 --- a/doc/APIreference/APIglobals.rst +++ b/doc/APIreference/APIglobals.rst @@ -534,6 +534,14 @@ shown in the table below. Their names are in the format ``mjKEY_XXX``. They corr Macros ^^^^^^ + +.. _mjUSESINGLE: + +mjUSESINGLE +~~~~~~~~~~~ + +Compile-time flag, see :ref:`mjtNum`. + .. _mjDISABLED: mjDISABLED diff --git a/doc/APIreference/APItypes.rst b/doc/APIreference/APItypes.rst index 5ee07e6a..e5ff73ff 100644 --- a/doc/APIreference/APItypes.rst +++ b/doc/APIreference/APItypes.rst @@ -54,24 +54,28 @@ The two types below are defined in `mjtnum.h `__ + with the build-time flag ``mjUSESINGLE``. If this symbol is not defined, MuJoCo will use double-precision floating + point, as usual. If ``mjUSESINGLE`` is defined, MuJoCo will use single-precision floating point. See :ref:`mjtNum`. + + Relatedly, fixed various type errors that prevented building with single-precision. MJX ~~~ -7. Added support for :ref:`elliptic friction cones`. -8. Fixed a bug that resulted in less-optimal linesearch solutions for some difficult constraint settings. -9. Fixed a bug in the Newton solver that sometimes resulted in less-optimal gradients. +8. Added support for :ref:`elliptic friction cones`. +9. Fixed a bug that resulted in less-optimal linesearch solutions for some difficult constraint settings. +10. Fixed a bug in the Newton solver that sometimes resulted in less-optimal gradients. .. youtube:: P83tKA1iz2Y @@ -39,8 +45,8 @@ MJX Simulate ^^^^^^^^ -10. Added improved tutorial video. -11. Improved the Brownian noise generator. +11. Added improved tutorial video. +12. Improved the Brownian noise generator. |br| |br| |br| |br| diff --git a/include/mujoco/mjtnum.h b/include/mujoco/mjtnum.h index b9d78ea1..e50b5ff5 100644 --- a/include/mujoco/mjtnum.h +++ b/include/mujoco/mjtnum.h @@ -17,12 +17,8 @@ //---------------------------------- floating-point definition ------------------------------------- -// compile-time configuration options -#define mjUSEDOUBLE // single or double precision for mjtNum - - // floating point data type and minval -#ifdef mjUSEDOUBLE +#ifndef mjUSESINGLE typedef double mjtNum; #define mjMINVAL 1E-15 // minimum value in any denominator #else diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index 95893542..1c85099f 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -893,7 +893,7 @@ MJAPI int mjs_isWarning(mjSpec* s); //---------------------------------- Standard math ------------------------------------------------- -#ifdef mjUSEDOUBLE +#if !defined(mjUSESINGLE) #define mju_sqrt sqrt #define mju_exp exp #define mju_sin sin diff --git a/sample/testspeed.cc b/sample/testspeed.cc index 66dabf3a..09757888 100644 --- a/sample/testspeed.cc +++ b/sample/testspeed.cc @@ -34,18 +34,17 @@ mjData* d[maxthread]; // per-thread statistics int contacts[maxthread]; int constraints[maxthread]; -double simtime[maxthread]; - +mjtNum simtime[maxthread]; // timer -std::chrono::steady_clock::time_point tm_start; mjtNum gettm(void) { - std::chrono::duration elapsed; - elapsed = std::chrono::steady_clock::now() - tm_start; + using std::chrono::steady_clock; + using Microseconds = std::chrono::duration; + static steady_clock::time_point tm_start = steady_clock::now(); + auto elapsed = Microseconds(steady_clock::now() - tm_start); return elapsed.count(); } - // deallocate and print message int finish(const char* msg = NULL, mjModel* m = NULL) { // deallocate model @@ -87,7 +86,7 @@ void simulate(int id, int nstep, mjtNum* ctrl) { constraints[id] = 0; // run and time - double start = gettm(); + mjtNum start = gettm(); for (int i=0; i < nstep; i++) { // inject pseudo-random control noise mju_copy(d[id]->ctrl, ctrl + i*m->nu, m->nu); @@ -126,7 +125,7 @@ int main(int argc, char** argv) { // read arguments int nstep = 10000, nthread = 0, npoolthread = 0; // inject small noise by default, to avoid fixed contact state - mjtNum ctrlnoise = 0.01; + double ctrlnoise = 0.01; if (argc > 2 && (std::sscanf(argv[2], "%d", &nstep) != 1 || nstep <= 0)) { return finish("Invalid nstep argument"); } @@ -149,7 +148,7 @@ int main(int argc, char** argv) { // get filename, determine file type std::string filename(argv[1]); - bool binary = (filename.find(".mjb") != std::string::npos); + bool binary = (filename.find(".mjb") != std::string::npos); // NOLINT // load model char error[1000] = "Could not load binary model"; @@ -191,8 +190,11 @@ int main(int argc, char** argv) { nstep, nthread > 1 ? " per thread" : "", m->opt.timestep); + if (sizeof(mjtNum) == 4) { + std::printf(", using single-precision"); + } if (npoolthread > 1) { - std::printf(", using %d threads for engine-internal threadpool", npoolthread); + std::printf(", using %d threads", npoolthread); } std::printf("...\n\n"); diff --git a/simulate/main.cc b/simulate/main.cc index 30139f8f..397a83c9 100644 --- a/simulate/main.cc +++ b/simulate/main.cc @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include #include @@ -334,7 +335,7 @@ void PhysicsLoop(mj::Simulate& sim) { // misalignment condition: distance from target sim time is bigger than syncmisalign bool misaligned = - mju_abs(Seconds(elapsedCPU).count()/slowdown - elapsedSim) > syncMisalign; + std::abs(Seconds(elapsedCPU).count()/slowdown - elapsedSim) > syncMisalign; // out-of-sync (for any reason): reset sync times, step if (elapsedSim < 0 || elapsedCPU.count() < 0 || syncCPU.time_since_epoch().count() == 0 || diff --git a/simulate/simulate.cc b/simulate/simulate.cc index a86c7343..a424e310 100644 --- a/simulate/simulate.cc +++ b/simulate/simulate.cc @@ -1165,7 +1165,9 @@ void CopyPose(mj::Simulate* sim, const mjModel* m, const mjData* d) { // millisecond timer, for MuJoCo built-in profiler mjtNum Timer() { - return Milliseconds(mj::Simulate::Clock::now().time_since_epoch()).count(); + static auto start = mj::Simulate::Clock::now(); + auto elapsed = Milliseconds(mj::Simulate::Clock::now() - start); + return elapsed.count(); } // clear all times diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index 6e47d384..e17c2bad 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -35,9 +35,9 @@ #endif #ifdef mjUSEPLATFORMSIMD - #if defined(__AVX__) && defined(mjUSEDOUBLE) + #if defined(__AVX__) && !defined(mjUSESINGLE) #define mjUSEAVX - #endif // defined(__AVX__) && defined(mjUSEDOUBLE) + #endif // defined(__AVX__) && !defined(mjUSESINGLE) #endif // mjUSEPLATFORMSIMD diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index 526a1334..6740f2ae 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -31,7 +31,7 @@ #include "engine/engine_util_spatial.h" #ifdef mjUSEPLATFORMSIMD - #if defined(__AVX__) && defined(mjUSEDOUBLE) + #if defined(__AVX__) && !defined(mjUSESINGLE) #define mjUSEAVX #include "immintrin.h" #endif diff --git a/src/engine/engine_util_blas.c b/src/engine/engine_util_blas.c index bb920b86..77ed4194 100644 --- a/src/engine/engine_util_blas.c +++ b/src/engine/engine_util_blas.c @@ -19,7 +19,7 @@ #include #ifdef mjUSEPLATFORMSIMD - #if defined(__AVX__) && defined(mjUSEDOUBLE) + #if defined(__AVX__) && !defined(mjUSESINGLE) #define mjUSEAVX #include "immintrin.h" #endif @@ -609,7 +609,7 @@ void mju_addToScl(mjtNum* res, const mjtNum* vec, mjtNum scl, int n) { void mju_addScl(mjtNum* res, const mjtNum* vec1, const mjtNum* vec2, mjtNum scl, int n) { int i = 0; -#if defined(__AVX__) && defined(mjUSEAVX) && defined(mjUSEDOUBLE) +#if defined(__AVX__) && defined(mjUSEAVX) && !defined(mjUSESINGLE) int n_4 = n - 4; // vector part diff --git a/src/engine/engine_util_blas.h b/src/engine/engine_util_blas.h index eaac6360..882c9783 100644 --- a/src/engine/engine_util_blas.h +++ b/src/engine/engine_util_blas.h @@ -26,7 +26,7 @@ extern "C" { //------------------------------ standard library functions ---------------------------------------- -#ifdef mjUSEDOUBLE +#if !defined(mjUSESINGLE) #define mju_sqrt sqrt #define mju_exp exp #define mju_sin sin @@ -59,7 +59,7 @@ extern "C" { #define mju_log10 log10f #define mju_floor floorf #define mju_ceil ceilf -#endif +#endif // !defined(mjUSESINGLE) //------------------------------ 3D vector and matrix-vector operations ---------------------------- diff --git a/src/engine/engine_util_sparse_avx.h b/src/engine/engine_util_sparse_avx.h index f962975c..c7f2f219 100644 --- a/src/engine/engine_util_sparse_avx.h +++ b/src/engine/engine_util_sparse_avx.h @@ -16,7 +16,7 @@ #define MUJOCO_SRC_ENGINE_ENGINE_UTIL_SPARSE_AVX_H_ #ifdef mjUSEPLATFORMSIMD -#if defined(__AVX__) && defined(mjUSEDOUBLE) +#if defined(__AVX__) && !defined(mjUSESINGLE) #define mjUSEAVX @@ -315,7 +315,7 @@ int mju_compare_avx(const int* vec1, const int* vec2, int n) { return !memcmp(vec1+i, vec2+i, (n-i)*sizeof(int)); } -#endif // defined(__AVX__) && defined(mjUSEDOUBLE) +#endif // defined(__AVX__) && !defined(mjUSESINGLE) #endif // mjUSEPLATFORMSIMD diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 2e791f25..0d551cdd 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -61,7 +61,6 @@ public const bool THIRD_PARTY_MUJOCO_INCLUDE_MJSPEC_H_ = true; public const bool THIRD_PARTY_MUJOCO_INCLUDE_MJTHREAD_H_ = true; public const int mjMAXTHREAD = 128; public const bool THIRD_PARTY_MUJOCO_INCLUDE_MJTNUM_H_ = true; -public const bool mjUSEDOUBLE = true; public const double mjMINVAL = 1e-15; public const bool THIRD_PARTY_MUJOCO_MJUI_H_ = true; public const int mjMAXUISECT = 10; From 2188cba4cd0b351d334bd084915920eea1cfc24b Mon Sep 17 00:00:00 2001 From: Nimrod Gileadi Date: Thu, 20 Jun 2024 03:45:26 -0700 Subject: [PATCH 08/52] Fix a memory leak when copy.deepcopy(data) is called. In MjDataWrapper.__deepcopy__ use the Python implementation of MjModelWrapper.__deepcopy__ to make an object that Python knows about and can release later. Fixes google-deepmind/mujoco#1572. PiperOrigin-RevId: 644967999 Change-Id: I95d093c672e136122e1939f69463a4d29dad82e8 --- doc/changelog.rst | 4 ++ python/mujoco/memory_leak_test.py | 69 +++++++++++++++++++++++++++++++ python/mujoco/structs.cc | 8 ++-- 3 files changed, 78 insertions(+), 3 deletions(-) create mode 100644 python/mujoco/memory_leak_test.py diff --git a/doc/changelog.rst b/doc/changelog.rst index cdef866c..7aa15b76 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -50,6 +50,10 @@ Simulate |br| |br| |br| |br| +Python bindings +^^^^^^^^^^^^^^^ +13. Fixed a memory leak when using ``copy.deepcopy()`` on a ``mujoco.MjData`` instance (:github:issue:`1572`). + Version 3.1.6 (Jun 3, 2024) --------------------------- diff --git a/python/mujoco/memory_leak_test.py b/python/mujoco/memory_leak_test.py new file mode 100644 index 00000000..47fae759 --- /dev/null +++ b/python/mujoco/memory_leak_test.py @@ -0,0 +1,69 @@ +# Copyright 2024 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. +# ============================================================================== +"""Test that copying mujoco.MjData multiple times doesn't leak memory.""" + +import copy +import textwrap + +from absl.testing import absltest +import mujoco + + +class MemoryLeakTest(absltest.TestCase): + + # Regression test for https://github.com/google-deepmind/mujoco/issues/1572 + def test_deepcopy_mjdata_leak(self): + # MuJoCo model with textures that take up significant memory. + model_xml = textwrap.dedent(""" + + + + + + + + + + + """) + limit = self._memory_limit(4 * 2**30) + try: + model = mujoco.MjModel.from_xml_string(model_xml) + data = mujoco.MjData(model) + total = 0 + for _ in range(1000): + mujoco.mj_step(model, data) + last_data = copy.deepcopy(data) + total += last_data.time + finally: + self._memory_limit(limit) + + def _memory_limit(self, limit_in_bytes: int) -> int: + """Limits max memory usage, and returns previous limit.""" + soft = -1 + try: + import resource # pylint: disable=g-import-not-at-top + soft, hard = resource.getrlimit(resource.RLIMIT_AS) + resource.setrlimit(resource.RLIMIT_AS, (limit_in_bytes, hard)) + except (ImportError, ValueError): + # On Windows or systems where setting resource limits fails, do nothing. + pass + return soft + + +if __name__ == '__main__': + absltest.main() diff --git a/python/mujoco/structs.cc b/python/mujoco/structs.cc index a52c8335..a49d4321 100644 --- a/python/mujoco/structs.cc +++ b/python/mujoco/structs.cc @@ -1907,9 +1907,11 @@ This is useful for example when the MJB is not available as a file on disk.)")); mjData.def("__copy__", [](const MjDataWrapper& other) { return MjDataWrapper(other); }); - mjData.def("__deepcopy__", [](const MjDataWrapper& other, py::dict) { - MjModelWrapper* model_copy = new MjModelWrapper(other.model()); - return MjDataWrapper(other, model_copy); + mjData.def("__deepcopy__", [](const MjDataWrapper& other, py::dict memo) { + // Use copy.deepcopy(model) to make a model that Python is aware of. + py::object new_model_py = + py::cast(other.model()).attr("__deepcopy__")(memo); + return MjDataWrapper(other, new_model_py.cast()); }); mjData.def(py::pickle( [](const MjDataWrapper& d) { // __getstate__ From 18634b6726b7be8a70982e184cc84e0e71672aa6 Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Thu, 20 Jun 2024 06:03:31 -0700 Subject: [PATCH 09/52] Add missing mj_deleteVFS to user unit tests. PiperOrigin-RevId: 645001843 Change-Id: I671514ea4cb7786e5d1f7f55404d4925e446a262 --- test/user/user_mesh_test.cc | 44 +++++++++++++++++++++------------- test/user/user_objects_test.cc | 12 +++++++++- 2 files changed, 38 insertions(+), 18 deletions(-) diff --git a/test/user/user_mesh_test.cc b/test/user/user_mesh_test.cc index 11fe0e9b..7fdd097e 100644 --- a/test/user/user_mesh_test.cc +++ b/test/user/user_mesh_test.cc @@ -16,6 +16,7 @@ #include #include +#include #include #include #include @@ -129,6 +130,7 @@ TEST_F(MjCMeshTest, LoadMSHWithVFS) { mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); EXPECT_THAT(model, IsNull()); EXPECT_THAT(error, HasSubstr("resource not found via provider or OS")); + mj_deleteVFS(vfs.get()); } TEST_F(MjCMeshTest, LoadOBJWithVFS) { @@ -154,6 +156,7 @@ TEST_F(MjCMeshTest, LoadOBJWithVFS) { mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); EXPECT_THAT(model, IsNull()); EXPECT_THAT(error, HasSubstr("resource not found via provider or OS")); + mj_deleteVFS(vfs.get()); } TEST_F(MjCMeshTest, LoadSTLWithVFS) { @@ -179,6 +182,7 @@ TEST_F(MjCMeshTest, LoadSTLWithVFS) { mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); EXPECT_THAT(model, IsNull()); EXPECT_THAT(error, HasSubstr("resource not found via provider or OS")); + mj_deleteVFS(vfs.get()); } // ------------- test content_type attributes ---------------------------------- @@ -206,6 +210,7 @@ TEST_F(MjCMeshTest, LoadMSHWithContentType) { mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); EXPECT_THAT(model, IsNull()); EXPECT_THAT(error, HasSubstr("resource not found via provider or OS")); + mj_deleteVFS(vfs.get()); } TEST_F(MjCMeshTest, LoadOBJWithContentType) { @@ -231,6 +236,7 @@ TEST_F(MjCMeshTest, LoadOBJWithContentType) { mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); EXPECT_THAT(model, IsNull()); EXPECT_THAT(error, HasSubstr("resource not found via provider or OS")); + mj_deleteVFS(vfs.get()); } TEST_F(MjCMeshTest, LoadSTLWithContentType) { @@ -256,6 +262,7 @@ TEST_F(MjCMeshTest, LoadSTLWithContentType) { mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); EXPECT_THAT(model, IsNull()); EXPECT_THAT(error, HasSubstr("resource not found via provider or OS")); + mj_deleteVFS(vfs.get()); } TEST_F(MjCMeshTest, LoadMSHWithContentTypeError) { @@ -281,6 +288,7 @@ TEST_F(MjCMeshTest, LoadMSHWithContentTypeError) { mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); EXPECT_THAT(model, IsNull()); EXPECT_THAT(error, HasSubstr("unsupported content type: 'model/unknown'")); + mj_deleteVFS(vfs.get()); } TEST_F(MjCMeshTest, LoadMSHWithContentTypeParam) { @@ -306,11 +314,12 @@ TEST_F(MjCMeshTest, LoadMSHWithContentTypeParam) { mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); EXPECT_THAT(model, IsNull()); EXPECT_THAT(error, HasSubstr("resource not found via provider or OS")); + mj_deleteVFS(vfs.get()); } -// ------------- test vertex de-duplication (STL) ------------------------------ +// ------------- test vertex deduplication (STL) ------------------------------ -TEST_F(MjCMeshTest, DeDuplicateSTLVertices) { +TEST_F(MjCMeshTest, DeduplicateSTLVertices) { const std::string xml_path = GetTestDataFilePath(kDuplicateVerticesPath); char error[1024]; size_t error_sz = 1024; @@ -795,15 +804,16 @@ TEST_F(MjCMeshTest, ExactConcaveInertia) { mjtNum d_cube = .5 - model->body_ipos[5]; mjtNum d_hole = .55 - model->body_ipos[5]; mjtNum I1 = I_cube - m_hole*(.8*.8 + .8*.8)/12; - mjtNum I2 = I_cube - m_hole*(.8*.8 + .9*.9)/12 + m_cube*d_cube*d_cube - m_hole*d_hole*d_hole; - EXPECT_LE(fabs(model->body_mass[1] - m_concave_cube), max_abs_err); - EXPECT_LE(fabs(model->body_mass[2] - m_concave_cube), max_abs_err); - EXPECT_LE(fabs(model->body_mass[3] - m_concave_cube), max_abs_err); - EXPECT_LE(fabs(model->body_mass[4] - m_concave_cube), max_abs_err); + mjtNum I2 = I_cube - m_hole*(.8*.8 + .9*.9)/12 + + m_cube*d_cube*d_cube - m_hole*d_hole*d_hole; + EXPECT_LE(mju_abs(model->body_mass[1] - m_concave_cube), max_abs_err); + EXPECT_LE(mju_abs(model->body_mass[2] - m_concave_cube), max_abs_err); + EXPECT_LE(mju_abs(model->body_mass[3] - m_concave_cube), max_abs_err); + EXPECT_LE(mju_abs(model->body_mass[4] - m_concave_cube), max_abs_err); for (int i = 3; i < 15; i += 3) { - EXPECT_LE(fabs(model->body_inertia[i] - I1), max_abs_err); - EXPECT_LE(fabs(model->body_inertia[i+1] - I2), max_abs_err); - EXPECT_LE(fabs(model->body_inertia[i+2] - I2), max_abs_err); + EXPECT_LE(mju_abs(model->body_inertia[i] - I1), max_abs_err); + EXPECT_LE(mju_abs(model->body_inertia[i+1] - I2), max_abs_err); + EXPECT_LE(mju_abs(model->body_inertia[i+2] - I2), max_abs_err); } mj_deleteModel(model); } @@ -815,10 +825,10 @@ TEST_F(MjCMeshTest, ExactConvexInertia) { // https://en.wikipedia.org/wiki/List_of_moments_of_inertia mjtNum m_solid_cube = 1.; mjtNum I_solid_cube = 1./6. * m_solid_cube; - EXPECT_LE(fabs(model->body_mass[1] - m_solid_cube), max_abs_err); - EXPECT_LE(fabs(model->body_mass[2] - m_solid_cube), max_abs_err); + EXPECT_LE(mju_abs(model->body_mass[1] - m_solid_cube), max_abs_err); + EXPECT_LE(mju_abs(model->body_mass[2] - m_solid_cube), max_abs_err); for (int i = 3; i < 9; i++) { - EXPECT_LE(fabs(model->body_inertia[i] - I_solid_cube), max_abs_err); + EXPECT_LE(mju_abs(model->body_inertia[i] - I_solid_cube), max_abs_err); } mj_deleteModel(model); } @@ -830,10 +840,10 @@ TEST_F(MjCMeshTest, ExactShellInertia) { // see https://en.wikipedia.org/wiki/List_of_moments_of_inertia mjtNum m_hollow_cube = 6.; mjtNum I_hollow_cube = 5./18. * m_hollow_cube; - EXPECT_LE(fabs(model->body_mass[1] - m_hollow_cube), max_abs_err); - EXPECT_LE(fabs(model->body_inertia[3] - I_hollow_cube), max_abs_err); - EXPECT_LE(fabs(model->body_inertia[4] - I_hollow_cube), max_abs_err); - EXPECT_LE(fabs(model->body_inertia[5] - I_hollow_cube), max_abs_err); + EXPECT_LE(mju_abs(model->body_mass[1] - m_hollow_cube), max_abs_err); + EXPECT_LE(mju_abs(model->body_inertia[3] - I_hollow_cube), max_abs_err); + EXPECT_LE(mju_abs(model->body_inertia[4] - I_hollow_cube), max_abs_err); + EXPECT_LE(mju_abs(model->body_inertia[5] - I_hollow_cube), max_abs_err); mj_deleteModel(model); } diff --git a/test/user/user_objects_test.cc b/test/user/user_objects_test.cc index bb4e7e3e..9328f52f 100644 --- a/test/user/user_objects_test.cc +++ b/test/user/user_objects_test.cc @@ -70,6 +70,7 @@ TEST_F(VfsTest, HFieldPngWithVFS) { EXPECT_THAT(model, IsNull()); EXPECT_THAT(error, HasSubstr("resource not found via provider or OS filesystem")); + mj_deleteVFS(vfs.get()); } TEST_F(VfsTest, HFieldCustomWithVFS) { @@ -97,6 +98,7 @@ TEST_F(VfsTest, HFieldCustomWithVFS) { EXPECT_THAT(model, IsNull()); EXPECT_THAT(error, HasSubstr("resource not found via provider or OS filesystem")); + mj_deleteVFS(vfs.get()); } TEST_F(VfsTest, TexturePngWithVFS) { @@ -125,6 +127,7 @@ TEST_F(VfsTest, TexturePngWithVFS) { EXPECT_THAT(model, IsNull()); EXPECT_THAT(error, HasSubstr("resource not found via provider or OS filesystem")); + mj_deleteVFS(vfs.get()); } TEST_F(VfsTest, TextureCustomWithVFS) { @@ -153,6 +156,7 @@ TEST_F(VfsTest, TextureCustomWithVFS) { EXPECT_THAT(model, IsNull()); EXPECT_THAT(error, HasSubstr("resource not found via provider or OS filesystem")); + mj_deleteVFS(vfs.get()); } // ------------------------ test content_type attribute ------------------------ @@ -185,6 +189,7 @@ TEST_F(ContentTypeTest, HFieldPngWithContentType) { EXPECT_THAT(model, IsNull()); EXPECT_THAT(error, HasSubstr("resource not found via provider or OS filesystem")); + mj_deleteVFS(vfs.get()); } TEST_F(ContentTypeTest, HFieldCustomWithContentType) { @@ -213,6 +218,7 @@ TEST_F(ContentTypeTest, HFieldCustomWithContentType) { EXPECT_THAT(model, IsNull()); EXPECT_THAT(error, HasSubstr("resource not found via provider or OS filesystem")); + mj_deleteVFS(vfs.get()); } TEST_F(ContentTypeTest, HFieldWithContentTypeError) { @@ -240,6 +246,7 @@ TEST_F(ContentTypeTest, HFieldWithContentTypeError) { mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); EXPECT_THAT(model, IsNull()); EXPECT_THAT(error, HasSubstr("unsupported content type: 'image/jpeg'")); + mj_deleteVFS(vfs.get()); } TEST_F(ContentTypeTest, TexturePngWithContentType) { @@ -268,6 +275,7 @@ TEST_F(ContentTypeTest, TexturePngWithContentType) { EXPECT_THAT(model, IsNull()); EXPECT_THAT(error, HasSubstr("resource not found via provider or OS filesystem")); + mj_deleteVFS(vfs.get()); } TEST_F(ContentTypeTest, TextureCustomWithContentType) { @@ -297,6 +305,7 @@ TEST_F(ContentTypeTest, TextureCustomWithContentType) { EXPECT_THAT(model, IsNull()); EXPECT_THAT(error, HasSubstr("resource not found via provider or OS filesystem")); + mj_deleteVFS(vfs.get()); } TEST_F(ContentTypeTest, TextureWithContentTypeError) { @@ -325,6 +334,7 @@ TEST_F(ContentTypeTest, TextureWithContentTypeError) { mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); EXPECT_THAT(model, IsNull()); EXPECT_THAT(error, HasSubstr("unsupported content type: 'image/jpeg'")); + mj_deleteVFS(vfs.get()); } TEST_F(ContentTypeTest, TextureLoadPng) { @@ -370,7 +380,7 @@ TEST_F(ContentTypeTest, TextureLoadPng) { EXPECT_THAT(model, NotNull()); mj_deleteModel(model); - mj_deleteFileVFS(vfs.get(), filename); + mj_deleteVFS(vfs.get()); } // ------------------------ test keyframes ------------------------------------- From 73cd15344e1d63d3546cb1a5262214404021ce87 Mon Sep 17 00:00:00 2001 From: Nimrod Gileadi Date: Thu, 20 Jun 2024 07:30:12 -0700 Subject: [PATCH 10/52] Fix a couple of mjtNum build errors. PiperOrigin-RevId: 645024056 Change-Id: Id283f550a4e2d927c214a86e77d42b90fb02824e --- test/engine/engine_plugin_test.cc | 6 +++--- test/engine/engine_support_test.cc | 10 +++++----- test/engine/engine_util_spatial_test.cc | 22 +++++++++++----------- test/fixture.cc | 19 +++++++++++++++---- test/user/user_mesh_test.cc | 4 ++-- 5 files changed, 36 insertions(+), 25 deletions(-) diff --git a/test/engine/engine_plugin_test.cc b/test/engine/engine_plugin_test.cc index 53ae1553..3a0c639f 100644 --- a/test/engine/engine_plugin_test.cc +++ b/test/engine/engine_plugin_test.cc @@ -240,7 +240,7 @@ int RegisterSensorPlugin() { TestSensor::DestroyCount()++; }; - plugin.reset = +[](const mjModel* m, double* plugin_state, void* plugin_data, + plugin.reset = +[](const mjModel* m, mjtNum* plugin_state, void* plugin_data, int instance) { auto sensor = reinterpret_cast(plugin_data); sensor->Reset(); @@ -283,7 +283,7 @@ int RegisterActuatorPlugin() { TestActuator::DestroyCount()++; }; - plugin.reset = +[](const mjModel* m, double* plugin_state, void* plugin_data, + plugin.reset = +[](const mjModel* m, mjtNum* plugin_state, void* plugin_data, int instance) { auto actuator = reinterpret_cast(plugin_data); actuator->Reset(); @@ -338,7 +338,7 @@ int RegisterPassivePlugin() { d->plugin_data[instance] = 0; }; - plugin.reset = +[](const mjModel* m, double* plugin_state, void* plugin_data, + plugin.reset = +[](const mjModel* m, mjtNum* plugin_state, void* plugin_data, int instance) { auto passive = reinterpret_cast(plugin_data); passive->Reset(); diff --git a/test/engine/engine_support_test.cc b/test/engine/engine_support_test.cc index c18dc7c0..93e69bfa 100644 --- a/test/engine/engine_support_test.cc +++ b/test/engine/engine_support_test.cc @@ -546,13 +546,13 @@ TEST_F(AddMTest, DenseSameAsSparse) { } // dense zero matrix - std::vector dst_sparse = std::vector(nv * nv, 0.0); + std::vector dst_sparse(nv * nv, 0.0); // sparse zero matrix - std::vector dst_dense = std::vector(nv * nv, 0.0); - std::vector rownnz = std::vector(nv, nv); - std::vector rowadr = std::vector(nv, 0); - std::vector colind = std::vector(nv * nv, 0); + std::vector dst_dense(nv * nv, 0.0); + std::vector rownnz(nv, nv); + std::vector rowadr(nv, 0); + std::vector colind(nv * nv, 0); // set sparse structure for (int i = 0; i < nv; i++) { diff --git a/test/engine/engine_util_spatial_test.cc b/test/engine/engine_util_spatial_test.cc index 159db210..678252f0 100644 --- a/test/engine/engine_util_spatial_test.cc +++ b/test/engine/engine_util_spatial_test.cc @@ -166,38 +166,38 @@ TEST_F(Euler2QuatTest, BadSeqLength) { } TEST_F(Euler2QuatTest, Euler2Quat) { - double quat[4] = {0}; - double tol = 1e-14; + mjtNum quat[4] = {0}; + mjtNum tol = 1e-14; char seq[] = "xyz"; - double euler[3] = {mjPI, 0, 0}; - double expected[4] = {0, 1, 0, 0}; + mjtNum euler[3] = {mjPI, 0, 0}; + mjtNum expected[4] = {0, 1, 0, 0}; mju_euler2Quat(quat, euler, seq); EXPECT_THAT(quat, Pointwise(DoubleNear(tol), expected)); euler[1] = mjPI; - double expected2[4] = {0, 0, 0, 1}; + mjtNum expected2[4] = {0, 0, 0, 1}; mju_euler2Quat(quat, euler, seq); EXPECT_THAT(quat, Pointwise(DoubleNear(tol), expected2)); char seq2[] = "XYZ"; - double expected3[4] = {0, 0, 0, -1}; + mjtNum expected3[4] = {0, 0, 0, -1}; mju_euler2Quat(quat, euler, seq2); EXPECT_THAT(quat, Pointwise(DoubleNear(tol), expected3)); - double euler2[3] = {2*mjPI, 2*mjPI, 2*mjPI}; - double expected4[4] = {-1, 0, 0, 0}; + mjtNum euler2[3] = {2*mjPI, 2*mjPI, 2*mjPI}; + mjtNum expected4[4] = {-1, 0, 0, 0}; mju_euler2Quat(quat, euler2, seq); EXPECT_THAT(quat, Pointwise(DoubleNear(tol), expected4)); mju_euler2Quat(quat, euler2, seq2); EXPECT_THAT(quat, Pointwise(DoubleNear(tol), expected4)); - double euler3[3] = {mjPI/2, mjPI/2, mjPI/2}; - double expected5[4] = {0, mju_sqrt(.5), 0, mju_sqrt(.5)}; + mjtNum euler3[3] = {mjPI/2, mjPI/2, mjPI/2}; + mjtNum expected5[4] = {0, mju_sqrt(.5), 0, mju_sqrt(.5)}; mju_euler2Quat(quat, euler3, seq); EXPECT_THAT(quat, Pointwise(DoubleNear(tol), expected5)); mju_euler2Quat(quat, euler3, seq2); - double expected6[4] = {mju_sqrt(.5), 0, mju_sqrt(.5), 0}; + mjtNum expected6[4] = {mju_sqrt(.5), 0, mju_sqrt(.5), 0}; EXPECT_THAT(quat, Pointwise(DoubleNear(tol), expected6)); } diff --git a/test/fixture.cc b/test/fixture.cc index 6a141ee3..4e231408 100644 --- a/test/fixture.cc +++ b/test/fixture.cc @@ -177,6 +177,17 @@ std::vector GetCtrlNoise(const mjModel* m, int nsteps, return ctrl; } +template +auto Compare(T val1, T val2); + +auto Compare(char val1, char val2) { + return val1 != val2; +} + +auto Compare(unsigned char val1, unsigned char val2) { + return val1 != val2; +} + // The maximum spacing between a normalised floating point number x and an // adjacent normalised number is 2 epsilon |x|; a factor 10 is added accounting // for losses during non-idempotent operations such as vector normalizations. @@ -185,13 +196,13 @@ auto Compare(T val1, T val2) { using ReturnType = std::conditional_t, float, double>; ReturnType error; - if (mju_abs(val1) <= 1 || mju_abs(val2) <= 1) { + if (std::abs(val1) <= 1 || std::abs(val2) <= 1) { // Absolute precision for small numbers - error = mju_abs(val1-val2); + error = std::abs(val1-val2); } else { // Relative precision for larger numbers - ReturnType magnitude = mju_abs(val1) + mju_abs(val2); - error = mju_abs(val1/magnitude - val2/magnitude) / magnitude; + ReturnType magnitude = std::abs(val1) + std::abs(val2); + error = std::abs(val1/magnitude - val2/magnitude) / magnitude; } ReturnType safety_factor = 200; return error < safety_factor * std::numeric_limits::epsilon() diff --git a/test/user/user_mesh_test.cc b/test/user/user_mesh_test.cc index 7fdd097e..02e6a6c1 100644 --- a/test/user/user_mesh_test.cc +++ b/test/user/user_mesh_test.cc @@ -873,8 +873,8 @@ TEST_F(MjCMeshTest, MeshPosQuat) { // Apply the inverted mesh_pos and inverted mesh_quat to the geom's pos and // quat. It should match the originally specified values. - double recovered_pos[3]; - double recovered_quat[4]; + mjtNum recovered_pos[3]; + mjtNum recovered_quat[4]; mju_mulPose(recovered_pos, recovered_quat, &model->geom_pos[0], &model->geom_quat[0], inverse_mesh_pos, inverse_mesh_quat); From b0b934cb5f8d2f37b203a2f787944832658f669e Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Fri, 21 Jun 2024 05:17:04 -0700 Subject: [PATCH 11/52] Separate CCD code from support/center logic. PiperOrigin-RevId: 645355865 Change-Id: I71303ffd7bbee37c5e8ac7e770f810768780c2e6 --- src/engine/engine_collision_convex.c | 121 ++++++++++++++------------- src/engine/engine_collision_convex.h | 13 ++- 2 files changed, 74 insertions(+), 60 deletions(-) diff --git a/src/engine/engine_collision_convex.c b/src/engine/engine_collision_convex.c index 54f72ad5..eb2628dc 100644 --- a/src/engine/engine_collision_convex.c +++ b/src/engine/engine_collision_convex.c @@ -34,25 +34,31 @@ // ccd center function void mjccd_center(const void *obj, ccd_vec3_t *center) { - const mjtCCD* ccd = (const mjtCCD*)obj; - int g = ccd->geom; - int f = ccd->flex; - int e = ccd->elem; - int v = ccd->vert; + mjc_center(center->v, (const mjtCCObj*) obj); +} + + + +// center function for convex collision algorithms +void mjc_center(mjtNum res[3], const mjtCCObj *obj) { + int g = obj->geom; + int f = obj->flex; + int e = obj->elem; + int v = obj->vert; // return geom position if (g >= 0) { - mju_copy3(center->v, ccd->data->geom_xpos + 3*g); + mju_copy3(res, obj->data->geom_xpos + 3*g); } // return flex element position else if (e >= 0) { - mju_copy3(center->v, ccd->data->flexelem_aabb + 6*(ccd->model->flex_elemadr[f]+e)); + mju_copy3(res, obj->data->flexelem_aabb + 6*(obj->model->flex_elemadr[f]+e)); } // return flex vertex position else { - mju_copy3(center->v, ccd->data->flexvert_xpos + 3*(ccd->model->flex_vertadr[f]+v)); + mju_copy3(res, obj->data->flexvert_xpos + 3*(obj->model->flex_vertadr[f]+v)); } } @@ -60,21 +66,25 @@ void mjccd_center(const void *obj, ccd_vec3_t *center) { // ccd support function void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) { - const mjtCCD* ccd = (const mjtCCD*)obj; - const mjModel* m = ccd->model; - const mjData* d = ccd->data; - int g = ccd->geom; + mjc_support(vec->v, (mjtCCObj*) obj, _dir->v); +} + + + +// support function for convex collision algorithms +void mjc_support(mjtNum res[3], mjtCCObj* obj, const mjtNum dir[3]) { + const mjModel* m = obj->model; + const mjData* d = obj->data; + int g = obj->geom; //-------------------------- flex element or vertex ----------------------------- if (g < 0) { - int f = ccd->flex; + int f = obj->flex; int dim = m->flex_dim[f]; - mjtNum *res = vec->v; - const mjtNum *dir = _dir->v; // flex element - if (ccd->elem >= 0) { - int e = ccd->elem; + if (obj->elem >= 0) { + int e = obj->elem; const int* edata = m->flex_elem + m->flex_elemdataadr[f] + e*(dim+1); const mjtNum* vert = d->flexvert_xpos + 3*m->flex_vertadr[f]; @@ -92,14 +102,14 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) { } // add radius and margin/2 - mju_addToScl3(res, dir, m->flex_radius[f] + 0.5*ccd->margin); + mju_addToScl3(res, dir, m->flex_radius[f] + 0.5*obj->margin); return; } // flex vertex else { - const mjtNum* vert = d->flexvert_xpos + 3*(m->flex_vertadr[f] + ccd->vert); - mju_addScl3(res, vert, dir, m->flex_radius[f] + 0.5*ccd->margin); + const mjtNum* vert = d->flexvert_xpos + 3*(m->flex_vertadr[f] + obj->vert); + mju_addScl3(res, vert, dir, m->flex_radius[f] + 0.5*obj->margin); return; } } @@ -111,30 +121,29 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) { mjtNum tmp, vdot; const mjtNum* size = m->geom_size+3*g; // geom sizes - mjtNum dir[3]; // direction in geom local frame - mjtNum res[3]; // result in geom local frame + mjtNum local_dir[3]; // direction in geom local frame // rotate dir to geom local frame - mju_mulMatTVec3(dir, d->geom_xmat+9*g, _dir->v); + mju_mulMatTVec3(local_dir, d->geom_xmat+9*g, dir); // compute result according to geom type switch ((mjtGeom) m->geom_type[g]) { case mjGEOM_SPHERE: - mju_scl3(res, dir, size[0]); + mju_scl3(res, local_dir, size[0]); break; case mjGEOM_CAPSULE: // start with sphere - mju_scl3(res, dir, size[0]); + mju_scl3(res, local_dir, size[0]); // add cylinder contribution - res[2] += mju_sign(dir[2]) * size[1]; + res[2] += mju_sign(local_dir[2]) * size[1]; break; case mjGEOM_ELLIPSOID: // find support point on unit sphere: scale dir by ellipsoid sizes and renormalize for (int i=0; i < 3; i++) { - res[i] = dir[i] * size[i]; + res[i] = local_dir[i] * size[i]; } mju_normalize3(res); @@ -146,21 +155,21 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) { case mjGEOM_CYLINDER: // set result in XY plane: support on circle - tmp = mju_sqrt(dir[0]*dir[0] + dir[1]*dir[1]); + tmp = mju_sqrt(local_dir[0]*local_dir[0] + local_dir[1]*local_dir[1]); if (tmp > mjMINVAL) { - res[0] = dir[0]/tmp*size[0]; - res[1] = dir[1]/tmp*size[0]; + res[0] = local_dir[0]/tmp*size[0]; + res[1] = local_dir[1]/tmp*size[0]; } else { res[0] = res[1] = 0; } // set result in Z direction - res[2] = mju_sign(dir[2]) * size[1]; + res[2] = mju_sign(local_dir[2]) * size[1]; break; case mjGEOM_BOX: for (int i=0; i < 3; i++) { - res[i] = mju_sign(dir[i]) * size[i]; + res[i] = mju_sign(local_dir[i]) * size[i]; } break; @@ -176,9 +185,9 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) { // search all vertices, find best for (int i=0; i < m->mesh_vertnum[m->geom_dataid[g]]; i++) { // vdot = dot(vertex, dir) - vdot = dir[0] * (mjtNum)vertdata[3*i] + - dir[1] * (mjtNum)vertdata[3*i+1] + - dir[2] * (mjtNum)vertdata[3*i+2]; + vdot = local_dir[0] * (mjtNum)vertdata[3*i] + + local_dir[1] * (mjtNum)vertdata[3*i+1] + + local_dir[2] * (mjtNum)vertdata[3*i+2]; // update best if (vdot > tmp) { @@ -188,7 +197,7 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) { } // record best vertex index, in globalid format - ((mjtCCD*)ccd)->meshindex = ibest; + obj->meshindex = ibest; } // hill-climb using graph data @@ -202,9 +211,9 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) { // init with first vertex in convex hull ibest = 0; - tmp = dir[0] * (mjtNum)vertdata[3*vert_globalid[0]] + - dir[1] * (mjtNum)vertdata[3*vert_globalid[0]+1] + - dir[2] * (mjtNum)vertdata[3*vert_globalid[0]+2]; + tmp = local_dir[0] * (mjtNum)vertdata[3*vert_globalid[0]] + + local_dir[1] * (mjtNum)vertdata[3*vert_globalid[0]+1] + + local_dir[2] * (mjtNum)vertdata[3*vert_globalid[0]+2]; // hill-climb until no change change = 1; @@ -213,10 +222,10 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) { change = 0; int i = vert_edgeadr[ibest]; while ((locid=edge_localid[i]) >= 0) { - // vdot = dot(vertex, dir) - vdot = dir[0] * (mjtNum)vertdata[3*vert_globalid[locid]] + - dir[1] * (mjtNum)vertdata[3*vert_globalid[locid]+1] + - dir[2] * (mjtNum)vertdata[3*vert_globalid[locid]+2]; + // vdot = dot(vertex, local_dir) + vdot = local_dir[0] * (mjtNum)vertdata[3*vert_globalid[locid]] + + local_dir[1] * (mjtNum)vertdata[3*vert_globalid[locid]+1] + + local_dir[2] * (mjtNum)vertdata[3*vert_globalid[locid]+2]; // update best if (vdot > tmp) { @@ -231,7 +240,7 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) { } // record best vertex index, in locid format - ((mjtCCD*)ccd)->meshindex = ibest; + obj->meshindex = ibest; // map best index to globalid ibest = vert_globalid[ibest]; @@ -255,16 +264,16 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) { mjERROR("ccd support function is undefined for geom type %d", m->geom_type[g]); } - // add dir*margin/2 to result + // add local_dir*margin/2 to result for (int i=0; i < 3; i++) { - res[i] += dir[i] * ccd->margin/2; + res[i] += local_dir[i] * obj->margin/2; } // rotate result to global frame - mju_mulMatVec3(vec->v, d->geom_xmat+9*g, res); + mju_mulMatVec3(res, d->geom_xmat+9*g, res); // add geom position - mju_addTo3(vec->v, d->geom_xpos+3*g); + mju_addTo3(res, d->geom_xpos+3*g); } @@ -280,7 +289,7 @@ static void mjc_initCCD(ccd_t* ccd, const mjModel* m) { // find single convex-convex collision, using libccd -static int mjc_MPRIteration(mjtCCD* obj1, mjtCCD* obj2, const ccd_t* ccd, +static int mjc_MPRIteration(mjtCCObj* obj1, mjtCCObj* obj2, const ccd_t* ccd, const mjModel* m, const mjData* d, mjContact* con, mjtNum margin) { ccd_vec3_t dir, pos; @@ -351,8 +360,8 @@ static void mju_rotateFrame(const mjtNum origin[3], const mjtNum rot[9], int mjc_Convex(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin) { ccd_t ccd; - mjtCCD obj1 = {m, d, g1, -1, -1, -1, -1, margin, {1, 0, 0, 0}}; - mjtCCD obj2 = {m, d, g2, -1, -1, -1, -1, margin, {1, 0, 0, 0}}; + mjtCCObj obj1 = {m, d, g1, -1, -1, -1, -1, margin, {1, 0, 0, 0}}; + mjtCCObj obj2 = {m, d, g2, -1, -1, -1, -1, margin, {1, 0, 0, 0}}; // init ccd structure mjc_initCCD(&ccd, m); @@ -480,7 +489,7 @@ int mjc_PlaneConvex(const mjModel* m, const mjData* d, mjGETINFO mjtNum dist, dif[3], normal[3] = {mat1[2], mat1[5], mat1[8]}; ccd_vec3_t dir, vec; - mjtCCD obj = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}}; + mjtCCObj obj = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}}; // get support point in -normal direction ccdVec3Set(&dir, -mat1[2], -mat1[5], -mat1[8]); @@ -661,7 +670,7 @@ int mjc_ConvexHField(const mjModel* m, const mjData* d, // ccd-related ccd_vec3_t dirccd, vecccd; ccd_real_t depth; - mjtCCD obj = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}}; + mjtCCObj obj = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}}; ccd_t ccd; // point size1 to hfield size instead of geom1 size @@ -1094,8 +1103,8 @@ void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, in int mjc_ConvexElem(const mjModel* m, const mjData* d, mjContact* con, int g1, int f1, int e1, int v1, int f2, int e2, mjtNum margin) { ccd_t ccd; - mjtCCD obj1 = {m, d, g1, -1, f1, e1, v1, margin, {1, 0, 0, 0}}; - mjtCCD obj2 = {m, d, -1, -1, f2, e2, -1, margin, {1, 0, 0, 0}}; + mjtCCObj obj1 = {m, d, g1, -1, f1, e1, v1, margin, {1, 0, 0, 0}}; + mjtCCObj obj2 = {m, d, -1, -1, f2, e2, -1, margin, {1, 0, 0, 0}}; // init ccd structure mjc_initCCD(&ccd, m); @@ -1142,7 +1151,7 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con, // ccd-related ccd_vec3_t dirccd, vecccd; ccd_real_t depth; - mjtCCD obj = {m, d, -1, -1, f, e, -1, margin, {1, 0, 0, 0}}; + mjtCCObj obj = {m, d, -1, -1, f, e, -1, margin, {1, 0, 0, 0}}; ccd_t ccd; //------------------------------------- AABB computation, box-box test diff --git a/src/engine/engine_collision_convex.h b/src/engine/engine_collision_convex.h index 3b9b797f..37aafce5 100644 --- a/src/engine/engine_collision_convex.h +++ b/src/engine/engine_collision_convex.h @@ -25,6 +25,7 @@ #include #include +#include #define mjGETINFO_HFIELD \ const mjtNum* pos1 = d->geom_xpos + 3*g1; \ @@ -38,8 +39,8 @@ extern "C" { #endif -// ccd general object type -struct _mjtCCD { +// internal object type for convex collision algorithms +struct _mjtCCObj { const mjModel* model; const mjData* data; int geom; @@ -50,13 +51,17 @@ struct _mjtCCD { mjtNum margin; mjtNum rotate[4]; }; -typedef struct _mjtCCD mjtCCD; +typedef struct _mjtCCObj mjtCCObj; +// support function for convex collision algorithms +void mjc_support(mjtNum res[3], mjtCCObj* obj, const mjtNum dir[3]); + +// center function for convex collision algorithms +void mjc_center(mjtNum res[3], const mjtCCObj *obj); // ccd support function void mjccd_support(const void *obj, const ccd_vec3_t *dir, ccd_vec3_t *vec); - // pairwise geom collision functions using ccd int mjc_PlaneConvex (const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin); From 1e8454820dffa1c626e4b5c4ef7c99d35eda5511 Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Fri, 21 Jun 2024 06:16:40 -0700 Subject: [PATCH 12/52] Implement initial GJK algorithm. PiperOrigin-RevId: 645368315 Change-Id: I40a1e1023fd9f14a59e11a9dcc6ac22df241b6ae --- src/engine/CMakeLists.txt | 2 + src/engine/engine_collision_gjk.c | 439 +++++++++++++++++++++++ src/engine/engine_collision_gjk.h | 40 +++ test/engine/CMakeLists.txt | 3 + test/engine/engine_collision_gjk_test.cc | 136 +++++++ 5 files changed, 620 insertions(+) create mode 100644 src/engine/engine_collision_gjk.c create mode 100644 src/engine/engine_collision_gjk.h create mode 100644 test/engine/engine_collision_gjk_test.cc diff --git a/src/engine/CMakeLists.txt b/src/engine/CMakeLists.txt index 0aacf637..e659bd1d 100644 --- a/src/engine/CMakeLists.txt +++ b/src/engine/CMakeLists.txt @@ -21,6 +21,8 @@ set(MUJOCO_ENGINE_SRCS engine_collision_convex.h engine_collision_driver.c engine_collision_driver.h + engine_collision_gjk.c + engine_collision_gjk.h engine_collision_primitive.c engine_collision_primitive.h engine_collision_sdf.c diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c new file mode 100644 index 00000000..c7c8c97d --- /dev/null +++ b/src/engine/engine_collision_gjk.c @@ -0,0 +1,439 @@ +// Copyright 2024 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 "engine/engine_collision_gjk.h" + +#include + +#include +#include "engine/engine_util_blas.h" +#include "engine/engine_util_spatial.h" +#include "engine/engine_collision_convex.h" + +// Computes the shortest distance between the origin and an n-simplex (n <= 3) and returns the +// barycentric coordinates of the closest point in the simplex. This is the so called distance +// sub-algorithm of the original 1988 GJK algorithm. +// +// We have adapted the Signed Volume method for our approach from the paper: +// Improving the GJK Algorithm for Faster and More Reliable Distance Queries Between Two +// Convex Objects, Montanari et al, ToG 2017. +static void signedVolume(mjtNum lambda[4], const mjtNum simplex[12], int n); + +// these internal functions compute the barycentric coordinates of the closest point +// to the origin in the n-simplex, where n = 3, 2, 1 respectively +static void S3D(mjtNum lambda[4], const mjtNum simplex[12]); +static void S2D(mjtNum lambda[3], const mjtNum simplex[9]); +static void S1D(mjtNum lambda[2], const mjtNum simplex[6]); + +// helper function to compute the support point in the Minkowski difference +static void support(mjtNum res[3], mjtCCObj* obj1, mjtCCObj* obj2, const mjtNum dir[3]); + +// linear algebra utility functions +static mjtNum det3(const mjtNum v1[3], const mjtNum v2[3], const mjtNum v3[3]); +static void lincomb(mjtNum res[3], const mjtNum* coef, const mjtNum* v, int n); + + + +// returns the distance between the two given objects given an initial guess x0 +mjtNum mj_gjk(const mjGjkConfig* config, mjtCCObj* obj1, mjtCCObj* obj2, const mjtNum x0[3]) { + // our current simplex with max 4 vertices due to only 3 dimensions + mjtNum simplex[12]; + int n = 0; // number of vertices in the simplex + + // the kth approximation point with initial value x_0 + mjtNum x_k[3]; + mju_copy3(x_k, x0); + + int N = config->max_iterations; + for (size_t k = 0; k < N; k++) { + mjtNum s_k[3]; // the kth support point + mjtNum lambda[4]; // barycentric coordinates for x_k + + // compute the kth support point in the direction of -(x_k) + mjtNum x_k_neg[3]; + mju_scl3(x_k_neg, x_k, -1); + mju_normalize3(x_k_neg); + support(s_k, obj1, obj2, x_k_neg); + + // the stopping criteria relies on the Frank-Wolfe duality gap given by + // f(x_k) - f(x_min) <= < grad f(x_k), (x_k - s_k) > + mjtNum diff[3]; + mju_sub3(diff, x_k, s_k); + if (2*mju_dot3(x_k, diff) < config->tolerance) { + return mju_norm3(x_k); + } + + // copy new support point into the simplex + mju_copy3(simplex + 3*n++, s_k); + + // run the distance subalgorithm to compute the barycentric coordinates + // of the closest point to the origin in the simplex + signedVolume(lambda, simplex, n); + lincomb(x_k, lambda, simplex, 4); + + // for lambda[i] == 0, remove the ith vertex from the simplex + n = 0; + for (int i = 0; i < 4; i++) { + if (lambda[i] == 0) continue; + mju_copy3(simplex + 3*n++, simplex + 3*i); + } + } + + return mju_norm3(x_k); +} + + + +// helper function to compute the support point in the Minkowski difference +static void support(mjtNum res[3], mjtCCObj* obj1, mjtCCObj* obj2, const mjtNum dir[3]) { + mjtNum s1[3], s2[3], dir_neg[3]; + mju_scl3(dir_neg, dir, -1); + + // compute S_{A-B}(dir) = S_A(dir) - S_B(-dir) + mjc_support(s1, obj1, dir); + mjc_support(s2, obj2, dir_neg); + mju_sub3(res, s1, s2); +} + + + +// linear combination of n 3D vectors: +// res = coef[0]*v[0] + ... + coef[n-1]*v[3*(n-1)] +static void lincomb(mjtNum res[3], const mjtNum* coef, const mjtNum* v, int n) { + mju_zero3(res); + for (int i = 0; i < n; i++) { + if (coef[i] == 0) continue; + res[0] += coef[i] * v[3*i + 0]; + res[1] += coef[i] * v[3*i + 1]; + res[2] += coef[i] * v[3*i + 2]; + } +} + + + +// returns determinant of the 3x3 matrix with columns v1, v2, v3 +static mjtNum det3(const mjtNum v1[3], const mjtNum v2[3], const mjtNum v3[3]) { + mjtNum temp[3]; + mju_cross(temp, v2, v3); + return mju_dot3(v1, temp); +} + + + +// returns true only when a and b are both strictly positive or both strictly negative +static int compareSigns(mjtNum a, mjtNum b) { + if (a > 0 && b > 0) return 1; + if (a < 0 && b < 0) return 1; + return 0; +} + + + +// computes the barycentric coordinates of the closest point to the origin in the n-simplex +void signedVolume(mjtNum lambda[4], const mjtNum simplex[12], int n) { + int r = n - 1; // spatial dimension of the simplex + mju_zero4(lambda); + + if (r == 3) { + S3D(lambda, simplex); + } else if (r == 2) { + S2D(lambda, simplex); + } else if (r == 1) { + S1D(lambda, simplex); + } else { + lambda[0] = 1; + } +} + + + +static void S3D(mjtNum lambda[4], const mjtNum simplex[12]) { + // the four vertices of the 3-simplex that correspond to 4 support points + const mjtNum* s1 = simplex; + const mjtNum* s2 = simplex + 3; + const mjtNum* s3 = simplex + 6; + const mjtNum* s4 = simplex + 9; + + // the matrix M is given by + // [[ s1_x, s2_x, s3_x, s4_x ], + // [ s1_y, s2_y, s3_y, s4_y ], + // [ s1_z, s2_z, s3_z, s4_z ], + // [ 1, 1, 1, 1 ]] + // we want to solve M*lambda = P, where P = [p_x, p_y, p_z, 1] with [p_x, p_y, p_z] is the + // origin projected onto the simplex + + // compute cofactors to find det(M) + mjtNum C41 = -det3(s2, s3, s4); + mjtNum C42 = det3(s1, s3, s4); + mjtNum C43 = -det3(s1, s2, s4); + mjtNum C44 = det3(s1, s2, s3); + + // note that m_det = 6*SignVol(simplex) with C4i corresponding to the volume of the 3-simplex + // with vertices {s1, s2, s3, 0} - si + mjtNum m_det = C41 + C42 + C43 + C44; + + int comp1 = compareSigns(m_det, C41), + comp2 = compareSigns(m_det, C42), + comp3 = compareSigns(m_det, C43), + comp4 = compareSigns(m_det, C44); + + // if all signs are the same then the origin is inside the simplex + if (comp1 && comp2 && comp3 && comp4) { + lambda[0] = C41 / m_det; + lambda[1] = C42 / m_det; + lambda[2] = C43 / m_det; + lambda[3] = C44 / m_det; + return; + } + + // find the smallest distance, and use the corresponding barycentric coordinates + mjtNum dist; + + if (!comp1) { + mjtNum lambda_2d[3], verts[9], x[3]; + mju_copy3(verts, s2); + mju_copy3(verts + 3, s3); + mju_copy3(verts + 6, s4); + S2D(lambda_2d, verts); + lincomb(x, lambda_2d, verts, 3); + dist = mju_norm3(x); + lambda[1] = lambda_2d[0]; + lambda[2] = lambda_2d[1]; + lambda[3] = lambda_2d[2]; + } + + if (!comp2) { + mjtNum lambda_2d[3], verts[9], x[3]; + mju_copy3(verts, s1); + mju_copy3(verts + 3, s3); + mju_copy3(verts + 6, s4); + S2D(lambda_2d, verts); + lincomb(x, lambda_2d, verts, 3); + mjtNum d = mju_norm3(x); + if (d < dist) { + lambda[0] = lambda_2d[0]; + lambda[2] = lambda_2d[1]; + lambda[3] = lambda_2d[2]; + dist = d; + } + } + + if (!comp3) { + mjtNum lambda_2d[3], verts[9], x[3]; + mju_copy3(verts, s1); + mju_copy3(verts + 3, s2); + mju_copy3(verts + 6, s4); + S2D(lambda_2d, verts); + lincomb(x, lambda_2d, verts, 3); + mjtNum d = mju_norm3(x); + if (d < dist) { + lambda[0] = lambda_2d[0]; + lambda[1] = lambda_2d[1]; + lambda[3] = lambda_2d[2]; + dist = d; + } + } + + if (!comp4) { + mjtNum lambda_2d[3], verts[9], x[3]; + mju_copy3(verts, s1); + mju_copy3(verts + 3, s2); + mju_copy3(verts + 6, s3); + S2D(lambda_2d, verts); + lincomb(x, lambda_2d, verts, 3); + mjtNum d = mju_norm3(x); + if (d < dist) { + lambda[0] = lambda_2d[0]; + lambda[1] = lambda_2d[1]; + lambda[2] = lambda_2d[2]; + dist = d; + } + } +} + + + +static void S2D(mjtNum lambda[3], const mjtNum simplex[9]) { + // the three vertices of the 2-simplex that correspond to 3 support points + const mjtNum* s1 = simplex; + const mjtNum* s2 = simplex + 3; + const mjtNum* s3 = simplex + 6; + + // compute normal + mjtNum diff1[3], diff2[3], n[3]; + mju_sub3(diff1, s2, s1); + mju_sub3(diff2, s3, s2); + mju_cross(n, diff1, diff2); + + // project origin + mjtNum p_o[3]; + mju_scl3(p_o, n, mju_dot3(n, s1) / mju_dot3(n, n)); + + int index; + mjtNum mu_max = 0; + int k = 1, l = 2; + + for (int i = 0; i < 3; i++) { + mjtNum mu = s2[k]*s3[l] + s1[k]*s2[l] + s3[k]*s1[l] - s2[k]*s1[l] - s3[k]*s2[l] - s1[k]*s3[l]; + if (mju_abs(mu) >= mju_abs(mu_max)) { + mu_max = mu; + index = i; + } + k = l; l = i; + } + + // exclude index component + mjtNum s1_2D[2], s2_2D[2], s3_2D[2], p_o_2D[2]; + + if (index == 0) { + s1_2D[0] = s1[1]; + s1_2D[1] = s1[2]; + + s2_2D[0] = s2[1]; + s2_2D[1] = s2[2]; + + s3_2D[0] = s3[1]; + s3_2D[1] = s3[2]; + + p_o_2D[0] = p_o[1]; + p_o_2D[1] = p_o[2]; + } else if (index == 1) { + s1_2D[0] = s1[9]; + s1_2D[1] = s1[2]; + + s2_2D[0] = s2[0]; + s2_2D[1] = s2[2]; + + s3_2D[0] = s3[0]; + s3_2D[1] = s3[2]; + + p_o_2D[0] = p_o[0]; + p_o_2D[1] = p_o[2]; + } else { + s1_2D[0] = s1[0]; + s1_2D[1] = s1[1]; + + s2_2D[0] = s2[0]; + s2_2D[1] = s2[1]; + + s3_2D[0] = s3[0]; + s3_2D[1] = s3[1]; + + p_o_2D[0] = p_o[0]; + p_o_2D[1] = p_o[1]; + } + + // substitute p_o as a vertex in simplex + mjtNum C1 = p_o_2D[0]*s2_2D[1] + p_o_2D[1]*s3_2D[0] + s2_2D[0]*s3_2D[1] + - p_o_2D[0]*s3_2D[1] - p_o_2D[1]*s2_2D[0] - s3_2D[0]*s2_2D[1]; + + mjtNum C2 = p_o_2D[0]*s3_2D[1] + p_o_2D[1]*s1_2D[0] + s3_2D[0]*s1_2D[1] + - p_o_2D[0]*s1_2D[1] - p_o_2D[1]*s3_2D[0] - s1_2D[0]*s3_2D[1]; + + mjtNum C3 = p_o_2D[0]*s1_2D[1] + p_o_2D[1]*s2_2D[0] + s1_2D[0]*s2_2D[1] + - p_o_2D[0]*s2_2D[1] - p_o_2D[1]*s1_2D[0] - s2_2D[0]*s1_2D[1]; + + int comp1 = compareSigns(mu_max, C1), + comp2 = compareSigns(mu_max, C2), + comp3 = compareSigns(mu_max, C3); + + // inside the simplex + if (comp1 && comp2 && comp3) { + lambda[0] = C1 / mu_max; + lambda[1] = C2 / mu_max; + lambda[2] = C3 / mu_max; + return; + } + + // find the smallest distance, and use the corresponding barycentric coordinates + mjtNum dist; + + if (!comp1) { + mjtNum lambda_1d[4], verts[6], x[3]; + mju_copy3(verts, s2); + mju_copy3(verts + 3, s3); + S1D(lambda_1d, verts); + lincomb(x, lambda_1d, verts, 2); + dist = mju_norm3(x); + lambda[1] = lambda_1d[0]; + lambda[2] = lambda_1d[1]; + } + + if (!comp2) { + mjtNum lambda_1d[4], verts[6], x[3]; + mju_copy3(verts, s1); + mju_copy3(verts + 3, s3); + S1D(lambda_1d, verts); + lincomb(x, lambda_1d, verts, 2); + mjtNum d = mju_norm3(x); + if (d < dist) { + lambda[0] = lambda_1d[0]; + lambda[2] = lambda_1d[1]; + dist = d; + } + } + + if (!comp3) { + mjtNum lambda_1d[4], verts[6], x[3]; + mju_copy3(verts, s1); + mju_copy3(verts + 3, s2); + S1D(lambda_1d, verts); + lincomb(x, lambda_1d, verts, 2); + mjtNum d = mju_norm3(x); + if (d < dist) { + lambda[0] = lambda_1d[0]; + lambda[1] = lambda_1d[1]; + dist = d; + } + } +} + + + +static void S1D(mjtNum lambda[2], const mjtNum simplex[6]) { + // the two vertices of the 1-simplex correspond to 2 support points + const mjtNum* s1 = simplex; + const mjtNum* s2 = simplex + 3; + + // find projection of origin onto the 1-simplex: + // p_o = s2 - / * (s2 - s1) + mjtNum p_o[3]; + mjtNum diff[3]; + mju_sub3(diff, s2, s1); + mjtNum temp1 = mju_dot3(s2, diff); + mjtNum temp2 = mju_dot3(diff, diff); + mju_addScl3(p_o, s2, diff, - temp1 / temp2); + + // find the axis with the largest projection "shadow" of the simplex + mjtNum mu_max = 0; + int index; + for (int i = 0; i < 3; i++) { + mjtNum mu = s1[i] - s2[i]; + if (mju_abs(mu) >= mju_abs(mu_max)) { + mu_max = mu; + index = i; + } + } + + mjtNum C1 = p_o[index] - s2[index]; + mjtNum C2 = s1[index] - p_o[index]; + + // inside the simplex + if (compareSigns(mu_max, C1) && compareSigns(mu_max, C2)) { + lambda[0] = C1 / mu_max; + lambda[1] = C2 / mu_max; + } else { + lambda[0] = 1; + } +} diff --git a/src/engine/engine_collision_gjk.h b/src/engine/engine_collision_gjk.h new file mode 100644 index 00000000..281b0026 --- /dev/null +++ b/src/engine/engine_collision_gjk.h @@ -0,0 +1,40 @@ +// Copyright 2024 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. + +#ifndef MUJOCO_SRC_ENGINE_ENGINE_COLLISION_GJK_H_ +#define MUJOCO_SRC_ENGINE_ENGINE_COLLISION_GJK_H_ + +#include +#include +#include "engine/engine_collision_convex.h" + +#ifdef __cplusplus +extern "C" { +#endif + +// internal struct with settings for GJK +struct _mjGjkConfig { + mjtNum max_iterations; + mjtNum tolerance; +}; +typedef struct _mjGjkConfig mjGjkConfig; + +// Returns the distance between the two objects given an initial guess x0. +MJAPI mjtNum mj_gjk(const mjGjkConfig* config, mjtCCObj* obj1, mjtCCObj* obj2, const mjtNum x0[3]); + +#ifdef __cplusplus +} +#endif + +#endif // MUJOCO_SRC_ENGINE_ENGINE_COLLISION_GJK_H_ diff --git a/test/engine/CMakeLists.txt b/test/engine/CMakeLists.txt index f35bb8b6..b844e061 100644 --- a/test/engine/CMakeLists.txt +++ b/test/engine/CMakeLists.txt @@ -21,6 +21,9 @@ target_link_libraries(engine_collision_convex_test fixture gmock) mujoco_test(engine_collision_driver_test) target_link_libraries(engine_collision_driver_test fixture gmock) +mujoco_test(engine_collision_gjk_test) +target_link_libraries(engine_collision_gjk_test fixture gmock ccd) + mujoco_test(engine_core_constraint_test) target_link_libraries(engine_core_constraint_test fixture gmock) diff --git a/test/engine/engine_collision_gjk_test.cc b/test/engine/engine_collision_gjk_test.cc new file mode 100644 index 00000000..29780cc0 --- /dev/null +++ b/test/engine/engine_collision_gjk_test.cc @@ -0,0 +1,136 @@ +// Copyright 2024 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. + +// Tests for engine/engine_collision_gjk.c. + +#include "src/engine/engine_collision_gjk.h" + +#include + +#include "src/engine/engine_collision_convex.h" +#include +#include +#include "test/fixture.h" +#include +#include + +namespace mujoco { +namespace { + +using ::testing::NotNull; + +mjtNum run_gjk(mjModel* m, mjData* d, int g1, int g2, mjtNum* x_0) { + mjGjkConfig config = {100, 1e-6}; + mjtCCObj obj1 = {m, d, g1, -1, -1, -1, -1, 0, {1, 0, 0, 0}}; + mjtCCObj obj2 = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}}; + return mj_gjk(&config, &obj1, &obj2, x_0); +} + +using MjGjkTest = MujocoTest; + +TEST_F(MjGjkTest, SphereSphereIntersect) { + mjtNum x_0[3] = {-2, 0, 0}; + static constexpr char xml[] = R"( + + + + + + + )"; + + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); + + mjData* data = mj_makeData(model); + mj_forward(model, data); + + int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); + int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + mjtNum dist = run_gjk(model, data, geom1, geom2, x_0); + + EXPECT_EQ(dist, 0); + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(MjGjkTest, SphereSphere) { + mjtNum x_0[3] = {-2, 0, 0}; + static constexpr char xml[] = R"( + + + + + + + + + + + + + )"; + + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); + + mjData* data = mj_makeData(model); + mj_forward(model, data); + + int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); + int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + mjtNum dist = run_gjk(model, data, geom1, geom2, x_0); + + EXPECT_EQ(dist, 1); + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(MjGjkTest, BoxBox) { + mjtNum x_0[3] = {-3, .5, 0}; + static constexpr char xml[] = R"( + + + + + + + )"; + + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); + + mjData* data = mj_makeData(model); + mj_forward(model, data); + + int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); + int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + mjtNum dist = run_gjk(model, data, geom1, geom2, x_0); + + EXPECT_EQ(dist, 1); + mj_deleteData(data); + mj_deleteModel(model); +} + +} // namespace +} // namespace mujoco From 14fe527ea2efb83f263cdda9ad1fd436f190c9b4 Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Fri, 21 Jun 2024 07:03:20 -0700 Subject: [PATCH 13/52] Fix small typo/bug in gjk implementation. PiperOrigin-RevId: 645378191 Change-Id: Icdba53b30c934ac3059bb6f7b6d1081d71823131 --- src/engine/engine_collision_gjk.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c index c7c8c97d..42a3043a 100644 --- a/src/engine/engine_collision_gjk.c +++ b/src/engine/engine_collision_gjk.c @@ -309,7 +309,7 @@ static void S2D(mjtNum lambda[3], const mjtNum simplex[9]) { p_o_2D[0] = p_o[1]; p_o_2D[1] = p_o[2]; } else if (index == 1) { - s1_2D[0] = s1[9]; + s1_2D[0] = s1[0]; s1_2D[1] = s1[2]; s2_2D[0] = s2[0]; From e66b9a36b2bd6d9d811cf663bf4a727fc0678b33 Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Fri, 21 Jun 2024 08:11:48 -0700 Subject: [PATCH 14/52] Remove mj_makeEmptyFileVFS and mj_findFileVFS from MuJoCo API. PiperOrigin-RevId: 645393666 Change-Id: Iabe939c88f60b80dce20f84c7ff9a6396717cc84 --- doc/APIreference/functions.rst | 22 +-------- doc/changelog.rst | 36 +++++++++------ doc/includes/references.h | 2 - include/mujoco/mujoco.h | 10 +--- introspect/functions.py | 48 +------------------- src/user/user_vfs.h | 6 +-- unity/Runtime/Bindings/MjBindings.cs | 6 --- unity/Runtime/Tools/MjVfs.cs | 12 ----- unity/Tests/Editor/Tools/MjVFSEditorTests.cs | 7 --- 9 files changed, 32 insertions(+), 117 deletions(-) diff --git a/doc/APIreference/functions.rst b/doc/APIreference/functions.rst index c55c15ea..87a8d02b 100644 --- a/doc/APIreference/functions.rst +++ b/doc/APIreference/functions.rst @@ -1203,7 +1203,7 @@ mj_defaultVFS .. mujoco-include:: mj_defaultVFS -Initialize VFS to empty (no deallocation). +Initialize an empty VFS, mj_deleteVFS must be called to deallocate the VFS. .. _mj_addFileVFS: @@ -1224,15 +1224,6 @@ mj_addBufferVFS Add file to VFS from buffer, return 0: success, 1: full, 2: repeated name, -1: failed to load. -.. _mj_findFileVFS: - -mj_findFileVFS -~~~~~~~~~~~~~~ - -.. mujoco-include:: mj_findFileVFS - -Return file index in VFS, or -1 if not found in VFS. - .. _mj_deleteFileVFS: mj_deleteFileVFS @@ -1249,16 +1240,7 @@ mj_deleteVFS .. mujoco-include:: mj_deleteVFS -Delete all files from VFS. - -.. _mj_makeEmptyFileVFS: - -mj_makeEmptyFileVFS -~~~~~~~~~~~~~~~~~~~ - -.. mujoco-include:: mj_makeEmptyFileVFS - -deprecated: use mj_copyBufferVFS. +Delete all files from VFS and deallocates VFS internal memory. .. _Initialization: diff --git a/doc/changelog.rst b/doc/changelog.rst index 7aa15b76..7a4bfdd2 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -7,7 +7,17 @@ Upcoming version (not yet released) General ^^^^^^^ -1. Added a new API for :doc:`procedural model manipulation`. Fixes :github:issue:`364`. +.. admonition:: Breaking API changes + :class: attention + + 1. Removed deprecated ``mj_makeEmptyFileVFS`` and ``mj_findFileVFS`` functions. + + **Migration:** Use ``mj_addBufferVFS`` to copy a buffer into a VFS file directly. + + 2. Calls to ``mj_defaultVFS`` may allocate memory inside VFS, and the corresponding + ``mj_deleteVFS`` must be called to deallocate any internal allocated memory. + +3. Added a new API for :doc:`procedural model manipulation`. Fixes :github:issue:`364`. Still missing: - Detailed documentation. @@ -17,15 +27,15 @@ General :align: right :width: 240px -2. Added support for orthographic cameras. This is available for both fixed cameras and the free camera, using the +4. Added support for orthographic cameras. This is available for both fixed cameras and the free camera, using the :ref:`camera/orthographic` and :ref:`global/orthographic` attributes, respectively. -3. Added :ref:`maxhullvert`, the maximum number of vertices in a mesh's convex hull. -4. Added :ref:`mj_setKeyframe` for saving the current state into a model keyframe. -5. Added support for ``ball`` joints in the URDF parser ("spherical" in URDF). -6. Deprecated :ref:`mju_rotVecMat` and :ref:`mju_rotVecMatT` in favor of :ref:`mju_mulMatVec3` and +5. Added :ref:`maxhullvert`, the maximum number of vertices in a mesh's convex hull. +6. Added :ref:`mj_setKeyframe` for saving the current state into a model keyframe. +7. Added support for ``ball`` joints in the URDF parser ("spherical" in URDF). +8. Deprecated :ref:`mju_rotVecMat` and :ref:`mju_rotVecMatT` in favor of :ref:`mju_mulMatVec3` and :ref:`mju_mulMatTVec3`. These functions names and argument ordering are more consistent with the rest of the API. -7. Replaced ``mjUSEDOUBLE`` which was previously hard-coded in +9. Replaced ``mjUSEDOUBLE`` which was previously hard-coded in `mjtnum.h `__ with the build-time flag ``mjUSESINGLE``. If this symbol is not defined, MuJoCo will use double-precision floating point, as usual. If ``mjUSESINGLE`` is defined, MuJoCo will use single-precision floating point. See :ref:`mjtNum`. @@ -34,9 +44,9 @@ General MJX ~~~ -8. Added support for :ref:`elliptic friction cones`. -9. Fixed a bug that resulted in less-optimal linesearch solutions for some difficult constraint settings. -10. Fixed a bug in the Newton solver that sometimes resulted in less-optimal gradients. +10. Added support for :ref:`elliptic friction cones`. +11. Fixed a bug that resulted in less-optimal linesearch solutions for some difficult constraint settings. +12. Fixed a bug in the Newton solver that sometimes resulted in less-optimal gradients. .. youtube:: P83tKA1iz2Y @@ -45,14 +55,14 @@ MJX Simulate ^^^^^^^^ -11. Added improved tutorial video. -12. Improved the Brownian noise generator. +13. Added improved tutorial video. +14. Improved the Brownian noise generator. |br| |br| |br| |br| Python bindings ^^^^^^^^^^^^^^^ -13. Fixed a memory leak when using ``copy.deepcopy()`` on a ``mujoco.MjData`` instance (:github:issue:`1572`). +15. Fixed a memory leak when using ``copy.deepcopy()`` on a ``mujoco.MjData`` instance (:github:issue:`1572`). Version 3.1.6 (Jun 3, 2024) --------------------------- diff --git a/doc/includes/references.h b/doc/includes/references.h index 2e3e3429..0f71cf53 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -3068,10 +3068,8 @@ typedef struct mjvSceneState_ mjvSceneState; void mj_defaultVFS(mjVFS* vfs); int mj_addFileVFS(mjVFS* vfs, const char* directory, const char* filename); int mj_addBufferVFS(mjVFS* vfs, const char* name, const void* buffer, int nbuffer); -int mj_findFileVFS(const mjVFS* vfs, const char* filename); int mj_deleteFileVFS(mjVFS* vfs, const char* filename); void mj_deleteVFS(mjVFS* vfs); -int mj_makeEmptyFileVFS(mjVFS* vfs, const char* filename, int filesize); mjModel* mj_loadXML(const char* filename, const mjVFS* vfs, char* error, int error_sz); mjSpec* mj_parseXML(const char* filename, const mjVFS* vfs, char* error, int error_sz); mjSpec* mj_parseXMLString(const char* xml, const mjVFS* vfs, char* error, int error_sz); diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index 1c85099f..6cb6f7e5 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -74,7 +74,7 @@ MJAPI extern const char* mjRNDSTRING[mjNRNDFLAG][3]; //---------------------------------- Virtual file system ------------------------------------------- -// Initialize VFS to empty (no deallocation). +// Initialize an empty VFS, mj_deleteVFS must be called to deallocate the VFS. MJAPI void mj_defaultVFS(mjVFS* vfs); // Add file to VFS, return 0: success, 1: full, 2: repeated name, -1: failed to load. @@ -83,18 +83,12 @@ MJAPI int mj_addFileVFS(mjVFS* vfs, const char* directory, const char* filename) // Add file to VFS from buffer, return 0: success, 1: full, 2: repeated name, -1: failed to load. MJAPI int mj_addBufferVFS(mjVFS* vfs, const char* name, const void* buffer, int nbuffer); -// Return file index in VFS, or -1 if not found in VFS. -MJAPI int mj_findFileVFS(const mjVFS* vfs, const char* filename); - // Delete file from VFS, return 0: success, -1: not found in VFS. MJAPI int mj_deleteFileVFS(mjVFS* vfs, const char* filename); -// Delete all files from VFS. +// Delete all files from VFS and deallocates VFS internal memory. MJAPI void mj_deleteVFS(mjVFS* vfs); -// deprecated: use mj_copyBufferVFS. -MJAPI int mj_makeEmptyFileVFS(mjVFS* vfs, const char* filename, int filesize); - //---------------------------------- Parse and compile --------------------------------------------- diff --git a/introspect/functions.py b/introspect/functions.py index 9733755b..58985da5 100644 --- a/introspect/functions.py +++ b/introspect/functions.py @@ -38,7 +38,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), ), ), - doc='Initialize VFS to empty (no deallocation).', + doc='Initialize an empty VFS, mj_deleteVFS must be called to deallocate the VFS.', # pylint: disable=line-too-long )), ('mj_addFileVFS', FunctionDecl( @@ -96,26 +96,6 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), doc='Add file to VFS from buffer, return 0: success, 1: full, 2: repeated name, -1: failed to load.', # pylint: disable=line-too-long )), - ('mj_findFileVFS', - FunctionDecl( - name='mj_findFileVFS', - return_type=ValueType(name='int'), - parameters=( - FunctionParameterDecl( - name='vfs', - type=PointerType( - inner_type=ValueType(name='mjVFS', is_const=True), - ), - ), - FunctionParameterDecl( - name='filename', - type=PointerType( - inner_type=ValueType(name='char', is_const=True), - ), - ), - ), - doc='Return file index in VFS, or -1 if not found in VFS.', - )), ('mj_deleteFileVFS', FunctionDecl( name='mj_deleteFileVFS', @@ -148,31 +128,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), ), ), - doc='Delete all files from VFS.', - )), - ('mj_makeEmptyFileVFS', - FunctionDecl( - name='mj_makeEmptyFileVFS', - return_type=ValueType(name='int'), - parameters=( - FunctionParameterDecl( - name='vfs', - type=PointerType( - inner_type=ValueType(name='mjVFS'), - ), - ), - FunctionParameterDecl( - name='filename', - type=PointerType( - inner_type=ValueType(name='char', is_const=True), - ), - ), - FunctionParameterDecl( - name='filesize', - type=ValueType(name='int'), - ), - ), - doc='deprecated: use mj_copyBufferVFS.', + doc='Delete all files from VFS and deallocates VFS internal memory.', )), ('mj_loadXML', FunctionDecl( diff --git a/src/user/user_vfs.h b/src/user/user_vfs.h index 80a7fcef..8896ac6d 100644 --- a/src/user/user_vfs.h +++ b/src/user/user_vfs.h @@ -27,17 +27,17 @@ extern "C" { #endif -// initialize to empty (no deallocation) +// Initialize an empty VFS, mj_deleteVFS must be called to deallocate the VFS MJAPI void mj_defaultVFS(mjVFS* vfs); // add file to VFS, return 0: success, 1: full, 2: repeated name, -1: not found on disk MJAPI int mj_addFileVFS(mjVFS* vfs, const char* directory, const char* filename); -// deprecated: use mj_copyBufferVFS +// deprecated: use mj_addBufferVFS MJAPI int mj_makeEmptyFileVFS(mjVFS* vfs, const char* filename, int filesize); // add file from buffer into VFS, return 0: success, 1: full, 2: repeated name, -1: failed to load -MJAPI int mj_copyBufferVFS(mjVFS* vfs, const char* filename, const void* buffer, int nbuffer); +MJAPI int mj_addBufferVFS(mjVFS* vfs, const char* filename, const void* buffer, int nbuffer); // return file index in VFS, or -1 if not found in VFS MJAPI int mj_findFileVFS(const mjVFS* vfs, const char* filename); diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 0d551cdd..c6afdeff 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -6376,18 +6376,12 @@ public static unsafe extern int mj_addFileVFS(void* vfs, [MarshalAs(UnmanagedTyp [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern int mj_addBufferVFS(void* vfs, [MarshalAs(UnmanagedType.LPStr)]string name, void* buffer, int nbuffer); -[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] -public static unsafe extern int mj_findFileVFS(void* vfs, [MarshalAs(UnmanagedType.LPStr)]string filename); - [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern int mj_deleteFileVFS(void* vfs, [MarshalAs(UnmanagedType.LPStr)]string filename); [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern void mj_deleteVFS(void* vfs); -[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] -public static unsafe extern int mj_makeEmptyFileVFS(void* vfs, [MarshalAs(UnmanagedType.LPStr)]string filename, int filesize); - [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern mjModel_* mj_loadXML([MarshalAs(UnmanagedType.LPStr)]string filename, void* vfs, StringBuilder error, int error_sz); diff --git a/unity/Runtime/Tools/MjVfs.cs b/unity/Runtime/Tools/MjVfs.cs index a52da4b2..14174e6a 100644 --- a/unity/Runtime/Tools/MjVfs.cs +++ b/unity/Runtime/Tools/MjVfs.cs @@ -59,20 +59,8 @@ public sealed class MjVfs : IDisposable { } } - // Searches the VFS for the specified file and returns its index. - // The index then can be used to retrieve the file contents from Data.filedata array. - public unsafe int FindFile(string filename) { - return mj_findFileVFS(_unmanagedVfs.ToPointer(), filename); - } - // Loads a model from the specified file. - // The file is assumed to be located in the filesystem. If it's not found, the method will throw - // an ArgumentException. public unsafe MujocoLib.mjModel_* LoadXML(string filename) { - if (FindFile(filename) < 0) { - throw new ArgumentException($"File {filename} was not added to the VFS."); - } - var errorBuf = new StringBuilder(1024); MujocoLib.mjModel_* model = MujocoLib.mj_loadXML( filename, _unmanagedVfs.ToPointer(), errorBuf, errorBuf.Capacity); diff --git a/unity/Tests/Editor/Tools/MjVFSEditorTests.cs b/unity/Tests/Editor/Tools/MjVFSEditorTests.cs index b43ea404..12ab6562 100644 --- a/unity/Tests/Editor/Tools/MjVFSEditorTests.cs +++ b/unity/Tests/Editor/Tools/MjVFSEditorTests.cs @@ -42,12 +42,5 @@ namespace Mujoco { _vfs.AddFile(filename, contents); Assert.That(_vfs.FilesCount, Is.EqualTo(1)); } - - [Test] - public unsafe void LocatingFileUsingMujocoLib() { - var filename = "filename"; - _vfs.AddFile(filename, "contents"); - Assert.That(_vfs.FindFile(filename), Is.EqualTo(0)); - } } } From ee6a1dba286ab4b548d0b9421ec1000f6c91e2ef Mon Sep 17 00:00:00 2001 From: Emo Todorov Date: Mon, 24 Jun 2024 05:30:25 -0700 Subject: [PATCH 15/52] UI improvements. Fixes #1274. Copybara import of the project: -- a1297d997d0b3cc624bbd1e704ffb7fbb2c5db9d by Emo Todorov : UI imrovements When OpenGL buffer size is too small to hold entire UI with all sections open, older sections (as determined by latest mouse click) are automatically closed as needed. To test this new functionality, uncomment the test near the top of mjui_resize(). UI items now have userid, which can be used for event handling. It is set through the (new) last field of mjuiDef, called otherint. Sections titles can now have a checkmark - in which case they are rendered differently. Set via mjuiDef.otherint: 0- none (as before), 1- rendering changed but box not shown, 2- box shown. This can be used to label sections of special interest to the application. -- b0405bfb3005586ef1d2656ea9cc43c608225927 by Emo Todorov : more ui improvements Implemented mjPRESERVE state for adding sections and separators, so you don't have to keep track of old states. simulate.cc is now modified to use this new functionality. I also made the separators in the Physics section collapsible. If you don't like it, change them back to state 1. Also added functionality for handling section checkboxes. -- 10f4c49be62a6605a38b7c0c7bfae1629024ba31 by Emo Todorov : even more ui improvements Selection boxes that extend below the current UI height are finally rendered in full. Sections and collapsible separators now have rounded corners. The radius can be controlled from theme spacing. More theme color settings were added. In particular, color gradients can now be disabled (the two end colors are set independently). -- 96c2dc601a25d31c93b21f8e089d349e729e829e by Emo Todorov : final ui improvements Cleaned up the code a bit. Added more control for color gradients. -- c9eddb679f36df3f0745d37e3fa90c4e66ee6b98 by Emo Todorov : bug fix Fixed some rendering bugs in the new functionality. COPYBARA_INTEGRATE_REVIEW=https://github.com/google-deepmind/mujoco/pull/1749 from emotodorov:ui_improve ddfc21dbcf0482b8f0d5eacab21847c302f71440 PiperOrigin-RevId: 646060325 Change-Id: Idb27aae7cd0311d00f26387bcadf5891175e867d --- doc/APIreference/APItypes.rst | 12 + doc/includes/references.h | 23 +- include/mujoco/mjui.h | 26 +- introspect/enums.py | 10 + introspect/structs.py | 87 +- mjx/requirements.txt | 3 +- simulate/simulate.cc | 108 ++- simulate/simulate.h | 6 +- src/ui/ui_main.c | 1088 +++++++++++++++++--------- unity/Runtime/Bindings/MjBindings.cs | 18 + 10 files changed, 946 insertions(+), 435 deletions(-) diff --git a/doc/APIreference/APItypes.rst b/doc/APIreference/APItypes.rst index e5ff73ff..db4c797e 100644 --- a/doc/APIreference/APItypes.rst +++ b/doc/APIreference/APItypes.rst @@ -582,6 +582,18 @@ Item types used in the UI framework. .. mujoco-include:: mjtItem + +.. _mjtSection: + +mjtSection +~~~~~~~~~~ + +State of a UI section. + +.. mujoco-include:: mjtSection + + + .. _tySpecEnums: Spec diff --git a/doc/includes/references.h b/doc/includes/references.h index 0f71cf53..d951fa75 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -2286,6 +2286,11 @@ typedef enum mjtItem_ { // UI item type mjNITEM // number of item types } mjtItem; +typedef enum mjtSection_ { // UI section state + mjSECT_CLOSED = 0, // closed state (regular section) + mjSECT_OPEN, // open state (regular section) + mjSECT_FIXED // fixed section: always open, no title +} mjtSection; struct mjuiState_ { // mouse and keyboard state // constants set by user int nrect; // number of rectangles used @@ -2333,6 +2338,8 @@ struct mjuiThemeSpacing_ { // UI visualization theme spacing int scroll; // scrollbar width int label; // label width int section; // section gap + int cornersect; // corner radius for section + int cornersep; // corner radius for separator int itemside; // item side gap int itemmid; // item middle gap int itemver; // item vertical gap @@ -2346,9 +2353,14 @@ struct mjuiThemeColor_ { // UI visualization theme color float master[3]; // master background float thumb[3]; // scrollbar thumb float secttitle[3]; // section title + float secttitle2[3]; // section title: bottom color + float secttitlecheck[3]; // section title with checkbox + float secttitlecheck2[3]; // section title with checkbox: bottom color float sectfont[3]; // section font float sectsymbol[3]; // section symbol float sectpane[3]; // section pane + float separator[3]; // separator title + float separator2[3]; // separator title: bottom color float shortcut[3]; // shortcut background float fontactive[3]; // font active float fontinactive[3]; // font inactive @@ -2398,6 +2410,7 @@ struct mjuiItem_ { // UI item void *pdata; // data pointer (type-specific) int sectionid; // id of section containing item int itemid; // id of item within section + int userid; // user-supplied id (for event handling) // type-specific properties union { @@ -2409,20 +2422,23 @@ struct mjuiItem_ { // UI item // internal mjrRect rect; // rectangle occupied by item + int skip; // item skipped due to closed separator }; typedef struct mjuiItem_ mjuiItem; struct mjuiSection_ { // UI section // properties char name[mjMAXUINAME]; // name - int state; // 0: closed, 1: open + int state; // section state (mjtSection) int modifier; // 0: none, 1: control, 2: shift; 4: alt int shortcut; // shortcut key; 0: undefined + int checkbox; // 0: none, 1: hidden, 2: unchecked, 2: checked int nitem; // number of items in use mjuiItem item[mjMAXUIITEM]; // preallocated array of items // internal mjrRect rtitle; // rectangle occupied by title mjrRect rcontent; // rectangle occupied by content + int lastclick; // last mouse click over this section }; typedef struct mjuiSection_ mjuiSection; struct mjUI_ { // entire UI @@ -2441,10 +2457,12 @@ struct mjUI_ { // entire UI int maxheight; // height when all sections open int scroll; // scroll from top of UI - // mouse focus + // mouse focus and count int mousesect; // 0: none, -1: scroll, otherwise 1+section int mouseitem; // item within section int mousehelp; // help button down: print shortcuts + int mouseclicks; // number of mouse clicks over UI + int mousesectcheck; // 0: none, otherwise 1+section // keyboard focus and edit int editsect; // 0: none, otherwise 1+section @@ -2465,6 +2483,7 @@ struct mjuiDef_ { // table passed to mjui_add() int state; // state void* pdata; // pointer to data char other[mjMAXUITEXT]; // string with type-specific properties + int otherint; // int with type-specific properties }; typedef struct mjuiDef_ mjuiDef; typedef enum mjtCatBit_ { // bitflags for mjvGeom category diff --git a/include/mujoco/mjui.h b/include/mujoco/mjui.h index c6d4c34d..7981d936 100644 --- a/include/mujoco/mjui.h +++ b/include/mujoco/mjui.h @@ -26,6 +26,7 @@ #define mjMAXUIRECT 25 // maximum number of rectangles #define mjSEPCLOSED 1000 // closed state of adjustable separator +#define mjPRESERVE 2000 // preserve section or separator state // key codes matching GLFW (user must remap for other frameworks) @@ -106,6 +107,13 @@ typedef enum mjtItem_ { // UI item type } mjtItem; +typedef enum mjtSection_ { // UI section state + mjSECT_CLOSED = 0, // closed state (regular section) + mjSECT_OPEN, // open state (regular section) + mjSECT_FIXED // fixed section: always open, no title +} mjtSection; + + // predicate function: set enable/disable based on item category typedef int (*mjfItemEnable)(int category, void* data); @@ -163,6 +171,8 @@ struct mjuiThemeSpacing_ { // UI visualization theme spacing int scroll; // scrollbar width int label; // label width int section; // section gap + int cornersect; // corner radius for section + int cornersep; // corner radius for separator int itemside; // item side gap int itemmid; // item middle gap int itemver; // item vertical gap @@ -180,9 +190,14 @@ struct mjuiThemeColor_ { // UI visualization theme color float master[3]; // master background float thumb[3]; // scrollbar thumb float secttitle[3]; // section title + float secttitle2[3]; // section title: bottom color + float secttitlecheck[3]; // section title with checkbox + float secttitlecheck2[3]; // section title with checkbox: bottom color float sectfont[3]; // section font float sectsymbol[3]; // section symbol float sectpane[3]; // section pane + float separator[3]; // separator title + float separator2[3]; // separator title: bottom color float shortcut[3]; // shortcut background float fontactive[3]; // font active float fontinactive[3]; // font inactive @@ -236,6 +251,7 @@ struct mjuiItem_ { // UI item void *pdata; // data pointer (type-specific) int sectionid; // id of section containing item int itemid; // id of item within section + int userid; // user-supplied id (for event handling) // type-specific properties union { @@ -247,6 +263,7 @@ struct mjuiItem_ { // UI item // internal mjrRect rect; // rectangle occupied by item + int skip; // item skipped due to closed separator }; typedef struct mjuiItem_ mjuiItem; @@ -256,15 +273,17 @@ typedef struct mjuiItem_ mjuiItem; struct mjuiSection_ { // UI section // properties char name[mjMAXUINAME]; // name - int state; // 0: closed, 1: open + int state; // section state (mjtSection) int modifier; // 0: none, 1: control, 2: shift; 4: alt int shortcut; // shortcut key; 0: undefined + int checkbox; // 0: none, 1: hidden, 2: unchecked, 2: checked int nitem; // number of items in use mjuiItem item[mjMAXUIITEM]; // preallocated array of items // internal mjrRect rtitle; // rectangle occupied by title mjrRect rcontent; // rectangle occupied by content + int lastclick; // last mouse click over this section }; typedef struct mjuiSection_ mjuiSection; @@ -287,10 +306,12 @@ struct mjUI_ { // entire UI int maxheight; // height when all sections open int scroll; // scroll from top of UI - // mouse focus + // mouse focus and count int mousesect; // 0: none, -1: scroll, otherwise 1+section int mouseitem; // item within section int mousehelp; // help button down: print shortcuts + int mouseclicks; // number of mouse clicks over UI + int mousesectcheck; // 0: none, otherwise 1+section // keyboard focus and edit int editsect; // 0: none, otherwise 1+section @@ -315,6 +336,7 @@ struct mjuiDef_ { // table passed to mjui_add() int state; // state void* pdata; // pointer to data char other[mjMAXUITEXT]; // string with type-specific properties + int otherint; // int with type-specific properties }; typedef struct mjuiDef_ mjuiDef; diff --git a/introspect/enums.py b/introspect/enums.py index 7d22f36f..0542e17e 100644 --- a/introspect/enums.py +++ b/introspect/enums.py @@ -800,4 +800,14 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjNITEM', 14), ]), )), + ('mjtSection', + EnumDecl( + name='mjtSection', + declname='enum mjtSection_', + values=dict([ + ('mjSECT_CLOSED', 0), + ('mjSECT_OPEN', 1), + ('mjSECT_FIXED', 2), + ]), + )), ]) diff --git a/introspect/structs.py b/introspect/structs.py index b3e92f77..47ad0e6a 100644 --- a/introspect/structs.py +++ b/introspect/structs.py @@ -11350,6 +11350,16 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=ValueType(name='int'), doc='section gap', ), + StructFieldDecl( + name='cornersect', + type=ValueType(name='int'), + doc='corner radius for section', + ), + StructFieldDecl( + name='cornersep', + type=ValueType(name='int'), + doc='corner radius for separator', + ), StructFieldDecl( name='itemside', type=ValueType(name='int'), @@ -11416,6 +11426,30 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='section title', ), + StructFieldDecl( + name='secttitle2', + type=ArrayType( + inner_type=ValueType(name='float'), + extents=(3,), + ), + doc='section title: bottom color', + ), + StructFieldDecl( + name='secttitlecheck', + type=ArrayType( + inner_type=ValueType(name='float'), + extents=(3,), + ), + doc='section title with checkbox', + ), + StructFieldDecl( + name='secttitlecheck2', + type=ArrayType( + inner_type=ValueType(name='float'), + extents=(3,), + ), + doc='section title with checkbox: bottom color', + ), StructFieldDecl( name='sectfont', type=ArrayType( @@ -11440,6 +11474,22 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='section pane', ), + StructFieldDecl( + name='separator', + type=ArrayType( + inner_type=ValueType(name='float'), + extents=(3,), + ), + doc='separator title', + ), + StructFieldDecl( + name='separator2', + type=ArrayType( + inner_type=ValueType(name='float'), + extents=(3,), + ), + doc='separator title: bottom color', + ), StructFieldDecl( name='shortcut', type=ArrayType( @@ -11602,6 +11652,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=ValueType(name='int'), doc='id of item within section', ), + StructFieldDecl( + name='userid', + type=ValueType(name='int'), + doc='user-supplied id (for event handling)', + ), AnonymousUnionDecl( fields=( StructFieldDecl( @@ -11631,6 +11686,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=ValueType(name='mjrRect'), doc='rectangle occupied by item', ), + StructFieldDecl( + name='skip', + type=ValueType(name='int'), + doc='item skipped due to closed separator', + ), ), )), ('mjuiSection', @@ -11649,7 +11709,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([ StructFieldDecl( name='state', type=ValueType(name='int'), - doc='0: closed, 1: open', + doc='section state (mjtSection)', ), StructFieldDecl( name='modifier', @@ -11661,6 +11721,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=ValueType(name='int'), doc='shortcut key; 0: undefined', ), + StructFieldDecl( + name='checkbox', + type=ValueType(name='int'), + doc='0: none, 1: hidden, 2: unchecked, 2: checked', + ), StructFieldDecl( name='nitem', type=ValueType(name='int'), @@ -11684,6 +11749,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=ValueType(name='mjrRect'), doc='rectangle occupied by content', ), + StructFieldDecl( + name='lastclick', + type=ValueType(name='int'), + doc='last mouse click over this section', + ), ), )), ('mjUI', @@ -11763,6 +11833,16 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=ValueType(name='int'), doc='help button down: print shortcuts', ), + StructFieldDecl( + name='mouseclicks', + type=ValueType(name='int'), + doc='number of mouse clicks over UI', + ), + StructFieldDecl( + name='mousesectcheck', + type=ValueType(name='int'), + doc='0: none, otherwise 1+section', + ), StructFieldDecl( name='editsect', type=ValueType(name='int'), @@ -11851,6 +11931,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='string with type-specific properties', ), + StructFieldDecl( + name='otherint', + type=ValueType(name='int'), + doc='int with type-specific properties', + ), ), )), ]) diff --git a/mjx/requirements.txt b/mjx/requirements.txt index 49434299..927cd134 100644 --- a/mjx/requirements.txt +++ b/mjx/requirements.txt @@ -35,7 +35,8 @@ jaxlib==0.4.18; python_version >= '3.9' \ --hash=sha256:9593ff69f424947567e206f3e356b2a2df55ca68e6d815d5adc6cae308e8f652 \ --hash=sha256:2b17b3f05b3bbf8e0ddb85fba339525ac03bac21c9f26d0f83dcea1b1654353e \ --hash=sha256:b35ec08984e2aa5e96ba3f3f8b88e90dee0283649e037f213dec8e85638fa17d \ - --hash=sha256:0bcc4768d29be80d20fd542aafd3a02510a5b1e47c7953beef8b03a9941fa64d + --hash=sha256:0bcc4768d29be80d20fd542aafd3a02510a5b1e47c7953beef8b03a9941fa64d \ + --hash=sha256:1e4b0a0d2cfad3905123af2aa0afc2f6fcfaa9a2f74eb247cd5f140645817ab2 pip==23.3.1 \ --hash=sha256:55eb67bb6171d37447e82213be585b75fe2b12b359e993773aca4de9247a052b pytest==7.4.2 \ diff --git a/simulate/simulate.cc b/simulate/simulate.cc index a424e310..3dc3d239 100644 --- a/simulate/simulate.cc +++ b/simulate/simulate.cc @@ -117,13 +117,13 @@ enum { // file section of UI const mjuiDef defFile[] = { - {mjITEM_SECTION, "File", 1, nullptr, "AF"}, - {mjITEM_BUTTON, "Save xml", 2, nullptr, ""}, - {mjITEM_BUTTON, "Save mjb", 2, nullptr, ""}, - {mjITEM_BUTTON, "Print model", 2, nullptr, "CM"}, - {mjITEM_BUTTON, "Print data", 2, nullptr, "CD"}, - {mjITEM_BUTTON, "Quit", 1, nullptr, "CQ"}, - {mjITEM_BUTTON, "Screenshot", 2, nullptr, "CP"}, + {mjITEM_SECTION, "File", mjPRESERVE, nullptr, "AF"}, + {mjITEM_BUTTON, "Save xml", 2, nullptr, ""}, + {mjITEM_BUTTON, "Save mjb", 2, nullptr, ""}, + {mjITEM_BUTTON, "Print model", 2, nullptr, "CM"}, + {mjITEM_BUTTON, "Print data", 2, nullptr, "CD"}, + {mjITEM_BUTTON, "Quit", 1, nullptr, "CQ"}, + {mjITEM_BUTTON, "Screenshot", 2, nullptr, "CP"}, {mjITEM_END} }; @@ -674,15 +674,15 @@ void UpdateWatch(mj::Simulate* sim, const mjModel* m, const mjData* d) { //---------------------------------- UI construction ----------------------------------------------- // make physics section of UI -void MakePhysicsSection(mj::Simulate* sim, int oldstate) { +void MakePhysicsSection(mj::Simulate* sim) { mjOption* opt = sim->is_passive_ ? &sim->scnstate_.model.opt : &sim->m_->opt; mjuiDef defPhysics[] = { - {mjITEM_SECTION, "Physics", oldstate, nullptr, "AP"}, + {mjITEM_SECTION, "Physics", mjPRESERVE, nullptr, "AP"}, {mjITEM_SELECT, "Integrator", 2, &(opt->integrator), "Euler\nRK4\nimplicit\nimplicitfast"}, {mjITEM_SELECT, "Cone", 2, &(opt->cone), "Pyramidal\nElliptic"}, {mjITEM_SELECT, "Jacobian", 2, &(opt->jacobian), "Dense\nSparse\nAuto"}, {mjITEM_SELECT, "Solver", 2, &(opt->solver), "PGS\nCG\nNewton"}, - {mjITEM_SEPARATOR, "Algorithmic Parameters", 1}, + {mjITEM_SEPARATOR, "Algorithmic Parameters", mjPRESERVE}, {mjITEM_EDITNUM, "Timestep", 2, &(opt->timestep), "1 0 1"}, {mjITEM_EDITINT, "Iterations", 2, &(opt->iterations), "1 0 1000"}, {mjITEM_EDITNUM, "Tolerance", 2, &(opt->tolerance), "1 0 1"}, @@ -695,22 +695,22 @@ void MakePhysicsSection(mj::Simulate* sim, int oldstate) { {mjITEM_EDITNUM, "API Rate", 2, &(opt->apirate), "1 0 1000"}, {mjITEM_EDITINT, "SDF Iter", 2, &(opt->sdf_iterations), "1 1 20"}, {mjITEM_EDITINT, "SDF Init", 2, &(opt->sdf_initpoints), "1 1 100"}, - {mjITEM_SEPARATOR, "Physical Parameters", 1}, + {mjITEM_SEPARATOR, "Physical Parameters", mjPRESERVE}, {mjITEM_EDITNUM, "Gravity", 2, opt->gravity, "3"}, {mjITEM_EDITNUM, "Wind", 2, opt->wind, "3"}, {mjITEM_EDITNUM, "Magnetic", 2, opt->magnetic, "3"}, {mjITEM_EDITNUM, "Density", 2, &(opt->density), "1"}, {mjITEM_EDITNUM, "Viscosity", 2, &(opt->viscosity), "1"}, {mjITEM_EDITNUM, "Imp Ratio", 2, &(opt->impratio), "1"}, - {mjITEM_SEPARATOR, "Disable Flags", 1}, + {mjITEM_SEPARATOR, "Disable Flags", mjPRESERVE}, {mjITEM_END} }; mjuiDef defEnableFlags[] = { - {mjITEM_SEPARATOR, "Enable Flags", 1}, + {mjITEM_SEPARATOR, "Enable Flags", mjPRESERVE}, {mjITEM_END} }; mjuiDef defOverride[] = { - {mjITEM_SEPARATOR, "Contact Override", 1}, + {mjITEM_SEPARATOR, "Contact Override", mjPRESERVE}, {mjITEM_EDITNUM, "Margin", 2, &(opt->o_margin), "1"}, {mjITEM_EDITNUM, "Sol Imp", 2, &(opt->o_solimp), "5"}, {mjITEM_EDITNUM, "Sol Ref", 2, &(opt->o_solref), "2"}, @@ -718,7 +718,7 @@ void MakePhysicsSection(mj::Simulate* sim, int oldstate) { {mjITEM_END} }; mjuiDef defDisableActuator[] = { - {mjITEM_SEPARATOR, "Actuator Group Enable", 1}, + {mjITEM_SEPARATOR, "Actuator Group Enable", mjPRESERVE}, {mjITEM_CHECKBYTE, "Act Group 0", 2, sim->enableactuator+0, ""}, {mjITEM_CHECKBYTE, "Act Group 1", 2, sim->enableactuator+1, ""}, {mjITEM_CHECKBYTE, "Act Group 2", 2, sim->enableactuator+2, ""}, @@ -757,12 +757,12 @@ void MakePhysicsSection(mj::Simulate* sim, int oldstate) { // make rendering section of UI -void MakeRenderingSection(mj::Simulate* sim, const mjModel* m, int oldstate) { +void MakeRenderingSection(mj::Simulate* sim, const mjModel* m) { mjuiDef defRendering[] = { { mjITEM_SECTION, "Rendering", - oldstate, + mjPRESERVE, nullptr, "AR" }, @@ -876,18 +876,18 @@ void MakeRenderingSection(mj::Simulate* sim, const mjModel* m, int oldstate) { } // make visualization section of UI -void MakeVisualizationSection(mj::Simulate* sim, const mjModel* m, int oldstate) { +void MakeVisualizationSection(mj::Simulate* sim, const mjModel* m) { mjStatistic* stat = sim->is_passive_ ? &sim->scnstate_.model.stat : &sim->m_->stat; mjVisual* vis = sim->is_passive_ ? &sim->scnstate_.model.vis : &sim->m_->vis; mjuiDef defVisualization[] = { - {mjITEM_SECTION, "Visualization", oldstate, nullptr, "AV"}, + {mjITEM_SECTION, "Visualization", mjPRESERVE, nullptr, "AV"}, {mjITEM_SEPARATOR, "Headlight", 1}, {mjITEM_RADIO, "Active", 5, &(vis->headlight.active), "Off\nOn"}, {mjITEM_EDITFLOAT, "Ambient", 2, &(vis->headlight.ambient), "3"}, {mjITEM_EDITFLOAT, "Diffuse", 2, &(vis->headlight.diffuse), "3"}, {mjITEM_EDITFLOAT, "Specular", 2, &(vis->headlight.specular), "3"}, - {mjITEM_SEPARATOR, "Free Camera", 1}, + {mjITEM_SEPARATOR, "Free Camera", 1}, {mjITEM_RADIO, "Orthographic", 2, &(vis->global.orthographic), "No\nYes"}, {mjITEM_EDITFLOAT, "Field of view", 2, &(vis->global.fovy), "1"}, {mjITEM_EDITNUM, "Center", 2, &(stat->center), "3"}, @@ -937,9 +937,9 @@ void MakeVisualizationSection(mj::Simulate* sim, const mjModel* m, int oldstate) } // make group section of UI -void MakeGroupSection(mj::Simulate* sim, int oldstate) { +void MakeGroupSection(mj::Simulate* sim) { mjuiDef defGroup[] = { - {mjITEM_SECTION, "Group enable", oldstate, nullptr, "AG"}, + {mjITEM_SECTION, "Group enable", mjPRESERVE, nullptr, "AG"}, {mjITEM_SEPARATOR, "Geom groups", 1}, {mjITEM_CHECKBYTE, "Geom 0", 2, sim->opt.geomgroup, " 0"}, {mjITEM_CHECKBYTE, "Geom 1", 2, sim->opt.geomgroup+1, " 1"}, @@ -997,9 +997,9 @@ void MakeGroupSection(mj::Simulate* sim, int oldstate) { } // make joint section of UI -void MakeJointSection(mj::Simulate* sim, int oldstate) { +void MakeJointSection(mj::Simulate* sim) { mjuiDef defJoint[] = { - {mjITEM_SECTION, "Joint", oldstate, nullptr, "AJ"}, + {mjITEM_SECTION, "Joint", mjPRESERVE, nullptr, "AJ"}, {mjITEM_END} }; mjuiDef defSlider[] = { @@ -1050,9 +1050,9 @@ void MakeJointSection(mj::Simulate* sim, int oldstate) { } // make control section of UI -void MakeControlSection(mj::Simulate* sim, int oldstate) { +void MakeControlSection(mj::Simulate* sim) { mjuiDef defControl[] = { - {mjITEM_SECTION, "Control", oldstate, nullptr, "AC"}, + {mjITEM_SECTION, "Control", mjPRESERVE, nullptr, "AC"}, {mjITEM_BUTTON, "Clear all", 2}, {mjITEM_END} }; @@ -1107,35 +1107,17 @@ void MakeControlSection(mj::Simulate* sim, int oldstate) { // make model-dependent UI sections void MakeUiSections(mj::Simulate* sim, const mjModel* m, const mjData* d) { - // get section open-close state, UI 0 - int oldstate0[NSECT0]; - for (int i=0; iui0.nsect>i) { - oldstate0[i] = sim->ui0.sect[i].state; - } - } - - // get section open-close state, UI 1 - int oldstate1[NSECT1]; - for (int i=0; iui1.nsect>i) { - oldstate1[i] = sim->ui1.sect[i].state; - } - } - // clear model-dependent sections of UI sim->ui0.nsect = SECT_PHYSICS; sim->ui1.nsect = 0; // make - MakePhysicsSection(sim, oldstate0[SECT_PHYSICS]); - MakeRenderingSection(sim, m, oldstate0[SECT_RENDERING]); - MakeVisualizationSection(sim, m, oldstate0[SECT_VISUALIZATION]); - MakeGroupSection(sim, oldstate0[SECT_GROUP]); - MakeJointSection(sim, oldstate1[SECT_JOINT]); - MakeControlSection(sim, oldstate1[SECT_CONTROL]); + MakePhysicsSection(sim); + MakeRenderingSection(sim, m); + MakeVisualizationSection(sim, m); + MakeGroupSection(sim); + MakeJointSection(sim); + MakeControlSection(sim); } //---------------------------------- utility functions --------------------------------------------- @@ -1265,7 +1247,7 @@ int ComputeFontScale(const mj::PlatformUIAdapter& platform_ui) { fs = 150; } fs = mju_round(fs * 0.02) * 50; - fs = mjMIN(250, mjMAX(100, fs)); + fs = mjMIN(300, mjMAX(100, fs)); return fs; } @@ -1322,9 +1304,22 @@ void UiLayout(mjuiState* state) { rect[3].height = rect[0].height; } +// modify UI void UiModify(mjUI* ui, mjuiState* state, mjrContext* con) { mjui_resize(ui, con); - mjr_addAux(ui->auxid, ui->width, ui->maxheight, ui->spacing.samples, con); + + // remake aux buffer only if missing or different + int id = ui->auxid; + if (con->auxFBO[id] == 0 || + con->auxFBO_r[id] == 0 || + con->auxColor[id] == 0 || + con->auxColor_r[id] == 0 || + con->auxWidth[id] != ui->width || + con->auxHeight[id] != ui->maxheight || + con->auxSamples[id] != ui->spacing.samples) { + mjr_addAux(id, ui->width, ui->maxheight, ui->spacing.samples, con); + } + UiLayout(state); mjui_update(-1, -1, ui, state, con); } @@ -1504,7 +1499,7 @@ void UiEvent(mjuiState* state) { // remake joint section if joint group changed if (it->name[0]=='J' && it->name[1]=='o') { sim->ui1.nsect = SECT_JOINT; - MakeJointSection(sim, sim->ui1.sect[SECT_JOINT].state); + MakeJointSection(sim); sim->ui1.nsect = NSECT1; UiModify(&sim->ui1, state, &sim->platform_ui->mjr_context()); } @@ -2457,7 +2452,7 @@ void Simulate::Render() { if (pending_.ui_remake_ctrl) { if (this->ui1_enable && this->ui1.sect[SECT_CONTROL].state) { this->ui1.nsect = SECT_CONTROL; - MakeControlSection(this, this->ui1.sect[SECT_CONTROL].state); + MakeControlSection(this); this->ui1.nsect = NSECT1; UiModify(&this->ui1, &this->uistate, &this->platform_ui->mjr_context()); } @@ -2645,12 +2640,15 @@ void Simulate::RenderLoop() { this->platform_ui->SetEventCallback(UiEvent); this->platform_ui->SetLayoutCallback(UiLayout); - // populate uis with standard sections + // populate uis with standard sections, open some sections initially this->ui0.userdata = this; this->ui1.userdata = this; mjui_add(&this->ui0, defFile); mjui_add(&this->ui0, this->def_option); mjui_add(&this->ui0, this->def_simulation); + this->ui0.sect[0].state = 1; + this->ui0.sect[1].state = 1; + this->ui0.sect[2].state = 1; mjui_add(&this->ui0, this->def_watch); UiModify(&this->ui0, &this->uistate, &this->platform_ui->mjr_context()); UiModify(&this->ui1, &this->uistate, &this->platform_ui->mjr_context()); diff --git a/simulate/simulate.h b/simulate/simulate.h index 5c385a74..bffacb37 100644 --- a/simulate/simulate.h +++ b/simulate/simulate.h @@ -265,7 +265,7 @@ class Simulate { // Constant arrays needed for the option section of UI and the UI interface // TODO setting the size here is not ideal const mjuiDef def_option[13] = { - {mjITEM_SECTION, "Option", 1, nullptr, "AO"}, + {mjITEM_SECTION, "Option", mjPRESERVE, nullptr, "AO"}, {mjITEM_CHECKINT, "Help", 2, &this->help, " #290"}, {mjITEM_CHECKINT, "Info", 2, &this->info, " #291"}, {mjITEM_CHECKINT, "Profiler", 2, &this->profiler, " #292"}, @@ -287,7 +287,7 @@ class Simulate { // simulation section of UI const mjuiDef def_simulation[14] = { - {mjITEM_SECTION, "Simulation", 1, nullptr, "AS"}, + {mjITEM_SECTION, "Simulation", mjPRESERVE, nullptr, "AS"}, {mjITEM_RADIO, "", 5, &this->run, "Pause\nRun"}, {mjITEM_BUTTON, "Reset", 2, nullptr, " #259"}, {mjITEM_BUTTON, "Reload", 5, nullptr, "CL"}, @@ -306,7 +306,7 @@ class Simulate { // watch section of UI const mjuiDef def_watch[5] = { - {mjITEM_SECTION, "Watch", 0, nullptr, "AW"}, + {mjITEM_SECTION, "Watch", mjPRESERVE, nullptr, "AW"}, {mjITEM_EDITTXT, "Field", 2, this->field, "qpos"}, {mjITEM_EDITINT, "Index", 2, &this->index, "1"}, {mjITEM_STATIC, "Value", 2, nullptr, " "}, diff --git a/src/ui/ui_main.c b/src/ui/ui_main.c index 523d9cca..6ccf1421 100644 --- a/src/ui/ui_main.c +++ b/src/ui/ui_main.c @@ -14,6 +14,7 @@ #include "ui/ui_main.h" +#include #include #include @@ -26,142 +27,165 @@ // theme spacing 0 : tight static const mjuiThemeSpacing themeSpacing0 = { - 270, // int total; - 15, // int scroll; - 120, // int label; - 8, // int section; - 4, // int itemside; - 4, // int itemmid; - 4, // int itemver; - 8, // int texthor; - 4, // int textver; - 30, // int linescroll; - 4 // int samples; + 270, // total + 15, // scroll + 120, // label + 8, // section + 6, // cornersect + 6, // cornersep + 4, // itemside + 4, // itemmid + 4, // itemver + 8, // texthor + 4, // textver + 30, // linescroll + 4 // samples }; // theme spacing 1 : wide static const mjuiThemeSpacing themeSpacing1 = { - 310, // int total; - 15, // int scroll; - 120, // int label; - 10, // int section; - 7, // int itemside; - 7, // int itemmid; - 7, // int itemver; - 10, // int texthor; - 5, // int textver; - 30, // int linescroll; - 4 // int samples; + 310, // total + 15, // scroll + 120, // label + 10, // section + 10, // cornersect + 10, // cornersep + 7, // itemside + 7, // itemmid + 7, // itemver + 10, // texthor + 5, // textver + 30, // linescroll + 4 // samples }; // theme color 0 : default static const mjuiThemeColor themeColor0 = { - {0.25, 0.25, 0.25}, // float master[3]; - {0.12, 0.12, 0.12}, // float thumb[3]; - {0.6, 0.2, 0.2}, // float secttitle[3]; - {1.0, 1.0, 1.0}, // float sectfont[3]; - {0.7, 0.7, 0.7}, // float sectsymbol[3]; - {0.1, 0.1, 0.1}, // float sectpane[3]; - {0.0, 0.0, 1.0}, // float shortcut[3]; - {1.0, 1.0, 1.0}, // float fontactive[3]; - {0.5, 0.5, 0.5}, // float fontinactive[3]; - {0.3, 0.3, 0.3}, // float decorinactive[3]; - {0.4, 0.4, 0.4}, // float decorinactive2[3]; - {0.6, 0.4, 0.4}, // float button[3]; - {0.4, 0.4, 0.7}, // float check[3]; - {0.4, 0.6, 0.4}, // float radio[3]; - {0.4, 0.6, 0.6}, // float select[3]; - {0.2, 0.3, 0.3}, // float select2[3]; - {0.3, 0.2, 0.3}, // float slider[3]; - {0.6, 0.4, 0.6}, // float slider2[3]; - {0.6, 0.6, 0.4}, // float edit[3]; - {0.7, 0.0, 0.0}, // float edit2[3]; - {0.9, 0.9, 0.9} // float cursor[3]; + {0.25, 0.25, 0.25}, // master + {0.12, 0.12, 0.12}, // thumb + {0.6, 0.2, 0.2}, // secttitle + {0.1, 0.1, 0.1}, // secttitle2 + {0.45, 0.17, 0.17}, // secttitlecheck + {0.45, 0.17, 0.17}, // secttitlecheck2 + {1.0, 1.0, 1.0}, // sectfont + {0.7, 0.7, 0.7}, // sectsymbol + {0.1, 0.1, 0.1}, // sectpane + {0.25, 0.25, 0.25}, // separator + {0.1, 0.1, 0.1}, // separator2 + {0.0, 0.0, 1.0}, // shortcut + {1.0, 1.0, 1.0}, // fontactive + {0.5, 0.5, 0.5}, // fontinactive + {0.3, 0.3, 0.3}, // decorinactive + {0.4, 0.4, 0.4}, // decorinactive2 + {0.6, 0.4, 0.4}, // button + {0.4, 0.4, 0.7}, // check + {0.4, 0.6, 0.4}, // radio + {0.4, 0.6, 0.6}, // select + {0.2, 0.3, 0.3}, // select2 + {0.3, 0.2, 0.3}, // slider + {0.6, 0.4, 0.6}, // slider2 + {0.6, 0.6, 0.4}, // edit + {0.7, 0.0, 0.0}, // edit2 + {0.9, 0.9, 0.9} // cursor }; // theme color 1 : orange static const mjuiThemeColor themeColor1 = { - {0.2, 0.2, 0.2}, // float master[3]; - {0.12, 0.12, 0.12}, // float thumb[3]; - {0.3, 0.3, 0.3}, // float secttitle[3]; - {0.8, 0.8, 0.8}, // float sectfont[3]; - {0.7, 0.7, 0.7}, // float sectsymbol[3]; - {0.15, 0.15, 0.15}, // float sectpane[3]; - {0.0, 0.0, 1.0}, // float shortcut[3]; - {0.9, 0.9, 0.9}, // float fontactive[3]; - {0.5, 0.5, 0.5}, // float fontinactive[3]; - {0.2, 0.2, 0.2}, // float decorinactive[3]; - {0.25, 0.25, 0.25}, // float decorinactive2[3]; - {0.6, 0.4, 0.2}, // float button[3]; - {0.6, 0.4, 0.2}, // float check[3]; - {0.6, 0.4, 0.2}, // float radio[3]; - {0.6, 0.4, 0.2}, // float select[3]; - {0.3, 0.2, 0.1}, // float select2[3]; - {0.2, 0.2, 0.2}, // float slider[3]; - {0.6, 0.4, 0.2}, // float slider2[3]; - {0.6, 0.4, 0.2}, // float edit[3]; - {0.7, 0.0, 0.0}, // float edit2[3]; - {0.9, 0.9, 0.9} // float cursor[3]; + {0.2, 0.2, 0.2}, // master + {0.12, 0.12, 0.12}, // thumb + {0.3, 0.3, 0.3}, // secttitle + {0.15, 0.15, 0.15}, // secttitle2 + {0.25, 0.25, 0.25}, // secttitlecheck + {0.25, 0.25, 0.25}, // secttitlecheck2 + {0.8, 0.8, 0.8}, // sectfont + {0.7, 0.7, 0.7}, // sectsymbol + {0.15, 0.15, 0.15}, // sectpane + {0.2, 0.2, 0.2}, // separator + {0.15, 0.15, 0.15}, // separator2 + {0.0, 0.0, 1.0}, // shortcut + {0.9, 0.9, 0.9}, // fontactive + {0.5, 0.5, 0.5}, // fontinactive + {0.2, 0.2, 0.2}, // decorinactive + {0.25, 0.25, 0.25}, // decorinactive2 + {0.6, 0.4, 0.2}, // button + {0.6, 0.4, 0.2}, // check + {0.6, 0.4, 0.2}, // radio + {0.6, 0.4, 0.2}, // select + {0.3, 0.2, 0.1}, // select2 + {0.2, 0.2, 0.2}, // slider + {0.6, 0.4, 0.2}, // slider2 + {0.6, 0.4, 0.2}, // edit + {0.7, 0.0, 0.0}, // edit2 + {0.9, 0.9, 0.9} // cursor }; // theme color 2 : white static const mjuiThemeColor themeColor2 = { - {0.9, 0.9, 0.9}, // float master[3]; - {0.7, 0.7, 0.7}, // float thumb[3]; - {0.8, 0.8, 0.8}, // float secttitle[3]; - {0.0, 0.0, 0.8}, // float sectfont[3]; - {0.0, 0.0, 0.8}, // float sectsymbol[3]; - {1.0, 1.0, 1.0}, // float sectpane[3]; - {0.0, 1.0, 1.0}, // float shortcut[3]; - {0.0, 0.0, 0.0}, // float fontactive[3]; - {0.7, 0.7, 0.7}, // float fontinactive[3]; - {0.95, 0.95, 0.95}, // float decorinactive[3]; - {0.9, 0.9, 0.9}, // float decorinactive2[3]; - {0.8, 0.8, 0.8}, // float button[3]; - {0.8, 0.8, 0.8}, // float check[3]; - {0.8, 0.8, 0.8}, // float radio[3]; - {0.8, 0.8, 0.8}, // float select[3]; - {0.9, 0.9, 0.9}, // float select2[3]; - {0.95, 0.95, 0.95}, // float slider[3]; - {0.8, 0.8, 0.8}, // float slider2[3]; - {0.8, 0.8, 0.8}, // float edit[3]; - {1.0, 0.3, 0.3}, // float edit2[3]; - {0.2, 0.2, 0.2} // float cursor[3]; + {0.9, 0.9, 0.9}, // master + {0.7, 0.7, 0.7}, // thumb + {0.8, 0.8, 0.8}, // secttitle + {1.0, 1.0, 1.0}, // secttitle2 + {0.95, 0.95, 0.95}, // secttitlecheck + {0.95, 0.95, 0.95}, // secttitlecheck2 + {0.0, 0.0, 0.8}, // sectfont + {0.0, 0.0, 0.8}, // sectsymbol + {1.0, 1.0, 1.0}, // sectpane + {0.9, 0.9, 0.9}, // separator + {1.0, 1.0, 1.0}, // separator2 + {0.0, 1.0, 1.0}, // shortcut + {0.0, 0.0, 0.0}, // fontactive + {0.7, 0.7, 0.7}, // fontinactive + {0.95, 0.95, 0.95}, // decorinactive + {0.9, 0.9, 0.9}, // decorinactive2 + {0.8, 0.8, 0.8}, // button + {0.8, 0.8, 0.8}, // check + {0.8, 0.8, 0.8}, // radio + {0.8, 0.8, 0.8}, // select + {0.9, 0.9, 0.9}, // select2 + {0.95, 0.95, 0.95}, // slider + {0.8, 0.8, 0.8}, // slider2 + {0.8, 0.8, 0.8}, // edit + {1.0, 0.3, 0.3}, // edit2 + {0.2, 0.2, 0.2} // cursor }; // theme color 3 : black static const mjuiThemeColor themeColor3 = { - {0.15, 0.15, 0.15}, // float master[3]; - {0.3, 0.3, 0.3}, // float thumb[3]; - {0.25, 0.25, 0.25}, // float secttitle[3]; - {1.0, 0.3, 0.3}, // float sectfont[3]; - {1.0, 0.3, 0.3}, // float sectsymbol[3]; - {0.0, 0.0, 0.0}, // float sectpane[3]; - {0.0, 0.0, 1.0}, // float shortcut[3]; - {1.0, 1.0, 1.0}, // float fontactive[3]; - {0.4, 0.4, 0.4}, // float fontinactive[3]; - {0.1, 0.1, 0.1}, // float decorinactive[3]; - {0.15, 0.15, 0.15}, // float decorinactive2[3]; - {0.3, 0.3, 0.3}, // float button[3]; - {0.3, 0.3, 0.3}, // float check[3]; - {0.3, 0.3, 0.3}, // float radio[3]; - {0.3, 0.3, 0.3}, // float select[3]; - {0.15, 0.15, 0.15}, // float select2[3]; - {0.15, 0.15, 0.15}, // float slider[3]; - {0.3, 0.3, 0.3}, // float slider2[3]; - {0.3, 0.3, 0.3}, // float edit[3]; - {0.8, 0.2, 0.2}, // float edit2[3]; - {0.8, 0.8, 0.8} // float cursor[3]; + {0.15, 0.15, 0.15}, // master + {0.3, 0.3, 0.3}, // thumb + {0.25, 0.25, 0.25}, // secttitle + {0.0, 0.0, 0.0}, // secttitle2 + {0.2, 0.2, 0.2}, // secttitlecheck + {0.2, 0.2, 0.2}, // secttitlecheck2 + {1.0, 0.3, 0.3}, // sectfont + {1.0, 0.3, 0.3}, // sectsymbol + {0.0, 0.0, 0.0}, // sectpane + {0.15, 0.15, 0.15}, // separator + {0.0, 0.0, 0.0}, // separator2 + {0.0, 0.0, 1.0}, // shortcut + {1.0, 1.0, 1.0}, // fontactive + {0.4, 0.4, 0.4}, // fontinactive + {0.1, 0.1, 0.1}, // decorinactive + {0.15, 0.15, 0.15}, // decorinactive2 + {0.3, 0.3, 0.3}, // button + {0.3, 0.3, 0.3}, // check + {0.3, 0.3, 0.3}, // radio + {0.3, 0.3, 0.3}, // select + {0.15, 0.15, 0.15}, // select2 + {0.15, 0.15, 0.15}, // slider + {0.3, 0.3, 0.3}, // slider2 + {0.3, 0.3, 0.3}, // edit + {0.8, 0.2, 0.2}, // edit2 + {0.8, 0.8, 0.8} // cursor }; - //------------------------------------ Utility functions ------------------------------------------- // scale from abstract pixels to framebuffer units @@ -172,7 +196,7 @@ static int SCL(int sz, const mjrContext* con) { // init OpenGL -static void initOpenGL(const mjUI* ui, const mjrContext* con) { +static void initOpenGL(const mjrRect* r, const mjrContext* con) { // set OpenGL options glDisable(GL_NORMALIZE); glDisable(GL_DEPTH_TEST); @@ -186,12 +210,12 @@ static void initOpenGL(const mjUI* ui, const mjrContext* con) { // standard 2D projection, in framebuffer units glMatrixMode(GL_PROJECTION); glLoadIdentity(); - glOrtho(0, ui->width, 0, ui->height, -1, 1); + glOrtho(0, r->width, 0, r->height, -1, 1); glMatrixMode(GL_MODELVIEW); glLoadIdentity(); // set viewport - glViewport(0, 0, ui->width, ui->height); + glViewport(r->left, r->bottom, r->width, r->height); } @@ -277,10 +301,72 @@ static void drawrectangle(mjrRect rect, const float* rgb, const float* rgbback, +// round corners of rectangle +static void roundcorner(mjrRect rect, int flg_skipbottom, int flg_separator, + const mjUI* ui, const mjrContext* con) { + // get rounding from theme, exit if disabled + int cornerspec = flg_separator ? ui->spacing.cornersep : ui->spacing.cornersect; + if (cornerspec == 0) { + return; + } + + // quarter-circle divisions and radius + int ndivide = 10; + double radius = cornerspec * 0.01 * con->fontScale; + + // draw fans in the four corners, optionally skip bottom corners + for (int ic = (flg_skipbottom ? 2 : 0); ic < 4; ++ic) { + // set corner + double corner[2]; + switch (ic) { + case 0: // bottom-left + corner[0] = rect.left; + corner[1] = rect.bottom; + break; + + case 1: // bottom-right + corner[0] = rect.left + rect.width; + corner[1] = rect.bottom; + break; + + case 2: // top-right + corner[0] = rect.left + rect.width; + corner[1] = rect.bottom + rect.height; + break; + + default: // top-left + corner[0] = rect.left; + corner[1] = rect.bottom + rect.height; + } + + // orient fan to point inside + double angle = ic * 0.5 * mjPI; + + // compute circle center: opposite to corner + double center[2]; + center[0] = corner[0] + mju_sqrt(2.0) * radius * cos(angle + 0.25 * mjPI); + center[1] = corner[1] + mju_sqrt(2.0) * radius * sin(angle + 0.25 * mjPI); + + // fill with erase color, start trinagle_fan from corner + glColor3fv(flg_separator ? ui->color.sectpane : ui->color.master); + glBegin(GL_TRIANGLE_FAN); + glVertex2d(corner[0], corner[1]); + + // compute vertices of quarter-circle + for (int i = 0; i <= ndivide; i++) { + double a = angle + mjPI + 0.5 * mjPI * (double)i / (double)ndivide; + glVertex2d(center[0] + radius * cos(a), center[1] + radius * sin(a)); + } + glEnd(); + } +} + + + // draw oval static void drawoval(mjrRect rect, const float* rgb, const float* rgbback, const mjrContext* con) { - const int ndivide = 20; + const int ndivide = 15; // require horizontal if (rect.height > rect.width) { @@ -334,8 +420,9 @@ static void drawoval(mjrRect rect, const float* rgb, const float* rgbback, -// draw section open/closed symbol: section -static void drawsymbol(mjrRect rect, int flg_open, int flg_sep, +// draw open/closed symbol in title +// type: 0- section, 1- section with checkbox, 2- separator +static void drawsymbol(mjrRect rect, int flg_open, int type, const mjUI* ui, const mjrContext* con) { // size and center int texthor = SCL(ui->spacing.texthor, con); @@ -344,7 +431,7 @@ static void drawsymbol(mjrRect rect, int flg_open, int flg_sep, int d = mju_round(con->charHeight*0.33); // separator size - if (flg_sep) { + if (type == 2) { d = mju_round(con->charHeight*0.28); } @@ -360,7 +447,7 @@ static void drawsymbol(mjrRect rect, int flg_open, int flg_sep, // closed else { - // solid + // solid outside glColor3fv(ui->color.sectsymbol); glBegin(GL_TRIANGLES); glVertex2i(cx, cy-d); @@ -368,23 +455,36 @@ static void drawsymbol(mjrRect rect, int flg_open, int flg_sep, glVertex2i(cx-2*d, cy); glEnd(); - // empty - double margin = con->fontScale * 0.015; - double u = 0.5*sqrt(5.0)*margin; - double y = d - u - 0.5*margin; - if (flg_sep) { + // set color for inside + switch (type) { + case 0: // section glColor3f( - (ui->color.master[0] + ui->color.sectpane[0]) * 0.5, - (ui->color.master[1] + ui->color.sectpane[1]) * 0.5, - (ui->color.master[2] + ui->color.sectpane[2]) * 0.5 - ); - } else { + (ui->color.secttitle[0] + ui->color.secttitle2[0]) * 0.5, + (ui->color.secttitle[1] + ui->color.secttitle2[1]) * 0.5, + (ui->color.secttitle[2] + ui->color.secttitle2[2]) * 0.5 + ); + break; + + case 1: // section with checkbox glColor3f( - (ui->color.secttitle[0] + ui->color.sectpane[0]) * 0.5, - (ui->color.secttitle[1] + ui->color.sectpane[1]) * 0.5, - (ui->color.secttitle[2] + ui->color.sectpane[2]) * 0.5 - ); + (ui->color.secttitlecheck[0] + ui->color.secttitlecheck2[0]) * 0.5, + (ui->color.secttitlecheck[1] + ui->color.secttitlecheck2[1]) * 0.5, + (ui->color.secttitlecheck[2] + ui->color.secttitlecheck2[2]) * 0.5 + ); + break; + + case 2: // separator + glColor3f( + (ui->color.separator[0] + ui->color.separator2[0]) * 0.5, + (ui->color.separator[1] + ui->color.separator2[1]) * 0.5, + (ui->color.separator[2] + ui->color.separator2[2]) * 0.5 + ); } + + // draw inside + double margin = con->fontScale * 0.015; + double u = 0.5 * sqrt(5.0) * margin; + double y = d - u - 0.5 * margin; glBegin(GL_TRIANGLES); glVertex2d(cx-margin, cy-y); glVertex2d(cx-margin, cy+y); @@ -636,6 +736,7 @@ static int insideoval(int x, int y, mjrRect r) { // find mouse location in UI; y already inverted // sect: -1: thumb, -2: slider down, -3: slider up, positive: 1+section // item: -1: section title or scroll, non-negative: item number +// item: -2 in checkbox on section title static void findmouse(const mjUI* ui, const mjuiState* ins, const mjrContext* con, int* sect, int* item) { // clear @@ -689,6 +790,16 @@ static void findmouse(const mjUI* ui, const mjuiState* ins, const mjrContext* co if (s->state < 2 && inside(x, y, s->rtitle)) { *sect = n+1; *item = -1; + + // in checkbox + if (s->checkbox > 0) { + mjrRect rcheck = s->rtitle; + rcheck.width = mjMIN(rcheck.height, rcheck.width); + if (inside(x, y, rcheck)) { + *item = -2; + } + } + return; } @@ -1152,18 +1263,32 @@ void mjui_add(mjUI* ui, const mjuiDef* def) { if (strlen(def[n].name) >= mjMAXUINAME-1) { mju_error("mjui_add: section name too long"); } - if (def[n].state < 0 || def[n].state > 2) { + if (def[n].state != mjSECT_CLOSED && def[n].state != mjSECT_OPEN && + def[n].state != mjSECT_FIXED && def[n].state != mjPRESERVE) { mju_error("mjui_add: invalid section state"); } - // add section, clear + // add section, save state ui->nsect++; mjuiSection* se = ui->sect + (ui->nsect-1); - memset(se, 0, sizeof(mjuiSection)); + int oldstate = se->state; - // copy data + // clear, but preserve item states + int itemstate[mjMAXUIITEM]; + for (int i = 0; i < mjMAXUIITEM; ++i) { + itemstate[i] = se->item[i].state; + } + memset(se, 0, sizeof(mjuiSection)); + for (int i = 0; i < mjMAXUIITEM; ++i) { + se->item[i].state = itemstate[i]; + } + + // set or restore section state + se->state = (def[n].state == mjPRESERVE ? oldstate : def[n].state); + + // copy remaining data mjSTRNCPY(se->name, def[n].name); - se->state = def[n].state; + se->checkbox = def[n].otherint; parseshortcut(def[n].other, &(se->modifier), &(se->shortcut)); } @@ -1191,18 +1316,28 @@ void mjui_add(mjUI* ui, const mjuiDef* def) { mju_error("mjui_add: invalid item state"); } - // add item, clear + // add item, save state, clear se->nitem++; mjuiItem* it = se->item + (se->nitem-1); + int oldstate = it->state; memset(it, 0, sizeof(mjuiItem)); + // set or restore state for collapsible separator, copy state for others + if (def[n].type == mjITEM_SEPARATOR && def[n].state == mjPRESERVE) { + // mjSEPCLOSED makes separator collapsible + it->state = (oldstate < mjSEPCLOSED ? mjSEPCLOSED : oldstate); + } + else { + it->state = def[n].state; + } + // copy common data it->type = def[n].type; - it->state = def[n].state; it->pdata = def[n].pdata; mjSTRNCPY(it->name, def[n].name); it->sectionid = ui->nsect - 1; it->itemid = se->nitem - 1; + it->userid = def[n].otherint; // data pointer check if (it->type > mjITEM_BUTTON && it->pdata == 0) { @@ -1343,10 +1478,38 @@ void mjui_addToSection(mjUI* ui, int sect, const mjuiDef* def) { -// Compute UI sizes. -void mjui_resize(mjUI* ui, const mjrContext* con) { +// set item skip flags within section, but not in pass 0 +static void setitemskip(mjuiSection* s, int pass) { + int skip = 0; + + // process section items + for (int i = 0; i < s->nitem; ++i) { + mjuiItem* it = s->item + i; + + // pass 0: nothing is skipped + if (pass == 0) { + it->skip = 0; + continue; + } + + // item is a separator: update skip state for subsequent items + if (it->type == mjITEM_SEPARATOR) { + skip = (it->state == mjSEPCLOSED); + } + + // item is not a separator: set skip state + else { + it->skip = skip; + } + } +} + + + +// Compute UI sizes: internal fuction, may be called twice per resize +static void tryresize(mjUI* ui, const mjrContext* con) { // scale theme sizes - int w_master = SCL(ui->spacing.total, con); + int w_master = SCL(ui->spacing.total, con); int w_scroll = SCL(ui->spacing.scroll, con); int g_section = SCL(ui->spacing.section, con); int g_itemside = SCL(ui->spacing.itemside, con); @@ -1361,183 +1524,279 @@ void mjui_resize(mjUI* ui, const mjrContext* con) { // column width int colwidth = (w_master - w_scroll - 2*g_section - 2*g_itemside - g_itemmid)/2; - // init UI sizes - int height = 0; - int maxheight = 0; + // pass 0 includes skipped items, pass 1 does not + int Height, MaxHeight; + for (int pass = 0; pass < 2; ++pass) { + // init UI heights + int height = 0; + int maxheight = 0; - // process sections - int skip; - for (int n=0; n < ui->nsect; n++) { - // vertical padding before section + // process sections + for (int n = 0; n < ui->nsect; n++) { + // vertical padding before section + height += g_section; + maxheight += g_section; + + // get section pointer + mjuiSection* s = ui->sect + n; + + // set item skip flags for section, depending on pass + setitemskip(s, pass); + + // title rectangle + s->rtitle.left = g_section; + s->rtitle.width = w_master - w_scroll - 2 * g_section; + if (s->state == mjSECT_FIXED) { // fixed section: no title + s->rtitle.bottom = height; + s->rtitle.height = 0; + } + else { // regular section with title + s->rtitle.bottom = height + textheight; + s->rtitle.height = textheight; + } + + // count title height + height += s->rtitle.height; + maxheight += s->rtitle.height; + + // init content rectangle + s->rcontent.left = s->rtitle.left; + s->rcontent.width = s->rtitle.width; + s->rcontent.height = 0; + s->rcontent.bottom = 0; + + // process items within section + for (int i = 0; i < s->nitem; i++) { + // get item pointer, clear rectangle + mjuiItem* it = s->item + i; + memset(&it->rect, 0, sizeof(mjrRect)); + + // item is skipped: nothing to do + if (it->skip) { + continue; + } + + // vertical padding before item + s->rcontent.height += it->type == mjITEM_SEPARATOR ? g_section : g_itemver; + + // packed pair of items + if (i < s->nitem - 1 && s->item[i + 1].type == it->type && + (it->type == mjITEM_BUTTON || + it->type == mjITEM_CHECKINT || + it->type == mjITEM_CHECKBYTE)) { + // get next item pointer + mjuiItem* it1 = s->item + (i + 1); + + // this item rectangle + it->rect.left = s->rcontent.left + g_itemside; + it->rect.width = colwidth; + it->rect.height = textheight; + + // next item rectangle (set bottom here) + it1->rect.left = s->rcontent.left + g_itemside + colwidth + g_itemmid; + it1->rect.width = colwidth; + it1->rect.height = textheight; + it1->rect.bottom = height + s->rcontent.height + it->rect.height; + + // advance + i++; + } + + // single-line item + else { + // common left border (except for labeled controls at the end) + it->rect.left = s->rcontent.left + g_itemside; + + // static + if (it->type == mjITEM_STATIC) { + it->rect.width = s->rcontent.width - 2 * g_itemside; + it->rect.height = (con->charHeight + g_textver) * it->multi.nelem; + } + + // single column + else if (it->type == mjITEM_BUTTON || + it->type == mjITEM_CHECKINT || + it->type == mjITEM_CHECKBYTE) { + it->rect.width = colwidth; + it->rect.height = textheight; + } + + // radio + else if (it->type == mjITEM_RADIO) { + int ncol = ui->radiocol ? ui->radiocol : 2; + int nrow = (it->multi.nelem - 1) / ncol + 1; + it->rect.width = s->rcontent.width - 2 * g_itemside; + it->rect.height = textheight * nrow; + } + + // separator, select, slider, edit, radioline + else { + it->rect.width = s->rcontent.width - 2 * g_itemside; + it->rect.height = textheight; + } + + // add room for label + if (it->name[0] && + (it->type >= mjITEM_RADIO || + it->type >= mjITEM_RADIOLINE || + it->type == mjITEM_STATIC)) { + it->rect.left = s->rcontent.left + g_itemside + g_label; + it->rect.width = s->rcontent.width - (2 * g_itemside + g_label); + } + } + + // set bottom, count height + it->rect.bottom = height + s->rcontent.height + it->rect.height; + s->rcontent.height += it->rect.height; + } + + // vertical padding after last item, compute bottom + s->rcontent.height += g_itemver; + s->rcontent.bottom = height + s->rcontent.height; + + // count content height + if (s->state != mjSECT_CLOSED) { + height += s->rcontent.height; + } + maxheight += s->rcontent.height; + } + + // vertical padding after last section height += g_section; maxheight += g_section; - // get section pointer - mjuiSection* s = ui->sect + n; - - // title rectangle - s->rtitle.left = g_section; - s->rtitle.width = w_master - w_scroll - 2*g_section; - if (s->state < 2) { - s->rtitle.bottom = height + textheight; - s->rtitle.height = textheight; - } else { - s->rtitle.bottom = height; - s->rtitle.height = 0; + // save data: maxheight from pass 0, height from pass 1 + if (pass == 0) { + MaxHeight = maxheight; } - - // count title height - height += s->rtitle.height; - maxheight += s->rtitle.height; - - // init content rectangle - s->rcontent.left = s->rtitle.left; - s->rcontent.width = s->rtitle.width; - s->rcontent.height = 0; - - // process items within section - for (int i=0; i < s->nitem; i++) { - // get item pointer - mjuiItem* it = s->item + i; - - // save section rcontent - mjrRect oldcontent = s->rcontent; - - // determine skip (collapsed separator before item) - skip = 0; - if (i > 0 && it->type != mjITEM_SEPARATOR) { - for (int k=i-1; k >= 0; k--) { - if (s->item[k].type == mjITEM_SEPARATOR) { - // collapsed state: skip items below it - if (s->item[k].state == mjSEPCLOSED) { - skip = 1; - } - - break; - } - } - } - - // vertical padding before item - s->rcontent.height += it->type == mjITEM_SEPARATOR ? g_section : g_itemver; - - // packed pair of items - if (i < s->nitem-1 && s->item[i+1].type == it->type && - (it->type == mjITEM_BUTTON || - it->type == mjITEM_CHECKINT || - it->type == mjITEM_CHECKBYTE)) { - // get next item pointer - mjuiItem* it1 = s->item + (i+1); - - // this item rectangle - it->rect.left = s->rcontent.left + g_itemside; - it->rect.width = colwidth; - it->rect.height = textheight; - - // next item rectangle (set bottom here) - it1->rect.left = s->rcontent.left + g_itemside + colwidth + g_itemmid; - it1->rect.width = colwidth; - it1->rect.height = textheight; - it1->rect.bottom = height + s->rcontent.height + it->rect.height; - - // skip second item in pair - if (skip) { - it1->rect.width = 0; - it1->rect.height = 0; - } - - // advance - i++; - } - - // single-line item - else { - // common left border (except for labeled controls at the end) - it->rect.left = s->rcontent.left + g_itemside; - - // static - if (it->type == mjITEM_STATIC) { - it->rect.width = s->rcontent.width - 2*g_itemside; - it->rect.height = (con->charHeight+g_textver)*it->multi.nelem; - } - - // single column - else if (it->type == mjITEM_BUTTON || - it->type == mjITEM_CHECKINT || - it->type == mjITEM_CHECKBYTE) { - it->rect.width = colwidth; - it->rect.height = textheight; - } - - // radio - else if (it->type == mjITEM_RADIO) { - int ncol = ui->radiocol ? ui->radiocol : 2; - int nrow = (it->multi.nelem-1)/ncol + 1; - it->rect.width = s->rcontent.width - 2*g_itemside; - it->rect.height = textheight*nrow; - } - - // separator, select, slider, edit, radioline - else { - it->rect.width = s->rcontent.width - 2*g_itemside; - it->rect.height = textheight; - } - - // add room for label - if (it->name[0] && - (it->type >= mjITEM_RADIO || - it->type >= mjITEM_RADIOLINE || - it->type == mjITEM_STATIC)) { - it->rect.left = s->rcontent.left + g_itemside + g_label; - it->rect.width = s->rcontent.width - (2*g_itemside + g_label); - } - } - - // set bottom, count height - it->rect.bottom = height + s->rcontent.height + it->rect.height; - s->rcontent.height += it->rect.height; - - // skip item - if (skip) { - maxheight += s->rcontent.height - oldcontent.height; - s->rcontent = oldcontent; - it->rect.width = 0; - it->rect.height = 0; - } + else { + Height = height; } - - // vertical padding after last item, compute bottom - s->rcontent.height += g_itemver; - s->rcontent.bottom = height + s->rcontent.height; - - // count content height - if (s->state) { - height += s->rcontent.height; - } - maxheight += s->rcontent.height; } - // vertical padding after last section - height += g_section; - maxheight += g_section; - // invert bottom for all sections and items for (int n=0; n < ui->nsect; n++) { // section mjuiSection* s = ui->sect + n; - s->rtitle.bottom = height - s->rtitle.bottom; - s->rcontent.bottom = height - s->rcontent.bottom; + s->rtitle.bottom = Height - s->rtitle.bottom; + s->rcontent.bottom = Height - s->rcontent.bottom; // items for (int i=0; i < s->nitem; i++) { - s->item[i].rect.bottom = height - s->item[i].rect.bottom; + s->item[i].rect.bottom = Height - s->item[i].rect.bottom; } } // assign UI sizes ui->width = w_master; - ui->height = height; - ui->maxheight = maxheight; + ui->height = Height; + ui->maxheight = MaxHeight; +} + + + +// insertion sort of groups of ints: increasing order of leading int +static void insertionsortgroup(int* list, int num, int stride) { + // allocate buffer of 10 ints, cannot handle more + if (stride > 10) { + mju_error("insertionsortgroup cannot handle stride greater than 10"); + } + int x[10]; + + for (int i = 1; i < num; i++) { + memcpy(x, list + i * stride, sizeof(int) * stride); + + int j = i - 1; + while (j >= 0 && list[j * stride] > x[0]) { + memcpy(list + (j + 1) * stride, list + j * stride, sizeof(int) * stride); + j--; + } + + memcpy(list + (j + 1) * stride, x, sizeof(int) * stride); + } +} + + + +// Compute UI sizes. +void mjui_resize(mjUI* ui, const mjrContext* con) { + // get maximum buffer size allowed by OpenGL driver + int maxBufferSize = 0; + glGetIntegerv(GL_MAX_RENDERBUFFER_SIZE, &maxBufferSize); + + // USED FOR TESTING OF SMALL BUFFER SIZES + // maxBufferSize = 3000; + + // resize with current section states, clamp maxheight + tryresize(ui, con); + ui->maxheight = mjMIN(ui->maxheight, maxBufferSize); + + // if height is too large, close some sections + if (ui->height > ui->maxheight) { + // init new height with section gaps + int hnew = (ui->nsect + 1) * SCL(ui->spacing.section, con); + + // add titles of regular sections and contents of fixed sections + for (int n = 0; n < ui->nsect; ++n) { + if (ui->sect[n].state == mjSECT_FIXED) { + hnew += ui->sect[n].rcontent.height; + } + else { + hnew += ui->sect[n].rtitle.height; + } + } + + // if fixed height is too big, nothing we can do + if (hnew > ui->maxheight) { + mju_error("fixed section height already too big, closing sections cannot help"); + } + + // sort open sections by lastclick + int nopen = 0; + int sortbuf[2 * mjMAXUISECT] = { 0 }; + for (int n = 0; n < ui->nsect; ++n) { + if (ui->sect[n].state == mjSECT_OPEN) { + sortbuf[2 * nopen] = ui->sect[n].lastclick; + sortbuf[2 * nopen + 1] = n; + ++nopen; + } + } + insertionsortgroup(sortbuf, nopen, 2); + + // nothing is open; SHOULD NOT OCCUR + if (nopen == 0) { + mju_error("internal error: expected some sections to be open"); + } + + // keep most recent sections: as many as can fit in maxheight + for (int i = nopen - 1; i >= 0; --i) { + // section fits: add height + if (hnew + ui->sect[sortbuf[2 * i + 1]].rcontent.height <= ui->maxheight) { + hnew += ui->sect[sortbuf[2 * i + 1]].rcontent.height; + } + + // section does not fit: mark for closing + else { + sortbuf[2 * i] = -1; + } + } + + // close sections that were marked + for (int i = 0; i < nopen; ++i) { + if (sortbuf[2 * i] == -1) { + ui->sect[sortbuf[2 * i + 1]].state = mjSECT_CLOSED; + } + } + + // resize with new section states, clamp maxheight again + tryresize(ui, con); + ui->maxheight = mjMIN(ui->maxheight, maxBufferSize); + + // make sure tryresize did what we expected; SHOULD NOT OCCUR + if (ui->height != hnew) { + mju_error("internal error: tryresize produced unexpeced ui height"); + } + } } @@ -1653,7 +1912,8 @@ void mjui_update(int section, int item, const mjUI* ui, // start rendering mjr_setAux(ui->auxid, con); - initOpenGL(ui, con); + mjrRect rgl = { 0, 0, ui->width, ui->height }; + initOpenGL(&rgl, con); // all sections: clear background if (section < 0) { @@ -1684,34 +1944,81 @@ void mjui_update(int section, int item, const mjUI* ui, // redraw section title and pane if (section < 0 || item < 0) { - // title shown - if (s->state < 2) { - // interpolated rectangle - r = s->rtitle; - glBegin(GL_QUADS); - glColor3fv(ui->color.sectpane); - glVertex2i(r.left, r.bottom); - glVertex2i(r.left+r.width, r.bottom); - glColor3fv(ui->color.secttitle); - glVertex2i(r.left+r.width, r.bottom+r.height); - glVertex2i(r.left, r.bottom+r.height); - glEnd(); + r = s->rtitle; - // symbol and text - drawsymbol(s->rtitle, s->state, 0, ui, con); - drawtext(s->name, s->rtitle.left+g_texthor, s->rtitle.bottom+g_textver, - 2*maxwidth, ui->color.sectfont, con); + // title shown + if (s->state != mjSECT_FIXED) { + // section without checkbox + if (s->checkbox == 0) { + // interpolated rectangle + glBegin(GL_QUADS); + glColor3fv(ui->color.secttitle2); + glVertex2i(r.left, r.bottom); + glVertex2i(r.left + r.width, r.bottom); + glColor3fv(ui->color.secttitle); + glVertex2i(r.left + r.width, r.bottom + r.height); + glVertex2i(r.left, r.bottom + r.height); + glEnd(); + + // symbol and text + drawsymbol(r, s->state, 0, ui, con); + drawtext(s->name, r.left + g_texthor, + r.bottom + g_textver, 2 * maxwidth, + ui->color.sectfont, con); + } + + // section with checkbox + else { + glBegin(GL_QUADS); + glColor3fv(ui->color.secttitlecheck2); + glVertex2i(r.left, r.bottom); + glVertex2i(r.left + r.width, r.bottom); + glColor3fv(ui->color.secttitlecheck); + glVertex2i(r.left + r.width, r.bottom + r.height); + glVertex2i(r.left, r.bottom + r.height); + glEnd(); + + // symbol and text with offset + drawsymbol(r, s->state, 1, ui, con); + drawtext(s->name, r.left + r.height, + r.bottom + g_textver, 2 * maxwidth - r.height, + ui->color.sectfont, con); + + // draw checkmark as specified + if (s->checkbox > 1) { + int cgap = r.height / 4; + mjrRect cr = { r.left + cgap, r.bottom + cgap, r.height - 2 * cgap, r.height - 2 * cgap }; + float rgb[3] = { + 0.5f * (ui->color.secttitlecheck[0] + ui->color.secttitlecheck2[0]), + 0.5f * (ui->color.secttitlecheck[1] + ui->color.secttitlecheck2[1]), + 0.5f * (ui->color.secttitlecheck[2] + ui->color.secttitlecheck2[2]) + }; + drawrectangle(cr, ui->color.sectsymbol, + s->checkbox == 2 ? rgb : NULL, con); + } + } // shortcut if (ui->mousehelp && s->shortcut) { - shortcuthelp(s->rtitle, s->modifier, s->shortcut, ui, con); + shortcuthelp(r, s->modifier, s->shortcut, ui, con); } } // content pane, active only - if (s->state) { + if (s->state != mjSECT_CLOSED) { drawrectangle(s->rcontent, ui->color.sectpane, NULL, con); } + + // round corners + mjrRect rround = s->rtitle; + if (s->state == mjSECT_FIXED) { + rround = s->rcontent; + } + else if (s->state == mjSECT_OPEN) { + rround.bottom = s->rcontent.bottom; + rround.height = s->rtitle.height + s->rcontent.height; + } + roundcorner(rround, 0, 0, ui, con); } // closed: skip items @@ -1754,10 +2061,10 @@ void mjui_update(int section, int item, const mjUI* ui, // background r = it->rect; glBegin(GL_QUADS); - glColor3fv(ui->color.sectpane); + glColor3fv(ui->color.separator2); glVertex2i(r.left, r.bottom); glVertex2i(r.left+r.width, r.bottom); - glColor3fv(ui->color.master); + glColor3fv(ui->color.separator); glVertex2i(r.left+r.width, r.bottom+r.height); glVertex2i(r.left, r.bottom+r.height); glEnd(); @@ -1768,13 +2075,12 @@ void mjui_update(int section, int item, const mjUI* ui, it->rect.bottom+g_textver, it->rect.width-2*g_texthor, ui->color.sectfont, con); - // symbol - if (it->state == mjSEPCLOSED+1) { - drawsymbol(it->rect, 1, 1, ui, con); - } else if (it->state == mjSEPCLOSED) { - drawsymbol(it->rect, 0, 1, ui, con); + // symbol and round corners for collapsible + if (it->state >= mjSEPCLOSED) { + int flg_open = (it->state == mjSEPCLOSED + 1); + drawsymbol(it->rect, flg_open, 2, ui, con); + roundcorner(it->rect, flg_open, 1, ui, con); } - break; case mjITEM_STATIC: @@ -1945,7 +2251,7 @@ void mjui_update(int section, int item, const mjUI* ui, it->rect.width-2*g_texthor, rgbfont, con); } - // draw tracking at the end + // draw tracking in mjui_render() break; case mjITEM_SLIDERINT: @@ -2062,46 +2368,6 @@ void mjui_update(int section, int item, const mjUI* ui, } } - // select tracking - if (ui->mousesect > 0 && ui->mouseitem >= 0) { - // get item pointer - const mjuiItem* it = ui->sect[ui->mousesect-1].item + ui->mouseitem; - - // proceed if select type - if (it->type == mjITEM_SELECT) { - // margin - r = it->rect; - r.left -= g_itemside; - r.width += 2*g_itemside; - r.height = it->multi.nelem * cellheight + g_itemside; - r.bottom -= r.height; - drawrectangle(r, ui->color.sectpane, NULL, con); - - // box - r = it->rect; - r.height = it->multi.nelem * cellheight; - r.bottom -= r.height; - drawrectangle(r, ui->color.select2, NULL, con); - - // hightlight row under mouse - int k = findselect(it, ui, state, con); - if (k >= 0) { - mjrRect r1 = r; - r1.bottom = r.bottom + (it->multi.nelem-1-k)*cellheight; - r1.height = cellheight; - drawrectangle(r1, ui->color.select, NULL, con); - } - - // values - for (int k=0; k < it->multi.nelem; k++) { - drawtext(it->multi.name[k], - r.left+g_texthor, - r.bottom+g_textver+(it->multi.nelem-1-k)*cellheight, - r.width-2*g_texthor, ui->color.fontactive, con); - } - } - } - // stop rendering mjr_restoreBuffer(con); } @@ -2114,6 +2380,11 @@ mjuiItem* mjui_event(mjUI* ui, mjuiState* state, const mjrContext* con) { mjuiItem* it; ui->editchanged = NULL; + // count mouse clicks over UI + if (state->type == mjEVENT_PRESS) { + ++ui->mouseclicks; + } + // non-left mouse events: handle shortcut help if ((state->type == mjEVENT_PRESS || state->type == mjEVENT_MOVE || state->type == mjEVENT_RELEASE) && state->button != mjBUTTON_LEFT) { @@ -2139,6 +2410,11 @@ mjuiItem* mjui_event(mjUI* ui, mjuiState* state, const mjrContext* con) { it_cur = ui->sect[sect_cur-1].item + item_cur; } + // update section lastclick + if (sect_cur > 0 && state->type == mjEVENT_PRESS) { + ui->sect[sect_cur - 1].lastclick = ui->mouseclicks; + } + // get recorded mouse section and item int sect_rec = ui->mousesect; int item_rec = -1; @@ -2242,19 +2518,33 @@ mjuiItem* mjui_event(mjUI* ui, mjuiState* state, const mjrContext* con) { // section title else if (sect_cur > 0 && item_cur < 0) { - // double-click: make all sections like this + mjuiSection* se = ui->sect + sect_cur - 1; + + // handle section checkbox + if (item_cur == -2 && se->checkbox > 0) { + ui->mousesectcheck = sect_cur; + return NULL; // leave it to user, because sections may interact + } + else { + ui->mousesectcheck = 0; + } + + // double-click: make all sections like this (exclude fixed) if (state->doubleclick) { for (int i=0; i < ui->nsect; i++) { - if (ui->sect[i].state < 2 && ui->sect[sect_cur-1].state < 2) { - ui->sect[i].state = ui->sect[sect_cur-1].state; + if (ui->sect[i].state != mjSECT_FIXED && se->state != mjSECT_FIXED) { + ui->sect[i].state = se->state; } } } - // single click: toggle section state + // single click: toggle section state (exclude fixed) else { - if (ui->sect[sect_cur-1].state < 2) { - ui->sect[sect_cur-1].state = 1 - ui->sect[sect_cur-1].state; + if (se->state == mjSECT_OPEN) { + se->state = mjSECT_CLOSED; + } + else if (se->state == mjSECT_CLOSED) { + se->state = mjSECT_OPEN; } } @@ -2607,7 +2897,7 @@ void mjui_render(mjUI* ui, const mjuiState* state, const mjrContext* con) { mjr_blitAux(ui->auxid, raux, rect.left, rect.bottom + mjMAX(0, rect.height - ui->height + ui->scroll), con); - // draw scrollbar on top if needed + // draw scrollbar over blit if needed if (ui->height > rect.height) { // construct rectangles mjrRect bar; @@ -2618,4 +2908,60 @@ void mjui_render(mjUI* ui, const mjuiState* state, const mjrContext* con) { mjr_rectangle(thumb, ui->color.thumb[0], ui->color.thumb[1], ui->color.thumb[2], 1); } + + // draw selection box tracking over blit if needed + if (ui->mousesect > 0 && ui->mouseitem >= 0) { + // get item pointer + const mjuiItem* it = ui->sect[ui->mousesect-1].item + ui->mouseitem; + + // proceed if select type + if (it->type == mjITEM_SELECT) { + // get relevant sizes + int g_texthor = SCL(ui->spacing.texthor, con); + int g_textver = SCL(ui->spacing.textver, con); + int g_itemside = SCL(ui->spacing.itemside, con); + int cellheight = con->charHeight + 2 * g_textver; + int offset = mjMAX(0, rect.height - ui->height + ui->scroll) - + mjMAX(0, ui->height - ui->scroll - rect.height); + + // margin + mjrRect r = it->rect; + r.left -= g_itemside; + r.width += 2*g_itemside; + r.height = it->multi.nelem * cellheight + g_itemside; + r.bottom -= r.height; + r.bottom += offset; + r.left += rect.left; + mjr_rectangle(r, ui->color.sectpane[0], + ui->color.sectpane[1], ui->color.sectpane[2], 1); + + // box + r = it->rect; + r.height = it->multi.nelem * cellheight; + r.bottom -= r.height; + r.bottom += offset; + r.left += rect.left; + mjr_rectangle(r, ui->color.select2[0], + ui->color.select2[1], ui->color.select2[2], 1); + + // hightlight row under mouse + int k = findselect(it, ui, state, con); + if (k >= 0) { + mjrRect r1 = r; + r1.bottom = r.bottom + (it->multi.nelem-1-k)*cellheight; + r1.height = cellheight; + mjr_rectangle(r1, ui->color.select[0], + ui->color.select[1], ui->color.select[2], 1); + } + + // text values + initOpenGL(&rect, con); + for (int k=0; k < it->multi.nelem; k++) { + drawtext(it->multi.name[k], + r.left+g_texthor - rect.left, + r.bottom+g_textver+(it->multi.nelem-1-k)*cellheight, + r.width-2*g_texthor, ui->color.fontactive, con); + } + } + } } diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index c6afdeff..649005f7 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -71,6 +71,7 @@ public const int mjMAXUIMULTI = 35; public const int mjMAXUIEDIT = 7; public const int mjMAXUIRECT = 25; public const int mjSEPCLOSED = 1000; +public const int mjPRESERVE = 2000; public const int mjKEY_ESCAPE = 256; public const int mjKEY_ENTER = 257; public const int mjKEY_TAB = 258; @@ -482,6 +483,11 @@ public enum mjtEvent : int{ mjEVENT_REDRAW = 7, mjEVENT_FILESDROP = 8, } +public enum mjtSection : int{ + mjSECT_CLOSED = 0, + mjSECT_OPEN = 1, + mjSECT_FIXED = 2, +} public enum mjtCatBit : int{ mjCAT_STATIC = 1, mjCAT_DYNAMIC = 2, @@ -5722,6 +5728,8 @@ public unsafe struct mjuiThemeSpacing_ { public int scroll; public int label; public int section; + public int cornersect; + public int cornersep; public int itemside; public int itemmid; public int itemver; @@ -5736,9 +5744,14 @@ public unsafe struct mjuiThemeColor_ { public fixed float master[3]; public fixed float thumb[3]; public fixed float secttitle[3]; + public fixed float secttitle2[3]; + public fixed float secttitlecheck[3]; + public fixed float secttitlecheck2[3]; public fixed float sectfont[3]; public fixed float sectsymbol[3]; public fixed float sectpane[3]; + public fixed float separator[3]; + public fixed float separator2[3]; public fixed float shortcut[3]; public fixed float fontactive[3]; public fixed float fontinactive[3]; @@ -5786,9 +5799,11 @@ public unsafe struct mjuiSection_ { public int state; public int modifier; public int shortcut; + public int checkbox; public int nitem; public mjrRect_ rtitle; public mjrRect_ rcontent; + public int lastclick; } [StructLayout(LayoutKind.Sequential)] @@ -5807,6 +5822,8 @@ public unsafe struct mjUI_ { public int mousesect; public int mouseitem; public int mousehelp; + public int mouseclicks; + public int mousesectcheck; public int editsect; public int edititem; public int editcursor; @@ -5833,6 +5850,7 @@ public unsafe struct mjuiDef_ { public int state; public void* pdata; public fixed sbyte other[300]; + public int otherint; } [StructLayout(LayoutKind.Sequential)] From 9e9a0c637e5f6469cbd26cc94c03b81040483edb Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 24 Jun 2024 09:27:00 -0700 Subject: [PATCH 16/52] Fix docstring of `mjData.arena`. Fixes #1753 PiperOrigin-RevId: 646121139 Change-Id: I46c7fb32b0e705fb3b8301272d7fc55ba0dacd7c --- doc/includes/references.h | 4 ++-- include/mujoco/mjdata.h | 4 ++-- introspect/structs.py | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/doc/includes/references.h b/doc/includes/references.h index d951fa75..c6c06f5c 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -179,8 +179,8 @@ struct mjData_ { //-------------------- end of info header // buffers - void* buffer; // main buffer; all pointers point in it (nbuffer bytes) - void* arena; // arena+stack buffer (nstack*sizeof(mjtNum) bytes) + void* buffer; // main buffer; all pointers point in it (nbuffer bytes) + void* arena; // arena+stack buffer (narena bytes) //-------------------- main inputs and outputs of the computation diff --git a/include/mujoco/mjdata.h b/include/mujoco/mjdata.h index 344c688e..7c5ecb50 100644 --- a/include/mujoco/mjdata.h +++ b/include/mujoco/mjdata.h @@ -207,8 +207,8 @@ struct mjData_ { //-------------------- end of info header // buffers - void* buffer; // main buffer; all pointers point in it (nbuffer bytes) - void* arena; // arena+stack buffer (nstack*sizeof(mjtNum) bytes) + void* buffer; // main buffer; all pointers point in it (nbuffer bytes) + void* arena; // arena+stack buffer (narena bytes) //-------------------- main inputs and outputs of the computation diff --git a/introspect/structs.py b/introspect/structs.py index 47ad0e6a..f24f8dd1 100644 --- a/introspect/structs.py +++ b/introspect/structs.py @@ -4329,14 +4329,14 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=PointerType( inner_type=ValueType(name='void'), ), - doc='main buffer; all pointers point in it (nbuffer bytes)', # pylint: disable=line-too-long + doc='main buffer; all pointers point in it (nbuffer bytes)', # pylint: disable=line-too-long ), StructFieldDecl( name='arena', type=PointerType( inner_type=ValueType(name='void'), ), - doc='arena+stack buffer (nstack*sizeof(mjtNum) bytes)', # pylint: disable=line-too-long + doc='arena+stack buffer (narena bytes)', # pylint: disable=line-too-long ), StructFieldDecl( name='qpos', From d8b85245a77b88b24811fa0f3ceb4e2c7afdc10e Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Tue, 25 Jun 2024 01:45:10 -0700 Subject: [PATCH 17/52] Informative error if mis-naming the top-level default class. Also: - Move the the top-level default class name into the mjCModel constructor. - Fix bug in default class propagation in frame elements. PiperOrigin-RevId: 646384669 Change-Id: Icc6154124361a234b84a07e2b5ecaa7c8b18ebf7 --- src/user/user_model.cc | 1 + src/xml/xml_native_reader.cc | 15 ++++--- test/xml/xml_native_reader_test.cc | 72 ++++++++++++++++++++++++++++++ test/xml/xml_native_writer_test.cc | 6 ++- 4 files changed, 86 insertions(+), 8 deletions(-) diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 7bd1ebbe..23f94634 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -97,6 +97,7 @@ mjCModel::mjCModel() { //------------------------ master default set defaults_.push_back(new mjCDef); + defaults_.back()->name = "main"; // world body mjCBody* world = new mjCBody(this); diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index c28cc35f..250f2bfe 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -2673,17 +2673,15 @@ void mjXReader::Default(XMLElement* section, int parentid) { mjsDefault* def; int thisid; - // create new default, except at top level (already added in mjCModel ctor) + // create new default, except at top level (already added in mjCModel constructor) text.clear(); ReadAttrTxt(section, "class", text); if (text.empty()) { - if (parentid>=0) { + if (parentid >= 0) { throw mjXError(section, "empty class name"); - } else { - text = "main"; } } - if (parentid>=0) { + if (parentid >= 0) { def = mjs_addDefault(model, text.c_str(), parentid, &thisid); if (!def) { throw mjXError(section, "repeated default class name"); @@ -2691,7 +2689,9 @@ void mjXReader::Default(XMLElement* section, int parentid) { } else { thisid = 0; def = mjs_getSpecDefault(model); - mjs_setString(def->name, text.c_str()); + if (!text.empty() && text != "main") { + throw mjXError(section, "top-level default class 'main' cannot be renamed"); + } } // iterate over elements other than nested defaults @@ -3497,6 +3497,9 @@ void mjXReader::Body(XMLElement* section, mjsBody* pbody, mjsFrame* frame) { mjs_setString(pchild->info, std::string("line " + std::to_string(elem->GetLineNum())).c_str()); + // set default from class or childclass + mjs_setDefault(pchild->element, childdef ? childdef : def); + // read attributes std::string name, childclass; if (ReadAttrTxt(elem, "name", name)) { diff --git a/test/xml/xml_native_reader_test.cc b/test/xml/xml_native_reader_test.cc index 1d13f7d1..f71e66fa 100644 --- a/test/xml/xml_native_reader_test.cc +++ b/test/xml/xml_native_reader_test.cc @@ -464,6 +464,39 @@ TEST_F(XMLReaderTest, InvalidDoubleOrientation) { } } +TEST_F(XMLReaderTest, ClassOverridesChildclass) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << error.data(); + EXPECT_EQ(model->geom_size[3*0], 2); + EXPECT_EQ(model->geom_size[3*1], 3); + EXPECT_EQ(model->geom_size[3*2], 2); + EXPECT_EQ(model->geom_size[3*3], 3); + mj_deleteModel(model); +} + TEST_F(XMLReaderTest, RepeatedDefaultName) { static constexpr char xml[] = R"( @@ -511,6 +544,45 @@ TEST_F(XMLReaderTest, InvalidDefaultClassName) { HasSubstr("Element 'geom'"), HasSubstr("line 10"))); } +TEST_F(XMLReaderTest, InvalidTopDefaultClassName) { + static constexpr char xml[] = R"( + + + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, IsNull()) << error.data(); + EXPECT_THAT(error.data(), + HasSubstr("top-level default class 'main' cannot be renamed")); +} + +TEST_F(XMLReaderTest, ValidTopDefaultClassName) { + static constexpr char xml[] = R"( + + + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << error.data(); + mj_deleteModel(model); +} + // ------------------------ test including ------------------------------------- // tiny RGB 2 x 3 PNG file diff --git a/test/xml/xml_native_writer_test.cc b/test/xml/xml_native_writer_test.cc index 09176bd5..96134397 100644 --- a/test/xml/xml_native_writer_test.cc +++ b/test/xml/xml_native_writer_test.cc @@ -727,8 +727,10 @@ TEST_F(XMLWriterTest, WritesActuatorDefaults) { TEST_F(XMLWriterTest, WritesFrameDefaults) { static constexpr char xml[] = R"( - - + + + + From 5a513a0afbc78dcbf610d5b808e76db5982fdac0 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Tue, 25 Jun 2024 09:15:08 -0700 Subject: [PATCH 18/52] `simulate` improvements - Add RGBA color editing for visualization - Make some separator sections closed by default PiperOrigin-RevId: 646502122 Change-Id: Id863d983b7fb53e6bbe86a92833876e40b74a5d8 --- simulate/simulate.cc | 94 ++++++++++++++++++++++++-------------------- 1 file changed, 51 insertions(+), 43 deletions(-) diff --git a/simulate/simulate.cc b/simulate/simulate.cc index 3dc3d239..17ae8db6 100644 --- a/simulate/simulate.cc +++ b/simulate/simulate.cc @@ -752,6 +752,20 @@ void MakePhysicsSection(mj::Simulate* sim) { // add actuator group enable/disable mjui_add(&sim->ui0, defDisableActuator); + + // make some subsections closed by default + for (int i=0; i < sim->ui0.sect[SECT_PHYSICS].nitem; i++) { + mjuiItem* it = sim->ui0.sect[SECT_PHYSICS].item + i; + + // close less useful subsections + if (it->type == mjITEM_SEPARATOR) { + if (mju::strcmp_arr(it->name, "Actuator Group Enable") && + mju::strcmp_arr(it->name, "Contact Override") && + mju::strcmp_arr(it->name, "Physical Parameters")) { + it->state = mjSEPCLOSED+1; + } + } + } } @@ -759,50 +773,18 @@ void MakePhysicsSection(mj::Simulate* sim) { // make rendering section of UI void MakeRenderingSection(mj::Simulate* sim, const mjModel* m) { mjuiDef defRendering[] = { - { - mjITEM_SECTION, - "Rendering", - mjPRESERVE, - nullptr, - "AR" - }, - { - mjITEM_SELECT, - "Camera", - 2, - &(sim->camera), - "Free\nTracking" - }, - { - mjITEM_SELECT, - "Label", - 2, - &(sim->opt.label), + {mjITEM_SECTION, "Rendering", mjPRESERVE, nullptr, "AR"}, + {mjITEM_SELECT, "Camera", 2, &(sim->camera), "Free\nTracking"}, + {mjITEM_SELECT, "Label", 2, &(sim->opt.label), "None\nBody\nJoint\nGeom\nSite\nCamera\nLight\nTendon\n" "Actuator\nConstraint\nFlex\nSkin\nSelection\nSel Pnt\nContact\nForce\nIsland" }, - { - mjITEM_SELECT, - "Frame", - 2, - &(sim->opt.frame), + {mjITEM_SELECT, "Frame", 2, &(sim->opt.frame), "None\nBody\nGeom\nSite\nCamera\nLight\nContact\nWorld" }, - { - mjITEM_BUTTON, - "Copy camera", - 2, - nullptr, - "" - }, - { - mjITEM_SEPARATOR, - "Model Elements", - 1 - }, - { - mjITEM_END - } + {mjITEM_BUTTON, "Copy camera", 2, nullptr, ""}, + {mjITEM_SEPARATOR, "Model Elements", 1}, + {mjITEM_END} }; mjuiDef defOpenGL[] = { {mjITEM_SEPARATOR, "OpenGL Effects", 1}, @@ -911,8 +893,8 @@ void MakeVisualizationSection(mj::Simulate* sim, const mjModel* m) { {mjITEM_EDITFLOAT, "Haze", 2, &(vis->map.haze), "1"}, {mjITEM_EDITFLOAT, "Shadow clip", 2, &(vis->map.shadowclip), "1"}, {mjITEM_EDITFLOAT, "Shadow scale", 2, &(vis->map.shadowscale), "1"}, - {mjITEM_SEPARATOR, "Scale", 1}, - {mjITEM_EDITNUM, "All [meansize]", 2, &(stat->meansize), "1"}, + {mjITEM_SEPARATOR, "Scale", mjPRESERVE}, + {mjITEM_EDITNUM, "All (meansize)", 2, &(stat->meansize), "1"}, {mjITEM_EDITFLOAT, "Force width", 2, &(vis->scale.forcewidth), "1"}, {mjITEM_EDITFLOAT, "Contact width", 2, &(vis->scale.contactwidth), "1"}, {mjITEM_EDITFLOAT, "Contact height", 2, &(vis->scale.contactheight), "1"}, @@ -929,17 +911,43 @@ void MakeVisualizationSection(mj::Simulate* sim, const mjModel* m) { {mjITEM_EDITFLOAT, "Frame width", 2, &(vis->scale.framewidth), "1"}, {mjITEM_EDITFLOAT, "Constraint", 2, &(vis->scale.constraint), "1"}, {mjITEM_EDITFLOAT, "Slider-crank", 2, &(vis->scale.slidercrank), "1"}, + {mjITEM_SEPARATOR, "RGBA", mjPRESERVE}, + {mjITEM_EDITFLOAT, "fog", 2, &(vis->rgba.fog), "4"}, + {mjITEM_EDITFLOAT, "haze", 2, &(vis->rgba.haze), "4"}, + {mjITEM_EDITFLOAT, "force", 2, &(vis->rgba.force), "4"}, + {mjITEM_EDITFLOAT, "inertia", 2, &(vis->rgba.inertia), "4"}, + {mjITEM_EDITFLOAT, "joint", 2, &(vis->rgba.joint), "4"}, + {mjITEM_EDITFLOAT, "actuator", 2, &(vis->rgba.actuator), "4"}, + {mjITEM_EDITFLOAT, "actnegative", 2, &(vis->rgba.actuatornegative), "4"}, + {mjITEM_EDITFLOAT, "actpositive", 2, &(vis->rgba.actuatorpositive), "4"}, + {mjITEM_EDITFLOAT, "com", 2, &(vis->rgba.com), "4"}, + {mjITEM_EDITFLOAT, "camera", 2, &(vis->rgba.camera), "4"}, + {mjITEM_EDITFLOAT, "light", 2, &(vis->rgba.light), "4"}, + {mjITEM_EDITFLOAT, "selectpoint", 2, &(vis->rgba.selectpoint), "4"}, + {mjITEM_EDITFLOAT, "connect", 2, &(vis->rgba.connect), "4"}, + {mjITEM_EDITFLOAT, "contactpoint", 2, &(vis->rgba.contactpoint), "4"}, + {mjITEM_EDITFLOAT, "contactforce", 2, &(vis->rgba.contactforce), "4"}, + {mjITEM_EDITFLOAT, "contactfriction", 2, &(vis->rgba.contactfriction), "4"}, + {mjITEM_EDITFLOAT, "contacttorque", 2, &(vis->rgba.contacttorque), "4"}, + {mjITEM_EDITFLOAT, "contactgap", 2, &(vis->rgba.contactgap), "4"}, + {mjITEM_EDITFLOAT, "rangefinder", 2, &(vis->rgba.rangefinder), "4"}, + {mjITEM_EDITFLOAT, "constraint", 2, &(vis->rgba.constraint), "4"}, + {mjITEM_EDITFLOAT, "slidercrank", 2, &(vis->rgba.slidercrank), "4"}, + {mjITEM_EDITFLOAT, "crankbroken", 2, &(vis->rgba.crankbroken), "4"}, + {mjITEM_EDITFLOAT, "frustum", 2, &(vis->rgba.frustum), "4"}, + {mjITEM_EDITFLOAT, "bv", 2, &(vis->rgba.bv), "4"}, + {mjITEM_EDITFLOAT, "bvactive", 2, &(vis->rgba.bvactive), "4"}, {mjITEM_END} }; - // add rendering standard + // add visualization section mjui_add(&sim->ui0, defVisualization); } // make group section of UI void MakeGroupSection(mj::Simulate* sim) { mjuiDef defGroup[] = { - {mjITEM_SECTION, "Group enable", mjPRESERVE, nullptr, "AG"}, + {mjITEM_SECTION, "Group enable", mjPRESERVE, nullptr, "AG"}, {mjITEM_SEPARATOR, "Geom groups", 1}, {mjITEM_CHECKBYTE, "Geom 0", 2, sim->opt.geomgroup, " 0"}, {mjITEM_CHECKBYTE, "Geom 1", 2, sim->opt.geomgroup+1, " 1"}, From 5e39fc05642d3655e7067ac61dc7587e1f2547e8 Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Tue, 25 Jun 2024 09:59:52 -0700 Subject: [PATCH 19/52] Improvements to GJK implementation: - Fix various bugs - Improve readability - Add support for recovering witness points. PiperOrigin-RevId: 646515019 Change-Id: I295cf90d885e32013e595ea7aad07ed6c1869239 --- src/engine/engine_collision_convex.c | 24 +-- src/engine/engine_collision_convex.h | 12 +- src/engine/engine_collision_gjk.c | 185 ++++++++++++++--------- src/engine/engine_collision_gjk.h | 13 +- test/engine/engine_collision_gjk_test.cc | 88 +++++++++-- 5 files changed, 214 insertions(+), 108 deletions(-) diff --git a/src/engine/engine_collision_convex.c b/src/engine/engine_collision_convex.c index eb2628dc..5ba14c02 100644 --- a/src/engine/engine_collision_convex.c +++ b/src/engine/engine_collision_convex.c @@ -34,13 +34,13 @@ // ccd center function void mjccd_center(const void *obj, ccd_vec3_t *center) { - mjc_center(center->v, (const mjtCCObj*) obj); + mjc_center(center->v, (const mjCCDObj*) obj); } // center function for convex collision algorithms -void mjc_center(mjtNum res[3], const mjtCCObj *obj) { +void mjc_center(mjtNum res[3], const mjCCDObj *obj) { int g = obj->geom; int f = obj->flex; int e = obj->elem; @@ -66,13 +66,13 @@ void mjc_center(mjtNum res[3], const mjtCCObj *obj) { // ccd support function void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) { - mjc_support(vec->v, (mjtCCObj*) obj, _dir->v); + mjc_support(vec->v, (mjCCDObj*) obj, _dir->v); } // support function for convex collision algorithms -void mjc_support(mjtNum res[3], mjtCCObj* obj, const mjtNum dir[3]) { +void mjc_support(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) { const mjModel* m = obj->model; const mjData* d = obj->data; int g = obj->geom; @@ -289,7 +289,7 @@ static void mjc_initCCD(ccd_t* ccd, const mjModel* m) { // find single convex-convex collision, using libccd -static int mjc_MPRIteration(mjtCCObj* obj1, mjtCCObj* obj2, const ccd_t* ccd, +static int mjc_MPRIteration(mjCCDObj* obj1, mjCCDObj* obj2, const ccd_t* ccd, const mjModel* m, const mjData* d, mjContact* con, mjtNum margin) { ccd_vec3_t dir, pos; @@ -360,8 +360,8 @@ static void mju_rotateFrame(const mjtNum origin[3], const mjtNum rot[9], int mjc_Convex(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin) { ccd_t ccd; - mjtCCObj obj1 = {m, d, g1, -1, -1, -1, -1, margin, {1, 0, 0, 0}}; - mjtCCObj obj2 = {m, d, g2, -1, -1, -1, -1, margin, {1, 0, 0, 0}}; + mjCCDObj obj1 = {m, d, g1, -1, -1, -1, -1, margin, {1, 0, 0, 0}, {0, 0, 0}}; + mjCCDObj obj2 = {m, d, g2, -1, -1, -1, -1, margin, {1, 0, 0, 0}, {0, 0, 0}}; // init ccd structure mjc_initCCD(&ccd, m); @@ -489,7 +489,7 @@ int mjc_PlaneConvex(const mjModel* m, const mjData* d, mjGETINFO mjtNum dist, dif[3], normal[3] = {mat1[2], mat1[5], mat1[8]}; ccd_vec3_t dir, vec; - mjtCCObj obj = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}}; + mjCCDObj obj = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0}}; // get support point in -normal direction ccdVec3Set(&dir, -mat1[2], -mat1[5], -mat1[8]); @@ -670,7 +670,7 @@ int mjc_ConvexHField(const mjModel* m, const mjData* d, // ccd-related ccd_vec3_t dirccd, vecccd; ccd_real_t depth; - mjtCCObj obj = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}}; + mjCCDObj obj = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0}}; ccd_t ccd; // point size1 to hfield size instead of geom1 size @@ -1103,8 +1103,8 @@ void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, in int mjc_ConvexElem(const mjModel* m, const mjData* d, mjContact* con, int g1, int f1, int e1, int v1, int f2, int e2, mjtNum margin) { ccd_t ccd; - mjtCCObj obj1 = {m, d, g1, -1, f1, e1, v1, margin, {1, 0, 0, 0}}; - mjtCCObj obj2 = {m, d, -1, -1, f2, e2, -1, margin, {1, 0, 0, 0}}; + mjCCDObj obj1 = {m, d, g1, -1, f1, e1, v1, margin, {1, 0, 0, 0}, {0, 0, 0}}; + mjCCDObj obj2 = {m, d, -1, -1, f2, e2, -1, margin, {1, 0, 0, 0}, {0, 0, 0}}; // init ccd structure mjc_initCCD(&ccd, m); @@ -1151,7 +1151,7 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con, // ccd-related ccd_vec3_t dirccd, vecccd; ccd_real_t depth; - mjtCCObj obj = {m, d, -1, -1, f, e, -1, margin, {1, 0, 0, 0}}; + mjCCDObj obj = {m, d, -1, -1, f, e, -1, margin, {1, 0, 0, 0}, {0, 0, 0}}; ccd_t ccd; //------------------------------------- AABB computation, box-box test diff --git a/src/engine/engine_collision_convex.h b/src/engine/engine_collision_convex.h index 37aafce5..24f38648 100644 --- a/src/engine/engine_collision_convex.h +++ b/src/engine/engine_collision_convex.h @@ -23,6 +23,7 @@ #include +#include #include #include #include @@ -39,8 +40,8 @@ extern "C" { #endif -// internal object type for convex collision algorithms -struct _mjtCCObj { +// internal object type for convex collision detection +struct _mjCCDObj { const mjModel* model; const mjData* data; int geom; @@ -50,14 +51,15 @@ struct _mjtCCObj { int vert; mjtNum margin; mjtNum rotate[4]; + mjtNum x0[3]; // initial guess of the witness point }; -typedef struct _mjtCCObj mjtCCObj; +typedef struct _mjCCDObj mjCCDObj; // support function for convex collision algorithms -void mjc_support(mjtNum res[3], mjtCCObj* obj, const mjtNum dir[3]); +MJAPI void mjc_support(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]); // center function for convex collision algorithms -void mjc_center(mjtNum res[3], const mjtCCObj *obj); +MJAPI void mjc_center(mjtNum res[3], const mjCCDObj *obj); // ccd support function void mjccd_support(const void *obj, const ccd_vec3_t *dir, ccd_vec3_t *vec); diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c index 42a3043a..7ddfd86a 100644 --- a/src/engine/engine_collision_gjk.c +++ b/src/engine/engine_collision_gjk.c @@ -17,6 +17,7 @@ #include #include +#include #include "engine/engine_util_blas.h" #include "engine/engine_util_spatial.h" #include "engine/engine_collision_convex.h" @@ -37,34 +38,38 @@ static void S2D(mjtNum lambda[3], const mjtNum simplex[9]); static void S1D(mjtNum lambda[2], const mjtNum simplex[6]); // helper function to compute the support point in the Minkowski difference -static void support(mjtNum res[3], mjtCCObj* obj1, mjtCCObj* obj2, const mjtNum dir[3]); +static void support(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2, + const mjtNum dir[3]); // linear algebra utility functions static mjtNum det3(const mjtNum v1[3], const mjtNum v2[3], const mjtNum v3[3]); static void lincomb(mjtNum res[3], const mjtNum* coef, const mjtNum* v, int n); +// returns the distance between the two objects. The witness points are +// recoverable from x_0 in obj1 and obj2. +mjtNum mj_gjk(const mjCCDConfig* config, mjCCDObj* obj1, mjCCDObj* obj2) { + mjtNum simplex[12]; // our current simplex with max 4 vertices due to only 3 dimensions + int n = 0; // number of vertices in the simplex + mjtNum x_k[3]; // the kth approximation point with initial value x_0 - -// returns the distance between the two given objects given an initial guess x0 -mjtNum mj_gjk(const mjGjkConfig* config, mjtCCObj* obj1, mjtCCObj* obj2, const mjtNum x0[3]) { - // our current simplex with max 4 vertices due to only 3 dimensions - mjtNum simplex[12]; - int n = 0; // number of vertices in the simplex - - // the kth approximation point with initial value x_0 - mjtNum x_k[3]; - mju_copy3(x_k, x0); + // segregated simplices and points for the two objects to recover witness points + mjtNum simplex1[12], simplex2[12]; + mjtNum* x1_k = obj1->x0; + mjtNum* x2_k = obj2->x0; + mju_sub3(x_k, x1_k, x2_k); int N = config->max_iterations; for (size_t k = 0; k < N; k++) { - mjtNum s_k[3]; // the kth support point - mjtNum lambda[4]; // barycentric coordinates for x_k + mjtNum s1[3], s2[3]; // the support points in obj1 and obj2 + mjtNum s_k[3]; // the kth support point of Minkowski difference + mjtNum lambda[4]; // barycentric coordinates for x_k // compute the kth support point in the direction of -(x_k) mjtNum x_k_neg[3]; mju_scl3(x_k_neg, x_k, -1); mju_normalize3(x_k_neg); - support(s_k, obj1, obj2, x_k_neg); + support(s1, s2, obj1, obj2, x_k_neg); + mju_sub3(s_k, s1, s2); // the stopping criteria relies on the Frank-Wolfe duality gap given by // f(x_k) - f(x_min) <= < grad f(x_k), (x_k - s_k) > @@ -74,36 +79,57 @@ mjtNum mj_gjk(const mjGjkConfig* config, mjtCCObj* obj1, mjtCCObj* obj2, const m return mju_norm3(x_k); } + // TODO(kylebayes): signedVolume has been written to assume the first vertex is the latest + // support to be added. Once the logic has been updated, then this hack should be removed. + for (int i = n; i > 0; i--) { + // shift the simplex vertices to the right + mju_copy3(simplex + 3*i, simplex + 3*(i-1)); + mju_copy3(simplex1 + 3*i, simplex1 + 3*(i-1)); + mju_copy3(simplex2 + 3*i, simplex2 + 3*(i-1)); + } + // copy new support point into the simplex - mju_copy3(simplex + 3*n++, s_k); + mju_copy3(simplex, s_k); + + // copy new support point into the individual simplexes + mju_copy3(simplex1, s1); + mju_copy3(simplex2, s2); // run the distance subalgorithm to compute the barycentric coordinates // of the closest point to the origin in the simplex - signedVolume(lambda, simplex, n); + signedVolume(lambda, simplex, ++n); lincomb(x_k, lambda, simplex, 4); + // compute the approximate witness points + lincomb(x1_k, lambda, simplex1, 4); + lincomb(x2_k, lambda, simplex2, 4); + // for lambda[i] == 0, remove the ith vertex from the simplex n = 0; for (int i = 0; i < 4; i++) { if (lambda[i] == 0) continue; + // recover simplex for the two objects + mju_copy3(simplex1 + 3*n, simplex1 + 3*i); + mju_copy3(simplex2 + 3*n, simplex2 + 3*i); + + // simplex in Minkowski difference mju_copy3(simplex + 3*n++, simplex + 3*i); } } - return mju_norm3(x_k); } // helper function to compute the support point in the Minkowski difference -static void support(mjtNum res[3], mjtCCObj* obj1, mjtCCObj* obj2, const mjtNum dir[3]) { - mjtNum s1[3], s2[3], dir_neg[3]; +static void support(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2, + const mjtNum dir[3]) { + mjtNum dir_neg[3]; mju_scl3(dir_neg, dir, -1); // compute S_{A-B}(dir) = S_A(dir) - S_B(-dir) mjc_support(s1, obj1, dir); mjc_support(s2, obj2, dir_neg); - mju_sub3(res, s1, s2); } @@ -198,20 +224,7 @@ static void S3D(mjtNum lambda[4], const mjtNum simplex[12]) { } // find the smallest distance, and use the corresponding barycentric coordinates - mjtNum dist; - - if (!comp1) { - mjtNum lambda_2d[3], verts[9], x[3]; - mju_copy3(verts, s2); - mju_copy3(verts + 3, s3); - mju_copy3(verts + 6, s4); - S2D(lambda_2d, verts); - lincomb(x, lambda_2d, verts, 3); - dist = mju_norm3(x); - lambda[1] = lambda_2d[0]; - lambda[2] = lambda_2d[1]; - lambda[3] = lambda_2d[2]; - } + mjtNum dist = mjMAXVAL; if (!comp2) { mjtNum lambda_2d[3], verts[9], x[3]; @@ -221,12 +234,11 @@ static void S3D(mjtNum lambda[4], const mjtNum simplex[12]) { S2D(lambda_2d, verts); lincomb(x, lambda_2d, verts, 3); mjtNum d = mju_norm3(x); - if (d < dist) { - lambda[0] = lambda_2d[0]; - lambda[2] = lambda_2d[1]; - lambda[3] = lambda_2d[2]; - dist = d; - } + lambda[0] = lambda_2d[0]; + lambda[1] = 0; + lambda[2] = lambda_2d[1]; + lambda[3] = lambda_2d[2]; + dist = d; } if (!comp3) { @@ -240,6 +252,7 @@ static void S3D(mjtNum lambda[4], const mjtNum simplex[12]) { if (d < dist) { lambda[0] = lambda_2d[0]; lambda[1] = lambda_2d[1]; + lambda[2] = 0; lambda[3] = lambda_2d[2]; dist = d; } @@ -257,6 +270,24 @@ static void S3D(mjtNum lambda[4], const mjtNum simplex[12]) { lambda[0] = lambda_2d[0]; lambda[1] = lambda_2d[1]; lambda[2] = lambda_2d[2]; + lambda[3] = 0; + dist = d; + } + } + + if (!comp1) { + mjtNum lambda_2d[3], verts[9], x[3]; + mju_copy3(verts, s2); + mju_copy3(verts + 3, s3); + mju_copy3(verts + 6, s4); + S2D(lambda_2d, verts); + lincomb(x, lambda_2d, verts, 3); + mjtNum d = mju_norm3(x); + if (d < dist) { + lambda[0] = 0; + lambda[1] = lambda_2d[0]; + lambda[2] = lambda_2d[1]; + lambda[3] = lambda_2d[2]; dist = d; } } @@ -273,30 +304,29 @@ static void S2D(mjtNum lambda[3], const mjtNum simplex[9]) { // compute normal mjtNum diff1[3], diff2[3], n[3]; mju_sub3(diff1, s2, s1); - mju_sub3(diff2, s3, s2); + mju_sub3(diff2, s3, s1); mju_cross(n, diff1, diff2); // project origin mjtNum p_o[3]; mju_scl3(p_o, n, mju_dot3(n, s1) / mju_dot3(n, n)); - int index; mjtNum mu_max = 0; - int k = 1, l = 2; - for (int i = 0; i < 3; i++) { - mjtNum mu = s2[k]*s3[l] + s1[k]*s2[l] + s3[k]*s1[l] - s2[k]*s1[l] - s3[k]*s2[l] - s1[k]*s3[l]; - if (mju_abs(mu) >= mju_abs(mu_max)) { - mu_max = mu; - index = i; - } - k = l; l = i; - } + // Below are the minors M_i4 of the matrix M given by + // [[ s1_x, s2_x, s3_x, s4_x ], + // [ s1_y, s2_y, s3_y, s4_y ], + // [ s1_z, s2_z, s3_z, s4_z ], + // [ 1, 1, 1, 1 ]] + mjtNum M_14 = s2[1]*s3[2] - s2[2]*s3[1] - s1[1]*s3[2] + s1[2]*s3[1] + s1[1]*s2[2] - s1[2]*s2[1]; + mjtNum M_24 = s2[0]*s3[2] - s2[2]*s3[0] - s1[0]*s3[2] + s1[2]*s3[0] + s1[0]*s2[2] - s1[2]*s2[0]; + mjtNum M_34 = s2[0]*s3[1] - s2[1]*s3[0] - s1[0]*s3[1] + s1[1]*s3[0] + s1[0]*s2[1] - s1[1]*s2[0]; - // exclude index component + // exclude one of the axes with the largest projection of the simplex using the computed minors mjtNum s1_2D[2], s2_2D[2], s3_2D[2], p_o_2D[2]; - - if (index == 0) { + mjtNum mu1 = mju_abs(M_14), mu2 = mju_abs(M_24), mu3 = mju_abs(M_34); + if (mu1 >= mu2 && mu1 >= mu3) { + mu_max = mu1; s1_2D[0] = s1[1]; s1_2D[1] = s1[2]; @@ -308,7 +338,8 @@ static void S2D(mjtNum lambda[3], const mjtNum simplex[9]) { p_o_2D[0] = p_o[1]; p_o_2D[1] = p_o[2]; - } else if (index == 1) { + } else if (mu2 >= mu3) { + mu_max = mu2; s1_2D[0] = s1[0]; s1_2D[1] = s1[2]; @@ -321,6 +352,7 @@ static void S2D(mjtNum lambda[3], const mjtNum simplex[9]) { p_o_2D[0] = p_o[0]; p_o_2D[1] = p_o[2]; } else { + mu_max = mu3; s1_2D[0] = s1[0]; s1_2D[1] = s1[1]; @@ -357,35 +389,23 @@ static void S2D(mjtNum lambda[3], const mjtNum simplex[9]) { } // find the smallest distance, and use the corresponding barycentric coordinates - mjtNum dist; - - if (!comp1) { - mjtNum lambda_1d[4], verts[6], x[3]; - mju_copy3(verts, s2); - mju_copy3(verts + 3, s3); - S1D(lambda_1d, verts); - lincomb(x, lambda_1d, verts, 2); - dist = mju_norm3(x); - lambda[1] = lambda_1d[0]; - lambda[2] = lambda_1d[1]; - } + mjtNum dist = mjMAXVAL; if (!comp2) { - mjtNum lambda_1d[4], verts[6], x[3]; + mjtNum lambda_1d[2], verts[6], x[3]; mju_copy3(verts, s1); mju_copy3(verts + 3, s3); S1D(lambda_1d, verts); lincomb(x, lambda_1d, verts, 2); mjtNum d = mju_norm3(x); - if (d < dist) { - lambda[0] = lambda_1d[0]; - lambda[2] = lambda_1d[1]; - dist = d; - } + lambda[0] = lambda_1d[0]; + lambda[1] = 0; + lambda[2] = lambda_1d[1]; + dist = d; } if (!comp3) { - mjtNum lambda_1d[4], verts[6], x[3]; + mjtNum lambda_1d[2], verts[6], x[3]; mju_copy3(verts, s1); mju_copy3(verts + 3, s2); S1D(lambda_1d, verts); @@ -394,6 +414,22 @@ static void S2D(mjtNum lambda[3], const mjtNum simplex[9]) { if (d < dist) { lambda[0] = lambda_1d[0]; lambda[1] = lambda_1d[1]; + lambda[2] = 0; + dist = d; + } + } + + if (!comp1) { + mjtNum lambda_1d[2], verts[6], x[3]; + mju_copy3(verts, s2); + mju_copy3(verts + 3, s3); + S1D(lambda_1d, verts); + lincomb(x, lambda_1d, verts, 2); + mjtNum d = mju_norm3(x); + if (d < dist) { + lambda[1] = lambda_1d[0]; + lambda[2] = lambda_1d[1]; + lambda[0] = 0; dist = d; } } @@ -435,5 +471,6 @@ static void S1D(mjtNum lambda[2], const mjtNum simplex[6]) { lambda[1] = C2 / mu_max; } else { lambda[0] = 1; + lambda[1] = 0; } } diff --git a/src/engine/engine_collision_gjk.h b/src/engine/engine_collision_gjk.h index 281b0026..ead02eba 100644 --- a/src/engine/engine_collision_gjk.h +++ b/src/engine/engine_collision_gjk.h @@ -23,15 +23,16 @@ extern "C" { #endif -// internal struct with settings for GJK -struct _mjGjkConfig { - mjtNum max_iterations; +// internal configuration for convex collision detection +struct _mjCCDConfig { + int max_iterations; mjtNum tolerance; }; -typedef struct _mjGjkConfig mjGjkConfig; +typedef struct _mjCCDConfig mjCCDConfig; -// Returns the distance between the two objects given an initial guess x0. -MJAPI mjtNum mj_gjk(const mjGjkConfig* config, mjtCCObj* obj1, mjtCCObj* obj2, const mjtNum x0[3]); +// Returns the distance between the two objects. The witness points are +// recoverable from x_0 in obj1 and obj2. +MJAPI mjtNum mj_gjk(const mjCCDConfig* config, mjCCDObj* obj1, mjCCDObj* obj2); #ifdef __cplusplus } diff --git a/test/engine/engine_collision_gjk_test.cc b/test/engine/engine_collision_gjk_test.cc index 29780cc0..22cc7297 100644 --- a/test/engine/engine_collision_gjk_test.cc +++ b/test/engine/engine_collision_gjk_test.cc @@ -29,18 +29,27 @@ namespace mujoco { namespace { using ::testing::NotNull; +using ::testing::ElementsAre; -mjtNum run_gjk(mjModel* m, mjData* d, int g1, int g2, mjtNum* x_0) { - mjGjkConfig config = {100, 1e-6}; - mjtCCObj obj1 = {m, d, g1, -1, -1, -1, -1, 0, {1, 0, 0, 0}}; - mjtCCObj obj2 = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}}; - return mj_gjk(&config, &obj1, &obj2, x_0); +constexpr mjtNum kTolerance = 1e-6; +constexpr int kMaxIterations = 1000; + +static mjtNum run_gjk(mjModel* m, mjData* d, int g1, int g2, mjtNum x1[3], + mjtNum x2[3]) { + mjCCDConfig config = {kMaxIterations, kTolerance}; + mjCCDObj obj1 = {m, d, g1, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0}}; + mjCCDObj obj2 = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0}}; + mjc_center(obj1.x0, &obj1); + mjc_center(obj2.x0, &obj2); + mjtNum dist = mj_gjk(&config, &obj1, &obj2); + if (x1 != nullptr) mju_copy3(x1, obj1.x0); + if (x2 != nullptr) mju_copy3(x2, obj2.x0); + return dist; } using MjGjkTest = MujocoTest; TEST_F(MjGjkTest, SphereSphereIntersect) { - mjtNum x_0[3] = {-2, 0, 0}; static constexpr char xml[] = R"(