13 #include "gmock/gmock.h" 14 #include "gtest/gtest.h" 26 TEST(Dynamics, cheetah3model) {
35 std::vector<int> parentRef{0, 0, 0, 0, 0, 0, 5, 6, 7,
36 5, 9, 10, 5, 12, 13, 5, 15, 16};
41 inertiaSumRef << 0.4352, -0.0000, -0.0044, 0, 0.6230, -0.0000, -0.0000,
42 1.0730, 0.0000, -0.6230, 0, -0.0822, -0.0044, 0.0000, 1.0071, 0.0000,
43 0.0822, 0, 0, -0.6230, 0.0000, 42.6540, 0, 0, 0.6230, 0, 0.0822, 0,
44 42.6540, 0, -0.0000, -0.0822, 0, 0, 0, 42.6540;
48 for (
size_t i = 0; i < 18; i++) {
52 EXPECT_TRUE(
almostEqual(inertiaSum, inertiaSumRef, .0001));
58 TEST(Dynamics, cheetah3ModelTransforms) {
62 for (
size_t i = 0; i < 18; i++) {
63 XTotal = XTotal + cheetah.
_Xtree[i];
64 XRotTotal = XRotTotal + cheetah.
_Xrot[i];
67 Xtr << 11.0000, 0.0000, 0, 0, 0, 0, -0.0000, 11.0000, 0, 0, 0, 0, 0, 0,
68 19.0000, 0, 0, 0, 0, -1.3680, 0, 11.0000, 0.0000, 0, 1.3680, 0, 0.0000,
69 -0.0000, 11.0000, 0, 0.0000, 0, 0, 0, 0, 19.0000;
70 Xrtr << 11.0000, 0.0000, 0, 0, 0, 0, -0.0000, 11.0000, 0, 0, 0, 0, 0, 0,
71 19.0000, 0, 0, 0, 0, 0, 0.0000, 11.0000, 0.0000, 0, 0, 0, 0, -0.0000,
72 11.0000, 0, 0, 0, 0, 0, 0, 19.0000;
83 TEST(Dynamics, simulatorDynamicsABANoExternalForceCheetah3) {
92 bodyVel << 1, 2, 3, 4, 5, 6;
97 for (
size_t i = 0; i < 12; i++) {
100 tau[i] = (i + 1) * -30.;
118 vbdRef << 455.5224, 62.1684, -70.56, -11.4505, 10.2354, -15.2394;
121 qddRef << -0.7924, -0.2205, -0.9163, -1.6136, -0.4328, -1.6911, -2.9878,
122 -0.9358, -2.6194, -3.3773, -1.3235, -3.1598;
128 for (
size_t i = 0; i < 12; i++) {
138 Quat<double> quat2Ref(-0.8962, -0.0994, -0.2610, -0.3446);
141 v2Ref << 912.0449, 126.3367, -138.1201, -18.9011, 25.4709, -24.4788;
153 TEST(Dynamics, inverseDynamicsNoContacts) {
160 bodyVel << 1, 2, 3, 4, 5, 6;
165 for (
size_t i = 0; i < 12; i++) {
168 tauref[i] = (i + 1) * -30.;
181 vbd << 455.5224, 62.1684, -70.56, -11.4505, 10.2354, -15.2394;
184 qdd << -0.7924, -0.2205, -0.9163, -1.6136, -0.4328, -1.6911, -2.9878, -0.9358,
185 -2.6194, -3.3773, -1.3235, -3.1598;
195 nu_dot.tail(12) = qdd;
204 DVec<double> component_ID_result = H * nu_dot + Cqd + G;
212 EXPECT_TRUE(
almostEqual(component_ID_result, genForce, 1e-6));
220 TEST(Dynamics, contactJacobians) {
227 bodyVel << 1, 2, 3, 4, 5, 6;
232 for (
size_t i = 0; i < 12; i++) {
235 tauref[i] = (i + 1) * -30.;
248 vbd << 45.5224, 62.1684, -70.56, -11.4505, 10.2354, -15.2394;
251 qdd << -0.7924, -0.2205, -0.9163, -1.6136, -0.4328, -1.6911, -2.9878, -0.9358,
252 -2.6194, -3.3773, -1.3235, -3.1598;
265 nu_dot.tail(12) = qdd;
270 Vec3<double> pos0, posf, vel0, vel0_fromJ, velf, acc0, J0dqd;
271 pos0 = cheetahModel.
_pGC[15];
272 vel0 = cheetahModel.
_vGC[15];
274 vel0_fromJ = J0 * nu;
275 acc0 = J0 * nu_dot + cheetahModel.
_Jcdqd[15];
285 double ang = omega0.norm();
290 Quat<double> quatD(std::cos(ang / 2), ee[0], ee[1], ee[2]);
292 quatNew = quatNew / quatNew.norm();
305 posf = cheetahModel.
_pGC[15];
306 velf = cheetahModel.
_vGC[15];
310 vel_fd = (posf - pos0) / dt;
311 acc_fd = (velf - vel0) / dt;
324 TEST(Dynamics, simulatorDynamicsWithExternalForceCheetah3) {
332 bodyVel << 1, 2, 3, 4, 5, 6;
337 for (
size_t i = 0; i < 12; i++) {
340 tau[i] = (i + 1) * -30.;
352 for (
size_t i = 0; i < 18; i++) {
353 for (
size_t j = 0; j < 6; j++) {
354 forces[i][j] = i + j + 1;
361 sim.
step(0.0, tau, 5e5, 5e3);
366 vbdRef << 806.6664, 44.1266, 33.8287, -10.1360, 2.1066, 12.1677;
369 qddRef << -0.5630, -0.1559, -1.0182, -0.9012, -0.3585, -1.6170, -2.0995,
370 -0.8959, -2.7325, -2.0808, -1.3134, -3.1206;
376 for (
size_t i = 0; i < 12; i++) {
387 TEST(Dynamics, simulatorFootPosVelCheetah3) {
395 bodyVel << 1, 2, 3, 4, 5, 6;
400 for (
size_t i = 0; i < 12; i++) {
403 tau[i] = (i + 1) * -30.;
415 for (
size_t i = 0; i < 18; i++) {
416 for (
size_t j = 0; j < 6; j++) {
417 forces[i][j] = i + j + 1;
424 sim.
step(0.0, tau, 5e5, 5e3);
vector< Mat6< T >, Eigen::aligned_allocator< Mat6< T > > > _Xrot
DVec< T > inverseDynamics(const FBModelStateDerivative< T > &dState)
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
typename Eigen::Matrix< T, 3, 3 > Mat3
DVec< T > generalizedCoriolisForce()
Data structure containing parameters for quadruped robot.
typename Eigen::Matrix< T, Eigen::Dynamic, Eigen::Dynamic > DMat
const std::vector< int > & getParentVector()
Utility function to build a Cheetah 3 Quadruped object.
typename Eigen::Matrix< T, 4, 1 > Quat
vectorAligned< Vec3< T > > _Jcdqd
vector< Mat6< T >, Eigen::aligned_allocator< Mat6< T > > > _Xtree
typename Eigen::Matrix< T, 3, 1 > Vec3
void integrate(T dt)
Integrate to find new _state.
const std::vector< SpatialInertia< T >, Eigen::aligned_allocator< SpatialInertia< T > > > & getBodyInertiaVector()
void setState(const FBModelState< T > &state)
TEST(Dynamics, cheetah3model)
typename Eigen::Matrix< T, 3, Eigen::Dynamic > D3Mat
typename Eigen::Matrix< T, 6, 1 > SVec
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)
Quat< typename T::Scalar > quatProduct(const Eigen::MatrixBase< T > &q1, const Eigen::MatrixBase< T > &q2)
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)
const FBModelState< T > & getState() const
void setState(const FBModelState< T > &state)
typename std::vector< T, Eigen::aligned_allocator< T >> vectorAligned
const FBModelStateDerivative< T > & getDState() const
typename Eigen::Matrix< T, Eigen::Dynamic, 1 > DVec
vectorAligned< D3Mat< T > > _Jc
Rigid Body Dynamics Simulator with Collisions.
Mat3< typename T::Scalar > quaternionToRotationMatrix(const Eigen::MatrixBase< T > &q)
const FloatingBaseModel< T > & getModel()
DVec< T > generalizedGravityForce()
typename Eigen::Matrix< T, 3, 3 > RotMat
Implementation of Rigid Body Floating Base model data structure.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Quat< T > bodyOrientation