Add test for rotational Jacobian formula used in weld constraints

PiperOrigin-RevId: 470947389 Change-Id: I39b9495e3e6d6cf8994e603d6eb69e3e5322e734
2022-08-30 03:42:20 -07:00
parent 6b6fff69ba
commit eb65273fa1
3 changed files with 169 additions and 4 deletions
@@ -70,10 +70,10 @@ void mj_jacSparseSimple(const mjModel* m, const mjData* d,
                        int body, int flg_second, int NV, int start);

 // dense or sparse Jacobian difference for two body points: pos2 - pos1, global
-int mj_jacDifPair(const mjModel* m, const mjData* d, int* chain,
-                  int b1, int b2, const mjtNum pos1[3], const mjtNum pos2[3],
-                  mjtNum* jac1p, mjtNum* jac2p, mjtNum* jacdifp,
-                  mjtNum* jac1r, mjtNum* jac2r, mjtNum* jacdifr);
+MJAPI int mj_jacDifPair(const mjModel* m, const mjData* d, int* chain,
+                        int b1, int b2, const mjtNum pos1[3], const mjtNum pos2[3],
+                        mjtNum* jac1p, mjtNum* jac2p, mjtNum* jacdifp,
+                        mjtNum* jac1r, mjtNum* jac2r, mjtNum* jacdifr);


 //-------------------------- name functions --------------------------------------------------------
@@ -18,6 +18,9 @@ target_link_libraries(engine_collision_convex_test fixture gmock)
 mujoco_test(engine_collision_driver_test)
 target_link_libraries(engine_collision_driver_test fixture gmock)

+mujoco_test(engine_core_constraint_test)
+target_link_libraries(engine_core_constraint_test fixture gmock)
+
 mujoco_test(engine_core_smooth_test)
 target_link_libraries(engine_core_smooth_test fixture gmock)

@@ -0,0 +1,162 @@
+// 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 engine/engine_core_constraint.c.
+
+#include <cstddef>
+#include <string>
+
+#include <gmock/gmock.h>
+#include <gtest/gtest.h>
+#include <mujoco/mjmodel.h>
+#include <mujoco/mujoco.h>
+#include "src/engine/engine_support.h"
+#include "test/fixture.h"
+
+namespace mujoco {
+namespace {
+
+using ::testing::Pointwise;
+using ::testing::DoubleNear;
+using CoreConstraintTest = MujocoTest;
+
+std::vector<mjtNum> AsVector(const mjtNum* array, int n) {
+  return std::vector<mjtNum>(array, array + n);
+}
+
+// compute rotation residual following formula in mj_instantiateEquality
+void RotationResidual(const mjModel *model, mjData *data,
+                        const mjtNum qpos[7], const mjtNum dqpos[6],
+                        mjtNum res[3]) {
+  // copy configuration, compute required quantities with mj_step1
+  mju_copy(data->qpos, qpos, 7);
+
+  // perturb configuration if given
+  if (dqpos) {
+    mj_integratePos(model, data->qpos, dqpos, 1);
+  }
+
+  // update relevant quantities
+  mj_step1(model, data);
+
+  // compute orientation residual
+  mjtNum quat1[4], quat2[4], quat3[4];
+  mju_copy4(quat1, data->xquat+4*1);
+  mju_negQuat(quat2, data->xquat+4*2);
+  mju_mulQuat(quat3, quat2, quat1);
+  mju_copy3(res, quat3+1);
+}
+
+// validate rotational Jacobian used in welds
+TEST_F(CoreConstraintTest, WeldRotJacobian) {
+  constexpr char xml[] = R"(
+  <mujoco>
+    <option jacobian="dense"/>
+    <worldbody>
+      <body>
+        <joint type="ball"/>
+        <geom size=".1"/>
+      </body>
+      <body pos=".5 0 0">
+        <joint axis="1 0 0" pos="0 0 .01"/>
+        <joint axis="0 1 0" pos=".02 0 0"/>
+        <joint axis="0 0 1" pos="0 .03 0"/>
+        <geom size=".1"/>
+      </body>
+    </worldbody>
+  </mujoco>
+  )";
+  mjModel* model = LoadModelFromString(xml);
+  ASSERT_THAT(model, testing::NotNull());
+  ASSERT_EQ(model->nq, 7);
+  ASSERT_EQ(model->nv, 6);
+  static const int nv = 6;  // for increased readabilty
+  mjData* data = mj_makeData(model);
+
+  // arbitrary initial values for the ball and hinge joints
+  mjtNum qpos0[7] = {.5, .5, .5, .5, .7, .8, .9};
+
+  // compute required quantities using mj_step1
+  mj_step1(model, data);
+
+  // get orientation error
+  mjtNum res[3];
+  RotationResidual(model, data, qpos0, NULL, res);
+
+  // compute Jacobian with finite-differencing
+  mjtNum jacFD[3*nv];
+  mjtNum dqpos[nv] = {0};
+  mjtNum dres[3];
+  const mjtNum eps = 1e-6;
+  for (int i=0; i < nv; i++) {
+    // nudge i-th dof
+    dqpos[i] = eps;
+
+    // get nudged residual
+    RotationResidual(model, data, qpos0, dqpos, dres);
+
+    // remove nudge
+    dqpos[i] = 0.0;
+
+    // compute Jacobian column
+    for (int j=0; j < 3; j++) {
+      jacFD[nv*j + i] = (dres[j] - res[j]) / eps;
+    }
+  }
+
+  // reset mjData to qpos0
+  mju_copy(data->qpos, qpos0, 7);
+  mj_step1(model, data);
+
+  // intermediate quaternions quat1 and quat2
+  mjtNum quat1[4], negQuat2[4];
+  mju_copy4(quat1, data->xquat+4*1);
+  mju_negQuat(negQuat2, data->xquat+4*2);
+
+  // get analytical Jacobian following formula in mj_instantiateEquality
+  mjtNum jacdif[3*nv], jac0[3*nv], jac1[3*nv];
+  mjtNum point[3] = {0};
+
+  // rotational Jacobian difference
+  mj_jacDifPair(model, data, NULL, 2, 1, point, point,
+                NULL, NULL, NULL, jac0, jac1, jacdif);
+
+  // formula: 0.5 * neg(quat2) * (jac1-jac2) * quat1
+  mjtNum axis[3], quat3[4], quat4[4];
+  for (int j=0; j < nv; j++) {
+    // axis = [jac1-jac2]_col(j)
+    axis[0] = jacdif[0*nv+j];
+    axis[1] = jacdif[1*nv+j];
+    axis[2] = jacdif[2*nv+j];
+
+    // apply formula
+    mju_mulQuatAxis(quat3, negQuat2, axis);
+    mju_mulQuat(quat4, quat3, quat1);
+
+    // correct Jacobian
+    jacdif[0*nv+j] = 0.5*quat4[1];
+    jacdif[1*nv+j] = 0.5*quat4[2];
+    jacdif[2*nv+j] = 0.5*quat4[3];
+  }
+
+  // test that analytical and finite-differenced Jacobians match
+  EXPECT_THAT(AsVector(jacFD, 3*nv),
+              Pointwise(DoubleNear(eps), AsVector(jacdif, 3*nv)));
+
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
+}  // namespace
+}  // namespace mujoco