From 3b2dd06c5438cde3ee08a1c5cf5fa8636d661764 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Tue, 6 Feb 2024 01:50:01 -0800 Subject: [PATCH] Add compiler C API for mjmSite. PiperOrigin-RevId: 604573450 Change-Id: I6ba7fcece0942ddc0d109413ba5684b2df3fab96 --- src/user/CMakeLists.txt | 2 + src/user/user_api.cc | 110 +++++++++++++++++++++++++++++++++++ src/user/user_api.h | 92 +++++++++++++++++++++++++++++ src/user/user_objects.cc | 32 +++++++--- src/user/user_objects.h | 48 +++++---------- src/xml/xml_base.cc | 3 +- src/xml/xml_base.h | 2 +- src/xml/xml_native_reader.cc | 50 +++++++++------- src/xml/xml_native_reader.h | 2 +- src/xml/xml_util.cc | 6 +- src/xml/xml_util.h | 7 ++- test/user/CMakeLists.txt | 3 + test/user/user_api_test.cc | 58 ++++++++++++++++++ 13 files changed, 342 insertions(+), 73 deletions(-) create mode 100644 src/user/user_api.cc create mode 100644 src/user/user_api.h create mode 100644 test/user/user_api_test.cc diff --git a/src/user/CMakeLists.txt b/src/user/CMakeLists.txt index e75a0355..55c8b407 100644 --- a/src/user/CMakeLists.txt +++ b/src/user/CMakeLists.txt @@ -13,6 +13,8 @@ # limitations under the License. set(MUJOCO_USER_SRCS + user_api.cc + user_api.h user_composite.cc user_composite.h user_flexcomp.cc diff --git a/src/user/user_api.cc b/src/user/user_api.cc new file mode 100644 index 00000000..c32b779b --- /dev/null +++ b/src/user/user_api.cc @@ -0,0 +1,110 @@ +// Copyright 2021 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "user/user_api.h" +#include +#include + +#include +#include "user/user_model.h" +#include "user/user_objects.h" + + + +// create model +void* mjm_createModel() { + mjCModel* modelC = new mjCModel(); + return modelC; +} + + + +// delete model +void mjm_deleteModel(void* model) { + mjCModel* modelC = static_cast(model); + delete modelC; +} + + + +// add body to body +void* mjm_addBody(void* body, void* def) { + mjCDef* defC = static_cast(def); + mjCBody* bodyC = static_cast(body); + return bodyC->AddBody(defC); +} + + + +// add site to body +mjmSite* mjm_addSite(void* body, void* def) { + mjCDef* defC = static_cast(def); + mjCSite* siteC = static_cast(body)->AddSite(defC); + return &siteC->spec; +} + + + +// get object of given type +void* mjm_findObject(void* model, mjtObj type, const char* name) { + mjCModel* modelC = static_cast(model); + return modelC->FindObject(type, std::string(name)); +} + + + +// set parent frame of dest +void mjm_setFrame(void* dest, void* frame) { + mjCFrame* frameC = static_cast(frame); + mjCBase* baseC = static_cast(dest); + baseC->SetFrame(frameC); +} + + + +// set string +void mjm_setString(mjString dest, const char* text) { + std::string* str = reinterpret_cast(dest); + *str = std::string(text); +} + + + +// set double array +void mjm_setDouble(mjDouble dest, const double* array, int size) { + std::vector* v = reinterpret_cast*>(dest); + v->assign(size, 0.0); + for (int i = 0; i < size; ++i) { + (*v)[i] = array[i]; + } +} + + + +// get string +const char* mjm_getString(const mjString source) { + std::string* str = reinterpret_cast(source); + return str->c_str(); +} + + + +// get double array +const double* mjm_getDouble(const mjDouble source, int* size) { + std::vector* v = reinterpret_cast*>(source); + if (size) { + *size = v->size(); + } + return v->data(); +} diff --git a/src/user/user_api.h b/src/user/user_api.h new file mode 100644 index 00000000..893ff83e --- /dev/null +++ b/src/user/user_api.h @@ -0,0 +1,92 @@ +// Copyright 2021 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MUJOCO_SRC_USER_USER_API_H_ +#define MUJOCO_SRC_USER_USER_API_H_ + +#include +#include + +typedef struct _mjString* mjString; +typedef struct _mjDouble* mjDouble; +typedef struct _mjElement* mjElement; + + + +//---------------------------------- Public structs ------------------------------------------------ +typedef struct _mjmOrientation { + double axisangle[4]; // rotation axis and angle + double xyaxes[6]; // x and y axes + double zaxis[3]; // z axis (use minimal rotation) + double euler[3]; // euler rotations + double fullinertia[6]; // non-axis-aligned inertia matrix +} mjmOrientation; + +typedef struct _mjmSite { + mjElement element; // only used internally, do not modify + mjString name; // name + mjString classname; // class name + mjString info; // message appended to errors + mjtGeom type; // geom type for rendering + int group; // group id, used for visualization + double size[3]; // geom size for rendering + double pos[3]; // position + double quat[4]; // orientation + mjString material; // name of material for rendering + mjDouble userdata; // user data + float rgba[4]; // rgba when material is omitted + double fromto[6]; // alternative for capsule, cylinder, box, ellipsoid + mjmOrientation alt; // alternative orientation specification +} mjmSite; + +//---------------------------------- Public API ---------------------------------------------------- +#ifdef __cplusplus +extern "C" { +#endif + +// Create model. +MJAPI void* mjm_createModel(); + +// Delete model. +MJAPI void mjm_deleteModel(void* model); + +// Add body to body. +MJAPI void* mjm_addBody(void* body, void* def); + +// Add site to body. +MJAPI mjmSite* mjm_addSite(void* body, void* def); + +// Find object of given type. +MJAPI void* mjm_findObject(void* model, mjtObj type, const char* name); + +// Copy input text to destination string. +MJAPI void mjm_setString(mjString dest, const char* text); + +// Copy input array to destination vector. +MJAPI void mjm_setDouble(mjDouble dest, const double* array, int size); + +// Get const pointer to mjString data. +MJAPI const char* mjm_getString(mjString source); + +// Get const pointer to mjDouble data and its size. +MJAPI const double* mjm_getDouble(mjDouble source, int* size); + +// Set frame. +MJAPI void mjm_setFrame(void* dest, void* frame); + +#ifdef __cplusplus +} +#endif + +#endif // MUJOCO_SRC_USER_USER_API_H_ diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 1f399e2b..a8806eed 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -43,6 +43,7 @@ #include "engine/engine_util_solve.h" #include "engine/engine_util_spatial.h" #include "engine/engine_vfs.h" +#include "user/user_api.h" #include "user/user_model.h" #include "user/user_util.h" @@ -1931,25 +1932,35 @@ mjCSite::mjCSite(mjCModel* _model, mjCDef* _def) { spec.group = 0; mjuu_setvec(spec.quat, 1, 0, 0, 0); mjuu_setvec(spec.pos, 0, 0, 0); - spec.material = nullptr; + 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 = nullptr; + spec_userdata_.clear(); + spec.alt.axisangle[0] = spec.alt.xyaxes[0] = spec.alt.zaxis[0] = + spec.alt.euler[0] = spec.alt.fullinertia[0] = mjNAN; // clear internal variables body = 0; matid = -1; - // initialize private attributes - CopyFromSpec(); - // reset to default if given if (_def) { _def->site.CopyFromSpec(); *this = _def->site; } + // point to local, not to default + spec.element = (mjElement)this; + spec.name = (mjString)&name; + spec.info = (mjString)&info; + spec.classname = (mjString)&classname; + spec.material = (mjString)&spec_material_; + spec.userdata = (mjDouble)&spec_userdata_; + + // initialize private attributes in case object won't be compiled + CopyFromSpec(); + // set model, def model = _model; def = (_def ? _def : (_model ? _model->defaults[0] : 0)); @@ -1961,8 +1972,13 @@ void mjCSite::CopyFromSpec() { *static_cast(this) = spec; userdata_ = spec_userdata_; material_ = spec_material_; - userdata = userdata_.data(); - material = material_.data(); + userdata = (mjDouble)&userdata_; + material = (mjString)&material_; + mju_copy4(alt_.axisangle, alt.axisangle); + mju_copy(alt_.xyaxes, alt.xyaxes, 6); + mju_copy3(alt_.zaxis, alt.zaxis); + mju_copy3(alt_.euler, alt.euler); + mju_copy(alt_.fullinertia, alt.fullinertia, 6); } @@ -2034,7 +2050,7 @@ void mjCSite::Compile(void) { // alternative orientation else { - const char* err = alt.Set(quat, 0, model->degree, model->euler); + const char* err = alt_.Set(quat, 0, model->degree, model->euler); if (err) { throw mjCError(this, "orientation specification error '%s' in site %d", err, id); } diff --git a/src/user/user_objects.h b/src/user/user_objects.h index b64c073e..fa06467d 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -24,11 +24,10 @@ #include #include -#include "lodepng.h" - #include #include #include +#include "user/user_api.h" // forward declarations of all mjC/X classes class mjCError; @@ -109,18 +108,12 @@ class [[nodiscard]] mjCError { // alternative specifications of frame orientation -class mjCAlternative { +class mjCAlternative : public mjmOrientation { public: mjCAlternative(); // constuctor const char* Set(double* quat, double* inertia, // set frame quat and diag. inertia bool degree, // angle format: degree/radian const char* sequence); // euler sequence format: "xyz" - - double axisangle[4]; // rotation axis and angle - double xyaxes[6]; // x and y axes - double zaxis[3]; // z axis (use minimal rotation) - double euler[3]; // euler rotations - double fullinertia[6]; // non-axis-aligned inertia matrix }; @@ -458,19 +451,6 @@ class mjCGeom : public mjCBase { //------------------------- class mjCSite ---------------------------------------------------------- // Describes a site on a body -typedef struct _mjmSite { - mjtGeom type; // geom type for rendering - int group; // group id, used for visualization - double size[3]; // geom size for rendering - double pos[3]; // position - double quat[4]; // orientation - char* material; // name of material for rendering - double* userdata; // user data - float rgba[4]; // rgba when material is omitted - double fromto[6]; // alternative for capsule, cylinder, box, ellipsoid - mjCAlternative alt; // alternative orientation specification -} mjmSite; - class mjCSite : public mjCBase, private mjmSite { friend class mjCDef; friend class mjCBody; @@ -481,30 +461,30 @@ class mjCSite : public mjCBase, private mjmSite { public: mjmSite spec; // variables set by user - void set_material(std::string _material) { - spec_material_ = _material; - spec.material = spec_material_.data(); - } - void set_userdata(std::vector _userdata) { - spec_userdata_ = _userdata; - spec.userdata = spec_userdata_.data(); - } - std::vector& get_userdata() { return userdata_; } - std::string& get_material() { return material_; } + // use strings from mjCBase rather than mjStrings from mjmSite + using mjCBase::name; + using mjCBase::classname; + using mjCBase::info; + + // used by mjXWriter and mjCModel + const std::vector& get_userdata() { return userdata_; } + const std::string& get_material() { return material_; } - // variables computed by 'compile' and 'mjCBody::addSite' private: mjCSite(mjCModel* = 0, mjCDef* = 0); // constructor void Compile(void); // compiler void CopyFromSpec(); // copy spec into attributes + mjCAlternative alt_; + mjCAlternative spec_alt_; + // variable-size data std::string material_; std::vector userdata_; - std::string spec_material_; std::vector spec_userdata_; + // variables computed by 'compile' and 'mjCBody::addSite' mjCBody* body; // site's body int matid; // material id for rendering }; diff --git a/src/xml/xml_base.cc b/src/xml/xml_base.cc index d95b1f8d..82229747 100644 --- a/src/xml/xml_base.cc +++ b/src/xml/xml_base.cc @@ -21,6 +21,7 @@ #include #include +#include "user/user_api.h" #include "user/user_model.h" #include "user/user_objects.h" #include "tinyxml2.h" @@ -50,7 +51,7 @@ void mjXBase::SetModel(mjCModel* _model) { // read alternative orientation specification -void mjXBase::ReadAlternative(XMLElement* elem, mjCAlternative& alt) { +void mjXBase::ReadAlternative(XMLElement* elem, mjmOrientation& alt) { string text; int read = (int)(elem->Attribute("quat") != 0) + (ReadAttr(elem, "axisangle", 4, alt.axisangle, text) ? 1 : 0) + diff --git a/src/xml/xml_base.h b/src/xml/xml_base.h index 3e853abf..ddff3542 100644 --- a/src/xml/xml_base.h +++ b/src/xml/xml_base.h @@ -92,7 +92,7 @@ class mjXBase : public mjXUtil { void SetModel(mjCModel*); // read alternative orientation specification - static void ReadAlternative(tinyxml2::XMLElement* elem, mjCAlternative& alt); + static void ReadAlternative(tinyxml2::XMLElement* elem, mjmOrientation& alt); protected: mjCModel* model; // internally-allocated mjCModel object diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 94f792ed..3fcd95a8 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -33,6 +33,7 @@ #include "engine/engine_plugin.h" #include "engine/engine_util_errmem.h" #include "engine/engine_util_misc.h" +#include "user/user_api.h" #include "user/user_composite.h" #include "user/user_flexcomp.h" #include "user/user_model.h" @@ -1563,34 +1564,39 @@ void mjXReader::OneGeom(XMLElement* elem, mjCGeom* pgeom) { // site element parser -void mjXReader::OneSite(XMLElement* elem, mjCSite* site) { +void mjXReader::OneSite(XMLElement* elem, mjmSite& site) { int n; - string text; - mjmSite* psite = &site->spec; + string text, name, classname; std::vector userdata; std::string material; // read attributes - ReadAttrTxt(elem, "name", site->name); - ReadAttrTxt(elem, "class", site->classname); + ReadAttrTxt(elem, "name", name); + ReadAttrTxt(elem, "class", classname); if (MapValue(elem, "type", &n, geom_map, mjNGEOMTYPES)) { - psite->type = (mjtGeom)n; + site.type = (mjtGeom)n; } - ReadAttr(elem, "size", 3, psite->size, text, false, false); - ReadAttrInt(elem, "group", &psite->group); - ReadAttr(elem, "pos", 3, psite->pos, text); - ReadQuat(elem, "quat", psite->quat, text); + ReadAttr(elem, "size", 3, site.size, text, false, false); + ReadAttrInt(elem, "group", &site.group); + ReadAttr(elem, "pos", 3, site.pos, text); + ReadQuat(elem, "quat", site.quat, text); ReadAttrTxt(elem, "material", material); - ReadAttr(elem, "rgba", 4, psite->rgba, text); - ReadAttr(elem, "fromto", 6, psite->fromto, text); - ReadAlternative(elem, psite->alt); + ReadAttr(elem, "rgba", 4, site.rgba, text); + ReadAttr(elem, "fromto", 6, site.fromto, text); + ReadAlternative(elem, site.alt); ReadVector(elem, "user", userdata, text); - // set variable-size attributes - site->set_userdata(userdata); - site->set_material(material); + // set strings + mjm_setString(site.name, name.c_str()); + mjm_setString(site.classname, classname.c_str()); + mjm_setString(site.material, material.c_str()); - GetXMLPos(elem, site); + // set pointers + mjm_setDouble(site.userdata, userdata.data(), userdata.size()); + + // set info + mjm_setString(site.info, + std::string("line = " + std::to_string(elem->GetLineNum()) + ", column = -1").c_str()); } @@ -2177,7 +2183,7 @@ void mjXReader::OneComposite(XMLElement* elem, mjCBody* pbody, mjCDef* def) { ReadAttrInt(esite, "group", &dsite.group); ReadAttrTxt(esite, "material", material); ReadAttr(esite, "rgba", 4, dsite.rgba, text); - comp.def[0].site.set_material(material); + mjm_setString(dsite.material, material.c_str()); } // joint @@ -2487,7 +2493,7 @@ void mjXReader::Default(XMLElement* section, int parentid) { else if (name=="geom") OneGeom(elem, &def->geom); // read site - else if (name=="site") OneSite(elem, &def->site); + else if (name=="site") OneSite(elem, def->site.spec); // read camera else if (name=="camera") OneCamera(elem, &def->camera); @@ -3061,9 +3067,9 @@ void mjXReader::Body(XMLElement* section, mjCBody* pbody, mjCFrame* frame) { // site sub-element else if (name=="site") { // create site and parse - mjCSite* psite = pbody->AddSite(def); - OneSite(elem, psite); - psite->SetFrame(frame); + mjmSite* site = mjm_addSite(pbody, def); + OneSite(elem, *site); + mjm_setFrame(site, frame); } // camera sub-element diff --git a/src/xml/xml_native_reader.h b/src/xml/xml_native_reader.h index 45f9671e..6ccf5e36 100644 --- a/src/xml/xml_native_reader.h +++ b/src/xml/xml_native_reader.h @@ -59,7 +59,7 @@ class mjXReader : public mjXBase { void OneMaterial(tinyxml2::XMLElement* elem, mjCMaterial* pmaterial); void OneJoint(tinyxml2::XMLElement* elem, mjCJoint* pjoint); void OneGeom(tinyxml2::XMLElement* elem, mjCGeom* pgeom); - void OneSite(tinyxml2::XMLElement* elem, mjCSite* psite); + void OneSite(tinyxml2::XMLElement* elem, mjmSite& site); void OneCamera(tinyxml2::XMLElement* elem, mjCCamera* pcamera); void OneLight(tinyxml2::XMLElement* elem, mjCLight* plight); void OnePair(tinyxml2::XMLElement* elem, mjCPair* ppair); diff --git a/src/xml/xml_util.cc b/src/xml/xml_util.cc index 2d68ab6e..c838d8d6 100644 --- a/src/xml/xml_util.cc +++ b/src/xml/xml_util.cc @@ -1034,7 +1034,7 @@ template void mjXUtil::WriteAttr(XMLElement* elem, string name, int n, // write vector attribute, default = zero array -void mjXUtil::WriteVector(XMLElement* elem, string name, vector& vec) { +void mjXUtil::WriteVector(XMLElement* elem, string name, const vector& vec) { // proceed only if non-zero found bool ok = false; for (size_t i=0; i& vec) { // write vector attribute, default with same size -void mjXUtil::WriteVector(XMLElement* elem, string name, vector& vec, - vector& def) { +void mjXUtil::WriteVector(XMLElement* elem, string name, const vector& vec, + const vector& def) { // proceed only if non-zero found bool ok = false; for (size_t i=0; i attribute, with and without default - static void WriteVector(tinyxml2::XMLElement* elem, std::string name, std::vector& vec); - static void WriteVector(tinyxml2::XMLElement* elem, std::string name, std::vector& vec, - std::vector& def); + static void WriteVector(tinyxml2::XMLElement* elem, std::string name, + const std::vector& vec); + static void WriteVector(tinyxml2::XMLElement* elem, std::string name, + const std::vector& vec, const std::vector& def); // write attribute- string static void WriteAttrTxt(tinyxml2::XMLElement* elem, std::string name, std::string value); diff --git a/test/user/CMakeLists.txt b/test/user/CMakeLists.txt index eee664bb..0c824e96 100644 --- a/test/user/CMakeLists.txt +++ b/test/user/CMakeLists.txt @@ -18,6 +18,9 @@ target_link_libraries(user_model_test fixture gmock absl::str_format) mujoco_test(user_objects_test) target_link_libraries(user_objects_test fixture gmock) +mujoco_test(user_api_test) +target_link_libraries(user_api_test fixture gmock) + mujoco_test(user_flex_test) target_link_libraries(user_flex_test fixture gmock) diff --git a/test/user/user_api_test.cc b/test/user/user_api_test.cc new file mode 100644 index 00000000..49472d5a --- /dev/null +++ b/test/user/user_api_test.cc @@ -0,0 +1,58 @@ +// Copyright 2021 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Tests for user/user_api.cc. + + +#include +#include +#include "src/user/user_api.h" +#include +#include "test/fixture.h" + +namespace mujoco { +namespace { + +using ::testing::HasSubstr; + + +// ----------------------------- test set/get -------------------------------- + +TEST_F(MujocoTest, ReadWriteData) { + void* model = mjm_createModel(); + void* world = mjm_findObject(model, mjOBJ_BODY, "world"); + void* body = mjm_addBody(world, 0); + mjmSite* site = mjm_addSite(body, 0); + + { + double vec[10] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9}; + const char* str = "sitename"; + + mjm_setString(site->name, str); + mjm_setDouble(site->userdata, vec, 10); + } + + EXPECT_THAT(mjm_getString(site->name), HasSubstr("sitename")); + + int nsize; + const double* vec = mjm_getDouble(site->userdata, &nsize); + for (int i = 0; i < nsize; ++i) { + EXPECT_EQ(vec[i], i); + } + + mjm_deleteModel(model); +} + +} // namespace +} // namespace mujoco