statelpf.hxx
Go to the documentation of this file.
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2018-2019, LAAS-CNRS
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #include <pinocchio/algorithm/joint-configuration.hpp>
10 
11 #include "crocoddyl/core/utils/exception.hpp"
12 #include "statelpf.hpp"
13 
14 namespace sobec {
15 using namespace crocoddyl;
16 
17 template <typename Scalar>
18 StateLPFTpl<Scalar>::StateLPFTpl(
19  boost::shared_ptr<pinocchio::ModelTpl<Scalar> > model, std::size_t nu)
20  : Base(model->nq + model->nv + nu, 2 * model->nv + nu),
21  pinocchio_(model),
22  y0_(VectorXs::Zero(model->nq + model->nv + nu)) {
23  // In a multibody system, we could define the first joint using Lie groups.
24  // The current cases are free-flyer (SE3) and spherical (S03).
25  // Instead simple represents any joint that can model within the Euclidean
26  // manifold. The rest of joints use Euclidean algebra. We use this fact for
27  // computing Jdiff.
28 
29  nv_ = model->nv; // tangent configuration dimension
30  nq_ = model->nq; // configuration dimension
31  ny_ = nq_ + nv_ + nu; // augmented state dimension
32  ndy_ = 2 * nv_ + nu; // augmented state tangent space dimension
33  nw_ = nu; // unfiltered input dimension (nb of actuated joints)
34 
35  // Define internally the limits of the first joint
36  const std::size_t nq0 = model->joints[1].nq();
37  lb_.head(nq0) =
38  -std::numeric_limits<Scalar>::infinity() * VectorXs::Ones(nq0);
39  ub_.head(nq0) = std::numeric_limits<Scalar>::infinity() * VectorXs::Ones(nq0);
40  lb_.segment(nq0, nq_ - nq0) = pinocchio_->lowerPositionLimit.tail(nq_ - nq0);
41  ub_.segment(nq0, nq_ - nq0) = pinocchio_->upperPositionLimit.tail(nq_ - nq0);
42  lb_.segment(nq_, nv_) = -pinocchio_->velocityLimit;
43  ub_.segment(nq_, nv_) = pinocchio_->velocityLimit;
44  lb_.tail(nw_) = -pinocchio_->effortLimit.tail(nw_);
45  ub_.tail(nw_) = pinocchio_->effortLimit.tail(nw_);
46  Base::update_has_limits();
47 
48  y0_.head(nq_) = pinocchio::neutral(*pinocchio_.get());
49  y0_.tail(nv_ + nu) = VectorXs::Zero(nv_ + nu);
50 }
51 
52 template <typename Scalar>
53 StateLPFTpl<Scalar>::~StateLPFTpl() {}
54 
55 template <typename Scalar>
56 const std::size_t& StateLPFTpl<Scalar>::get_nw() const {
57  return nw_;
58 }
59 
60 template <typename Scalar>
61 const std::size_t& StateLPFTpl<Scalar>::get_ny() const {
62  return ny_;
63 }
64 
65 template <typename Scalar>
66 const std::size_t& StateLPFTpl<Scalar>::get_ndy() const {
67  return ndy_;
68 }
69 
70 template <typename Scalar>
71 typename MathBaseTpl<Scalar>::VectorXs StateLPFTpl<Scalar>::zero() const {
72  return y0_;
73 }
74 
75 template <typename Scalar>
76 typename MathBaseTpl<Scalar>::VectorXs StateLPFTpl<Scalar>::rand() const {
77  VectorXs yrand = VectorXs::Random(ny_);
78  yrand.head(nq_) = pinocchio::randomConfiguration(*pinocchio_.get());
79  return yrand;
80 }
81 
82 template <typename Scalar>
83 void StateLPFTpl<Scalar>::diff(const Eigen::Ref<const VectorXs>& y0,
84  const Eigen::Ref<const VectorXs>& y1,
85  Eigen::Ref<VectorXs> dyout) const {
86  if (static_cast<std::size_t>(y0.size()) != ny_) {
87  throw_pretty("Invalid argument: "
88  << "y0 has wrong dimension (it should be " +
89  std::to_string(ny_) + ")");
90  }
91  if (static_cast<std::size_t>(y1.size()) != ny_) {
92  throw_pretty("Invalid argument: "
93  << "y1 has wrong dimension (it should be " +
94  std::to_string(ny_) + ")");
95  }
96  if (static_cast<std::size_t>(dyout.size()) != ndy_) {
97  throw_pretty("Invalid argument: "
98  << "dyout has wrong dimension (it should be " +
99  std::to_string(ndy_) + ")");
100  }
101 
102  pinocchio::difference(*pinocchio_.get(), y0.head(nq_), y1.head(nq_),
103  dyout.head(nv_));
104  dyout.segment(nv_, nv_) = y1.segment(nq_, nv_) - y0.segment(nq_, nv_);
105  dyout.tail(nw_) = y1.tail(nw_) - y0.tail(nw_);
106 }
107 
108 template <typename Scalar>
109 void StateLPFTpl<Scalar>::integrate(const Eigen::Ref<const VectorXs>& y,
110  const Eigen::Ref<const VectorXs>& dy,
111  Eigen::Ref<VectorXs> yout) const {
112  if (static_cast<std::size_t>(y.size()) != ny_) {
113  throw_pretty("Invalid argument: "
114  << "y has wrong dimension (it should be " +
115  std::to_string(ny_) + ")");
116  }
117  if (static_cast<std::size_t>(dy.size()) != ndy_) {
118  throw_pretty("Invalid argument: "
119  << "dy has wrong dimension (it should be " +
120  std::to_string(ndy_) + ")");
121  }
122  if (static_cast<std::size_t>(yout.size()) != ny_) {
123  throw_pretty("Invalid argument: "
124  << "yout has wrong dimension (it should be " +
125  std::to_string(ny_) + ")");
126  }
127 
128  pinocchio::integrate(*pinocchio_.get(), y.head(nq_), dy.head(nv_),
129  yout.head(nq_));
130  yout.segment(nq_, nv_) = y.segment(nq_, nv_) + dy.segment(nv_, nv_);
131  yout.tail(nw_) = y.tail(nw_) + dy.tail(nw_);
132 }
133 
134 template <typename Scalar>
135 void StateLPFTpl<Scalar>::Jdiff(const Eigen::Ref<const VectorXs>& y0,
136  const Eigen::Ref<const VectorXs>& y1,
137  Eigen::Ref<MatrixXs> Jfirst,
138  Eigen::Ref<MatrixXs> Jsecond,
139  const Jcomponent firstsecond) const {
140  assert_pretty(
141  is_a_Jcomponent(firstsecond),
142  ("firstsecond must be one of the Jcomponent {both, first, second}"));
143  if (static_cast<std::size_t>(y0.size()) != ny_) {
144  throw_pretty("Invalid argument: "
145  << "y0 has wrong dimension (it should be " +
146  std::to_string(ny_) + ")");
147  }
148  if (static_cast<std::size_t>(y1.size()) != ny_) {
149  throw_pretty("Invalid argument: "
150  << "y1 has wrong dimension (it should be " +
151  std::to_string(ny_) + ")");
152  }
153 
154  if (firstsecond == first) {
155  if (static_cast<std::size_t>(Jfirst.rows()) != ndy_ ||
156  static_cast<std::size_t>(Jfirst.cols()) != ndy_) {
157  throw_pretty("Invalid argument: "
158  << "Jfirst has wrong dimension (it should be " +
159  std::to_string(ndy_) + "," + std::to_string(ndy_) +
160  ")");
161  }
162 
163  pinocchio::dDifference(*pinocchio_.get(), y0.head(nq_), y1.head(nq_),
164  Jfirst.topLeftCorner(nv_, nv_), pinocchio::ARG0);
165  Jfirst.block(nv_, nv_, nv_, nv_).diagonal().array() = (Scalar)-1;
166  Jfirst.bottomRightCorner(nw_, nw_).diagonal().array() = (Scalar)-1;
167  } else if (firstsecond == second) {
168  if (static_cast<std::size_t>(Jsecond.rows()) != ndy_ ||
169  static_cast<std::size_t>(Jsecond.cols()) != ndy_) {
170  throw_pretty("Invalid argument: "
171  << "Jsecond has wrong dimension (it should be " +
172  std::to_string(ndy_) + "," + std::to_string(ndy_) +
173  ")");
174  }
175  pinocchio::dDifference(*pinocchio_.get(), y0.head(nq_), y1.head(nq_),
176  Jsecond.topLeftCorner(nv_, nv_), pinocchio::ARG1);
177  Jsecond.block(nv_, nv_, nv_, nv_).diagonal().array() = (Scalar)1;
178  Jsecond.bottomRightCorner(nw_, nw_).diagonal().array() = (Scalar)1;
179  } else { // computing both
180  if (static_cast<std::size_t>(Jfirst.rows()) != ndy_ ||
181  static_cast<std::size_t>(Jfirst.cols()) != ndy_) {
182  throw_pretty("Invalid argument: "
183  << "Jfirst has wrong dimension (it should be " +
184  std::to_string(ndy_) + "," + std::to_string(ndy_) +
185  ")");
186  }
187  if (static_cast<std::size_t>(Jsecond.rows()) != ndy_ ||
188  static_cast<std::size_t>(Jsecond.cols()) != ndy_) {
189  throw_pretty("Invalid argument: "
190  << "Jsecond has wrong dimension (it should be " +
191  std::to_string(ndy_) + "," + std::to_string(ndy_) +
192  ")");
193  }
194  pinocchio::dDifference(*pinocchio_.get(), y0.head(nq_), y1.head(nq_),
195  Jfirst.topLeftCorner(nv_, nv_), pinocchio::ARG0);
196  pinocchio::dDifference(*pinocchio_.get(), y0.head(nq_), y1.head(nq_),
197  Jsecond.topLeftCorner(nv_, nv_), pinocchio::ARG1);
198  Jfirst.block(nv_, nv_, nv_, nv_).diagonal().array() = (Scalar)-1;
199  Jfirst.bottomRightCorner(nw_, nw_).diagonal().array() = (Scalar)-1;
200  Jsecond.block(nv_, nv_, nv_, nv_).diagonal().array() = (Scalar)1;
201  Jsecond.bottomRightCorner(nw_, nw_).diagonal().array() = (Scalar)1;
202  }
203 }
204 
205 template <typename Scalar>
206 void StateLPFTpl<Scalar>::Jintegrate(const Eigen::Ref<const VectorXs>& y,
207  const Eigen::Ref<const VectorXs>& dy,
208  Eigen::Ref<MatrixXs> Jfirst,
209  Eigen::Ref<MatrixXs> Jsecond,
210  const Jcomponent firstsecond,
211  const AssignmentOp op) const {
212  assert_pretty(
213  is_a_Jcomponent(firstsecond),
214  ("firstsecond must be one of the Jcomponent {both, first, second}"));
215  assert_pretty(is_a_AssignmentOp(op),
216  ("op must be one of the AssignmentOp {settop, addto, rmfrom}"));
217  if (firstsecond == first || firstsecond == both) {
218  if (static_cast<std::size_t>(Jfirst.rows()) != ndy_ ||
219  static_cast<std::size_t>(Jfirst.cols()) != ndy_) {
220  throw_pretty("Invalid argument: "
221  << "Jfirst has wrong dimension (it should be " +
222  std::to_string(ndy_) + "," + std::to_string(ndy_) +
223  ")");
224  }
225  switch (op) {
226  case setto:
227  pinocchio::dIntegrate(*pinocchio_.get(), y.head(nq_), dy.head(nv_),
228  Jfirst.topLeftCorner(nv_, nv_), pinocchio::ARG0,
229  pinocchio::SETTO);
230  Jfirst.block(nv_, nv_, nv_, nv_).diagonal().array() = (Scalar)1;
231  Jfirst.bottomRightCorner(nw_, nw_).diagonal().array() = (Scalar)1;
232  break;
233  case addto:
234  pinocchio::dIntegrate(*pinocchio_.get(), y.head(nq_), dy.head(nv_),
235  Jfirst.topLeftCorner(nv_, nv_), pinocchio::ARG0,
236  pinocchio::ADDTO);
237  Jfirst.block(nv_, nv_, nv_, nv_).diagonal().array() += (Scalar)1;
238  Jfirst.bottomRightCorner(nw_, nw_).diagonal().array() += (Scalar)1;
239  break;
240  case rmfrom:
241  pinocchio::dIntegrate(*pinocchio_.get(), y.head(nq_), dy.head(nv_),
242  Jfirst.topLeftCorner(nv_, nv_), pinocchio::ARG0,
243  pinocchio::RMTO);
244  Jfirst.block(nv_, nv_, nv_, nv_).diagonal().array() -= (Scalar)1;
245  Jfirst.bottomRightCorner(nw_, nw_).diagonal().array() -= (Scalar)1;
246  break;
247  default:
248  throw_pretty(
249  "Invalid argument: allowed operators: setto, addto, rmfrom");
250  break;
251  }
252  }
253  if (firstsecond == second || firstsecond == both) {
254  if (static_cast<std::size_t>(Jsecond.rows()) != ndy_ ||
255  static_cast<std::size_t>(Jsecond.cols()) != ndy_) {
256  throw_pretty("Invalid argument: "
257  << "Jsecond has wrong dimension (it should be " +
258  std::to_string(ndy_) + "," + std::to_string(ndy_) +
259  ")");
260  }
261  switch (op) {
262  case setto:
263  pinocchio::dIntegrate(*pinocchio_.get(), y.head(nq_), dy.head(nv_),
264  Jsecond.topLeftCorner(nv_, nv_), pinocchio::ARG1,
265  pinocchio::SETTO);
266  Jsecond.block(nv_, nv_, nv_, nv_).diagonal().array() = (Scalar)1;
267  Jsecond.bottomRightCorner(nw_, nw_).diagonal().array() = (Scalar)1;
268  break;
269  case addto:
270  pinocchio::dIntegrate(*pinocchio_.get(), y.head(nq_), dy.head(nv_),
271  Jsecond.topLeftCorner(nv_, nv_), pinocchio::ARG1,
272  pinocchio::ADDTO);
273  Jsecond.block(nv_, nv_, nv_, nv_).diagonal().array() += (Scalar)1;
274  Jsecond.bottomRightCorner(nw_, nw_).diagonal().array() += (Scalar)1;
275  break;
276  case rmfrom:
277  pinocchio::dIntegrate(*pinocchio_.get(), y.head(nq_), dy.head(nv_),
278  Jsecond.topLeftCorner(nv_, nv_), pinocchio::ARG1,
279  pinocchio::RMTO);
280  Jsecond.block(nv_, nv_, nv_, nv_).diagonal().array() -= (Scalar)1;
281  Jsecond.bottomRightCorner(nw_, nw_).diagonal().array() -= (Scalar)1;
282  break;
283  default:
284  throw_pretty(
285  "Invalid argument: allowed operators: setto, addto, rmfrom");
286  break;
287  }
288  }
289 }
290 
291 template <typename Scalar>
292 void StateLPFTpl<Scalar>::JintegrateTransport(
293  const Eigen::Ref<const VectorXs>& y, const Eigen::Ref<const VectorXs>& dy,
294  Eigen::Ref<MatrixXs> Jin, const Jcomponent firstsecond) const {
295  assert_pretty(
296  is_a_Jcomponent(firstsecond),
297  ("firstsecond must be one of the Jcomponent {both, first, second}"));
298 
299  switch (firstsecond) {
300  case first:
301  // pinocchio::dIntegrateTransport(*pinocchio_.get(), y.head(nq_),
302  // dy.head(nv_), Jin.topLeftCorner(nv_, nx_),
303  // pinocchio::ARG0);
304  pinocchio::dIntegrateTransport(*pinocchio_.get(), y.head(nq_),
305  dy.head(nv_), Jin.topRows(nv_),
306  pinocchio::ARG0);
307  break;
308  case second:
309  // pinocchio::dIntegrateTransport(*pinocchio_.get(), y.head(nq_),
310  // dy.head(nv_), Jin.topLeftCorner(nv_, nx_),
311  // pinocchio::ARG1);
312  pinocchio::dIntegrateTransport(*pinocchio_.get(), y.head(nq_),
313  dy.head(nv_), Jin.topRows(nv_),
314  pinocchio::ARG1);
315  break;
316  default:
317  throw_pretty(
318  "Invalid argument: firstsecond must be either first or second. both "
319  "not supported for this operation.");
320  break;
321  }
322 }
323 
324 template <typename Scalar>
325 const boost::shared_ptr<pinocchio::ModelTpl<Scalar> >&
326 StateLPFTpl<Scalar>::get_pinocchio() const {
327  return pinocchio_;
328 }
329 
330 } // namespace sobec
Definition: contact-force.hxx:11