15 #include "gmock/gmock.h" 16 #include "gtest/gtest.h" 25 TEST(MiniCheetah, miniCheetahModel1) {
33 std::vector<int> parentRef{0, 0, 0, 0, 0, 0, 5, 6, 7,
34 5, 9, 10, 5, 12, 13, 5, 15, 16};
48 0, 0, 0, 0.0663, 0, 0, 0.05, 0, -0.066336, 0, 0, 0, 0, 0.0497, 0, 0, 0, 0,
49 -0.066336, 0, 8.417, 0, 0, 0.066336, 0, 0, 0, 8.417, 0, 0, 0, 0, 0, 0,
54 for (
size_t i = 0; i < 18; i++) {
59 EXPECT_TRUE(
almostEqual(inertiaSum, inertiaSumRef, .0003));
65 TEST(MiniCheetah, miniCheetahModel2) {
69 for (
size_t i = 0; i < 18; i++) {
70 XTotal = XTotal + cheetah.
_Xtree[i];
71 XRotTotal = XRotTotal + cheetah.
_Xrot[i];
74 Xtr << 11.0000, 0.0000, 0, 0, 0, 0, -0.0000, 11.0000, 0, 0, 0, 0, 0, 0,
75 19.0000, 0, 0, 0, 0, -0.8360, 0, 11.0000, 0.0000, 0, 0.8360, 0, 0.0000,
76 -0.0000, 11.0000, 0, 0, 0, 0, 0, 0, 19.0000;
78 Xrtr << 11.0000, 0.0000, 0, 0, 0, 0, -0.0000, 11.0000, 0, 0, 0, 0, 0, 0,
79 19.0000, 0, 0, 0, 0, 0, 0, 11.0000, 0.0000, 0, 0, 0, 0.0000, -0.0000,
80 11.0000, 0, 0.0000, 0, 0, 0, 0, 19.0000;
90 TEST(MiniCheetah, simulatorDynamicsDoesntCrashMiniCheetah) {
103 TEST(MiniCheetah, simulatorDynamicsABANoExternalForceMiniCheetah) {
105 buildMiniCheetah<double>().buildModel();
112 bodyVel << 1, 2, 3, 4, 5, 6;
117 for (
size_t i = 0; i < 12; i++) {
120 tau[i] = (i + 1) * -3.;
138 vbdRef << 1217.31, -182.793, -171.524, -4.09393, 33.6798, -59.975;
141 qddRef << -1946.36, -1056.62, -1470.57, -3133.03, -1263.89, -2626.9, -6637.38,
142 -4071.88, -4219.14, -7554.08, -3392.44, -5112.85;
147 for (
size_t i = 0; i < 12; i++) {
159 TEST(MiniCheetah, simulatorDynamicsWithExternalForceMiniCheetah) {
161 buildMiniCheetah<double>().buildModel();
168 bodyVel << 1, 2, 3, 4, 5, 6;
173 for (
size_t i = 0; i < 12; i++) {
176 tau[i] = (i + 1) * -3.;
188 for (
size_t i = 0; i < 18; i++) {
189 for (
size_t j = 0; j < 6; j++) {
190 forces[i][j] = .3 * (i + j + 1);
197 sim.
step(0.0, tau, 5e5, 5e3);
202 vbdRef << 3153.46, 42.6931, 264.584, -3.80573, -53.3519, 68.5713;
205 qddRef << -1211.85, -1167.39, -2024.94, 394.414, -297.854, -2292.29, -1765.92,
206 -4040.78, -4917.41, 149.368, -2508.2, -4969.02;
211 for (
size_t i = 0; i < 12; i++) {
222 TEST(MiniCheetah, simulatorFootPosVelMiniCheetah) {
224 buildMiniCheetah<double>().buildModel();
231 bodyVel << 1, 2, 3, 4, 5, 6;
236 for (
size_t i = 0; i < 12; i++) {
239 tau[i] = (i + 1) * -30.;
251 for (
size_t i = 0; i < 18; i++) {
252 for (
size_t j = 0; j < 6; j++) {
253 forces[i][j] = i + j + 1;
260 sim.
step(0.0, tau, 5e5, 5e3);
274 TEST(MiniCheetah, hipLocationConvention) {
283 auto quadruped = buildMiniCheetah<double>();
285 for (
int i = 0; i < 4; i++) {
286 hipLocations[i] = quadruped.getHipLocation(i);
289 for (
int i = 0; i < 4; i++) {
292 EXPECT_TRUE(
almostEqual(hipLocations[i], hipLocationRef[i], .0001));
296 TEST(MiniCheetah, ContactPositionVelocity) {
298 buildMiniCheetah<double>().buildModel();
306 bodyVel << 0., 0., 0., 1.0, 0., 0.;
311 for (
size_t i = 0; i < 12; i++) {
314 tau[i] = (i + 1) * -30.;
326 for (
size_t i = 0; i < 18; i++) {
327 for (
size_t j = 0; j < 6; j++) {
328 forces[i][j] = i + j + 1;
368 TEST(MiniCheetah, InertiaProperty) {
370 buildMiniCheetah<double>().buildModel();
378 for (
size_t i = 0; i < 12; i++) {
vector< Mat6< T >, Eigen::aligned_allocator< Mat6< T > > > _Xrot
bool fpEqual(T a, T b, T tol)
void forwardKinematics()
Do forward kinematics for feet.
Mat3< T > coordinateRotation(CoordinateAxis axis, T theta)
typename Eigen::Matrix< T, 6, 6 > Mat6
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Vec3< T > dBodyPosition
Data structure containing parameters for quadruped robot.
typename Eigen::Matrix< T, Eigen::Dynamic, Eigen::Dynamic > DMat
TEST(MiniCheetah, miniCheetahModel1)
const std::vector< int > & getParentVector()
vector< Mat6< T >, Eigen::aligned_allocator< Mat6< T > > > _Xtree
Utility function to build a Mini Cheetah Quadruped object.
typename Eigen::Matrix< T, 3, 1 > Vec3
const std::vector< SpatialInertia< T >, Eigen::aligned_allocator< SpatialInertia< T > > > & getBodyInertiaVector()
void setState(const FBModelState< T > &state)
typename Eigen::Matrix< T, 6, 1 > SVec
const DMat< T > & getMassMatrix() const
const std::vector< SpatialInertia< T >, Eigen::aligned_allocator< SpatialInertia< T > > > & getRotorInertiaVector()
void setAllExternalForces(const vectorAligned< SVec< T >> &forces)
Quat< typename T::Scalar > rotationMatrixToQuaternion(const Eigen::MatrixBase< T > &r1)
bool almostEqual(const Eigen::MatrixBase< T > &a, const Eigen::MatrixBase< T > &b, T2 tol)
void step(T dt, const DVec< T > &tau, T kp, T kd)
Initialize simulator with given model.
void runABA(const DVec< T > &tau)
Simulate forward one step.
bool vectorEqual(const std::vector< T > &a, const std::vector< T > &b)
void setState(const FBModelState< T > &state)
Quat< typename T::Scalar > rpyToQuat(const Eigen::MatrixBase< T > &rpy)
typename std::vector< T, Eigen::aligned_allocator< T >> vectorAligned
const FBModelStateDerivative< T > & getDState() const
typename Eigen::Matrix< T, Eigen::Dynamic, 1 > DVec
Rigid Body Dynamics Simulator with Collisions.
const FloatingBaseModel< T > & getModel()
typename Eigen::Matrix< T, 3, 3 > RotMat
Implementation of Rigid Body Floating Base model data structure.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Quat< T > bodyOrientation