Line |
Branch |
Exec |
Source |
1 |
|
|
// |
2 |
|
|
// Copyright(c) 2019 INRIA |
3 |
|
|
// |
4 |
|
|
|
5 |
|
|
#include "pinocchio/multibody/model.hpp" |
6 |
|
|
#include "pinocchio/multibody/data.hpp" |
7 |
|
|
#include "pinocchio/algorithm/kinematics.hpp" |
8 |
|
|
#include "pinocchio/algorithm/joint-configuration.hpp" |
9 |
|
|
#include "pinocchio/multibody/sample-models.hpp" |
10 |
|
|
|
11 |
|
|
#include "pinocchio/spatial/classic-acceleration.hpp" |
12 |
|
|
|
13 |
|
|
#include <iostream> |
14 |
|
|
|
15 |
|
|
#include <boost/test/unit_test.hpp> |
16 |
|
|
#include <boost/utility/binary.hpp> |
17 |
|
|
|
18 |
|
|
BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) |
19 |
|
|
|
20 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_classic_acceleration) |
21 |
|
|
{ |
22 |
|
|
using namespace Eigen; |
23 |
|
|
using namespace pinocchio; |
24 |
|
|
|
25 |
|
|
static const int num_tests = 1e2; |
26 |
|
|
|
27 |
|
✗ |
Model model; |
28 |
|
✗ |
buildModels::humanoidRandom(model); |
29 |
|
|
|
30 |
|
✗ |
model.upperPositionLimit.head<3>().fill(100); |
31 |
|
✗ |
model.upperPositionLimit.segment<4>(3).setOnes(); |
32 |
|
✗ |
model.lowerPositionLimit.head<7>() = -model.upperPositionLimit.head<7>(); |
33 |
|
|
|
34 |
|
✗ |
VectorXd q(model.nq); |
35 |
|
✗ |
q = randomConfiguration(model); |
36 |
|
✗ |
VectorXd v(VectorXd::Random(model.nv)); |
37 |
|
✗ |
VectorXd a(VectorXd::Zero(model.nv)); |
38 |
|
|
|
39 |
|
✗ |
Data data_ref(model), data(model); |
40 |
|
|
|
41 |
|
✗ |
const std::string RF_joint_name = "rleg6_joint"; |
42 |
|
✗ |
const Model::JointIndex RF_joint_id = model.getJointId(RF_joint_name); |
43 |
|
|
|
44 |
|
✗ |
for (int k = 0; k < num_tests; ++k) |
45 |
|
|
{ |
46 |
|
✗ |
q = randomConfiguration(model); |
47 |
|
✗ |
v = VectorXd::Random(model.nv); |
48 |
|
|
|
49 |
|
✗ |
forwardKinematics(model, data, q, v, a); |
50 |
|
✗ |
const SE3 RF_world_transf = SE3(data.oMi[RF_joint_id].rotation(), SE3::Vector3::Zero()); |
51 |
|
|
|
52 |
|
✗ |
const Motion RF_v_global = RF_world_transf.act(data.v[RF_joint_id]); |
53 |
|
✗ |
const Motion RF_a_global = RF_world_transf.act(data.a[RF_joint_id]); |
54 |
|
✗ |
const Motion::Vector3 classic_acc = classicAcceleration(RF_v_global, RF_a_global); |
55 |
|
|
|
56 |
|
✗ |
Motion::Vector3 classic_acc_other_signature; |
57 |
|
✗ |
classicAcceleration(RF_v_global, RF_a_global, classic_acc_other_signature); |
58 |
|
✗ |
BOOST_CHECK(classic_acc_other_signature.isApprox(classic_acc)); |
59 |
|
|
|
60 |
|
|
// Computes with finite differences |
61 |
|
✗ |
const double eps = 1e-5; |
62 |
|
✗ |
const double eps2 = eps * eps; |
63 |
|
✗ |
forwardKinematics(model, data_ref, q); |
64 |
|
✗ |
const SE3::Vector3 pos = data_ref.oMi[RF_joint_id].translation(); |
65 |
|
|
|
66 |
|
✗ |
VectorXd v_plus = v + eps * a; |
67 |
|
✗ |
VectorXd q_plus = integrate(model, q, v * eps + a * eps2 / 2.); |
68 |
|
✗ |
forwardKinematics(model, data_ref, q_plus); |
69 |
|
✗ |
const SE3::Vector3 pos_plus = data_ref.oMi[RF_joint_id].translation(); |
70 |
|
|
|
71 |
|
✗ |
VectorXd v_minus = v - eps * a; |
72 |
|
✗ |
VectorXd q_minus = integrate(model, q, -v * eps - a * eps2 / 2.); |
73 |
|
✗ |
forwardKinematics(model, data_ref, q_minus); |
74 |
|
✗ |
const SE3::Vector3 pos_minus = data_ref.oMi[RF_joint_id].translation(); |
75 |
|
|
|
76 |
|
✗ |
const SE3::Vector3 classic_acc_ref = (pos_plus + pos_minus - 2. * pos) / eps2; |
77 |
|
|
|
78 |
|
✗ |
BOOST_CHECK(classic_acc.isApprox(classic_acc_ref, math::sqrt(eps) * 1e1)); |
79 |
|
|
} |
80 |
|
|
} |
81 |
|
|
|
82 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_classic_acceleration_with_placement) |
83 |
|
|
{ |
84 |
|
|
using namespace Eigen; |
85 |
|
|
using namespace pinocchio; |
86 |
|
|
|
87 |
|
|
static const int num_tests = 1e2; |
88 |
|
|
|
89 |
|
✗ |
Model model; |
90 |
|
✗ |
buildModels::humanoidRandom(model); |
91 |
|
|
|
92 |
|
✗ |
model.upperPositionLimit.head<3>().fill(1); |
93 |
|
✗ |
model.upperPositionLimit.segment<4>(3).setOnes(); |
94 |
|
✗ |
model.lowerPositionLimit.head<7>() = -model.upperPositionLimit.head<7>(); |
95 |
|
|
|
96 |
|
✗ |
VectorXd q(model.nq); |
97 |
|
✗ |
q = randomConfiguration(model); |
98 |
|
✗ |
VectorXd v(VectorXd::Random(model.nv)); |
99 |
|
✗ |
VectorXd a(VectorXd::Zero(model.nv)); |
100 |
|
|
|
101 |
|
✗ |
Data data_ref(model), data(model); |
102 |
|
|
|
103 |
|
✗ |
const std::string RF_joint_name = "rleg6_joint"; |
104 |
|
✗ |
const Model::JointIndex RF_joint_id = model.getJointId(RF_joint_name); |
105 |
|
|
|
106 |
|
✗ |
for (int k = 0; k < num_tests; ++k) |
107 |
|
|
{ |
108 |
|
✗ |
q = randomConfiguration(model); |
109 |
|
✗ |
v = VectorXd::Random(model.nv); |
110 |
|
|
|
111 |
|
✗ |
forwardKinematics(model, data, q, v, a); |
112 |
|
|
|
113 |
|
✗ |
const SE3 RF_world_transf = SE3(data.oMi[RF_joint_id].rotation(), SE3::Vector3::Zero()); |
114 |
|
|
|
115 |
|
✗ |
const Motion RF_v_global = RF_world_transf.act(data.v[RF_joint_id]); |
116 |
|
✗ |
const Motion RF_a_global = RF_world_transf.act(data.a[RF_joint_id]); |
117 |
|
✗ |
const Motion::Vector3 RF_classic_acc_ref = classicAcceleration(RF_v_global, RF_a_global); |
118 |
|
|
|
119 |
|
✗ |
const SE3 identity_placement = SE3::Identity(); |
120 |
|
|
Motion::Vector3 RF_classic_acc = |
121 |
|
✗ |
classicAcceleration(RF_v_global, RF_a_global, identity_placement); |
122 |
|
|
|
123 |
|
✗ |
BOOST_CHECK(RF_classic_acc.isApprox(RF_classic_acc_ref)); |
124 |
|
|
|
125 |
|
✗ |
const SE3 random_placement = SE3::Random(); |
126 |
|
|
|
127 |
|
✗ |
const Motion & v_A = data.v[RF_joint_id]; |
128 |
|
✗ |
const Motion & a_A = data.a[RF_joint_id]; |
129 |
|
|
|
130 |
|
✗ |
const Motion v_B = random_placement.actInv(data.v[RF_joint_id]); |
131 |
|
✗ |
const Motion a_B = random_placement.actInv(data.a[RF_joint_id]); |
132 |
|
|
|
133 |
|
✗ |
Motion::Vector3 classic_acc_B_ref = classicAcceleration(v_B, a_B); |
134 |
|
✗ |
Motion::Vector3 classic_acc_B = classicAcceleration(v_A, a_A, random_placement); |
135 |
|
|
|
136 |
|
✗ |
BOOST_CHECK(classic_acc_B.isApprox(classic_acc_B_ref)); |
137 |
|
|
|
138 |
|
|
// Check other signature |
139 |
|
✗ |
Motion::Vector3 classic_acc_B_other_signature; |
140 |
|
✗ |
classicAcceleration(v_A, a_A, random_placement, classic_acc_B_other_signature); |
141 |
|
|
|
142 |
|
✗ |
BOOST_CHECK(classic_acc_B_other_signature.isApprox(classic_acc_B)); |
143 |
|
|
} |
144 |
|
|
} |
145 |
|
|
|
146 |
|
|
BOOST_AUTO_TEST_SUITE_END() |
147 |
|
|
|