-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
kuka_iiwa_robot_inverse_dynamics_test.cc
144 lines (126 loc) · 6.33 KB
/
kuka_iiwa_robot_inverse_dynamics_test.cc
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
#include <gtest/gtest.h>
#include "drake/multibody/benchmarks/kuka_iiwa_robot/MG/MG_kuka_iiwa_robot.h"
#include "drake/multibody/benchmarks/kuka_iiwa_robot/drake_kuka_iiwa_robot.h"
namespace drake {
namespace multibody {
namespace benchmarks {
namespace kuka_iiwa_robot {
namespace {
using Eigen::Vector3d;
using Eigen::Matrix3d;
using test_utilities::SpatialKinematicsPVA;
using SpatialForced = SpatialForce<double>;
using Vector7d = Eigen::Matrix<double, 7, 1>;
// Compare Drake's MultibodyTree forces and motion with MotionGenesis solution.
void TestKukaArmInverseDynamics(const Eigen::Ref<const VectorX<double>>& q,
const Eigen::Ref<const VectorX<double>>& qDt,
const Eigen::Ref<const VectorX<double>>& qDDt,
const double gravity) {
// Kinematics: Get Drake's end-effector information, including:
// R_NG | Rotation matrix relating Nx, Ny, Nz to Gx, Gy, Gz.
// p_NoGo_N | Go's position from No, expressed in N.
// w_NG_N | G's angular velocity in N, expressed in N.
// v_NGo_N | Go's velocity in N, expressed in N.
// alpha_NG_N | G's angular acceleration in N, expressed in N.
// a_NGo_N | Go's acceleration in N, expressed in N.
DrakeKukaIIwaRobot<double> drake_kuka_robot(gravity);
const SpatialKinematicsPVA<double> drake_kinematics =
drake_kuka_robot.CalcEndEffectorKinematics(q, qDt, qDDt);
// Kinematics: Get corresponding MotionGenesis information.
MG::MGKukaIIwaRobot<double> MG_kuka_robot(gravity);
const SpatialKinematicsPVA<double> MG_kinematics =
MG_kuka_robot.CalcEndEffectorKinematics(q, qDt, qDDt);
// Kinematics: Compare Drake results with MotionGenesis (expected) results.
constexpr double kEpsilon = 10 * std::numeric_limits<double>::epsilon();
EXPECT_TRUE(drake_kinematics.rotation_matrix().matrix().isApprox(
MG_kinematics.rotation_matrix().matrix(), kEpsilon));
EXPECT_TRUE(drake_kinematics.position_vector().isApprox(
MG_kinematics.position_vector(), kEpsilon));
EXPECT_TRUE(drake_kinematics.angular_velocity().isApprox(
MG_kinematics.angular_velocity(), kEpsilon));
EXPECT_TRUE(drake_kinematics.translational_velocity().isApprox(
MG_kinematics.translational_velocity(), kEpsilon));
EXPECT_TRUE(drake_kinematics.angular_acceleration().isApprox(
MG_kinematics.angular_acceleration(), kEpsilon));
EXPECT_TRUE(drake_kinematics.translational_acceleration().isApprox(
MG_kinematics.translational_acceleration(), kEpsilon));
// Inverse dynamics: Get Drake's joint forces/torques, including:
// --------|-------------------------------------------------
// F_Ao_W | Spatial force on Ao from N, expressed in frame W (world).
// F_Bo_W | Spatial force on Bo from A, expressed in frame W (world).
// F_Co_W | Spatial force on Co from B, expressed in frame W (world).
// F_Do_W | Spatial force on Do from C, expressed in frame W (world).
// F_Eo_W | Spatial force on Eo from D, expressed in frame W (world).
// F_Fo_W | Spatial force on Fo from E, expressed in frame W (world).
// F_Go_W | Spatial force on Go from F, expressed in frame W (world).
// In Drake, this is done by creating a vector of the aforementioned 7 spatial
// forces and then calls a method to fill this vector of spatial forces.
// Consider a generic body B whose inboard frame (welded to B) is M with
// origin Mo. The spatial force on B at Mo expressed in W (world) if F_BMo_W.
const int number_of_links = drake_kuka_robot.get_number_of_rigid_bodies();
std::vector<SpatialForce<double>> F_BMo_W_array(number_of_links);
const KukaRobotJointReactionForces<double> forces =
drake_kuka_robot.CalcJointReactionForces(q, qDt, qDDt);
// Inverse dynamics: Get corresponding MotionGenesis information.
SpatialForced F_Ao_W, F_Bo_W, F_Co_W, F_Do_W, F_Eo_W, F_Fo_W, F_Go_W;
std::tie(F_Ao_W, F_Bo_W, F_Co_W, F_Do_W, F_Eo_W, F_Fo_W, F_Go_W) =
MG_kuka_robot.CalcJointReactionForcesExpressedInWorld(q, qDt, qDDt);
// Inverse dynamics: Compare Drake results with expected results.
const double tolerance = 40 * kEpsilon;
EXPECT_TRUE(F_Ao_W.IsApprox(forces.F_Ao_W, tolerance));
EXPECT_TRUE(F_Bo_W.IsApprox(forces.F_Bo_W, tolerance));
EXPECT_TRUE(F_Co_W.IsApprox(forces.F_Co_W, tolerance));
EXPECT_TRUE(F_Do_W.IsApprox(forces.F_Do_W, tolerance));
EXPECT_TRUE(F_Eo_W.IsApprox(forces.F_Eo_W, tolerance));
EXPECT_TRUE(F_Fo_W.IsApprox(forces.F_Fo_W, tolerance));
}
// Verify Drake's computed results for joint reaction forces/torques.
GTEST_TEST(KukaIIwaRobotKinematics, InverseDynamicsTestA) {
// Provide values for joint angles q and their 1st and 2nd time-derivatives.
// The values below correspond to a static configuration.
double q30 = M_PI / 6, q60 = M_PI / 3;
double qA = q60, qAdot = 0.0, qAddot = 0.0;
double qB = q30, qBdot = 0.0, qBddot = 0.0;
double qC = q60, qCdot = 0.0, qCddot = 0.0;
double qD = q30, qDdot = 0.0, qDddot = 0.0;
double qE = q60, qEdot = 0.0, qEddot = 0.0;
double qF = q30, qFdot = 0.0, qFddot = 0.0;
double qG = q60, qGdot = 0.0, qGddot = 0.0;
// Create a state with joint angles (q) and their time-derivatives (qDt).
Vector7d q, qDt, qDDt;
q << qA, qB, qC, qD, qE, qF, qG;
qDt << qAdot, qBdot, qCdot, qDdot, qEdot, qFdot, qGdot;
qDDt << qAddot, qBddot, qCddot, qDddot, qEddot, qFddot, qGddot;
// Test 1: Static configuration test (with and without gravity).
const double gravityA = 0;
const double gravityB = 9.8;
TestKukaArmInverseDynamics(q, qDt, qDDt, gravityA);
TestKukaArmInverseDynamics(q, qDt, qDDt, gravityB);
// Test 2: Includes non-zero velocity, centripetal acceleration, ...
qAdot = 0.1;
qBdot = 0.2;
qCdot = 0.3;
qDdot = 0.4;
qEdot = 0.5;
qFdot = 0.6;
qGdot = 0.7;
qDt << qAdot, qBdot, qCdot, qDdot, qEdot, qFdot, qGdot;
TestKukaArmInverseDynamics(q, qDt, qDDt, gravityA);
TestKukaArmInverseDynamics(q, qDt, qDDt, gravityB);
// Test 3: Includes non-zero velocity and more general non-zero acceleration.
qAddot = 0.7;
qBddot = 0.6;
qCddot = 0.5;
qDddot = 0.4;
qEddot = 0.3;
qFddot = 0.2;
qGddot = 0.1;
qDDt << qAddot, qBddot, qCddot, qDddot, qEddot, qFddot, qGddot;
TestKukaArmInverseDynamics(q, qDt, qDDt, gravityA);
TestKukaArmInverseDynamics(q, qDt, qDDt, gravityB);
}
} // namespace
} // namespace kuka_iiwa_robot
} // namespace benchmarks
} // namespace multibody
} // namespace drake