rnea-second-order-derivatives.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2017-2020 CNRS INRIA
3 //
4 
5 #include "pinocchio/multibody/model.hpp"
6 #include "pinocchio/multibody/data.hpp"
7 #include "pinocchio/algorithm/jacobian.hpp"
8 #include "pinocchio/algorithm/joint-configuration.hpp"
9 #include "pinocchio/algorithm/rnea-second-order-derivatives.hpp"
10 #include "pinocchio/algorithm/rnea-derivatives.hpp"
11 #include "pinocchio/parsers/sample-models.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_rnea_derivatives_SO) {
21  using namespace Eigen;
22  using namespace pinocchio;
23 
24  Model model;
25  buildModels::humanoidRandom(model);
26 
27  Data data(model), data_fd(model);
28 
29  model.lowerPositionLimit.head<3>().fill(-1.);
30  model.upperPositionLimit.head<3>().fill(1.);
31 
32  VectorXd q = randomConfiguration(model);
33  VectorXd v(VectorXd::Random(model.nv));
34  VectorXd a(VectorXd::Random(model.nv));
35 
36  // check with only q non-zero
37  Data::Tensor3x dtau2_dq(model.nv, model.nv, model.nv);
38  Data::Tensor3x dtau2_dv(model.nv, model.nv, model.nv);
39  Data::Tensor3x dtau2_dqdv(model.nv, model.nv, model.nv);
40  Data::Tensor3x dtau2_dadq(model.nv, model.nv, model.nv);
41  dtau2_dq.setZero();
42  dtau2_dv.setZero();
43  dtau2_dqdv.setZero();
44  dtau2_dadq.setZero();
45 
46  ComputeRNEASecondOrderDerivatives(model, data, q, VectorXd::Zero(model.nv),
47  VectorXd::Zero(model.nv), dtau2_dq, dtau2_dv,
48  dtau2_dqdv, dtau2_dadq);
49 
50  Data::Tensor3x dtau2_dq_fd(model.nv, model.nv, model.nv);
51  Data::Tensor3x dtau2_dv_fd(model.nv, model.nv, model.nv);
52  Data::Tensor3x dtau2_dqdv_fd(model.nv, model.nv, model.nv);
53  Data::Tensor3x dtau2_dadq_fd(model.nv, model.nv, model.nv);
54  dtau2_dq_fd.setZero();
55  dtau2_dv_fd.setZero();
56  dtau2_dqdv_fd.setZero();
57  dtau2_dadq_fd.setZero();
58 
59  MatrixXd drnea_dq_plus(MatrixXd::Zero(model.nv, model.nv));
60  MatrixXd drnea_dv_plus(MatrixXd::Zero(model.nv, model.nv));
61  MatrixXd drnea_da_plus(MatrixXd::Zero(model.nv, model.nv));
62 
63  MatrixXd temp1(MatrixXd::Zero(model.nv, model.nv));
64  MatrixXd temp2(MatrixXd::Zero(model.nv, model.nv));
65 
66  VectorXd v_eps(VectorXd::Zero(model.nv));
67  VectorXd q_plus(model.nq);
68  VectorXd v_plus(model.nv);
69 
70  MatrixXd rnea_partial_dq(MatrixXd::Zero(model.nv, model.nv));
71  MatrixXd rnea_partial_dv(MatrixXd::Zero(model.nv, model.nv));
72  MatrixXd rnea_partial_da(MatrixXd::Zero(model.nv, model.nv));
73 
74  computeRNEADerivatives(model, data, q, VectorXd::Zero(model.nv),
75  VectorXd::Zero(model.nv), rnea_partial_dq,
76  rnea_partial_dv, rnea_partial_da);
77 
78  const double alpha = 1e-7;
79 
80  for (int k = 0; k < model.nv; ++k) {
81  v_eps[k] += alpha;
82  q_plus = integrate(model, q, v_eps);
83  computeRNEADerivatives(model, data_fd, q_plus, VectorXd::Zero(model.nv),
84  VectorXd::Zero(model.nv), drnea_dq_plus,
85  drnea_dv_plus, drnea_da_plus);
86  temp1 = (drnea_dq_plus - rnea_partial_dq) / alpha;
87  for (int ii = 0; ii < model.nv; ii++) {
88  for (int jj = 0; jj < model.nv; jj++) {
89  dtau2_dq_fd(jj, ii, k) = temp1(jj, ii);
90  }
91  }
92  v_eps[k] -= alpha;
93  }
94 
95  Map<VectorXd> mq(dtau2_dq.data(), dtau2_dq.size());
96  Map<VectorXd> mq_fd(dtau2_dq_fd.data(), dtau2_dq_fd.size());
97  BOOST_CHECK(mq.isApprox(mq_fd, sqrt(alpha)));
98 
99  // Check with q and a non zero
100  dtau2_dq.setZero();
101  dtau2_dv.setZero();
102  dtau2_dqdv.setZero();
103  dtau2_dadq.setZero();
104  ComputeRNEASecondOrderDerivatives(model, data, q, VectorXd::Zero(model.nv), a,
105  dtau2_dq, dtau2_dv, dtau2_dqdv, dtau2_dadq);
106 
107  rnea_partial_dq.setZero();
108  rnea_partial_dv.setZero();
109  rnea_partial_da.setZero();
110 
111  dtau2_dq_fd.setZero();
112  dtau2_dadq_fd.setZero();
113  drnea_dq_plus.setZero();
114  drnea_dv_plus.setZero();
115  drnea_da_plus.setZero();
116 
117  computeRNEADerivatives(model, data, q, VectorXd::Zero(model.nv), a,
118  rnea_partial_dq, rnea_partial_dv, rnea_partial_da);
119 
120  for (int k = 0; k < model.nv; ++k) {
121  v_eps[k] += alpha;
122  q_plus = integrate(model, q, v_eps);
123  computeRNEADerivatives(model, data_fd, q_plus, VectorXd::Zero(model.nv), a,
124  drnea_dq_plus, drnea_dv_plus, drnea_da_plus);
125  temp1 = (drnea_dq_plus - rnea_partial_dq) / alpha;
126  temp2 = (drnea_da_plus - rnea_partial_da) / alpha;
127  temp2.triangularView<Eigen::StrictlyLower>() =
128  temp2.transpose().triangularView<Eigen::StrictlyLower>();
129  for (int ii = 0; ii < model.nv; ii++) {
130  for (int jj = 0; jj < model.nv; jj++) {
131  dtau2_dq_fd(jj, ii, k) = temp1(jj, ii);
132  dtau2_dadq_fd(jj, ii, k) = temp2(jj, ii);
133  }
134  }
135  v_eps[k] -= alpha;
136  }
137  Map<VectorXd> maq(dtau2_dadq.data(), dtau2_dadq.size());
138  Map<VectorXd> maq_fd(dtau2_dadq_fd.data(), dtau2_dadq_fd.size());
139 
140  BOOST_CHECK(mq.isApprox(mq_fd, sqrt(alpha)));
141  BOOST_CHECK(maq.isApprox(maq_fd, sqrt(alpha)));
142 
143  // Check with q,v and a non zero
144  dtau2_dq.setZero();
145  dtau2_dv.setZero();
146  dtau2_dqdv.setZero();
147  dtau2_dadq.setZero();
148  ComputeRNEASecondOrderDerivatives(model, data, q, v, a, dtau2_dq, dtau2_dv, dtau2_dqdv,
149  dtau2_dadq);
150 
151  rnea_partial_dq.setZero();
152  rnea_partial_dv.setZero();
153  rnea_partial_da.setZero();
154  computeRNEADerivatives(model, data, q, v, a, rnea_partial_dq, rnea_partial_dv,
155  rnea_partial_da);
156 
157  dtau2_dq_fd.setZero();
158  dtau2_dadq_fd.setZero();
159  drnea_dq_plus.setZero();
160  drnea_dv_plus.setZero();
161  drnea_da_plus.setZero();
162 
163  for (int k = 0; k < model.nv; ++k) {
164  v_eps[k] += alpha;
165  q_plus = integrate(model, q, v_eps);
166  computeRNEADerivatives(model, data_fd, q_plus, v, a, drnea_dq_plus,
167  drnea_dv_plus, drnea_da_plus);
168  temp1 = (drnea_dq_plus - rnea_partial_dq) / alpha;
169  temp2 = (drnea_da_plus - rnea_partial_da) / alpha;
170  temp2.triangularView<Eigen::StrictlyLower>() =
171  temp2.transpose().triangularView<Eigen::StrictlyLower>();
172  for (int ii = 0; ii < model.nv; ii++) {
173  for (int jj = 0; jj < model.nv; jj++) {
174  dtau2_dq_fd(jj, ii, k) = temp1(jj, ii);
175  dtau2_dadq_fd(jj, ii, k) = temp2(jj, ii);
176  }
177  }
178  v_eps[k] -= alpha;
179  }
180  dtau2_dv_fd.setZero();
181  dtau2_dqdv_fd.setZero();
182  for (int k = 0; k < model.nv; ++k) {
183  v_eps[k] += alpha;
184  v_plus = v + v_eps;
185  computeRNEADerivatives(model, data_fd, q, v_plus, a, drnea_dq_plus,
186  drnea_dv_plus, drnea_da_plus);
187  temp1 = (drnea_dv_plus - rnea_partial_dv) / alpha;
188  temp2 = (drnea_dq_plus - rnea_partial_dq) / alpha;
189  for (int ii = 0; ii < model.nv; ii++) {
190  for (int jj = 0; jj < model.nv; jj++) {
191  dtau2_dv_fd(jj, ii, k) = temp1(jj, ii);
192  dtau2_dqdv_fd(jj, ii, k) = temp2(jj, ii);
193  }
194  }
195  v_eps[k] -= alpha;
196  }
197  Map<VectorXd> mv(dtau2_dv.data(), dtau2_dv.size());
198  Map<VectorXd> mv_fd(dtau2_dv_fd.data(), dtau2_dv_fd.size());
199 
200  Map<VectorXd> mqv(dtau2_dqdv.data(), dtau2_dqdv.size());
201  Map<VectorXd> mqv_fd(dtau2_dqdv_fd.data(), dtau2_dqdv_fd.size());
202 
203  BOOST_CHECK(mq.isApprox(mq_fd, sqrt(alpha)));
204  BOOST_CHECK(maq.isApprox(maq_fd, sqrt(alpha)));
205  BOOST_CHECK(mv.isApprox(mv_fd, sqrt(alpha)));
206  BOOST_CHECK(mqv.isApprox(mqv_fd, sqrt(alpha)));
207 
208  Data data2(model);
209  ComputeRNEASecondOrderDerivatives(model, data2, q, v, a);
210 
211  Map<VectorXd> mq2(data2.d2tau_dqdq.data(), (data2.d2tau_dqdq).size());
212  Map<VectorXd> mv2(data2.d2tau_dvdv.data(), (data2.d2tau_dvdv).size());
213  Map<VectorXd> mqv2(data2.d2tau_dqdv.data(), (data2.d2tau_dqdv).size());
214  Map<VectorXd> maq2(data2.d2tau_dadq.data(), (data2.d2tau_dadq).size());
215 
216  BOOST_CHECK(mq.isApprox(mq2, sqrt(alpha)));
217  BOOST_CHECK(mv.isApprox(mv2, sqrt(alpha)));
218  BOOST_CHECK(mqv.isApprox(mqv2, sqrt(alpha)));
219  BOOST_CHECK(maq.isApprox(maq2, sqrt(alpha)));
220 }
221 
222 BOOST_AUTO_TEST_SUITE_END()
q
q
pinocchio::integrate
void integrate(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v, const Eigen::MatrixBase< ReturnType > &qout)
Integrate a configuration vector for the specified model for a tangent vector during one unit time...
Definition: joint-configuration.hpp:54
pinocchio::ModelTpl::lowerPositionLimit
ConfigVectorType lowerPositionLimit
Lower joint configuration limit.
Definition: src/multibody/model.hpp:139
pinocchio::randomConfiguration
void randomConfiguration(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &lowerLimits, const Eigen::MatrixBase< ConfigVectorIn2 > &upperLimits, const Eigen::MatrixBase< ReturnType > &qout)
Generate a configuration vector uniformly sampled among provided limits.
Definition: joint-configuration.hpp:224
fill
fill
pinocchio::Tensor::data
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar * data()
Definition: tensor.hpp:112
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(test_rnea_derivatives_SO)
Definition: rnea-second-order-derivatives.cpp:20
pinocchio::python::computeRNEADerivatives
bp::tuple computeRNEADerivatives(const Model &model, Data &data, const Eigen::VectorXd &q, const Eigen::VectorXd &v, const Eigen::VectorXd &a)
Definition: expose-rnea-derivatives.cpp:36
a
Vec3f a
size
FCL_REAL size() const
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:28
pinocchio::ComputeRNEASecondOrderDerivatives
void ComputeRNEASecondOrderDerivatives(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &a, const Tensor1 &d2tau_dqdq, const Tensor2 &d2tau_dvdv, const Tensor3 &dtau_dqdv, const Tensor4 &dtau_dadq)
Computes the Second-Order partial derivatives of the Recursive Newton Euler Algorithm w...
pinocchio::ModelTpl::nv
int nv
Dimension of the velocity vector space.
Definition: src/multibody/model.hpp:79
pinocchio::ModelTpl::upperPositionLimit
ConfigVectorType upperPositionLimit
Upper joint configuration limit.
Definition: src/multibody/model.hpp:142
pinocchio::buildModels::humanoidRandom
void humanoidRandom(ModelTpl< Scalar, Options, JointCollectionTpl > &model, bool usingFF=true)
Create a humanoid kinematic tree with 6-DOF limbs and random joint placements.
pinocchio::python::VectorXd
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > VectorXd
Definition: conversions.cpp:14
pinocchio::Tensor::size
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index size() const
Definition: tensor.hpp:107
pinocchio::ModelTpl::nq
int nq
Dimension of the configuration vector representation.
Definition: src/multibody/model.hpp:76
pinocchio::Tensor::setZero
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Tensor & setZero()
Definition: tensor.hpp:123

pinocchio
Author(s):
autogenerated on Fri Jun 23 2023 02:38:32