Allow orientation for user composite.

PiperOrigin-RevId: 740909723
Change-Id: Id38a55114d4257a461bbced57d35d6bb42a2597d

Author: quagla

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:
 

doc/XMLschema.rst

@@ -344,6 +344,8 @@
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
 |                                    |    |    | :ref:`vertex<body-composite-vertex>`                            | :ref:`initial<body-composite-initial>`                          | :ref:`curve<body-composite-curve>`                              | :ref:`size<body-composite-size>`                                | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+|                                    |    |    | :ref:`quat<body-composite-quat>`                                |                                                                 |                                                                 |                                                                 | |
+|                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
 +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
 | |_2| composite |br| |_2| |L|       |    | .. table::                                                                                                                                                                                                                                                                   |
 | :ref:`joint                        | \* |    :class: mjcf-attributes                                                                                                                                                                                                                                                   |

doc/changelog.rst

@@ -11,6 +11,11 @@ Upcoming version (not yet released)
    - The default value of the flag for toggling :ref:`internal flex contacts<flex-contact-internal>` was changed from
      "true" to "false". This feature has proven to be counterintuitive for users.
 
+General
+^^^^^^^
+- Add :ref:`orientation<body-composite-quat>` parameter to :ref:`composite<body-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

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,26 +261,29 @@ 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
             mju_error("Invalid composite shape: %d", curve[k]);
             break;
         }
       }
+      mjuu_rotVecQuat(v, v, quat);
+      uservert.insert(uservert.end(), v, v+3);
     }
   }
 

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

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