SpatialMotionTests.cpp
Go to the documentation of this file.
1 //
2 // Created by jordan on 9/24/16.
3 //
4 
5 #include <gtest/gtest.h>
6 #include "UnitTestUtils.hpp"
7 #include "Fixtures.h"
8 #include "rdl_dynamics/Kinematics.h"
9 
10 using namespace RobotDynamics::Math;
11 
12 class SpatialMotionTests : public testing::Test
13 {
14  public:
15  SpatialMotionTests()
16  {
17  }
18 
19  void SetUp()
20  {
21  }
22 
23  void TearDown()
24  {
25  }
26 };
27 
28 TEST_F(FixedBase3DoFPlanar, get_frame_vectors)
29 {
30  SpatialMotion v1(model->getReferenceFrame("body_a"), ReferenceFrame::getWorldFrame(), model->getReferenceFrame("body_a"), 1., 2., 3., 4., 5., 6.);
31 
32  FrameVector fvl = v1.getFramedLinearPart();
33  FrameVector fva = v1.getFramedAngularPart();
34 
35  EXPECT_STREQ(fvl.getReferenceFrame()->getName().c_str(), v1.getReferenceFrame()->getName().c_str());
36 
37  EXPECT_EQ(fva.x(), v1.wx());
38  EXPECT_EQ(fva.y(), v1.wy());
39  EXPECT_EQ(fva.z(), v1.wz());
40 
41  EXPECT_EQ(fvl.x(), v1.vx());
42  EXPECT_EQ(fvl.y(), v1.vy());
43  EXPECT_EQ(fvl.z(), v1.vz());
44 
45  v1.setIncludingFrame(model->getReferenceFrame("body_b"), MotionVector(0.1, -0.1, 0.2, -0.2, 0.3, -0.3));
46 
47  EXPECT_STREQ(v1.getReferenceFrame()->getName().c_str(), "body_b");
48  EXPECT_EQ(v1.wx(), 0.1);
49  EXPECT_EQ(v1.wy(), -0.1);
50  EXPECT_EQ(v1.wz(), 0.2);
51 
52  EXPECT_EQ(v1.vx(), -0.2);
53  EXPECT_EQ(v1.vy(), 0.3);
54  EXPECT_EQ(v1.vz(), -0.3);
55 
56  v1.setIncludingFrame(model->getReferenceFrame("body_c"), 3., 4., 5., 6., 7., 8.);
57 
58  EXPECT_STREQ(v1.getReferenceFrame()->getName().c_str(), "body_c");
59  EXPECT_EQ(v1.wx(), 3.);
60  EXPECT_EQ(v1.wy(), 4.);
61  EXPECT_EQ(v1.wz(), 5.);
62 
63  EXPECT_EQ(v1.vx(), 6.);
64  EXPECT_EQ(v1.vy(), 7.);
65  EXPECT_EQ(v1.vz(), 8.);
66 }
67 
68 TEST_F(FixedBase3DoFPlanar, changeFrameAndCopy)
69 {
70  SpatialMotion v1(model->getReferenceFrame("body_b"), ReferenceFrame::getWorldFrame(), model->getReferenceFrame("body_b"), 1., 2., 3., 4., 5., 6.);
71  SpatialMotion v2;
72 
73  randomizeStates();
74  updateKinematicsCustom(*model, &Q, nullptr, nullptr);
75 
76  v2 = v1.changeFrameAndCopy(ReferenceFrame::getWorldFrame());
77 
78  EXPECT_FALSE(v2.wx() == v1.wx());
79  EXPECT_TRUE(v2.wy() == v1.wy()); // its a rot about y axis, so this one isn't supposed to change
80  EXPECT_FALSE(v2.wz() == v1.wz());
81  EXPECT_FALSE(v2.vx() == v1.vx());
82  EXPECT_FALSE(v2.vy() == v1.vy());
83  EXPECT_FALSE(v2.vz() == v1.vz());
84 
85  v1.changeFrame(ReferenceFrame::getWorldFrame());
86 
87  EXPECT_TRUE(v2.wx() == v1.wx());
88  EXPECT_TRUE(v2.wy() == v1.wy());
89  EXPECT_TRUE(v2.wz() == v1.wz());
90  EXPECT_TRUE(v2.vx() == v1.vx());
91  EXPECT_TRUE(v2.vy() == v1.vy());
92  EXPECT_TRUE(v2.vz() == v1.vz());
93 
94  SpatialMotion v3(model->getReferenceFrame("body_b"), ReferenceFrame::getWorldFrame(), model->getReferenceFrame("body_b"), 1., 2., 3., 4., 5., 6.);
95  SpatialMotion v4;
96 
97  v4 = v3.changeFrameAndCopy(ReferenceFrame::getWorldFrame());
98 
99  EXPECT_FALSE(v3.wx() == v4.wx());
100  EXPECT_TRUE(v3.wy() == v4.wy()); // its a rot about y axis, so this one isn't supposed to change
101  EXPECT_FALSE(v3.wz() == v4.wz());
102  EXPECT_FALSE(v3.vx() == v4.vx());
103  EXPECT_FALSE(v3.vy() == v4.vy());
104  EXPECT_FALSE(v3.vz() == v4.vz());
105 
106  v3.changeFrame(ReferenceFrame::getWorldFrame());
107 
108  EXPECT_TRUE(v3.wx() == v4.wx());
109  EXPECT_TRUE(v3.wy() == v4.wy());
110  EXPECT_TRUE(v3.wz() == v4.wz());
111  EXPECT_TRUE(v3.vx() == v4.vx());
112  EXPECT_TRUE(v3.vy() == v4.vy());
113  EXPECT_TRUE(v3.vz() == v4.vz());
114 }
115 
116 TEST_F(FixedBase3DoFPlanar, testAddAndSubtract)
117 {
118  SpatialMotion v1(model->getReferenceFrame("body_a"), ReferenceFrame::getWorldFrame(), model->getReferenceFrame("body_a"), 0., 1., 0., 0., 0., 0.);
119 
120  SpatialMotion v2(model->getReferenceFrame("body_b"), ReferenceFrame::getWorldFrame(), model->getReferenceFrame("body_b"), 0., 1., 0., 0., 0., 0.);
121 
122  try
123  {
124  // Frame mismatches, should throw
125  SpatialMotion v3 = v1 + v2;
126  FAIL();
127  }
128  catch (ReferenceFrameException& e)
129  {
130  EXPECT_STREQ("Reference frames do not match!", e.what());
131  }
132 
133  SpatialMotion v3(model->getReferenceFrame("body_c"), model->getReferenceFrame("body_b"), ReferenceFrame::getWorldFrame(), 0., 1., 0., 0., 0., 0.);
134 
135  try
136  {
137  // Experessed in frames aren't the same, should throw
138  v3 += v2;
139  FAIL();
140  }
141  catch (ReferenceFrameException& e)
142  {
143  EXPECT_STREQ("Reference frames do not match!", e.what());
144  }
145 
146  SpatialMotion v4(model->getReferenceFrame("body_c"), model->getReferenceFrame("body_b"), model->getReferenceFrame("body_b"), 0., 1., 0., 0., 0., 0.);
147 
148  SpatialMotion v5 = v2 + v4;
149 
150  EXPECT_TRUE(unit_test_utils::checkSpatialVectorsEpsilonClose(v5, SpatialVector(0., 2., 0., 0., 0., 0.), unit_test_utils::TEST_PREC));
151 
152  SpatialMotion v6 = v5;
153 
154  EXPECT_TRUE(unit_test_utils::checkSpatialVectorsEpsilonClose(v5, SpatialVector(0., 2., 0., 0., 0., 0.), unit_test_utils::TEST_PREC));
155 
156  SpatialMotion v7(model->getReferenceFrame("body_c"), model->getReferenceFrame("body_b"), model->getReferenceFrame("body_b"), -0.2, 0.3, -0.4, 0.5, -0.6, 0.7);
157  SpatialMotion v8(model->getReferenceFrame("body_a"), model->getReferenceFrame("body_c"), model->getReferenceFrame("body_b"), -0.5, -0.3, 0.1, 0.1, -4.6, 1.7);
158 
159  EXPECT_EQ(v7.wx(), -0.2);
160  EXPECT_EQ(v7.wy(), 0.3);
161  EXPECT_EQ(v7.wz(), -0.4);
162  EXPECT_EQ(v7.vx(), 0.5);
163  EXPECT_EQ(v7.vy(), -0.6);
164  EXPECT_EQ(v7.vz(), 0.7);
165 
166  SpatialMotion v9 = v7 + v8;
167 
168  EXPECT_STREQ(v9.getReferenceFrame()->getName().c_str(), v8.getReferenceFrame()->getName().c_str());
169  EXPECT_STREQ(v9.getBaseFrame()->getName().c_str(), v7.getBaseFrame()->getName().c_str());
170  EXPECT_STREQ(v9.getReferenceFrame()->getName().c_str(), v7.getReferenceFrame()->getName().c_str());
171 
172  EXPECT_NEAR(v9.wx(), -0.7, unit_test_utils::TEST_PREC);
173  EXPECT_NEAR(v9.wy(), 0, unit_test_utils::TEST_PREC);
174  EXPECT_NEAR(v9.wz(), -0.3, unit_test_utils::TEST_PREC);
175  EXPECT_NEAR(v9.vx(), 0.6, unit_test_utils::TEST_PREC);
176  EXPECT_NEAR(v9.vy(), -5.2, unit_test_utils::TEST_PREC);
177  EXPECT_NEAR(v9.vz(), 2.4, unit_test_utils::TEST_PREC);
178 
179  SpatialMotion v10 = v9 - v8;
180 
181  EXPECT_TRUE(unit_test_utils::checkSpatialVectorsEpsilonClose(v10, v7, unit_test_utils::TEST_PREC));
182 
183  v10 = v9 - v7;
184 
185  EXPECT_TRUE(unit_test_utils::checkSpatialVectorsEpsilonClose(v10, v8, unit_test_utils::TEST_PREC));
186 
187  try
188  {
189  v8 -= v7;
190  FAIL();
191  }
192  catch (ReferenceFrameException& e)
193  {
194  EXPECT_STREQ(e.what(), "Cannot perform -= operation on spatial motion vectors due to a reference frame mismatch!");
195  }
196 
197  v7.setBodyFrame(model->getReferenceFrame("body_a"));
198  v7.setBaseFrame(model->getReferenceFrame("body_b"));
199  EXPECT_TRUE(v7.getBodyFrame() == model->getReferenceFrame("body_a"));
200  EXPECT_TRUE(v7.getBaseFrame() == model->getReferenceFrame("body_b"));
201  EXPECT_TRUE(v7.getReferenceFrame() == model->getReferenceFrame("body_b"));
202 }
203 
204 TEST_F(SpatialMotionTests, testTransformToSpatialVec)
205 {
206  Vector3d rot(1.1, 1.2, 1.3);
207  Vector3d trans(1.1, 1.2, 1.3);
208 
209  SpatialTransform X_st;
210  X_st.r = trans;
211 
212  SpatialMatrix X_66_matrix(SpatialMatrix::Zero(6, 6));
213  X_66_matrix = Xrotz_mat(rot[2]) * Xroty_mat(rot[1]) * Xrotx_mat(rot[0]) * Xtrans_mat(trans);
214  X_st.E = X_66_matrix.block<3, 3>(0, 0);
215 
216  std::shared_ptr<ReferenceFrame> frameA(new ReferenceFrame("frameA", ReferenceFrame::getWorldFrame(), X_st, 0, 0));
217 
218  MotionVector v(1.1, 2.1, 3.1, 4.1, 5.1, 6.1);
219 
220  SpatialMotion m(frameA, ReferenceFrame::getWorldFrame(), frameA, v);
221 
222  v.transform(X_st);
223  SpatialVector vec_out = m.transform_copy(X_st);
224 
225  EXPECT_TRUE(unit_test_utils::checkSpatialVectorsEpsilonClose(vec_out, v, unit_test_utils::TEST_PREC));
226 }
227 
228 TEST_F(SpatialMotionTests, testChangeFrame)
229 {
230  Vector3d rot(1.1, 1.2, 1.3);
231  Vector3d trans(1.1, 1.2, 1.3);
232 
233  SpatialTransform X_st;
234  X_st.r = trans;
235 
236  SpatialMatrix X_66_matrix(SpatialMatrix::Zero(6, 6));
237  X_66_matrix = Xrotz_mat(rot[2]) * Xroty_mat(rot[1]) * Xrotx_mat(rot[0]) * Xtrans_mat(trans);
238  X_st.E = X_66_matrix.block<3, 3>(0, 0);
239 
240  std::shared_ptr<ReferenceFrame> frameA(new ReferenceFrame("frameA", ReferenceFrame::getWorldFrame(), X_st, 0, 0));
241 
242  MotionVector v(1.1, 2.1, 3.1, 4.1, 5.1, 6.1);
243 
244  SpatialMotion m(frameA, ReferenceFrame::getWorldFrame(), frameA, v);
245 
246  v.transform(X_st.inverse());
247 
248  m.changeFrame(ReferenceFrame::getWorldFrame());
249 
250  EXPECT_TRUE(unit_test_utils::checkSpatialVectorsEpsilonClose(m, v, unit_test_utils::TEST_PREC));
251 
252  try
253  {
254  m.changeFrame(nullptr);
255  FAIL();
256  }
257  catch (ReferenceFrameException& e)
258  {
259  EXPECT_STREQ(e.what(), "Either this reference frame or desired reference frame is nullptr!");
260  }
261 }
262 
263 int main(int argc, char** argv)
264 {
265  ::testing::InitGoogleTest(&argc, argv);
266  return RUN_ALL_TESTS();
267 }
RobotDynamics::FrameObject::getReferenceFrame
ReferenceFramePtr getReferenceFrame() const
Get a pointer to the reference frame this FrameObject is expressed in.
Definition: FrameObject.hpp:52
RobotDynamics::Math::SpatialMotion::setIncludingFrame
void setIncludingFrame(ReferenceFramePtr referenceFrame, const SpatialVector &v)
Definition: SpatialMotion.hpp:169
RobotDynamics::Math::Xrotx_mat
SpatialMatrix Xrotx_mat(const double &xrot)
Creates a rotational transformation around the X-axis.
Definition: rdl_mathutils.cc:138
RobotDynamics::ReferenceFrameException
A custom exception for frame operations.
Definition: FrameExceptions.hpp:30
RobotDynamics::Math::MotionVector::vz
EIGEN_STRONG_INLINE double & vz()
Get a reference to the linear-z component.
Definition: MotionVector.hpp:165
RobotDynamics::Math::MotionVector::transform
void transform(const SpatialTransform &X)
Transforms a motion vector. Performs .
Definition: MotionVector.hpp:201
unit_test_utils::TEST_PREC
const double TEST_PREC
Definition: UnitTestUtils.hpp:22
Q
VectorNd Q
Definition: RdlTwoLegModelTests.cpp:29
RobotDynamics::Math::Xtrans_mat
SpatialMatrix Xtrans_mat(const Vector3d &displacement)
Creates a transformation of a linear displacement.
Definition: rdl_mathutils.cc:133
RobotDynamics::Math::SpatialMotion::getFramedAngularPart
FrameVector getFramedAngularPart() const
Get angular part of spatial motion as a frame vector.
Definition: SpatialMotion.hpp:120
RobotDynamics::Math::MotionVector::wx
EIGEN_STRONG_INLINE double & wx()
Get a reference to the angular-x component.
Definition: MotionVector.hpp:93
RobotDynamics::ReferenceFrame::getWorldFrame
static ReferenceFramePtr getWorldFrame()
Get a pointer to the world frame.
Definition: ReferenceFrame.hpp:267
RobotDynamics::Math::MotionVector::wy
EIGEN_STRONG_INLINE double & wy()
Get a reference to the angular-y component.
Definition: MotionVector.hpp:102
RobotDynamics::ReferenceFrameException::what
virtual const char * what() const
Definition: FrameExceptions.hpp:41
RobotDynamics::Math::SpatialMotion::getFramedLinearPart
FrameVector getFramedLinearPart() const
Get linear part of spatial motion as a frame vector.
Definition: SpatialMotion.hpp:111
main
int main(int argc, char **argv)
Definition: SpatialMotionTests.cpp:263
RobotDynamics::Math::MotionVector::vx
EIGEN_STRONG_INLINE double & vx()
Get a reference to the linear-x component.
Definition: MotionVector.hpp:147
RobotDynamics::Math::SpatialTransform
Compact representation of spatial transformations.
Definition: rdl_eigenmath.h:337
RobotDynamics::FrameObject::changeFrame
virtual void changeFrame(ReferenceFramePtr desiredFrame)
Change the ReferenceFrame this FrameObject is expressed in.
Definition: FrameObject.cpp:12
RobotDynamics::Math::SpatialTransform::inverse
SpatialTransform inverse() const
Returns inverse of transform.
Definition: rdl_eigenmath.h:488
RobotDynamics::Math::FrameVector
A FrameVector is a 3D vector with a ReferenceFrame, and all operations between FrameVectors and other...
Definition: FrameVector.hpp:33
RobotDynamics::Math::SpatialMotion
A SpatialMotion vector is a MotionVector with a RobotDynamics::ReferenceFrame it is expressed in...
Definition: SpatialMotion.hpp:35
RobotDynamics::ReferenceFrame
ReferenceFrame object used to tell what frame objects are expressed in. Every ReferenceFrame has a po...
Definition: ReferenceFrame.hpp:88
RobotDynamics::Math::SpatialMotion::getBaseFrame
ReferenceFramePtr getBaseFrame() const
Get a SpatialMotions SpatialMotion::baseFrame.
Definition: SpatialMotion.hpp:155
RobotDynamics::Math::MotionVector::wz
EIGEN_STRONG_INLINE double & wz()
Get a reference to the angular-z component.
Definition: MotionVector.hpp:111
RobotDynamics::Math::MotionVector::vy
EIGEN_STRONG_INLINE double & vy()
Get a reference to the linear-y component.
Definition: MotionVector.hpp:156
RobotDynamics::Math::Xroty_mat
SpatialMatrix Xroty_mat(const double &yrot)
Creates a rotational transformation around the Y-axis.
Definition: rdl_mathutils.cc:147
TEST_F
TEST_F(FixedBase3DoFPlanar, get_frame_vectors)
Definition: SpatialMotionTests.cpp:28
RobotDynamics::updateKinematicsCustom
void updateKinematicsCustom(Model &model, const Math::VectorNd *Q, const Math::VectorNd *QDot=nullptr, const Math::VectorNd *QDDot=nullptr)
Selectively updates model internal states of body positions, velocities and/or accelerations.
Definition: Kinematics.cc:72
RobotDynamics::Math
Math types such as vectors and matrices and utility functions.
Definition: ForceVector.hpp:21
unit_test_utils::checkSpatialVectorsEpsilonClose
static bool checkSpatialVectorsEpsilonClose(const RobotDynamics::Math::SpatialVector &v1, const RobotDynamics::Math::SpatialVector &v2, const double epsilon)
Definition: UnitTestUtils.hpp:306
RobotDynamics::Math::SpatialMotion::changeFrameAndCopy
SpatialMotion changeFrameAndCopy(ReferenceFramePtr referenceFrame) const
Copy and change frame.
Definition: SpatialMotion.hpp:135
RobotDynamics::Math::MotionVector::transform_copy
MotionVector transform_copy(const SpatialTransform &X) const
Copies, transforms, and returns a MotionVector. Performs .
Definition: MotionVector.hpp:217
RobotDynamics::Math::Xrotz_mat
SpatialMatrix Xrotz_mat(const double &zrot)
Creates a rotational transformation around the Z-axis.
Definition: rdl_mathutils.cc:156

rdl_dynamics
Author(s):
autogenerated on Tue Apr 20 2021 02:25:28