Allow orientation for user composite.

PiperOrigin-RevId: 740909723
Change-Id: Id38a55114d4257a461bbced57d35d6bb42a2597d

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);
     }
   }
 

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