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/multibody/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 {
22  using namespace Eigen;
23  using namespace pinocchio;
24 
25  Model model;
26  buildModels::humanoidRandom(model);
27 
28  Data data(model), data_fd(model);
29 
30  model.lowerPositionLimit.head<3>().fill(-1.);
31  model.upperPositionLimit.head<3>().fill(1.);
32 
33  VectorXd q = randomConfiguration(model);
34  VectorXd v(VectorXd::Random(model.nv));
35  VectorXd a(VectorXd::Random(model.nv));
36 
37  // check with only q non-zero
38  Data::Tensor3x dtau2_dq(model.nv, model.nv, model.nv);
39  Data::Tensor3x dtau2_dv(model.nv, model.nv, model.nv);
40  Data::Tensor3x dtau2_dqdv(model.nv, model.nv, model.nv);
41  Data::Tensor3x dtau2_dadq(model.nv, model.nv, model.nv);
42  dtau2_dq.setZero();
43  dtau2_dv.setZero();
44  dtau2_dqdv.setZero();
45  dtau2_dadq.setZero();
46 
47  ComputeRNEASecondOrderDerivatives(
48  model, data, q, VectorXd::Zero(model.nv), VectorXd::Zero(model.nv), dtau2_dq, dtau2_dv,
49  dtau2_dqdv, dtau2_dadq);
50 
51  Data::Tensor3x dtau2_dq_fd(model.nv, model.nv, model.nv);
52  Data::Tensor3x dtau2_dv_fd(model.nv, model.nv, model.nv);
53  Data::Tensor3x dtau2_dqdv_fd(model.nv, model.nv, model.nv);
54  Data::Tensor3x dtau2_dadq_fd(model.nv, model.nv, model.nv);
55  dtau2_dq_fd.setZero();
56  dtau2_dv_fd.setZero();
57  dtau2_dqdv_fd.setZero();
58  dtau2_dadq_fd.setZero();
59 
60  MatrixXd drnea_dq_plus(MatrixXd::Zero(model.nv, model.nv));
61  MatrixXd drnea_dv_plus(MatrixXd::Zero(model.nv, model.nv));
62  MatrixXd drnea_da_plus(MatrixXd::Zero(model.nv, model.nv));
63 
64  MatrixXd temp1(MatrixXd::Zero(model.nv, model.nv));
65  MatrixXd temp2(MatrixXd::Zero(model.nv, model.nv));
66 
67  VectorXd v_eps(VectorXd::Zero(model.nv));
68  VectorXd q_plus(model.nq);
69  VectorXd v_plus(model.nv);
70 
71  MatrixXd rnea_partial_dq(MatrixXd::Zero(model.nv, model.nv));
72  MatrixXd rnea_partial_dv(MatrixXd::Zero(model.nv, model.nv));
73  MatrixXd rnea_partial_da(MatrixXd::Zero(model.nv, model.nv));
74 
75  computeRNEADerivatives(
76  model, data, q, VectorXd::Zero(model.nv), VectorXd::Zero(model.nv), rnea_partial_dq,
77  rnea_partial_dv, rnea_partial_da);
78 
79  const double alpha = 1e-7;
80 
81  for (int k = 0; k < model.nv; ++k)
82  {
83  v_eps[k] += alpha;
84  q_plus = integrate(model, q, v_eps);
85  computeRNEADerivatives(
86  model, data_fd, q_plus, VectorXd::Zero(model.nv), VectorXd::Zero(model.nv), drnea_dq_plus,
87  drnea_dv_plus, drnea_da_plus);
88  temp1 = (drnea_dq_plus - rnea_partial_dq) / alpha;
89  for (int ii = 0; ii < model.nv; ii++)
90  {
91  for (int jj = 0; jj < model.nv; jj++)
92  {
93  dtau2_dq_fd(jj, ii, k) = temp1(jj, ii);
94  }
95  }
96  v_eps[k] -= alpha;
97  }
98 
99  Map<VectorXd> mq(dtau2_dq.data(), dtau2_dq.size());
100  Map<VectorXd> mq_fd(dtau2_dq_fd.data(), dtau2_dq_fd.size());
101  BOOST_CHECK(mq.isApprox(mq_fd, sqrt(alpha)));
102 
103  // Check with q and a non zero
104  dtau2_dq.setZero();
105  dtau2_dv.setZero();
106  dtau2_dqdv.setZero();
107  dtau2_dadq.setZero();
108  ComputeRNEASecondOrderDerivatives(
109  model, data, q, VectorXd::Zero(model.nv), a, dtau2_dq, dtau2_dv, dtau2_dqdv, dtau2_dadq);
110 
111  rnea_partial_dq.setZero();
112  rnea_partial_dv.setZero();
113  rnea_partial_da.setZero();
114 
115  dtau2_dq_fd.setZero();
116  dtau2_dadq_fd.setZero();
117  drnea_dq_plus.setZero();
118  drnea_dv_plus.setZero();
119  drnea_da_plus.setZero();
120 
121  computeRNEADerivatives(
122  model, data, q, VectorXd::Zero(model.nv), a, rnea_partial_dq, rnea_partial_dv, rnea_partial_da);
123 
124  for (int k = 0; k < model.nv; ++k)
125  {
126  v_eps[k] += alpha;
127  q_plus = integrate(model, q, v_eps);
128  computeRNEADerivatives(
129  model, data_fd, q_plus, VectorXd::Zero(model.nv), a, drnea_dq_plus, drnea_dv_plus,
130  drnea_da_plus);
131  temp1 = (drnea_dq_plus - rnea_partial_dq) / alpha;
132  temp2 = (drnea_da_plus - rnea_partial_da) / alpha;
133  temp2.triangularView<Eigen::StrictlyLower>() =
134  temp2.transpose().triangularView<Eigen::StrictlyLower>();
135  for (int ii = 0; ii < model.nv; ii++)
136  {
137  for (int jj = 0; jj < model.nv; jj++)
138  {
139  dtau2_dq_fd(jj, ii, k) = temp1(jj, ii);
140  dtau2_dadq_fd(jj, ii, k) = temp2(jj, ii);
141  }
142  }
143  v_eps[k] -= alpha;
144  }
145  Map<VectorXd> maq(dtau2_dadq.data(), dtau2_dadq.size());
146  Map<VectorXd> maq_fd(dtau2_dadq_fd.data(), dtau2_dadq_fd.size());
147 
148  BOOST_CHECK(mq.isApprox(mq_fd, sqrt(alpha)));
149  BOOST_CHECK(maq.isApprox(maq_fd, sqrt(alpha)));
150 
151  // Check with q,v and a non zero
152  dtau2_dq.setZero();
153  dtau2_dv.setZero();
154  dtau2_dqdv.setZero();
155  dtau2_dadq.setZero();
156  ComputeRNEASecondOrderDerivatives(
157  model, data, q, v, a, dtau2_dq, dtau2_dv, dtau2_dqdv, dtau2_dadq);
158 
159  rnea_partial_dq.setZero();
160  rnea_partial_dv.setZero();
161  rnea_partial_da.setZero();
162  computeRNEADerivatives(model, data, q, v, a, rnea_partial_dq, rnea_partial_dv, rnea_partial_da);
163 
164  dtau2_dq_fd.setZero();
165  dtau2_dadq_fd.setZero();
166  drnea_dq_plus.setZero();
167  drnea_dv_plus.setZero();
168  drnea_da_plus.setZero();
169 
170  for (int k = 0; k < model.nv; ++k)
171  {
172  v_eps[k] += alpha;
173  q_plus = integrate(model, q, v_eps);
174  computeRNEADerivatives(
175  model, data_fd, q_plus, v, a, drnea_dq_plus, drnea_dv_plus, drnea_da_plus);
176  temp1 = (drnea_dq_plus - rnea_partial_dq) / alpha;
177  temp2 = (drnea_da_plus - rnea_partial_da) / alpha;
178  temp2.triangularView<Eigen::StrictlyLower>() =
179  temp2.transpose().triangularView<Eigen::StrictlyLower>();
180  for (int ii = 0; ii < model.nv; ii++)
181  {
182  for (int jj = 0; jj < model.nv; jj++)
183  {
184  dtau2_dq_fd(jj, ii, k) = temp1(jj, ii);
185  dtau2_dadq_fd(jj, ii, k) = temp2(jj, ii);
186  }
187  }
188  v_eps[k] -= alpha;
189  }
190  dtau2_dv_fd.setZero();
191  dtau2_dqdv_fd.setZero();
192  for (int k = 0; k < model.nv; ++k)
193  {
194  v_eps[k] += alpha;
195  v_plus = v + v_eps;
196  computeRNEADerivatives(
197  model, data_fd, q, v_plus, a, drnea_dq_plus, drnea_dv_plus, drnea_da_plus);
198  temp1 = (drnea_dv_plus - rnea_partial_dv) / alpha;
199  temp2 = (drnea_dq_plus - rnea_partial_dq) / alpha;
200  for (int ii = 0; ii < model.nv; ii++)
201  {
202  for (int jj = 0; jj < model.nv; jj++)
203  {
204  dtau2_dv_fd(jj, ii, k) = temp1(jj, ii);
205  dtau2_dqdv_fd(jj, ii, k) = temp2(jj, ii);
206  }
207  }
208  v_eps[k] -= alpha;
209  }
210  Map<VectorXd> mv(dtau2_dv.data(), dtau2_dv.size());
211  Map<VectorXd> mv_fd(dtau2_dv_fd.data(), dtau2_dv_fd.size());
212 
213  Map<VectorXd> mqv(dtau2_dqdv.data(), dtau2_dqdv.size());
214  Map<VectorXd> mqv_fd(dtau2_dqdv_fd.data(), dtau2_dqdv_fd.size());
215 
216  BOOST_CHECK(mq.isApprox(mq_fd, sqrt(alpha)));
217  BOOST_CHECK(maq.isApprox(maq_fd, sqrt(alpha)));
218  BOOST_CHECK(mv.isApprox(mv_fd, sqrt(alpha)));
219  BOOST_CHECK(mqv.isApprox(mqv_fd, sqrt(alpha)));
220 
221  Data data2(model);
222  ComputeRNEASecondOrderDerivatives(model, data2, q, v, a);
223 
224  Map<VectorXd> mq2(data2.d2tau_dqdq.data(), (data2.d2tau_dqdq).size());
225  Map<VectorXd> mv2(data2.d2tau_dvdv.data(), (data2.d2tau_dvdv).size());
226  Map<VectorXd> mqv2(data2.d2tau_dqdv.data(), (data2.d2tau_dqdv).size());
227  Map<VectorXd> maq2(data2.d2tau_dadq.data(), (data2.d2tau_dadq).size());
228 
229  BOOST_CHECK(mq.isApprox(mq2, sqrt(alpha)));
230  BOOST_CHECK(mv.isApprox(mv2, sqrt(alpha)));
231  BOOST_CHECK(mqv.isApprox(mqv2, sqrt(alpha)));
232  BOOST_CHECK(maq.isApprox(maq2, sqrt(alpha)));
233 }
234 
235 BOOST_AUTO_TEST_SUITE_END()
pinocchio::DataTpl::d2tau_dvdv
Tensor3x d2tau_dvdv
SO Partial derivative of the joint torque vector with respect to the joint velocity.
Definition: multibody/data.hpp:552
pinocchio::DataTpl::d2tau_dqdq
Tensor3x d2tau_dqdq
SO Partial derivative of the joint torque vector with respect to the joint configuration.
Definition: multibody/data.hpp:548
Eigen
pinocchio::DataTpl
Definition: context/generic.hpp:25
pinocchio::DataTpl::d2tau_dqdv
Tensor3x d2tau_dqdv
SO Cross-Partial derivative of the joint torque vector with respect to the joint configuration/veloci...
Definition: multibody/data.hpp:556
rnea-derivatives.hpp
model.hpp
setup.data
data
Definition: cmake/cython/setup.in.py:48
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::computeRNEADerivatives
bp::tuple computeRNEADerivatives(const context::Model &model, context::Data &data, const context::VectorXs &q, const context::VectorXs &v, const context::VectorXs &a)
Definition: expose-rnea-derivatives.cpp:38
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:315
anymal-simulation.model
model
Definition: anymal-simulation.py:8
joint-configuration.hpp
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::Tensor::setZero
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Tensor & setZero()
Definition: tensor.hpp:161
reachable-workspace-with-collisions.alpha
float alpha
Definition: reachable-workspace-with-collisions.py:162
pinocchio::DataTpl::d2tau_dadq
Tensor3x d2tau_dadq
SO Cross-Partial derivative of the joint torque vector with respect to the joint acceleration/configu...
Definition: multibody/data.hpp:562
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:70
pinocchio::Tensor::size
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index size() const
Definition: tensor.hpp:145
cartpole.v
v
Definition: cartpole.py:153
data.hpp
q
q
pinocchio::Tensor::data
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar * data()
Definition: tensor.hpp:150
rnea-second-order-derivatives.hpp
a
Vec3f a
fill
fill
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(test_rnea_derivatives_SO)
Definition: rnea-second-order-derivatives.cpp:20
pinocchio::Tensor< Scalar, 3, Options >
sample-models.hpp
jacobian.hpp
pinocchio::ModelTpl
Definition: context/generic.hpp:20
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:27

pinocchio
Author(s):
autogenerated on Thu Dec 19 2024 03:41:32