Added tests for chains

gtdynamics/dynamics/Chain.cpp

@@ -142,6 +142,17 @@ gtsam::Vector3_ Chain::ChainConstraint3(
   // Get Expression for wrench
   gtsam::Vector6_ wrench(wrench_key);
 
+  // The constraint is true for the wrench exerted on the end-effector frame, so
+  // we need to adjoint from base to end-effector
+  gtsam::Matrix66 Ad_B_E = sMb_.AdjointMap();
+
+  // Create an expression of the wrench on the base multiplied by the adjoint
+  // map
+  const std::function<gtsam::Vector6(gtsam::Vector6)> f =
+      [Ad_B_E](const gtsam::Vector6 &wrench) { return Ad_B_E * wrench; };
+
+  gtsam::Vector6_ adjoint_wrench = gtsam::linearExpression(f, wrench, Ad_B_E);
+
   // Get expression for joint angles as a column vector of size 3.
   gtsam::Double_ angle0(JointAngleKey(joints[0]->id(), k)),
       angle1(JointAngleKey(joints[1]->id(), k)),
@@ -160,7 +171,7 @@ gtsam::Vector3_ Chain::ChainConstraint3(
                 std::placeholders::_2, std::placeholders::_3,
                 std::placeholders::_4, std::placeholders::_5,
                 std::placeholders::_6),
-      wrench, angles, torques);
+      adjoint_wrench, angles, torques);
 
   return torque_diff;
 }

tests/testChain.cpp

@@ -15,11 +15,13 @@
 #include <CppUnitLite/TestHarness.h>
 #include <gtdynamics/dynamics/Chain.h>
 #include <gtdynamics/optimizer/EqualityConstraint.h>
+#include <gtdynamics/optimizer/Optimizer.h>
 #include <gtdynamics/universal_robot/sdf.h>
 #include <gtsam/base/Matrix.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/nonlinear/factorTesting.h>
+#include <gtdynamics/factors/MinTorqueFactor.h>
 
 using namespace gtdynamics;
 using gtsam::assert_equal;
@@ -183,7 +185,7 @@ TEST(Chain, ChainConstraint) {
 
   // Create VectorExpressionEquality Constraint
   auto constraint = VectorExpressionEquality<3>(expression, tolerance);
-  Vector3 expected_values(1, 1.9, 1.3);
+  Vector3 expected_values(1, 0.46, 0.82);
   bool constraint_violation = constraint.feasible(init_values);
   Vector values = constraint(init_values);
   EXPECT(!constraint_violation);
@@ -570,7 +572,7 @@ TEST(Chain, ChainConstraintFactorJacobians) {
 
   // Create VectorExpressionEquality Constraint
   auto constraint = VectorExpressionEquality<3>(expression, tolerance);
-  Vector3 expected_values(1, 1.9, 1.3);
+  Vector3 expected_values(1, 0.46, 0.82);
   bool constraint_violation = constraint.feasible(init_values);
   Vector values = constraint(init_values);
   EXPECT(!constraint_violation);
@@ -583,6 +585,274 @@ TEST(Chain, ChainConstraintFactorJacobians) {
   EXPECT_CORRECT_FACTOR_JACOBIANS(*factor, init_values, 1e-7, 1e-3);
 }
 
+TEST(Chain, A1QuadOneLegCompare) {
+  // This test checks equality between torque calculation with and without
+  // chains. This assumes one static leg of the a1 quadruped with zero mass for
+  // the links besides the trunk.
+
+  auto robot =
+      CreateRobotFromFile(kUrdfPath + std::string("/a1/a1.urdf"), "a1");
+
+  // initialize joints of FL leg
+  JointSharedPtr j0,j1,j2;
+
+  for (auto&& joint : robot.joints()) {
+    if (joint->name().find("FL") != std::string::npos) {
+      if (joint->name().find("lower") != std::string::npos) {
+        j2 = joint;
+      }
+      if (joint->name().find("upper") != std::string::npos) {
+        j1 = joint;
+      }
+      if (joint->name().find("hip") != std::string::npos) {
+        j0 = joint;
+      }
+    }
+  }
+
+  // Calculate all relevant relative poses.
+  Pose3 M_T_H = j0->pMc();
+  Pose3 M_H_T = M_T_H.inverse();
+  Pose3 M_H_U = j1->pMc();
+  Pose3 M_U_H = M_H_U.inverse();
+  Pose3 M_U_L = j2->pMc();
+  Pose3 M_L_U = M_U_L.inverse();
+  Pose3 M_T_L = M_T_H * M_H_U * M_U_L;
+  Pose3 M_L_T = M_T_L.inverse();
+
+  // Set Gravity Wrench
+  Matrix gravity(1, 6);
+  gravity << 0.0, 0.0, 0.0, 0.0, 0.0, -10.0;
+
+  // Calculate all wrenches and torques without chains.
+  Matrix F_2_L = -gravity * M_T_L.AdjointMap();
+  Matrix F_1_U = F_2_L * M_L_U.AdjointMap();
+  Matrix F_0_H = F_1_U * M_U_H.AdjointMap();
+
+  // joint 0
+  Matrix tau0 = F_0_H * j0->cScrewAxis();
+  EXPECT(assert_equal(tau0(0, 0), -0.448145, 1e-6));  // regression
+
+  // joint 1
+  Matrix tau1 = F_1_U * j1->cScrewAxis();
+  EXPECT(assert_equal(tau1(0, 0), 1.70272, 1e-5));  // regression
+
+  // joint 2
+  Matrix tau2 = F_2_L * j2->cScrewAxis();
+  EXPECT(assert_equal(tau2(0, 0), 1.70272, 1e-5));  // regression
+
+  // Calculate all wrenches and torques with chains.
+  Chain chain1(M_T_H, j0->cScrewAxis());
+  Chain chain2(M_H_U, j1->cScrewAxis());
+  Chain chain3(M_U_L, j2->cScrewAxis());
+
+  std::vector<Chain> chains{chain1, chain2, chain3};
+
+  // Compose Chains
+  Chain composed = Chain::compose(chains);
+
+  // test for same values
+  Matrix torques = F_2_L * composed.axes();
+  EXPECT(assert_equal(torques(0, 0), tau0(0, 0), 1e-6));
+  EXPECT(assert_equal(torques(0, 1), tau1(0, 0), 1e-5));
+  EXPECT(assert_equal(torques(0, 2), tau2(0, 0), 1e-5));
+}
+
+std::vector<std::vector<JointSharedPtr>> getChainJoints(const Robot& robot) {
+  std::vector<JointSharedPtr> FR(3), FL(3), RR(3), RL(3);
+
+  int loc;
+  for (auto&& joint : robot.joints()) {
+    if (joint->name().find("lower") != std::string::npos) {
+      loc = 2;
+    }
+    if (joint->name().find("upper") != std::string::npos) {
+      loc = 1;
+    }
+    if (joint->name().find("hip") != std::string::npos) {
+      loc = 0;
+    }
+    if (joint->name().find("FR") != std::string::npos) {
+      FR[loc] = joint;
+    }
+    if (joint->name().find("FL") != std::string::npos) {
+      FL[loc] = joint;
+    }
+    if (joint->name().find("RR") != std::string::npos) {
+      RR[loc] = joint;
+    }
+    if (joint->name().find("RL") != std::string::npos) {
+      RL[loc] = joint;
+    }
+  }
+
+  std::vector<std::vector<JointSharedPtr>> chain_joints{FL, FR, RL, RR};
+
+  return chain_joints;
+}
+
+Chain BuildChain(std::vector<JointSharedPtr>& joints) {
+  auto j0 = joints[0];
+  auto j1 = joints[1];
+  auto j2 = joints[2];
+
+  // Calculate all relevant relative poses.
+  Pose3 M_T_H = j0->pMc();
+  Pose3 M_H_T = M_T_H.inverse();
+  Pose3 M_H_U = j1->pMc();
+  Pose3 M_U_H = M_H_U.inverse();
+  Pose3 M_U_L = j2->pMc();
+  Pose3 M_L_U = M_U_L.inverse();
+  Pose3 M_T_L = M_T_H * M_H_U * M_U_L;
+  Pose3 M_L_T = M_T_L.inverse();
+
+  // Create chains
+  Chain chain1(M_T_H, j0->cScrewAxis());
+  Chain chain2(M_H_U, j1->cScrewAxis());
+  Chain chain3(M_U_L, j2->cScrewAxis());
+
+  std::vector<Chain> chains{chain1, chain2, chain3};
+
+  Chain composed = Chain::compose(chains);
+
+  return composed;
+}
+
+std::vector<Chain> getComposedChains(
+    std::vector<std::vector<JointSharedPtr>>& chain_joints) {
+  Chain composed_fr, composed_fl, composed_rr, composed_rl;
+
+  composed_fr = BuildChain(chain_joints[0]);
+  composed_fl = BuildChain(chain_joints[1]);
+  composed_rr = BuildChain(chain_joints[2]);
+  composed_rl = BuildChain(chain_joints[3]);
+
+  std::vector<Chain> composed_chains{composed_fr, composed_fl, composed_rr,
+                                     composed_rl};
+
+  return composed_chains;
+}
+
+TEST(Chain, A1QuadStaticChainGraph) {
+  // This test uses chain constraints for each leg of the robot, a wrench
+  // constraint on the trunk, zero angles at the joints constraints (robot
+  // standing still) and minimum torque constraints.
+
+  auto robot =
+      CreateRobotFromFile(kUrdfPath + std::string("/a1/a1.urdf"), "a1");
+
+  // Get joint and composed chains for each leg
+  auto chain_joints = getChainJoints(robot);
+  auto composed_chains = getComposedChains(chain_joints);
+
+  // Initialize Constraints
+  EqualityConstraints constraints;
+
+  // Hard constraint on zero angles
+  double angle_tolerance = 1e-30;
+  for (int i = 0; i < 4; ++i) {
+    for (int j = 0; j < 3; ++j) {
+      const int joint_id = chain_joints[i][j]->id();
+      gtsam::Double_ angle(JointAngleKey(joint_id, 0));
+      constraints.emplace_shared<DoubleExpressionEquality>(angle,
+                                                           angle_tolerance);
+    }
+  }
+
+  // Get key for wrench at hip joints  on link 0 at time 0
+  const gtsam::Key wrench_key_fl =
+      gtdynamics::WrenchKey(0, 0, 0);  // fl hip joint id  = 0
+  const gtsam::Key wrench_key_fr =
+      gtdynamics::WrenchKey(0, 3, 0);  // fr hip joint id  = 3
+  const gtsam::Key wrench_key_rl =
+      gtdynamics::WrenchKey(0, 6, 0);  // rl hip joint id  = 6
+  const gtsam::Key wrench_key_rr =
+      gtdynamics::WrenchKey(0, 9, 0);  // rr hip joint id  = 9
+
+  // create expressions for these wrenches
+  gtsam::Vector6_ wrench_fl(wrench_key_fl);
+  gtsam::Vector6_ wrench_fr(wrench_key_fr);
+  gtsam::Vector6_ wrench_rl(wrench_key_rl);
+  gtsam::Vector6_ wrench_rr(wrench_key_rr);
+
+  // Set Gravity Wrench
+  gtsam::Vector6 gravity;
+  gravity << 0.0, 0.0, 0.0, 0.0, 0.0, -10.0;
+  gtsam::Vector6_ gravity_wrench(gravity);
+
+  // Create expression for wrench constraint on trunk
+  auto expression =
+      wrench_fl + wrench_fr + wrench_rl + wrench_rr + gravity_wrench;
+  gtsam::Vector6 wrench_tolerance;
+  wrench_tolerance << 1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6;
+  constraints.emplace_shared<VectorExpressionEquality<6>>(expression,
+                                                          wrench_tolerance);
+
+  // Get expressions for chains on each leg
+  auto expression_fl =
+      composed_chains[0].ChainConstraint3(chain_joints[0], wrench_key_fl, 0);
+  auto expression_fr =
+      composed_chains[1].ChainConstraint3(chain_joints[1], wrench_key_fr, 0);
+  auto expression_rl =
+      composed_chains[2].ChainConstraint3(chain_joints[2], wrench_key_rl, 0);
+  auto expression_rr =
+      composed_chains[3].ChainConstraint3(chain_joints[3], wrench_key_rr, 0);
+
+  gtsam::Vector3 torque_tolerance;
+  torque_tolerance << 1e-6, 1e-6, 1e-6;
+
+  constraints.emplace_shared<VectorExpressionEquality<3>>(expression_fl,
+                                                          torque_tolerance);
+  constraints.emplace_shared<VectorExpressionEquality<3>>(expression_fr,
+                                                          torque_tolerance);
+  constraints.emplace_shared<VectorExpressionEquality<3>>(expression_rl,
+                                                          torque_tolerance);
+  constraints.emplace_shared<VectorExpressionEquality<3>>(expression_rr,
+                                                          torque_tolerance);
+
+  // Constrain Minimum torque in actuators
+  gtsam::NonlinearFactorGraph graph;
+  auto cost_model = gtsam::noiseModel::Unit::Create(1);
+  for (auto&& joint : robot.joints()) {
+    MinTorqueFactor factor(TorqueKey(joint->id(), 0), cost_model);
+    graph.add(factor);
+  }
+
+  // Create initial values.
+  gtsam::Values init_values;
+  for (auto&& joint : robot.joints()) {
+    InsertJointAngle(&init_values, joint->id(), 0, 0.0);
+    InsertTorque(&init_values, joint->id(), 0, 0.0);
+  }
+
+  gtsam::Vector6 wrench_zero;
+  wrench_zero << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
+  init_values.insert(wrench_key_fl, wrench_zero);
+  init_values.insert(wrench_key_fr, wrench_zero);
+  init_values.insert(wrench_key_rl, wrench_zero);
+  init_values.insert(wrench_key_rr, wrench_zero);
+
+  /// Solve the constraint problem with LM optimizer.
+  Optimizer optimizer;
+  gtsam::Values results = optimizer.optimize(graph, constraints, init_values);
+
+  gtsam::Vector6 result_wrench_fl = results.at<gtsam::Vector6>(wrench_key_fl);
+  gtsam::Vector6 result_wrench_fr = results.at<gtsam::Vector6>(wrench_key_fr);
+  gtsam::Vector6 result_wrench_rl = results.at<gtsam::Vector6>(wrench_key_rl);
+  gtsam::Vector6 result_wrench_rr = results.at<gtsam::Vector6>(wrench_key_rr);
+
+  // We expect these wrenches to be close  to 2.5 N in Z direction.
+  EXPECT(assert_equal(result_wrench_fr(5), 2.5, 1e-4));
+  EXPECT(assert_equal(result_wrench_fl(5), 2.5, 1e-4));
+  EXPECT(assert_equal(result_wrench_rr(5), 2.5, 1e-4));
+  EXPECT(assert_equal(result_wrench_rl(5), 2.5, 1e-4));
+
+  for (auto&& joint : robot.joints()) {
+    const int id = joint->id();
+    EXPECT(assert_equal(results.at<double>(JointAngleKey(id)), 0.0, 1e-30));
+  }
+}
+
 int main() {
   TestResult tr;
   return TestRegistry::runAllTests(tr);