multibody.hxx
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 "crocoddyl/core/utils/exception.hpp"
10 #include "crocoddyl/multibody/states/multibody.hpp"
11 #include <pinocchio/algorithm/joint-configuration.hpp>
12 
13 namespace crocoddyl {
14 
15 template <typename Scalar>
16 StateMultibodyTpl<Scalar>::StateMultibodyTpl(boost::shared_ptr<pinocchio::ModelTpl<Scalar> > model)
17  : Base(model->nq + model->nv, 2 * model->nv),
18  pinocchio_(model),
19  x0_(VectorXs::Zero(model->nq + model->nv)),
20  joint_type_(Simple) {
21  x0_.head(nq_) = pinocchio::neutral(*pinocchio_.get());
22 
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 manifold.
26  // The rest of joints use Euclidean algebra. We use this fact for computing Jdiff.
27  std::size_t nq0 = 0;
28  if (model->joints[1].shortname() == "JointModelFreeFlyer") {
29  joint_type_ = FreeFlyer;
30  // Define internally the free-flyer limits
31  nq0 = 7;
32  lb_.template head<7>() = -std::numeric_limits<Scalar>::infinity() * VectorXs::Ones(7);
33  ub_.template head<7>() = std::numeric_limits<Scalar>::infinity() * VectorXs::Ones(7);
34  } else if (model->joints[1].shortname() == "JointModelSphericalZYX") {
35  joint_type_ = Spherical;
36  }
37 
38  lb_.segment(nq0, nq_ - nq0) = pinocchio_->lowerPositionLimit.tail(nq_ - nq0);
39  ub_.segment(nq0, nq_ - nq0) = pinocchio_->upperPositionLimit.tail(nq_ - nq0);
40  lb_.tail(nv_) = -pinocchio_->velocityLimit;
41  ub_.tail(nv_) = pinocchio_->velocityLimit;
42  Base::update_has_limits();
43 }
44 
45 template <typename Scalar>
46 StateMultibodyTpl<Scalar>::~StateMultibodyTpl() {}
47 
48 template <typename Scalar>
49 typename MathBaseTpl<Scalar>::VectorXs StateMultibodyTpl<Scalar>::zero() const {
50  return x0_;
51 }
52 
53 template <typename Scalar>
54 typename MathBaseTpl<Scalar>::VectorXs StateMultibodyTpl<Scalar>::rand() const {
55  VectorXs xrand = VectorXs::Random(nx_);
56  xrand.head(nq_) = pinocchio::randomConfiguration(*pinocchio_.get());
57  return xrand;
58 }
59 
60 template <typename Scalar>
61 void StateMultibodyTpl<Scalar>::diff(const Eigen::Ref<const VectorXs>& x0, const Eigen::Ref<const VectorXs>& x1,
62  Eigen::Ref<VectorXs> dxout) const {
63  if (static_cast<std::size_t>(x0.size()) != nx_) {
64  throw_pretty("Invalid argument: "
65  << "x0 has wrong dimension (it should be " + std::to_string(nx_) + ")");
66  }
67  if (static_cast<std::size_t>(x1.size()) != nx_) {
68  throw_pretty("Invalid argument: "
69  << "x1 has wrong dimension (it should be " + std::to_string(nx_) + ")");
70  }
71  if (static_cast<std::size_t>(dxout.size()) != ndx_) {
72  throw_pretty("Invalid argument: "
73  << "dxout has wrong dimension (it should be " + std::to_string(ndx_) + ")");
74  }
75 
76  pinocchio::difference(*pinocchio_.get(), x0.head(nq_), x1.head(nq_), dxout.head(nv_));
77  dxout.tail(nv_) = x1.tail(nv_) - x0.tail(nv_);
78 }
79 
80 template <typename Scalar>
81 void StateMultibodyTpl<Scalar>::integrate(const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& dx,
82  Eigen::Ref<VectorXs> xout) const {
83  if (static_cast<std::size_t>(x.size()) != nx_) {
84  throw_pretty("Invalid argument: "
85  << "x has wrong dimension (it should be " + std::to_string(nx_) + ")");
86  }
87  if (static_cast<std::size_t>(dx.size()) != ndx_) {
88  throw_pretty("Invalid argument: "
89  << "dx has wrong dimension (it should be " + std::to_string(ndx_) + ")");
90  }
91  if (static_cast<std::size_t>(xout.size()) != nx_) {
92  throw_pretty("Invalid argument: "
93  << "xout has wrong dimension (it should be " + std::to_string(nx_) + ")");
94  }
95 
96  pinocchio::integrate(*pinocchio_.get(), x.head(nq_), dx.head(nv_), xout.head(nq_));
97  xout.tail(nv_) = x.tail(nv_) + dx.tail(nv_);
98 }
99 
100 template <typename Scalar>
101 void StateMultibodyTpl<Scalar>::Jdiff(const Eigen::Ref<const VectorXs>& x0, const Eigen::Ref<const VectorXs>& x1,
102  Eigen::Ref<MatrixXs> Jfirst, Eigen::Ref<MatrixXs> Jsecond,
103  Jcomponent firstsecond) const {
104  assert_pretty(is_a_Jcomponent(firstsecond), ("firstsecond must be one of the Jcomponent {both, first, second}"));
105  if (static_cast<std::size_t>(x0.size()) != nx_) {
106  throw_pretty("Invalid argument: "
107  << "x0 has wrong dimension (it should be " + std::to_string(nx_) + ")");
108  }
109  if (static_cast<std::size_t>(x1.size()) != nx_) {
110  throw_pretty("Invalid argument: "
111  << "x1 has wrong dimension (it should be " + std::to_string(nx_) + ")");
112  }
113 
114  typedef Eigen::Block<Eigen::Ref<MatrixXs>, -1, 1, true> NColAlignedVectorBlock;
115  typedef Eigen::Block<Eigen::Ref<MatrixXs> > MatrixBlock;
116 
117  if (firstsecond == first) {
118  if (static_cast<std::size_t>(Jfirst.rows()) != ndx_ || static_cast<std::size_t>(Jfirst.cols()) != ndx_) {
119  throw_pretty("Invalid argument: "
120  << "Jfirst has wrong dimension (it should be " + std::to_string(ndx_) + "," + std::to_string(ndx_) +
121  ")");
122  }
123  Jfirst.setZero();
124  NColAlignedVectorBlock dx = Jfirst.template rightCols<1>();
125  MatrixBlock Jdq = Jfirst.bottomLeftCorner(nv_, nv_);
126 
127  diff(x1, x0, dx);
128  pinocchio::dIntegrate(*pinocchio_.get(), x1.head(nq_), dx.topRows(nv_), Jdq, pinocchio::ARG1);
129  updateJdiff(Jdq, Jfirst.topLeftCorner(nv_, nv_), false);
130 
131  Jdq.setZero();
132  dx.setZero();
133  Jfirst.bottomRightCorner(nv_, nv_).diagonal().array() = (Scalar)-1;
134  } else if (firstsecond == second) {
135  if (static_cast<std::size_t>(Jsecond.rows()) != ndx_ || static_cast<std::size_t>(Jsecond.cols()) != ndx_) {
136  throw_pretty("Invalid argument: "
137  << "Jsecond has wrong dimension (it should be " + std::to_string(ndx_) + "," +
138  std::to_string(ndx_) + ")");
139  }
140 
141  Jsecond.setZero();
142  NColAlignedVectorBlock dx = Jsecond.template rightCols<1>();
143  MatrixBlock Jdq = Jsecond.bottomLeftCorner(nv_, nv_);
144 
145  diff(x0, x1, dx);
146  pinocchio::dIntegrate(*pinocchio_.get(), x0.head(nq_), dx.topRows(nv_), Jdq, pinocchio::ARG1);
147  updateJdiff(Jdq, Jsecond.topLeftCorner(nv_, nv_));
148 
149  Jdq.setZero();
150  dx.setZero();
151  Jsecond.bottomRightCorner(nv_, nv_).diagonal().array() = (Scalar)1;
152  } else { // computing both
153  if (static_cast<std::size_t>(Jfirst.rows()) != ndx_ || static_cast<std::size_t>(Jfirst.cols()) != ndx_) {
154  throw_pretty("Invalid argument: "
155  << "Jfirst has wrong dimension (it should be " + std::to_string(ndx_) + "," + std::to_string(ndx_) +
156  ")");
157  }
158  if (static_cast<std::size_t>(Jsecond.rows()) != ndx_ || static_cast<std::size_t>(Jsecond.cols()) != ndx_) {
159  throw_pretty("Invalid argument: "
160  << "Jsecond has wrong dimension (it should be " + std::to_string(ndx_) + "," +
161  std::to_string(ndx_) + ")");
162  }
163  Jfirst.setZero();
164  Jsecond.setZero();
165 
166  // Computing Jfirst
167  NColAlignedVectorBlock dx1 = Jfirst.template rightCols<1>();
168  MatrixBlock Jdq1 = Jfirst.bottomLeftCorner(nv_, nv_);
169 
170  diff(x1, x0, dx1);
171  pinocchio::dIntegrate(*pinocchio_.get(), x1.head(nq_), dx1.topRows(nv_), Jdq1, pinocchio::ARG1);
172  updateJdiff(Jdq1, Jfirst.topLeftCorner(nv_, nv_), false);
173  Jdq1.setZero();
174  dx1.setZero();
175  Jfirst.bottomRightCorner(nv_, nv_).diagonal().array() = (Scalar)-1;
176 
177  // Computing Jsecond
178  NColAlignedVectorBlock dx2 = Jsecond.template rightCols<1>();
179  MatrixBlock Jdq2 = Jsecond.bottomLeftCorner(nv_, nv_);
180 
181  diff(x0, x1, dx2);
182  pinocchio::dIntegrate(*pinocchio_.get(), x0.head(nq_), dx2.topRows(nv_), Jdq2, pinocchio::ARG1);
183  updateJdiff(Jdq2, Jsecond.topLeftCorner(nv_, nv_));
184 
185  dx2.setZero();
186  Jdq2.setZero();
187  Jsecond.bottomRightCorner(nv_, nv_).diagonal().array() = (Scalar)1;
188  }
189 }
190 
191 template <typename Scalar>
192 void StateMultibodyTpl<Scalar>::Jintegrate(const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& dx,
193  Eigen::Ref<MatrixXs> Jfirst, Eigen::Ref<MatrixXs> Jsecond,
194  Jcomponent firstsecond) const {
195  assert_pretty(is_a_Jcomponent(firstsecond), ("firstsecond must be one of the Jcomponent {both, first, second}"));
196  if (static_cast<std::size_t>(x.size()) != nx_) {
197  throw_pretty("Invalid argument: "
198  << "x has wrong dimension (it should be " + std::to_string(nx_) + ")");
199  }
200  if (static_cast<std::size_t>(dx.size()) != ndx_) {
201  throw_pretty("Invalid argument: "
202  << "dx has wrong dimension (it should be " + std::to_string(ndx_) + ")");
203  }
204 
205  if (firstsecond == first) {
206  if (static_cast<std::size_t>(Jfirst.rows()) != ndx_ || static_cast<std::size_t>(Jfirst.cols()) != ndx_) {
207  throw_pretty("Invalid argument: "
208  << "Jfirst has wrong dimension (it should be " + std::to_string(ndx_) + "," + std::to_string(ndx_) +
209  ")");
210  }
211  Jfirst.setZero();
212 
213  pinocchio::dIntegrate(*pinocchio_.get(), x.head(nq_), dx.head(nv_), Jfirst.topLeftCorner(nv_, nv_),
214  pinocchio::ARG0);
215  Jfirst.bottomRightCorner(nv_, nv_).diagonal().array() = (Scalar)1;
216  } else if (firstsecond == second) {
217  if (static_cast<std::size_t>(Jsecond.rows()) != ndx_ || static_cast<std::size_t>(Jsecond.cols()) != ndx_) {
218  throw_pretty("Invalid argument: "
219  << "Jsecond has wrong dimension (it should be " + std::to_string(ndx_) + "," +
220  std::to_string(ndx_) + ")");
221  }
222  Jsecond.setZero();
223 
224  pinocchio::dIntegrate(*pinocchio_.get(), x.head(nq_), dx.head(nv_), Jsecond.topLeftCorner(nv_, nv_),
225  pinocchio::ARG1);
226  Jsecond.bottomRightCorner(nv_, nv_).diagonal().array() = (Scalar)1;
227  } else { // computing both
228  if (static_cast<std::size_t>(Jfirst.rows()) != ndx_ || static_cast<std::size_t>(Jfirst.cols()) != ndx_) {
229  throw_pretty("Invalid argument: "
230  << "Jfirst has wrong dimension (it should be " + std::to_string(ndx_) + "," + std::to_string(ndx_) +
231  ")");
232  }
233  if (static_cast<std::size_t>(Jsecond.rows()) != ndx_ || static_cast<std::size_t>(Jsecond.cols()) != ndx_) {
234  throw_pretty("Invalid argument: "
235  << "Jsecond has wrong dimension (it should be " + std::to_string(ndx_) + "," +
236  std::to_string(ndx_) + ")");
237  }
238 
239  // Computing Jfirst
240  Jfirst.setZero();
241  pinocchio::dIntegrate(*pinocchio_.get(), x.head(nq_), dx.head(nv_), Jfirst.topLeftCorner(nv_, nv_),
242  pinocchio::ARG0);
243  Jfirst.bottomRightCorner(nv_, nv_).diagonal().array() = (Scalar)1;
244 
245  // Computing Jsecond
246  Jsecond.setZero();
247  pinocchio::dIntegrate(*pinocchio_.get(), x.head(nq_), dx.head(nv_), Jsecond.topLeftCorner(nv_, nv_),
248  pinocchio::ARG1);
249  Jsecond.bottomRightCorner(nv_, nv_).diagonal().array() = (Scalar)1;
250  }
251 }
252 
253 template <typename Scalar>
254 const boost::shared_ptr<pinocchio::ModelTpl<Scalar> >& StateMultibodyTpl<Scalar>::get_pinocchio() const {
255  return pinocchio_;
256 }
257 
258 template <typename Scalar>
259 void StateMultibodyTpl<Scalar>::updateJdiff(const Eigen::Ref<const MatrixXs>& Jdq, Eigen::Ref<MatrixXs> Jd,
260  bool positive) const {
261  if (positive) {
262  Jd.diagonal() = Jdq.diagonal();
263 
264  // Needed only for systems with bases defined as SE3 and S03 group
265  if (joint_type_ == FreeFlyer) {
266  Jd.template block<6, 6>(0, 0) = Jdq.template block<6, 6>(0, 0).inverse();
267  } else if (joint_type_ == Spherical) {
268  Jd.template block<3, 3>(0, 0) = Jdq.template block<3, 3>(0, 0).inverse();
269  }
270  } else {
271  Jd.diagonal() = -Jdq.diagonal();
272 
273  // Needed only for systems with bases defined as SE3 and S03 group
274  if (joint_type_ == FreeFlyer) {
275  Jd.template block<6, 6>(0, 0) = -Jdq.template block<6, 6>(0, 0).inverse();
276  } else if (joint_type_ == Spherical) {
277  Jd.template block<3, 3>(0, 0) = -Jdq.template block<3, 3>(0, 0).inverse();
278  }
279  }
280 }
281 
282 } // namespace crocoddyl
Definition: action-base.hxx:11