test_cheetah3_model.cpp File Reference

#include "Dynamics/FloatingBaseModel.h"
#include "Dynamics/Quadruped.h"
#include "Utilities/utilities.h"
#include "include/Dynamics/Cheetah3.h"
#include "gmock/gmock.h"
#include "gtest/gtest.h"
#include <stdio.h>

Include dependency graph for test_cheetah3_model.cpp:

Go to the source code of this file.

Functions
	TEST (Cheetah3, simulatorDynamicsDoesntCrashCheetah3)
	TEST (Cheetah3, simulatorContactInertiaCheetah3)
	TEST (Cheetah3, simulatorTestForceCheetah3)

Function Documentation

TEST	(	Cheetah3	,
		simulatorDynamicsDoesntCrashCheetah3
	)

Creates a cheetah model and runs forward kinematics and ABA Doesn't test anyting - this is just to make sure it doesn't crash

Definition at line 25 of file test_cheetah3_model.cpp.

References FloatingBaseModel< T >::_nDof, FloatingBaseModel< T >::forwardKinematics(), FBModelStateDerivative< T >::qdd, and FloatingBaseModel< T >::runABA().

                                                     {
  FloatingBaseModel<double> cheetah = buildCheetah3<double>().buildModel();
  DVec<double> tau(12);
  FBModelStateDerivative<double> _dstate;
  _dstate.qdd = DVec<double>::Zero(cheetah._nDof - 6);

  cheetah.forwardKinematics();
  cheetah.runABA(tau, _dstate);
}

Here is the call graph for this function:

TEST	(	Cheetah3	,
		simulatorContactInertiaCheetah3
	)

Run the contact inertia algorithm for cheetah 3 Set a weird body orientation, velocity, q, dq, and tau Checks that it matches the result from J H^(-1) J^T

Definition at line 40 of file test_cheetah3_model.cpp.

References FloatingBaseModel< T >::_Jc, almostEqual(), FBModelState< T >::bodyOrientation, FBModelState< T >::bodyPosition, FBModelState< T >::bodyVelocity, FloatingBaseModel< T >::contactJacobians(), ori::coordinateRotation(), FloatingBaseModel< T >::invContactInertia(), FloatingBaseModel< T >::massMatrix(), FBModelState< T >::q, FBModelState< T >::qd, ori::rotationMatrixToQuaternion(), and FloatingBaseModel< T >::setState().

                                                {
  FloatingBaseModel<double> cheetahModel = buildCheetah3<double>().buildModel();

  RotMat<double> rBody = coordinateRotation(CoordinateAxis::X, .123) *
                         coordinateRotation(CoordinateAxis::Z, .232) *
                         coordinateRotation(CoordinateAxis::Y, .111);
  SVec<double> bodyVel;
  bodyVel << 1, 2, 3, 4, 5, 6;
  FBModelState<double> x;
  DVec<double> q(12);
  DVec<double> dq(12);
  DVec<double> tauref(12);
  for (size_t i = 0; i < 12; i++) {
    q[i] = i + 1;
    dq[i] = (i + 1) * 2;
    tauref[i] = (i + 1) * -30.;
  }

  // set state
  x.bodyOrientation = rotationMatrixToQuaternion(rBody.transpose());
  x.bodyVelocity = bodyVel;
  x.bodyPosition = Vec3<double>(6, 7, 8);
  x.q = q;
  x.qd = dq;

  // do aba
  cheetahModel.setState(x);

  cheetahModel.contactJacobians();
  DMat<double> H = cheetahModel.massMatrix();
  D3Mat<double> J0 = cheetahModel._Jc[15];
  DMat<double> LambdaInv1 = J0 * H.colPivHouseholderQr().solve(J0.transpose());

  D6Mat<double> forceOnly = D6Mat<double>::Zero(6, 3);
  forceOnly.bottomRows<3>() = Mat3<double>::Identity();

  DMat<double> LambdaInv2 = cheetahModel.invContactInertia(15, forceOnly);

  EXPECT_TRUE(almostEqual(LambdaInv1, LambdaInv2, .001));
}

Here is the call graph for this function:

TEST	(	Cheetah3	,
		simulatorTestForceCheetah3
	)

Check a test force for cheetah 3 Set a weird body orientation, velocity, q, dq, and tau Checks the result matches qdd = H^(-1) J^T F

Definition at line 86 of file test_cheetah3_model.cpp.

References FloatingBaseModel< T >::_Jc, almostEqual(), FloatingBaseModel< T >::applyTestForce(), FBModelState< T >::bodyOrientation, FBModelState< T >::bodyPosition, FBModelState< T >::bodyVelocity, FloatingBaseModel< T >::contactJacobians(), ori::coordinateRotation(), FBModelStateDerivative< T >::dBodyVelocity, FloatingBaseModel< T >::invContactInertia(), FloatingBaseModel< T >::massMatrix(), FBModelState< T >::q, FBModelState< T >::qd, FBModelStateDerivative< T >::qdd, ori::rotationMatrixToQuaternion(), and FloatingBaseModel< T >::setState().

                                           {
  FloatingBaseModel<double> cheetahModel = buildCheetah3<double>().buildModel();

  RotMat<double> rBody = coordinateRotation(CoordinateAxis::X, .123) *
                         coordinateRotation(CoordinateAxis::Z, .232) *
                         coordinateRotation(CoordinateAxis::Y, .111);
  SVec<double> bodyVel;
  bodyVel << 1, 2, 3, 4, 5, 6;
  FBModelState<double> x;
  DVec<double> q(12);
  DVec<double> dq(12);
  DVec<double> tauref(12);
  for (size_t i = 0; i < 12; i++) {
    q[i] = i + 1;
    dq[i] = (i + 1) * 2;
    tauref[i] = (i + 1) * -30.;
  }

  // set state
  x.bodyOrientation = rotationMatrixToQuaternion(rBody.transpose());
  x.bodyVelocity = bodyVel;
  x.bodyPosition = Vec3<double>(6, 7, 8);
  x.q = q;
  x.qd = dq;

  FBModelStateDerivative<double> dx;
  dx.qdd.setZero(12);

  cheetahModel.setState(x);

  cheetahModel.contactJacobians();
  DMat<double> H = cheetahModel.massMatrix();
  DMat<double> J0 = cheetahModel._Jc[15].bottomRows(1);

  Vec3<double> zforce;
  zforce << 0, 0, 1;

  double foot_accel_in_z = cheetahModel.applyTestForce(15, zforce, dx);

  DVec<double> qddFull = H.colPivHouseholderQr().solve(J0.transpose());
  DMat<double> LambdaInv = J0 * qddFull;

  SVec<double> dBodyVelocity = qddFull.head<6>();
  DVec<double> qdd = qddFull.tail(12);

  double foot_accel_2 = LambdaInv(0, 0);

  double LambdaInv2 = cheetahModel.invContactInertia(15, zforce);

  EXPECT_TRUE(almostEqual(dBodyVelocity, dx.dBodyVelocity, .001));
  EXPECT_TRUE(almostEqual(qdd, dx.qdd, 1e-6));
  EXPECT_TRUE(abs(foot_accel_2 - foot_accel_in_z) < .001);
  EXPECT_TRUE(abs(LambdaInv2 - foot_accel_in_z) < .001);

  qdd(0) += 5;
  EXPECT_FALSE(almostEqual(qdd, dx.qdd, 1e-6));
}

Here is the call graph for this function: