11 #include "gmock/gmock.h" 12 #include "gtest/gtest.h" 21 TEST(Spatial, axisRotation) {
24 X1 << 0.8384, 0.4580, -0.2955, 0, 0, 0, -0.4183, 0.8882, 0.1898, 0, 0, 0,
25 0.3494, -0.0355, 0.9363, 0, 0, 0, 0, 0, 0, 0.8384, 0.4580, -0.2955, 0, 0,
26 0, -0.4183, 0.8882, 0.1898, 0, 0, 0, 0.3494, -0.0355, 0.9363;
40 v1 << 1, 2, 3, 4, 5, 6;
41 v2 << 6, 5, 4, 3, 2, 1;
43 v4 << -7, 14, -7, -14, 28, -14;
45 v5 << 0, 0, 0, 0, 0, 0;
57 v1 << 1, 2, 3, 4, 5, 6;
58 v2 << 6, 5, 4, 3, 2, 1;
60 m1.transposeInPlace();
72 v1 << 1, 2, 3, 4, 5, 6;
73 v2 << 6, 5, 4, 3, 2, 1;
86 v1 << 1, 2, 3, 4, 5, 6;
87 v2 << 6, 5, 4, 3, 2, 1;
101 I << 1, 2, 3, 2, 4, 5, 3, 5, 6;
106 pref << 4204.5, 4618, 5037, 420, 4618, 5083.5, 5539, 462, 5037, 5539, 6047.5,
107 504, 420, 462, 504, 42;
109 ref << 11131, -4618, -5037, 0, -504, 462, -4618, 10252, -5539, 504, 0, -420,
110 -5037, -5539, 9288, -462, 420, 0, 0, 504, -462, 42, 0, 0, -504, 0, 420, 0,
111 42, 0, 462, -420, 0, 0, 0, 42;
129 a << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10;
130 aref << 1, 2, -3, 4, 5, 6, 7, -8, 9, -10;
141 TEST(Spatial, pluho_and_plux) {
152 Xref << 0.4120, -0.0587, -0.9093, 0, 0, 0, -0.8337, -0.4269, -0.3502, 0, 0, 0,
153 -0.3676, 0.9024, -0.2248, 0, 0, 0, -4.1941, 6.1091, -2.2948, 0.4120,
154 -0.0587, -0.9093, 0.8106, -3.6017, 2.4610, -0.8337, -0.4269, -0.3502,
155 -6.5385, -1.3064, 5.4477, -0.3676, 0.9024, -0.2248;
156 Href << 0.4120, -0.0587, -0.9093, 4.1015, -0.8337, -0.4269, -0.3502, 7.5706,
157 -0.3676, 0.9024, -0.2248, -1.6923, 0, 0, 0, 1.0000;
193 Xr_ref << 0.7072, 0, -0.7070, 0, 0, 0, 0, 1.0000, 0, 0, 0, 0, 0.7070, 0,
194 0.7072, 0, 0, 0, 0, 0, 0, 0.7072, 0, -0.7070, 0, 0, 0, 0, 1.0000, 0, 0, 0,
195 0, 0.7070, 0, 0.7072;
197 phi_r_ref << 0, 1, 0, 0, 0, 0;
199 Xp_ref << 1.0000, 0, 0, 0, 0, 0, 0, 1.0000, 0, 0, 0, 0, 0, 0, 1.0000, 0, 0, 0,
200 0, 0.2000, 0, 1.0000, 0, 0, -0.2000, 0, 0, 0, 1.0000, 0, 0, 0, 0, 0, 0,
203 phi_p_ref << 0, 0, 0, 0, 0, 1;
207 EXPECT_TRUE(
almostEqual(phi_r, phi_r_ref, .000001));
208 EXPECT_TRUE(
almostEqual(phi_p, phi_p_ref, .000001));
215 TEST(Spatial, mass_properties) {
217 a << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10;
219 I_ref << 5, 10, 9, 0, -4, 3, 10, 6, 8, 4, 0, -2, 9, 8, 7, -3, 2, 0, 0, 4, -3,
220 1, 0, 0, -4, 0, 2, 0, 1, 0, 3, -2, 0, 0, 0, 1;
232 I_ref << 2.0833, 0, 0, 0, 1.66667, 0, 0, 0, 1.083333;
241 TEST(Spatial, velocityConver) {
244 vs << 1, 2, 3, 4, 5, 6;
253 TEST(Spatial, pointTransform) {
274 fRef << 6, -12, 6, 1, 2, 3;
285 vspat << 1.93, 2.34, 3.345, -4.23, 5.8383, 6.921;
289 point << -23.23, 2.638, 9.324;
309 vspat << 0, 0, w, v, -w * p, 0;
310 aspat << 0, 0, 0, 0, -w * v, 0;
320 a1_expected << w * w * p, 0, 0;
321 a2_expected << 0, 0, 0;
326 vspat << 1.93, 2.34, 3.345, -4.23, 5.8383, 6.921;
327 aspat << -5.164, 68.4, 1.56879, -98.44, 8.14, 6.324;
328 p2 << 54.797, -6.1654, 3.64587;
340 v2 = v1 + w1.cross(p2);
341 vd2 = vd1 + wd1.cross(p2);
343 a1_expected = vd1 + w1.cross(v1);
344 a2_expected = vd2 + w2.cross(v2);
auto forceToSpatialForce(const Eigen::MatrixBase< T > &f, const Eigen::MatrixBase< T2 > &p)
auto spatialToLinearVelocity(const Eigen::MatrixBase< T > &v, const Eigen::MatrixBase< T2 > &x)
auto forceCrossProduct(const Eigen::MatrixBase< T > &a, const Eigen::MatrixBase< T > &b)
Mat3< T > coordinateRotation(CoordinateAxis axis, T theta)
typename Eigen::Matrix< T, 6, 6 > Mat6
Mat6< T > jointXform(JointType joint, CoordinateAxis axis, T q)
typename Eigen::Matrix< T, 3, 3 > Mat3
const Mat6< T > & getMatrix() const
typename Eigen::Matrix< T, 4, 4 > Mat4
typename Eigen::Matrix< T, 3, 1 > Vec3
Mat3< T > getInertiaTensor()
auto translationFromSXform(const Eigen::MatrixBase< T > &X)
auto spatialToLinearAcceleration(const Eigen::MatrixBase< T > &a, const Eigen::MatrixBase< T2 > &v)
typename Eigen::Matrix< T, 6, 6 > SXform
auto sXFormPoint(const Eigen::MatrixBase< T > &X, const Eigen::MatrixBase< T2 > &p)
typename Eigen::Matrix< T, 6, 1 > SVec
SXform< T > spatialRotation(CoordinateAxis axis, T theta)
auto motionCrossMatrix(const Eigen::MatrixBase< T > &v)
MassProperties< T > asMassPropertyVector()
auto createSXform(const Eigen::MatrixBase< T > &R, const Eigen::MatrixBase< T2 > &r)
Mat4< T > getPseudoInertia()
Class representing spatial inertia tensors.
auto sxformToHomogeneous(const Eigen::MatrixBase< T > &X)
bool almostEqual(const Eigen::MatrixBase< T > &a, const Eigen::MatrixBase< T > &b, T2 tol)
TEST(Spatial, axisRotation)
auto motionCrossProduct(const Eigen::MatrixBase< T > &a, const Eigen::MatrixBase< T > &b)
auto homogeneousToSXform(const Eigen::MatrixBase< T > &H)
typename Eigen::Matrix< T, 10, 1 > MassProperties
auto forceCrossMatrix(const Eigen::MatrixBase< T > &v)
Mat3< typename T::Scalar > rotInertiaOfBox(typename T::Scalar mass, const Eigen::MatrixBase< T > &dims)
typename Eigen::Matrix< T, 3, 3 > RotMat
auto rotationFromSXform(const Eigen::MatrixBase< T > &X)
Utility functions for manipulating spatial quantities.
auto invertSXform(const Eigen::MatrixBase< T > &X)