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