RdlSparseFactorizationTests.cpp
Go to the documentation of this file.
1 #include <gtest/gtest.h>
2 
3 #include "UnitTestUtils.hpp"
4 
5 #include "Fixtures.h"
6 #include "rdl_dynamics/Dynamics.h"
7 
8 using namespace std;
9 using namespace RobotDynamics;
10 using namespace RobotDynamics::Math;
11 
12 const double TEST_PREC = 1.0e-12;
13 
14 TEST_F(FloatingBase12DoF, TestSparseFactorizationLTL)
15 {
16  for (unsigned int i = 0; i < model->q_size; i++)
17  {
18  Q[i] = static_cast<double>(i + 1) * 0.1;
19  }
20 
21  MatrixNd H(MatrixNd::Zero(model->qdot_size, model->qdot_size));
22 
23  compositeRigidBodyAlgorithm(*model, Q, H);
24 
25  MatrixNd L(H);
26  SparseFactorizeLTL(*model, L);
27  MatrixNd LTL = L.transpose() * L;
28 
29  EXPECT_TRUE(unit_test_utils::checkMatrixNdEpsilonClose(H, LTL, TEST_PREC));
30 }
31 
32 TEST_F(FloatingBase12DoF, TestSparseSolveLx)
33 {
34  for (unsigned int i = 0; i < model->q_size; i++)
35  {
36  Q[i] = static_cast<double>(i + 1) * 0.1;
37  }
38 
39  MatrixNd H(MatrixNd::Zero(model->qdot_size, model->qdot_size));
40 
41  compositeRigidBodyAlgorithm(*model, Q, H);
42 
43  MatrixNd L(H);
44  SparseFactorizeLTL(*model, L);
45  VectorNd x = L * Q;
46 
47  SparseSolveLx(*model, L, x);
48 
49  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(Q, x, TEST_PREC));
50 }
51 
52 TEST_F(FloatingBase12DoF, TestSparseSolveLTx)
53 {
54  for (unsigned int i = 0; i < model->q_size; i++)
55  {
56  Q[i] = static_cast<double>(i + 1) * 0.1;
57  }
58 
59  MatrixNd H(MatrixNd::Zero(model->qdot_size, model->qdot_size));
60 
61  compositeRigidBodyAlgorithm(*model, Q, H);
62 
63  MatrixNd L(H);
64  SparseFactorizeLTL(*model, L);
65  VectorNd x = L.transpose() * Q;
66 
67  SparseSolveLTx(*model, L, x);
68 
69  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(Q, x, TEST_PREC));
70 }
71 
72 TEST_F(FixedBase6DoF12DoFFloatingBase, ForwardDynamicsContactsSparse)
73 {
74  ConstraintSet constraint_set_var1;
75 
76  constraint_set.addConstraint(contact_body_id, contact_point, Vector3d(1., 0., 0.));
77  constraint_set.addConstraint(contact_body_id, contact_point, Vector3d(0., 1., 0.));
78  constraint_set.addConstraint(child_2_id, contact_point, Vector3d(0., 1., 0.));
79 
80  constraint_set_var1 = constraint_set.Copy();
81  constraint_set_var1.bind(*model);
82  constraint_set.bind(*model);
83 
84  VectorNd QDDot_var1 = VectorNd::Constant(model->dof_count, 0.);
85 
86  Q[0] = 0.1;
87  Q[1] = -0.3;
88  Q[2] = 0.15;
89  Q[3] = -0.21;
90  Q[4] = -0.81;
91  Q[5] = 0.11;
92  Q[6] = 0.31;
93  Q[7] = -0.91;
94  Q[8] = 0.61;
95 
96  QDot[0] = 1.3;
97  QDot[1] = -1.7;
98  QDot[2] = 3;
99  QDot[3] = -2.5;
100  QDot[4] = 1.5;
101  QDot[5] = -5.5;
102  QDot[6] = 2.5;
103  QDot[7] = -1.5;
104  QDot[8] = -3.5;
105 
106  forwardDynamicsContactsKokkevis(*model, Q, QDot, Tau, constraint_set, QDDot);
107 
108  forwardDynamicsContactsRangeSpaceSparse(*model, Q, QDot, Tau, constraint_set_var1, QDDot_var1);
109 
110  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(QDDot, QDDot_var1, TEST_PREC));
111 }
112 
113 TEST_F(FloatingBase12DoF, TestSparseFactorizationMultiDof)
114 {
115  Model model_emulated;
116  Model model_3dof;
117 
118  Body body(1., Vector3d(1., 2., 1.), Matrix3d(1., 0., 0, 0., 1., 0., 0., 0., 1.));
119  Joint joint_emulated(SpatialVector(0., 1., 0., 0., 0., 0.), SpatialVector(1., 0., 0., 0., 0., 0.), SpatialVector(0., 0., 1., 0., 0., 0.));
120  Joint joint_3dof(JointTypeEulerYXZ);
121 
122  Joint joint_rot_y(SpatialVector(0., 1., 0., 0., 0., 0.));
123 
124  model_emulated.appendBody(SpatialTransform(Matrix3d::Identity(), Vector3d::Zero()), joint_rot_y, body);
125  unsigned int multdof_body_id_emulated = model_emulated.appendBody(SpatialTransform(Matrix3d::Identity(), Vector3d::Zero()), joint_emulated, body);
126  model_emulated.appendBody(SpatialTransform(Matrix3d::Identity(), Vector3d::Zero()), joint_emulated, body);
127  model_emulated.addBody(multdof_body_id_emulated, SpatialTransform(Matrix3d::Identity(), Vector3d::Zero()), joint_rot_y, body);
128  model_emulated.appendBody(SpatialTransform(Matrix3d::Identity(), Vector3d::Zero()), joint_emulated, body);
129 
130  model_3dof.appendBody(SpatialTransform(Matrix3d::Identity(), Vector3d::Zero()), joint_rot_y, body);
131  unsigned int multdof_body_id_3dof = model_3dof.appendBody(SpatialTransform(Matrix3d::Identity(), Vector3d::Zero()), joint_3dof, body);
132  model_3dof.appendBody(SpatialTransform(Matrix3d::Identity(), Vector3d::Zero()), joint_3dof, body);
133  model_3dof.addBody(multdof_body_id_3dof, SpatialTransform(Matrix3d::Identity(), Vector3d::Zero()), joint_rot_y, body);
134  model_3dof.appendBody(SpatialTransform(Matrix3d::Identity(), Vector3d::Zero()), joint_3dof, body);
135 
136  VectorNd q(VectorNd::Zero(model_emulated.q_size));
137  VectorNd qdot(VectorNd::Zero(model_emulated.qdot_size));
138  VectorNd qddot_emulated(VectorNd::Zero(model_emulated.qdot_size));
139  VectorNd qddot_3dof(VectorNd::Zero(model_emulated.qdot_size));
140  VectorNd tau(VectorNd::Zero(model_emulated.qdot_size));
141 
142  for (int i = 0; i < q.size(); i++)
143  {
144  q[i] = 1.1 * (static_cast<double>(i + 1));
145  qdot[i] = 0.55 * (static_cast<double>(i + 1));
146  qddot_emulated[i] = 0.23 * (static_cast<double>(i + 1));
147  qddot_3dof[i] = 0.22 * (static_cast<double>(i + 1));
148  tau[i] = 2.1 * (static_cast<double>(i + 1));
149  }
150 
151  MatrixNd H_emulated(MatrixNd::Zero(q.size(), q.size()));
152  MatrixNd H_3dof(MatrixNd::Zero(q.size(), q.size()));
153 
154  compositeRigidBodyAlgorithm(model_emulated, q, H_emulated);
155  compositeRigidBodyAlgorithm(model_3dof, q, H_3dof);
156 
157  VectorNd b(VectorNd::Zero(q.size()));
158  VectorNd x_emulated(VectorNd::Zero(q.size()));
159  VectorNd x_3dof(VectorNd::Zero(q.size()));
160 
161  for (unsigned int i = 0; i < b.size(); i++)
162  {
163  b[i] = static_cast<double>(i + 1) * 2.152;
164  }
165  b = H_emulated * b;
166 
167  SparseFactorizeLTL(model_emulated, H_emulated);
168  SparseFactorizeLTL(model_3dof, H_3dof);
169 
170  EXPECT_TRUE(unit_test_utils::checkMatrixNdEpsilonClose(H_emulated, H_3dof, TEST_PREC));
171 
172  x_emulated = b;
173  SparseSolveLx(model_emulated, H_emulated, x_emulated);
174  x_3dof = b;
175  SparseSolveLx(model_3dof, H_3dof, x_3dof);
176 
177  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(x_emulated, x_3dof, 1.0e-9));
178 
179  x_emulated = b;
180  SparseSolveLTx(model_emulated, H_emulated, x_emulated);
181  x_3dof = b;
182  SparseSolveLTx(model_3dof, H_3dof, x_3dof);
183 
184  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(x_emulated, x_3dof, 1.0e-9));
185 }
186 
187 TEST_F(FloatingBase12DoF, TestSparseFactorizationMultiDofAndFixed)
188 {
189  Model model_emulated;
190  Model model_3dof;
191 
192  Body body(1., Vector3d(1., 2., 1.), Matrix3d(1., 0., 0, 0., 1., 0., 0., 0., 1.));
193  Joint joint_emulated(SpatialVector(0., 1., 0., 0., 0., 0.), SpatialVector(1., 0., 0., 0., 0., 0.), SpatialVector(0., 0., 1., 0., 0., 0.));
194  Joint joint_3dof(JointTypeEulerYXZ);
195 
196  Joint joint_rot_y(SpatialVector(0., 1., 0., 0., 0., 0.));
197 
198  SpatialTransform translate_x(Matrix3d::Identity(), Vector3d(1., 0., 0.));
199 
200  model_emulated.appendBody(SpatialTransform(Matrix3d::Identity(), Vector3d::Zero()), joint_rot_y, body);
201  unsigned int multdof_body_id_emulated = model_emulated.appendBody(translate_x, joint_emulated, body);
202  model_emulated.appendBody(translate_x, joint_emulated, body);
203  model_emulated.addBody(multdof_body_id_emulated, translate_x, Joint(JointTypeFixed), body);
204  model_emulated.appendBody(translate_x, joint_emulated, body);
205 
206  model_3dof.appendBody(SpatialTransform(Matrix3d::Identity(), Vector3d::Zero()), joint_rot_y, body);
207  unsigned int multdof_body_id_3dof = model_3dof.appendBody(translate_x, joint_3dof, body);
208  model_3dof.appendBody(translate_x, joint_3dof, body);
209  model_3dof.addBody(multdof_body_id_3dof, translate_x, Joint(JointTypeFixed), body);
210  model_3dof.appendBody(translate_x, joint_3dof, body);
211 
212  VectorNd q(VectorNd::Zero(model_emulated.q_size));
213  VectorNd qdot(VectorNd::Zero(model_emulated.qdot_size));
214  VectorNd qddot_emulated(VectorNd::Zero(model_emulated.qdot_size));
215  VectorNd qddot_3dof(VectorNd::Zero(model_emulated.qdot_size));
216  VectorNd tau(VectorNd::Zero(model_emulated.qdot_size));
217 
218  for (int i = 0; i < q.size(); i++)
219  {
220  q[i] = 1.1 * (static_cast<double>(i + 1));
221  qdot[i] = 0.55 * (static_cast<double>(i + 1));
222  qddot_emulated[i] = 0.23 * (static_cast<double>(i + 1));
223  qddot_3dof[i] = 0.22 * (static_cast<double>(i + 1));
224  tau[i] = 2.1 * (static_cast<double>(i + 1));
225  }
226 
227  MatrixNd H_emulated(MatrixNd::Zero(q.size(), q.size()));
228  MatrixNd H_3dof(MatrixNd::Zero(q.size(), q.size()));
229 
230  compositeRigidBodyAlgorithm(model_emulated, q, H_emulated);
231  compositeRigidBodyAlgorithm(model_3dof, q, H_3dof);
232 
233  VectorNd b(VectorNd::Zero(q.size()));
234  VectorNd x_emulated(VectorNd::Zero(q.size()));
235  VectorNd x_3dof(VectorNd::Zero(q.size()));
236 
237  for (unsigned int i = 0; i < b.size(); i++)
238  {
239  b[i] = static_cast<double>(i + 1) * 2.152;
240  }
241  b = H_emulated * b;
242 
243  SparseFactorizeLTL(model_emulated, H_emulated);
244  SparseFactorizeLTL(model_3dof, H_3dof);
245 
246  EXPECT_TRUE(unit_test_utils::checkMatrixNdEpsilonClose(H_emulated, H_3dof, TEST_PREC));
247 
248  x_emulated = b;
249  SparseSolveLx(model_emulated, H_emulated, x_emulated);
250  x_3dof = b;
251  SparseSolveLx(model_3dof, H_3dof, x_3dof);
252 
253  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(x_emulated, x_3dof, 1.0e-9));
254 
255  x_emulated = b;
256  SparseSolveLTx(model_emulated, H_emulated, x_emulated);
257  x_3dof = b;
258  SparseSolveLTx(model_3dof, H_3dof, x_3dof);
259 
260  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(x_emulated, x_3dof, 1.0e-9));
261 }
262 
263 int main(int argc, char** argv)
264 {
265  ::testing::InitGoogleTest(&argc, argv);
266  return RUN_ALL_TESTS();
267 }
unit_test_utils::checkVectorNdEpsilonClose
static bool checkVectorNdEpsilonClose(const RobotDynamics::Math::VectorNd &v1, const RobotDynamics::Math::VectorNd &v2, const double epsilon)
Definition: UnitTestUtils.hpp:288
RobotDynamics::compositeRigidBodyAlgorithm
void compositeRigidBodyAlgorithm(Model &model, const Math::VectorNd &Q, Math::MatrixNd &H, bool update_kinematics=true)
Computes the joint space inertia matrix by using the Composite Rigid Body Algorithm.
Definition: Dynamics.cc:302
RobotDynamics::Body
Describes all properties of a single body.
Definition: Body.h:30
Tau
VectorNd Tau
Definition: RdlTwoLegModelTests.cpp:32
QDDot
VectorNd QDDot
Definition: RdlTwoLegModelTests.cpp:31
main
int main(int argc, char **argv)
Definition: RdlSparseFactorizationTests.cpp:263
RobotDynamics::ConstraintSet::Copy
ConstraintSet Copy()
Copies the constraints and resets its ConstraintSet::bound flag.
Definition: Contacts.h:191
QDot
VectorNd QDot
Definition: RdlTwoLegModelTests.cpp:30
Q
VectorNd Q
Definition: RdlTwoLegModelTests.cpp:29
RobotDynamics::JointTypeFixed
Fixed joint which causes the inertial properties to be merged with.
Definition: Joint.h:205
RobotDynamics::ConstraintSet::addConstraint
unsigned int addConstraint(unsigned int body_id, const Math::Vector3d &body_point, const Math::Vector3d &world_normal, const char *constraint_name=NULL, double normal_acceleration=0.)
Adds a constraint to the constraint set.
Definition: Contacts.cc:27
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::forwardDynamicsContactsRangeSpaceSparse
void forwardDynamicsContactsRangeSpaceSparse(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &Tau, ConstraintSet &CS, Math::VectorNd &QDDot)
Definition: Contacts.cc:362
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
TEST_PREC
const double TEST_PREC
Definition: RdlSparseFactorizationTests.cpp:12
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::ConstraintSet
Structure that contains both constraint information and workspace memory.
Definition: Contacts.h:168
TEST_F
TEST_F(FloatingBase12DoF, TestSparseFactorizationLTL)
Definition: RdlSparseFactorizationTests.cpp:14
RobotDynamics::Joint
Describes a joint relative to the predecessor body.
Definition: Joint.h:228
RobotDynamics::Math::VectorNd
Eigen::VectorXd VectorNd
Definition: rdl_eigenmath.h:25
unit_test_utils::checkMatrixNdEpsilonClose
static bool checkMatrixNdEpsilonClose(const RobotDynamics::Math::MatrixNd &t1, const RobotDynamics::Math::MatrixNd &t2, const double epsilon)
Definition: UnitTestUtils.hpp:154
RobotDynamics::Model
Contains all information about the rigid body model.
Definition: Model.h:121
RobotDynamics::ConstraintSet::bind
bool bind(const Model &model)
Initializes and allocates memory for the constraint set.
Definition: Contacts.cc:62
RobotDynamics::Math
Math types such as vectors and matrices and utility functions.
Definition: ForceVector.hpp:21
RobotDynamics::Math::SparseFactorizeLTL
void SparseFactorizeLTL(Model &model, Math::MatrixNd &H)
Definition: rdl_mathutils.cc:170
RobotDynamics::Math::MatrixNd
Eigen::MatrixXd MatrixNd
Definition: rdl_eigenmath.h:26
RobotDynamics::Math::SparseSolveLTx
void SparseSolveLTx(Model &model, Math::MatrixNd &L, Math::VectorNd &x)
Definition: rdl_mathutils.cc:233
RobotDynamics::Math::SparseSolveLx
void SparseSolveLx(Model &model, Math::MatrixNd &L, Math::VectorNd &x)
Definition: rdl_mathutils.cc:219
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::forwardDynamicsContactsKokkevis
void forwardDynamicsContactsKokkevis(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &Tau, ConstraintSet &CS, Math::VectorNd &QDDot)
Computes forward dynamics that accounts for active contacts in ConstraintSet.
Definition: Contacts.cc:682
RobotDynamics::JointTypeEulerYXZ
3 DoF joint that uses Euler YXZ convention (faster than emulated
Definition: Joint.h:201

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