diff --git a/src/user/CMakeLists.txt b/src/user/CMakeLists.txt index 55c8b407..035ef1fe 100644 --- a/src/user/CMakeLists.txt +++ b/src/user/CMakeLists.txt @@ -19,6 +19,7 @@ set(MUJOCO_USER_SRCS user_composite.h user_flexcomp.cc user_flexcomp.h + user_init.cc user_mesh.cc user_model.cc user_model.h diff --git a/src/user/user_api.h b/src/user/user_api.h index 8b30b485..9190ef19 100644 --- a/src/user/user_api.h +++ b/src/user/user_api.h @@ -155,9 +155,18 @@ MJAPI void mjm_setDefault(mjElement element, void* defspec); // Set frame. MJAPI void mjm_setFrame(mjElement dest, void* frame); -// Compute quat and inertia from body->fullinertia.. +// Compute quat and inertia from body->fullinertia. MJAPI const char* mjm_setFullInertia(mjmBody* body, double quat[4], double inertia[3]); + +//---------------------------------- Initialization functions -------------------------------------- + +// Default body attributes. +MJAPI void mjm_defaultBody(mjmBody& body); + +// Default site attributes. +MJAPI void mjm_defaultSite(mjmSite& site); + #ifdef __cplusplus } #endif diff --git a/src/user/user_init.cc b/src/user/user_init.cc new file mode 100644 index 00000000..22bc46e0 --- /dev/null +++ b/src/user/user_init.cc @@ -0,0 +1,54 @@ +// 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 +#include +#include +#include "user/user_api.h" +#include "user/user_util.h" + + +// Default body parameters +void mjm_defaultBody(mjmBody& body) { + memset(&body, 0, sizeof(mjmBody)); + + // set non-zero defaults + body.fullinertia[0] = mjNAN; + body.explicitinertial = (mjtByte)false; + body.mocap = (mjtByte)false; + body.quat[0] = 1; + body.iquat[0] = 1; + body.pos[0] = mjNAN; + body.ipos[0] = mjNAN; + body.alt.axisangle[0] = body.alt.xyaxes[0] = body.alt.zaxis[0] = body.alt.euler[0] = mjNAN; + body.ialt.axisangle[0] = body.ialt.xyaxes[0] = body.ialt.zaxis[0] = body.ialt.euler[0] = mjNAN; + body.plugin.active = false; + body.plugin.instance = nullptr; +} + + + +// Default site parameters +void mjm_defaultSite(mjmSite& site) { + memset(&site, 0, sizeof(mjmSite)); + + // set non-zero defaults + site.type = mjGEOM_SPHERE; + site.quat[0] = 1; + site.size[0] = site.size[1] = site.size[2] = 0.005; + site.fromto[0] = mjNAN; + site.alt.axisangle[0] = site.alt.xyaxes[0] = site.alt.zaxis[0] = site.alt.euler[0] = mjNAN; + site.rgba[0] = site.rgba[1] = site.rgba[2] = 0.5f; + site.rgba[3] = 1.0f; +} diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index ee8f9a4d..9f659a9c 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -552,33 +552,12 @@ mjCBody::mjCBody(mjCModel* _model) { // set model pointer model = _model; - spec.pos[0] = spec.ipos[0] = mjNAN; - - // clear variables - spec.fullinertia[0] = mjNAN; - spec.explicitinertial = false; - spec.mocap = false; - mjuu_setvec(spec.quat, 1, 0, 0, 0); - mjuu_setvec(spec.iquat, 1, 0, 0, 0); - mjuu_zerovec(spec.pos+1, 2); - mjuu_zerovec(spec.ipos+1, 2); - spec.mass = 0; - mjuu_setvec(spec.inertia, 0, 0, 0); + mjm_defaultBody(spec); parentid = -1; weldid = -1; dofnum = 0; lastdof = -1; subtreedofs = 0; - spec.gravcomp = 0; - spec_userdata_.clear(); - spec.alt.axisangle[0] = spec.alt.xyaxes[0] = spec.alt.zaxis[0] = - spec.alt.euler[0] = mjNAN; - spec.ialt.axisangle[0] = spec.ialt.xyaxes[0] = spec.ialt.zaxis[0] = - spec.ialt.euler[0] = mjNAN; - - spec.plugin.active = false; - spec.plugin.instance = nullptr; - contype = 0; conaffinity = 0; margin = 0; @@ -593,6 +572,7 @@ mjCBody::mjCBody(mjCModel* _model) { sites.clear(); cameras.clear(); lights.clear(); + spec_userdata_.clear(); // point to local spec.element = (mjElement)this; @@ -1982,23 +1962,13 @@ void mjCGeom::Compile(void) { // initialize default site mjCSite::mjCSite(mjCModel* _model, mjCDef* _def) { - // set defaults - spec.type = mjGEOM_SPHERE; - mjuu_setvec(spec.size, 0.005, 0.005, 0.005); - spec.group = 0; - mjuu_setvec(spec.quat, 1, 0, 0, 0); - mjuu_setvec(spec.pos, 0, 0, 0); - spec_material_.clear(); - spec.rgba[0] = spec.rgba[1] = spec.rgba[2] = 0.5f; - spec.rgba[3] = 1.0f; - spec.fromto[0] = mjNAN; - spec_userdata_.clear(); - spec.alt.axisangle[0] = spec.alt.xyaxes[0] = spec.alt.zaxis[0] = - spec.alt.euler[0] = mjNAN; + mjm_defaultSite(spec); // clear internal variables body = 0; matid = -1; + spec_material_.clear(); + spec_userdata_.clear(); // reset to default if given if (_def) {