13 #include "gmock/gmock.h" 14 #include "gtest/gtest.h" 25 TEST(Cheetah3, simulatorDynamicsDoesntCrashCheetah3) {
32 cheetah.
runABA(tau, _dstate);
40 TEST(Cheetah3, simulatorContactInertiaCheetah3) {
47 bodyVel << 1, 2, 3, 4, 5, 6;
52 for (
size_t i = 0; i < 12; i++) {
55 tauref[i] = (i + 1) * -30.;
71 DMat<double> LambdaInv1 = J0 * H.colPivHouseholderQr().solve(J0.transpose());
78 EXPECT_TRUE(
almostEqual(LambdaInv1, LambdaInv2, .001));
86 TEST(Cheetah3, simulatorTestForceCheetah3) {
93 bodyVel << 1, 2, 3, 4, 5, 6;
98 for (
size_t i = 0; i < 12; i++) {
101 tauref[i] = (i + 1) * -30.;
123 double foot_accel_in_z = cheetahModel.
applyTestForce(15, zforce, dx);
125 DVec<double> qddFull = H.colPivHouseholderQr().solve(J0.transpose());
131 double foot_accel_2 = LambdaInv(0, 0);
137 EXPECT_TRUE(abs(foot_accel_2 - foot_accel_in_z) < .001);
138 EXPECT_TRUE(abs(LambdaInv2 - foot_accel_in_z) < .001);
void runABA(const DVec< T > &tau, FBModelStateDerivative< T > &dstate)
typename Eigen::Matrix< T, 6, Eigen::Dynamic > D6Mat
Mat3< T > coordinateRotation(CoordinateAxis axis, T theta)
typename Eigen::Matrix< T, 3, 3 > Mat3
Data structure containing parameters for quadruped robot.
typename Eigen::Matrix< T, Eigen::Dynamic, Eigen::Dynamic > DMat
Utility function to build a Cheetah 3 Quadruped object.
typename Eigen::Matrix< T, 3, 1 > Vec3
typename Eigen::Matrix< T, 3, Eigen::Dynamic > D3Mat
typename Eigen::Matrix< T, 6, 1 > SVec
Quat< typename T::Scalar > rotationMatrixToQuaternion(const Eigen::MatrixBase< T > &r1)
bool almostEqual(const Eigen::MatrixBase< T > &a, const Eigen::MatrixBase< T > &b, T2 tol)
TEST(Cheetah3, simulatorDynamicsDoesntCrashCheetah3)
void setState(const FBModelState< T > &state)
DMat< T > invContactInertia(const int gc_index, const D6Mat< T > &force_directions)
T applyTestForce(const int gc_index, const Vec3< T > &force_ics_at_contact, FBModelStateDerivative< T > &dstate_out)
typename Eigen::Matrix< T, Eigen::Dynamic, 1 > DVec
vectorAligned< D3Mat< T > > _Jc
typename Eigen::Matrix< T, 3, 3 > RotMat
Implementation of Rigid Body Floating Base model data structure.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Quat< T > bodyOrientation