RdlImpulsesTests.cpp
Go to the documentation of this file.
1 #include <gtest/gtest.h>
2 #include <iostream>
3 
4 #include "UnitTestUtils.hpp"
5 
6 #include "rdl_dynamics/Model.h"
7 #include "rdl_dynamics/Contacts.h"
8 #include "rdl_dynamics/Dynamics.h"
9 #include "rdl_dynamics/Kinematics.h"
10 
11 using namespace std;
12 using namespace RobotDynamics;
13 using namespace RobotDynamics::Math;
14 
15 const double TEST_PREC = 1.0e-14;
16 
17 struct RdlImpulsesFixture : public testing::Test
18 {
19  RdlImpulsesFixture()
20  {
21  }
22 
23  void SetUp()
24  {
25  model = new Model;
26 
27  model->gravity = SpatialVector(0., 0., 0., 0., -9.81, 0.);
28 
29  // base body
30  base = Body(1., Vector3d(0., 1., 0.), Vector3d(1., 1., 1.));
31  joint_rotzyx = Joint(SpatialVector(0., 0., 1., 0., 0., 0.), SpatialVector(0., 1., 0., 0., 0., 0.), SpatialVector(1., 0., 0., 0., 0., 0.));
32  base_id = model->addBody(0, Xtrans(Vector3d(0., 0., 0.)), joint_rotzyx, base);
33 
34  // child body (3 DoF)
35  child = Body(1., Vector3d(0., 1., 0.), Vector3d(1., 1., 1.));
36  child_id = model->addBody(base_id, Xtrans(Vector3d(1., 0., 0.)), joint_rotzyx, child);
37 
38  Q = VectorNd::Zero(model->dof_count);
39  QDot = VectorNd::Zero(model->dof_count);
40  QDDot = VectorNd::Zero(model->dof_count);
41  Tau = VectorNd::Zero(model->dof_count);
42 
43  contact_body_id = child_id;
44  contact_point = Vector3d(0., 1., 0.);
45  contact_normal = Vector3d(0., 1., 0.);
46  }
47 
48  void TearDown()
49  {
50  delete model;
51  }
52 
53  Model* model;
54 
55  unsigned int base_id, child_id;
56  Body base, child;
57  Joint joint_rotzyx;
58 
59  VectorNd Q;
60  VectorNd QDot;
61  VectorNd QDDot;
62  VectorNd Tau;
63 
64  unsigned int contact_body_id;
65  Vector3d contact_point;
66  Vector3d contact_normal;
67  ConstraintSet constraint_set;
68 };
69 
70 TEST_F(RdlImpulsesFixture, TestContactImpulse)
71 {
72  constraint_set.addConstraint(contact_body_id, contact_point, Vector3d(1., 0., 0.), NULL, 0.);
73  constraint_set.addConstraint(contact_body_id, contact_point, Vector3d(0., 1., 0.), NULL, 0.);
74  constraint_set.addConstraint(contact_body_id, contact_point, Vector3d(0., 0., 1.), NULL, 0.);
75 
76  constraint_set.bind(*model);
77 
78  constraint_set.v_plus[0] = 0.;
79  constraint_set.v_plus[1] = 0.;
80  constraint_set.v_plus[2] = 0.;
81 
82  QDot[0] = 0.1;
83  QDot[1] = -0.2;
84  QDot[2] = 0.1;
85 
86  Vector3d point_velocity;
87  point_velocity = calcPointVelocity(*model, Q, QDot, contact_body_id, contact_point, true);
88 
89  VectorNd qdot_post(QDot.size());
90  computeContactImpulsesDirect(*model, Q, QDot, constraint_set, qdot_post);
91 
92  point_velocity = calcPointVelocity(*model, Q, qdot_post, contact_body_id, contact_point, true);
93 
94  // cout << "Point Velocity = " << point_velocity << endl;
95  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(Vector3d(0., 0., 0.), point_velocity, unit_test_utils::TEST_PREC));
96 }
97 
98 TEST_F(RdlImpulsesFixture, TestContactImpulseRotated)
99 {
100  constraint_set.addConstraint(contact_body_id, contact_point, Vector3d(1., 0., 0.), NULL, 0.);
101  constraint_set.addConstraint(contact_body_id, contact_point, Vector3d(0., 1., 0.), NULL, 0.);
102  constraint_set.addConstraint(contact_body_id, contact_point, Vector3d(0., 0., 1.), NULL, 0.);
103 
104  constraint_set.bind(*model);
105 
106  constraint_set.v_plus[0] = 0.;
107  constraint_set.v_plus[1] = 0.;
108  constraint_set.v_plus[2] = 0.;
109 
110  Q[0] = 0.2;
111  Q[1] = -0.5;
112  Q[2] = 0.1;
113  Q[3] = -0.4;
114  Q[4] = -0.1;
115  Q[5] = 0.4;
116 
117  QDot[0] = 0.1;
118  QDot[1] = -0.2;
119  QDot[2] = 0.1;
120 
121  Vector3d point_velocity;
122  point_velocity = calcPointVelocity(*model, Q, QDot, contact_body_id, contact_point, true);
123 
124  VectorNd qdot_post(QDot.size());
125  computeContactImpulsesDirect(*model, Q, QDot, constraint_set, qdot_post);
126 
127  point_velocity = calcPointVelocity(*model, Q, qdot_post, contact_body_id, contact_point, true);
128 
129  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(Vector3d(0., 0., 0.), point_velocity, TEST_PREC));
130 }
131 
132 TEST_F(RdlImpulsesFixture, TestContactImpulseRotatedCollisionVelocity)
133 {
134  constraint_set.addConstraint(contact_body_id, contact_point, Vector3d(1., 0., 0.), NULL, 1.);
135  constraint_set.addConstraint(contact_body_id, contact_point, Vector3d(0., 1., 0.), NULL, 2.);
136  constraint_set.addConstraint(contact_body_id, contact_point, Vector3d(0., 0., 1.), NULL, 3.);
137 
138  constraint_set.bind(*model);
139 
140  constraint_set.v_plus[0] = 1.;
141  constraint_set.v_plus[1] = 2.;
142  constraint_set.v_plus[2] = 3.;
143 
144  Q[0] = 0.2;
145  Q[1] = -0.5;
146  Q[2] = 0.1;
147  Q[3] = -0.4;
148  Q[4] = -0.1;
149  Q[5] = 0.4;
150 
151  QDot[0] = 0.1;
152  QDot[1] = -0.2;
153  QDot[2] = 0.1;
154 
155  Vector3d point_velocity;
156  point_velocity = calcPointVelocity(*model, Q, QDot, contact_body_id, contact_point, true);
157 
158  VectorNd qdot_post(QDot.size());
159  computeContactImpulsesDirect(*model, Q, QDot, constraint_set, qdot_post);
160 
161  point_velocity = calcPointVelocity(*model, Q, qdot_post, contact_body_id, contact_point, true);
162 
163  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(Vector3d(1., 2., 3.), point_velocity, TEST_PREC));
164 }
165 
166 TEST_F(RdlImpulsesFixture, TestContactImpulseRangeSpaceSparse)
167 {
168  Q[0] = 0.2;
169  Q[1] = -0.5;
170  Q[2] = 0.1;
171  Q[3] = -0.4;
172  Q[4] = -0.1;
173  Q[5] = 0.4;
174 
175  QDot[0] = 0.1;
176  QDot[1] = -0.2;
177  QDot[2] = 0.1;
178 
179  constraint_set.addConstraint(contact_body_id, contact_point, Vector3d(1., 0., 0.), NULL, 1.);
180  constraint_set.addConstraint(contact_body_id, contact_point, Vector3d(0., 1., 0.), NULL, 2.);
181  constraint_set.addConstraint(contact_body_id, contact_point, Vector3d(0., 0., 1.), NULL, 3.);
182 
183  constraint_set.bind(*model);
184 
185  constraint_set.v_plus[0] = 1.;
186  constraint_set.v_plus[1] = 2.;
187  constraint_set.v_plus[2] = 3.;
188 
189  ConstraintSet constraint_set_rangespace;
190  constraint_set_rangespace = constraint_set.Copy();
191  constraint_set_rangespace.bind(*model);
192 
193  VectorNd qdot_post_direct(QDot.size());
194  computeContactImpulsesDirect(*model, Q, QDot, constraint_set, qdot_post_direct);
195 
196  VectorNd qdot_post_rangespace(QDot.size());
197  computeContactImpulsesRangeSpaceSparse(*model, Q, QDot, constraint_set_rangespace, qdot_post_rangespace);
198 
199  Vector3d point_velocity_rangespace = calcPointVelocity(*model, Q, qdot_post_rangespace, contact_body_id, contact_point, true);
200  Vector3d point_velocity_direct = calcPointVelocity(*model, Q, qdot_post_direct, contact_body_id, contact_point, true);
201 
202  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(qdot_post_direct, qdot_post_rangespace, TEST_PREC));
203  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(Vector3d(1., 2., 3.), point_velocity_rangespace, TEST_PREC));
204  // check this
205  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(point_velocity_direct, point_velocity_rangespace, TEST_PREC));
206 }
207 
208 TEST_F(RdlImpulsesFixture, TestContactImpulseNullSpace)
209 {
210  Q[0] = 0.2;
211  Q[1] = -0.5;
212  Q[2] = 0.1;
213  Q[3] = -0.4;
214  Q[4] = -0.1;
215  Q[5] = 0.4;
216 
217  QDot[0] = 0.1;
218  QDot[1] = -0.2;
219  QDot[2] = 0.1;
220 
221  constraint_set.addConstraint(contact_body_id, contact_point, Vector3d(1., 0., 0.), NULL, 1.);
222  constraint_set.addConstraint(contact_body_id, contact_point, Vector3d(0., 1., 0.), NULL, 2.);
223  constraint_set.addConstraint(contact_body_id, contact_point, Vector3d(0., 0., 1.), NULL, 3.);
224 
225  constraint_set.bind(*model);
226 
227  constraint_set.v_plus[0] = 1.;
228  constraint_set.v_plus[1] = 2.;
229  constraint_set.v_plus[2] = 3.;
230 
231  ConstraintSet constraint_set_nullspace;
232  constraint_set_nullspace = constraint_set.Copy();
233  constraint_set_nullspace.bind(*model);
234 
235  VectorNd qdot_post_direct(QDot.size());
236  computeContactImpulsesDirect(*model, Q, QDot, constraint_set, qdot_post_direct);
237 
238  VectorNd qdot_post_nullspace(QDot.size());
239  computeContactImpulsesNullSpace(*model, Q, QDot, constraint_set, qdot_post_nullspace);
240 
241  Vector3d point_velocity_direct = calcPointVelocity(*model, Q, qdot_post_direct, contact_body_id, contact_point, true);
242  Vector3d point_velocity_nullspace = calcPointVelocity(*model, Q, qdot_post_nullspace, contact_body_id, contact_point, true);
243 
244  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(qdot_post_direct, qdot_post_nullspace, TEST_PREC));
245  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(Vector3d(1., 2., 3.), point_velocity_nullspace, TEST_PREC));
246  // check this
247  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(point_velocity_direct, point_velocity_nullspace, TEST_PREC));
248 }
249 
250 int main(int argc, char** argv)
251 {
252  ::testing::InitGoogleTest(&argc, argv);
253  return RUN_ALL_TESTS();
254 }
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::computeContactImpulsesDirect
void computeContactImpulsesDirect(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDotMinus, ConstraintSet &CS, Math::VectorNd &QDotPlus)
Computes contact gain by constructing and solving the full lagrangian equation.
Definition: Contacts.cc:383
TEST_F
TEST_F(RdlImpulsesFixture, TestContactImpulse)
Definition: RdlImpulsesTests.cpp:70
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
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
unit_test_utils::TEST_PREC
const double TEST_PREC
Definition: UnitTestUtils.hpp:22
TEST_PREC
const double TEST_PREC
Definition: RdlImpulsesTests.cpp:15
Q
VectorNd Q
Definition: RdlTwoLegModelTests.cpp:29
RobotDynamics::Math::Xtrans
SpatialTransform Xtrans(const Vector3d &r)
Get pure translation transform.
Definition: SpatialAlgebraOperators.h:369
RobotDynamics::Model::gravity
Math::MotionVector gravity
the cartesian vector of the gravity
Definition: Model.h:196
RobotDynamics::computeContactImpulsesRangeSpaceSparse
void computeContactImpulsesRangeSpaceSparse(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDotMinus, ConstraintSet &CS, Math::VectorNd &QDotPlus)
Resolves contact gain using solveContactSystemRangeSpaceSparse()
Definition: Contacts.cc:407
RobotDynamics::calcPointVelocity
Math::FrameVector calcPointVelocity(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, unsigned int body_id, const Math::Vector3d &point_position, bool update_kinematics=true)
Computes the velocity of a point on a body.
Definition: Kinematics.cc:1422
RdlImpulsesFixture::contact_body_id
unsigned int contact_body_id
Definition: RdlImpulsesTests.cpp:64
RobotDynamics::ConstraintSet
Structure that contains both constraint information and workspace memory.
Definition: Contacts.h:168
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
RdlImpulsesFixture::constraint_set
ConstraintSet constraint_set
Definition: RdlImpulsesTests.cpp:67
main
int main(int argc, char **argv)
Definition: RdlImpulsesTests.cpp:250
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::computeContactImpulsesNullSpace
void computeContactImpulsesNullSpace(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDotMinus, ConstraintSet &CS, Math::VectorNd &QDotPlus)
Resolves contact gain using solveContactSystemNullSpace()
Definition: Contacts.cc:419
RobotDynamics::Math
Math types such as vectors and matrices and utility functions.
Definition: ForceVector.hpp:21
unit_test_utils::checkVector3dEpsilonClose
static bool checkVector3dEpsilonClose(const RobotDynamics::Math::Vector3d &v1, const RobotDynamics::Math::Vector3d &v2, const double epsilon)
Definition: UnitTestUtils.hpp:256
RobotDynamics
Namespace for all structures of the RobotDynamics library.
Definition: Body.h:21

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