FrameOrientationTests.cpp
Go to the documentation of this file.
1 
2 
3 #include <gtest/gtest.h>
4 #include "rdl_dynamics/FrameOrientation.hpp"
5 #include "rdl_dynamics/Kinematics.h"
6 #include "UnitTestUtils.hpp"
7 
8 using namespace RobotDynamics;
9 using namespace RobotDynamics::Math;
10 
11 class FrameOrientationTest : public ::testing::Test
12 {
13  public:
14  FrameOrientationTest()
15  {
16  }
17 
18  void SetUp()
19  {
20  }
21 
22  void TearDown()
23  {
24  }
25 };
26 
27 TEST_F(FrameOrientationTest, setters)
28 {
29  ReferenceFramePtr f1(new ReferenceFrame("f1", ReferenceFrame::getWorldFrame(), SpatialTransform(), true, 0));
30  FrameOrientation orientation(f1, Quaternion());
31 
32  Quaternion q(1., 2., 3., 4.);
33 
34  orientation.set(q);
35  EXPECT_EQ(orientation.x(), 1.);
36  EXPECT_EQ(orientation.y(), 2.);
37  EXPECT_EQ(orientation.z(), 3.);
38  EXPECT_EQ(orientation.w(), 4.);
39 
40  SpatialTransform X = Xrotz(0.1);
41  Quaternion q2(Math::toQuaternion(X.E));
42 
43  orientation.set(X.E);
44 
45  EXPECT_EQ(orientation.x(), Math::toQuaternion(X.E).x());
46  EXPECT_EQ(orientation.y(), Math::toQuaternion(X.E).y());
47  EXPECT_EQ(orientation.z(), Math::toQuaternion(X.E).z());
48  EXPECT_EQ(orientation.w(), Math::toQuaternion(X.E).w());
49 
50  RobotDynamics::Math::FrameOrientation q3(f1, RobotDynamics::Math::AxisAngle(RobotDynamics::Math::toAxisAngle(orientation)));
51  RobotDynamics::Math::FrameOrientation q4(f1, X.E);
52  EXPECT_TRUE(q3.isApprox(q4, unit_test_utils::TEST_PREC));
53 
54  RobotDynamics::Math::Vector3d axis(-0.3, 0.1, 1.2);
55  axis.normalize();
56 
57  RobotDynamics::Math::AxisAngle axis_angle(-3.1, axis);
58  RobotDynamics::Math::Matrix3d E = RobotDynamics::Math::toMatrix(axis_angle);
59 
60  q3.set(axis_angle);
61  q4.set(E);
62  EXPECT_TRUE(q3.isApprox(q4, unit_test_utils::TEST_PREC));
63 }
64 
65 TEST_F(FrameOrientationTest, changeFrame)
66 {
67  SpatialTransform X1 = Xrotz(M_PI_2);
68  ReferenceFramePtr f1(new ReferenceFrame("f1", ReferenceFrame::getWorldFrame(), X1, true, 0));
69  SpatialTransform X2 = Xrotz(-M_PI_2);
70  ReferenceFramePtr f2(new ReferenceFrame("f2", f1, X2, true, 1));
71  SpatialTransform X3 = Xrotz(-M_PI_2);
72  ReferenceFramePtr f3(new ReferenceFrame("f3", f2, X3, true, 1));
73 
74  FrameOrientation orientation(f2, Quaternion());
75  orientation.changeFrame(f1);
76 
77  Quaternion expected(Math::toQuaternion(X2.E));
78  EXPECT_TRUE(unit_test_utils::checkVector4dEpsilonClose(orientation, expected, unit_test_utils::TEST_PREC));
79  EXPECT_STREQ(orientation.getReferenceFrame()->getName().c_str(), "f1");
80 
81  orientation.changeFrame(ReferenceFrame::getWorldFrame());
82 
83  expected = Quaternion(0., 0., 0., 1.);
84  EXPECT_TRUE(unit_test_utils::checkVector4dEpsilonClose(orientation, expected, unit_test_utils::TEST_PREC));
85  EXPECT_STREQ(orientation.getReferenceFrame()->getName().c_str(), "world");
86 
87  orientation.changeFrame(f3);
88  expected = Math::toQuaternion(Xrotz(M_PI_2).E);
89  EXPECT_TRUE(unit_test_utils::checkVector4dEpsilonClose(orientation, expected, unit_test_utils::TEST_PREC));
90  EXPECT_STREQ(orientation.getReferenceFrame()->getName().c_str(), "f3");
91 }
92 
93 TEST_F(FrameOrientationTest, changeFrameAndCopy)
94 {
95  SpatialTransform X1 = Xrotz(M_PI_2);
96  std::shared_ptr<ReferenceFrame> f1(new ReferenceFrame("f1", ReferenceFrame::getWorldFrame(), X1, true, 0));
97  SpatialTransform X2 = Xrotz(-M_PI_2);
98  std::shared_ptr<ReferenceFrame> f2(new ReferenceFrame("f2", f1, X2, true, 1));
99  SpatialTransform X3 = Xrotz(-M_PI_2);
100  std::shared_ptr<ReferenceFrame> f3(new ReferenceFrame("f3", f2, X3, true, 1));
101 
102  FrameOrientation orientation(f2, Quaternion());
103  FrameOrientation orientation_copy = orientation.changeFrameAndCopy(f1);
104 
105  Quaternion expected(Math::toQuaternion(X2.E));
106  EXPECT_TRUE(unit_test_utils::checkVector4dEpsilonClose(orientation_copy, expected, unit_test_utils::TEST_PREC));
107  EXPECT_STREQ(orientation.getReferenceFrame()->getName().c_str(), "f2");
108  EXPECT_STREQ(orientation_copy.getReferenceFrame()->getName().c_str(), "f1");
109 
110  orientation = orientation_copy;
111  orientation_copy = orientation.changeFrameAndCopy(ReferenceFrame::getWorldFrame());
112 
113  expected = Quaternion(0., 0., 0., 1.);
114  EXPECT_TRUE(unit_test_utils::checkVector4dEpsilonClose(orientation_copy, expected, unit_test_utils::TEST_PREC));
115  EXPECT_STREQ(orientation.getReferenceFrame()->getName().c_str(), "f1");
116  EXPECT_STREQ(orientation_copy.getReferenceFrame()->getName().c_str(), "world");
117 
118  orientation = orientation_copy;
119  orientation_copy = orientation.changeFrameAndCopy(f3);
120  expected = Math::toQuaternion(Xrotz(M_PI_2).E);
121  EXPECT_TRUE(unit_test_utils::checkVector4dEpsilonClose(orientation_copy, expected, unit_test_utils::TEST_PREC));
122  EXPECT_STREQ(orientation.getReferenceFrame()->getName().c_str(), "world");
123  EXPECT_STREQ(orientation_copy.getReferenceFrame()->getName().c_str(), "f3");
124 }
125 
126 TEST_F(FrameOrientationTest, model)
127 {
128  RobotDynamics::Body b(1., RobotDynamics::Math::Vector3d(1., 1., 1.), RobotDynamics::Math::Vector3d(1., 1., 1.));
129  RobotDynamics::Joint floating_base(RobotDynamics::JointType::JointTypeFloatingBase);
130 
131  RobotDynamics::Model model;
132  model.gravity = RobotDynamics::Math::MotionVector(0., 0., 0., 0., 0., -9.81);
133 
134  unsigned int floating_body_id = model.appendBody(RobotDynamics::Math::SpatialTransform(), floating_base, b, "b1");
135  unsigned int fb_id =
136  model.addBody(floating_body_id, RobotDynamics::Math::Xrotz(M_PI_2), RobotDynamics::Joint(RobotDynamics::JointType::JointTypeFixed), b, "fixed_body");
137  model.addBody(fb_id, RobotDynamics::Math::Xroty(M_PI_2), RobotDynamics::Joint(RobotDynamics::JointType::JointTypeFixed), b, "fixed_body_2");
138 
139  RobotDynamics::Math::VectorNd q_ = RobotDynamics::Math::VectorNd::Zero(model.q_size);
140  RobotDynamics::Math::VectorNd qdot_ = RobotDynamics::Math::VectorNd::Zero(model.qdot_size);
141  RobotDynamics::Math::VectorNd tau_ = RobotDynamics::Math::VectorNd::Zero(model.qdot_size);
142 
143  RobotDynamics::ReferenceFramePtr b1_frame = model.getReferenceFrame("b1");
144  RobotDynamics::ReferenceFramePtr fixed_body_frame = model.getReferenceFrame("fixed_body");
145  RobotDynamics::ReferenceFramePtr fixed_body2_frame = model.getReferenceFrame("fixed_body_2");
146 
147  model.SetQuaternion(floating_body_id, RobotDynamics::Math::Quaternion(0., 0., 0., 1.), q_);
148  RobotDynamics::updateKinematicsCustom(model, &q_, &qdot_, nullptr);
149 
150  FrameOrientation f(fixed_body_frame, RobotDynamics::Math::toQuaternion(RobotDynamics::Math::Xroty(M_PI_2).E));
151  f.changeFrame(model.worldFrame);
152 
153  RobotDynamics::Math::Quaternion q = RobotDynamics::Math::toQuaternion(model.worldFrame->getTransformToDesiredFrame(fixed_body2_frame).E);
154 
155  EXPECT_NEAR(q.x(), f.x(), unit_test_utils::TEST_PREC);
156  EXPECT_NEAR(q.y(), f.y(), unit_test_utils::TEST_PREC);
157  EXPECT_NEAR(q.z(), f.z(), unit_test_utils::TEST_PREC);
158  EXPECT_NEAR(q.w(), f.w(), unit_test_utils::TEST_PREC);
159 }
160 
161 int main(int argc, char** argv)
162 {
163  ::testing::InitGoogleTest(&argc, argv);
164  ::testing::FLAGS_gtest_death_test_style = "threadsafe";
165  return RUN_ALL_TESTS();
166 }
RobotDynamics::Math::Quaternion::w
EIGEN_STRONG_INLINE double & w()
Definition: Quaternion.h:106
RobotDynamics::FrameObject::getReferenceFrame
ReferenceFramePtr getReferenceFrame() const
Get a pointer to the reference frame this FrameObject is expressed in.
Definition: FrameObject.hpp:52
RobotDynamics::Body
Describes all properties of a single body.
Definition: Body.h:30
RobotDynamics::Model::SetQuaternion
void SetQuaternion(unsigned int i, const Math::Quaternion &quat, Math::VectorNd &Q) const
Sets the quaternion for body i (only valid if body i is connected by a JointTypeSpherical joint) ...
Definition: Model.h:609
RobotDynamics::Math::Xroty
SpatialTransform Xroty(const double &yrot)
Get transform with zero translation and pure rotation about y axis.
Definition: SpatialAlgebraOperators.h:286
unit_test_utils::TEST_PREC
const double TEST_PREC
Definition: UnitTestUtils.hpp:22
RobotDynamics::JointTypeFixed
Fixed joint which causes the inertial properties to be merged with.
Definition: Joint.h:205
RobotDynamics::Math::toQuaternion
static Quaternion toQuaternion(const Vector3d &axis, double angle_rad)
Get quaternion representation of axis and angle.
Definition: SpatialAlgebraOperators.h:63
unit_test_utils::checkVector4dEpsilonClose
static bool checkVector4dEpsilonClose(const RobotDynamics::Math::Vector4d &v1, const RobotDynamics::Math::Vector4d &v2, const double epsilon)
Definition: UnitTestUtils.hpp:275
RobotDynamics::ReferenceFrame::getWorldFrame
static ReferenceFramePtr getWorldFrame()
Get a pointer to the world frame.
Definition: ReferenceFrame.hpp:267
RobotDynamics::Model::gravity
Math::MotionVector gravity
the cartesian vector of the gravity
Definition: Model.h:196
RobotDynamics::Model::appendBody
unsigned int appendBody(const Math::SpatialTransform &joint_frame, const Joint &joint, const Body &body, std::string body_name="")
Adds a Body to the model such that the previously added Body is the Parent.
Definition: Model.cc:517
RobotDynamics::Model::worldFrame
ReferenceFramePtr worldFrame
Definition: Model.h:141
RobotDynamics::Model::addBody
unsigned int addBody(const unsigned int parent_id, const Math::SpatialTransform &joint_frame, const Joint &joint, const Body &body, std::string body_name="")
Connects a given body to the model.
Definition: Model.cc:271
RobotDynamics::Model::getReferenceFrame
ReferenceFramePtr getReferenceFrame(const std::string &frameName) const
Get a fixed or moveable body&#39;s reference frame.
Definition: Model.h:448
RobotDynamics::Math::Xrotz
SpatialTransform Xrotz(const double &zrot)
Get transform with zero translation and pure rotation about z axis.
Definition: SpatialAlgebraOperators.h:300
RobotDynamics::ReferenceFramePtr
std::shared_ptr< ReferenceFrame > ReferenceFramePtr
Definition: ReferenceFrame.hpp:72
RobotDynamics::Model::q_size
unsigned int q_size
The size of the -vector. For models without spherical joints the value is the same as Model::dof_coun...
Definition: Model.h:178
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::Quaternion::y
EIGEN_STRONG_INLINE double & y()
Definition: Quaternion.h:86
RobotDynamics::Joint
Describes a joint relative to the predecessor body.
Definition: Joint.h:228
RobotDynamics::ReferenceFrame
ReferenceFrame object used to tell what frame objects are expressed in. Every ReferenceFrame has a po...
Definition: ReferenceFrame.hpp:88
RobotDynamics::Math::Quaternion
Quaternion that are used for singularity free joints.
Definition: Quaternion.h:25
RobotDynamics::Math::VectorNd
Eigen::VectorXd VectorNd
Definition: rdl_eigenmath.h:25
RobotDynamics::Model
Contains all information about the rigid body model.
Definition: Model.h:121
RobotDynamics::JointTypeFloatingBase
A 6-DoF joint for floating-base (or freeflyer) systems.
Definition: Joint.h:204
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::Quaternion::x
EIGEN_STRONG_INLINE double & x()
Definition: Quaternion.h:76
RobotDynamics::Math
Math types such as vectors and matrices and utility functions.
Definition: ForceVector.hpp:21
RobotDynamics::Math::Quaternion::z
EIGEN_STRONG_INLINE double & z()
Definition: Quaternion.h:96
RobotDynamics::Math::FrameOrientation
A Frame object that represents an orientation(quaternion) relative to a reference frame...
Definition: FrameOrientation.hpp:30
TEST_F
TEST_F(FrameOrientationTest, setters)
Definition: FrameOrientationTests.cpp:27
RobotDynamics::Math::Quaternion::set
void set(double x, double y, double z, double w)
Definition: Quaternion.h:116
RobotDynamics::Math::FrameOrientation::changeFrameAndCopy
FrameOrientation changeFrameAndCopy(ReferenceFramePtr referenceFrame) const
Definition: FrameOrientation.hpp:96
main
int main(int argc, char **argv)
Definition: FrameOrientationTests.cpp:161
RobotDynamics::Math::AxisAngle
Eigen::AngleAxisd AxisAngle
Definition: rdl_eigenmath.h:27
RobotDynamics::Math::toMatrix
static Matrix3d toMatrix(const Quaternion &q)
Convert quaternion to rotation matrix.
Definition: SpatialAlgebraOperators.h:187
RobotDynamics
Namespace for all structures of the RobotDynamics library.
Definition: Body.h:21
RobotDynamics::Model::qdot_size
unsigned int qdot_size
The size of the.
Definition: Model.h:187
RobotDynamics::Math::toAxisAngle
static AxisAngle toAxisAngle(Quaternion q)
Get axis angle representation of a quaternion.
Definition: SpatialAlgebraOperators.h:132

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