Cheetah Software  1.0
test_LegController.cpp
Go to the documentation of this file.
1 
6 #include "Dynamics/MiniCheetah.h"
7 #include "Dynamics/Quadruped.h"
8 #include "gmock/gmock.h"
9 #include "gtest/gtest.h"
10 
15 TEST(LegController, JacobianAndFwdKinematics1) {
16  Quadruped<double> quadruped = buildMiniCheetah<double>();
17  Vec3<double> q0(1, 2, 3);
18  Vec3<double> dq(.001, .002, -.001);
19  Mat3<double> J;
20  Vec3<double> p0, p1Ref, p1;
21 
22  Vec3<double> q1 = q0 + dq;
23  computeLegJacobianAndPosition(quadruped, q0, &J, &p0, 0);
24  computeLegJacobianAndPosition(quadruped, q1, (Mat3<double>*)nullptr, &p1Ref,
25  0);
26 
27  p1 = p0 + J * dq;
28 
29  EXPECT_TRUE(almostEqual(p1, p1Ref, .001 * .001));
30 }
31 
35 TEST(LegController, JacobianAndFwdKinematics2) {
36  Quadruped<double> quadruped = buildMiniCheetah<double>();
37  Vec3<double> q0(4, 5, 6);
38  Vec3<double> p0;
39  Mat3<double> Jref;
40  Mat3<double> J;
41 
42  computeLegJacobianAndPosition(quadruped, q0, &Jref, &p0, 0);
43  double d = .001;
44 
45  for (int dim = 0; dim < 3; dim++) {
47  dq(dim) = d;
48  Vec3<double> q1 = q0 + dq;
49  Vec3<double> p1;
50  computeLegJacobianAndPosition(quadruped, q1, (Mat3<double>*)nullptr, &p1,
51  0);
52  Vec3<double> dp = p1 - p0;
53  J.block<3, 1>(0, dim) = dp / d;
54  }
55 
56  EXPECT_TRUE(almostEqual(J, Jref, .001));
57 }
58 
62 TEST(LegController, FwdKinematicsLegSign) {
63  Quadruped<double> quadruped = buildMiniCheetah<double>();
64  Vec3<double> q(0, 0, 0);
65  Vec3<double> p;
66  computeLegJacobianAndPosition(quadruped, q, (Mat3<double>*)nullptr, &p, 0);
67 
68  Vec3<double> pRef(0, -quadruped._abadLinkLength,
69  -quadruped._hipLinkLength - quadruped._kneeLinkLength);
70 
71  EXPECT_TRUE(almostEqual(pRef, p, .00001));
72 
73  computeLegJacobianAndPosition(quadruped, q, (Mat3<double>*)nullptr, &p, 1);
74 
75  Vec3<double> pRef2(0, quadruped._abadLinkLength,
76  -quadruped._hipLinkLength - quadruped._kneeLinkLength);
77 
78  EXPECT_TRUE(almostEqual(pRef2, p, .00001));
79 }
void computeLegJacobianAndPosition(Quadruped< T > &quad, Vec3< T > &q, Mat3< T > *J, Vec3< T > *p, int leg)
T _hipLinkLength
Definition: Quadruped.h:62
typename Eigen::Matrix< T, 3, 3 > Mat3
Definition: cppTypes.h:54
Data structure containing parameters for quadruped robot.
Utility function to build a Mini Cheetah Quadruped object.
typename Eigen::Matrix< T, 3, 1 > Vec3
Definition: cppTypes.h:26
T _kneeLinkLength
Definition: Quadruped.h:62
bool almostEqual(const Eigen::MatrixBase< T > &a, const Eigen::MatrixBase< T > &b, T2 tol)
Definition: MathUtilities.h:23
Common Leg Control Interface and Leg Control Algorithms.
TEST(LegController, JacobianAndFwdKinematics1)
T _abadLinkLength
Definition: Quadruped.h:62