From e8c67ca586e23ac62e8f8ef58e82945036edcbf2 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Wed, 26 Mar 2025 14:43:55 -0700 Subject: [PATCH] Allow orientation for user composite. PiperOrigin-RevId: 740909723 Change-Id: Id38a55114d4257a461bbced57d35d6bb42a2597d --- doc/XMLreference.rst | 9 +++++++-- doc/XMLschema.rst | 2 ++ doc/changelog.rst | 5 +++++ src/user/user_composite.cc | 12 ++++++++---- src/user/user_composite.h | 3 ++- src/xml/xml_native_reader.cc | 5 +++-- 6 files changed, 27 insertions(+), 9 deletions(-) diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index ca7258f8..5d052472 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -3003,8 +3003,13 @@ cable, which produces an inextensible chain of bodies connected with ball joints .. _body-composite-offset: :at:`offset`: :at-val:`real(3), "0 0 0"` - It specifies a 3D offset from the center of the parent body to the center of the grid of elements. The offset is - expressed in the local coordinate frame of the parent body. + It specifies a 3D offset from the center of the parent body to the center of the first body of the cable. The offset + is expressed in the local coordinate frame of the parent body. + +.. _body-composite-quat: + +:at:`quat`: :at-val:`real(4), "1 0 0 0"` + It specifies a quaternion that rotates the first body frame. The quaternion is expressed in the parent body frame. .. _body-composite-vertex: diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index 0f71f6a3..6cbc0121 100644 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -344,6 +344,8 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | | :ref:`vertex` | :ref:`initial` | :ref:`curve` | :ref:`size` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`quat` | | | | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_2| composite |br| |_2| |L| | | .. table:: | | :ref:`joint | \* | :class: mjcf-attributes | diff --git a/doc/changelog.rst b/doc/changelog.rst index 05d68306..fd565e76 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -11,6 +11,11 @@ Upcoming version (not yet released) - The default value of the flag for toggling :ref:`internal flex contacts` was changed from "true" to "false". This feature has proven to be counterintuitive for users. +General +^^^^^^^ +- Add :ref:`orientation` parameter to :ref:`composite`. Moreover, allow the + composite to be the direct child of a frame. + Bug fixes ^^^^^^^^^ - :ref:`mj_jacDot` was missing a term that accounts for the motion of the point with respect to diff --git a/src/user/user_composite.cc b/src/user/user_composite.cc index 98c3ed41..6cd21b51 100644 --- a/src/user/user_composite.cc +++ b/src/user/user_composite.cc @@ -52,6 +52,7 @@ mjCComposite::mjCComposite(void) { type = mjCOMPTYPE_PARTICLE; count[0] = count[1] = count[2] = 1; mjuu_setvec(offset, 0, 0, 0); + mjuu_setvec(quat, 1, 0, 0, 0); frame = nullptr; // plugin variables @@ -260,19 +261,20 @@ bool mjCComposite::MakeCable(mjCModel* model, mjsBody* body, char* error, int er // populate uservert if not specified if (uservert.empty()) { for (int ix=0; ix < count[0]; ix++) { + double v[3]; for (int k=0; k < 3; k++) { switch (curve[k]) { case mjCOMPSHAPE_LINE: - uservert.push_back(ix*size[0]/(count[0]-1)); + v[k] = ix*size[0]/(count[0]-1); break; case mjCOMPSHAPE_COS: - uservert.push_back(size[1]*cos(mjPI*ix*size[2]/(count[0]-1))); + v[k] = size[1]*cos(mjPI*ix*size[2]/(count[0]-1)); break; case mjCOMPSHAPE_SIN: - uservert.push_back(size[1]*sin(mjPI*ix*size[2]/(count[0]-1))); + v[k] = size[1]*sin(mjPI*ix*size[2]/(count[0]-1)); break; case mjCOMPSHAPE_ZERO: - uservert.push_back(0); + v[k] = 0; break; default: // SHOULD NOT OCCUR @@ -280,6 +282,8 @@ bool mjCComposite::MakeCable(mjCModel* model, mjsBody* body, char* error, int er break; } } + mjuu_rotVecQuat(v, v, quat); + uservert.insert(uservert.end(), v, v+3); } } diff --git a/src/user/user_composite.h b/src/user/user_composite.h index 0f826760..17378004 100644 --- a/src/user/user_composite.h +++ b/src/user/user_composite.h @@ -73,7 +73,8 @@ class mjCComposite { std::string prefix; // name prefix mjtCompType type; // composite type int count[3]; // geom count in each dimension - double offset[3]; // position offset for particle and grid + double offset[3]; // position offset + double quat[4]; // quaternion offset // currently used only for cable std::string initial; // root boundary type diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 44efc4ec..272b5a2c 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -287,8 +287,8 @@ const char* MJCF[nMJCF][mjXATTRNUM] = { {"<"}, {"config", "*", "2", "key", "value"}, {">"}, - {"composite", "*", "8", "prefix", "type", "count", "offset", - "vertex", "initial", "curve", "size"}, + {"composite", "*", "9", "prefix", "type", "count", "offset", + "vertex", "initial", "curve", "size", "quat"}, {"<"}, {"joint", "*", "17", "kind", "group", "stiffness", "damping", "armature", "solreffix", "solimpfix", "type", "axis", @@ -2402,6 +2402,7 @@ void mjXReader::OneComposite(XMLElement* elem, mjsBody* body, mjsFrame* frame, c } ReadAttr(elem, "count", 3, comp.count, text, false, false); ReadAttr(elem, "offset", 3, comp.offset, text); + ReadAttr(elem, "quat", 4, comp.quat, text); comp.frame = frame; // plugin