9 #include "crocoddyl/core/utils/exception.hpp" 10 #include "crocoddyl/multibody/states/multibody.hpp" 11 #include <pinocchio/algorithm/joint-configuration.hpp> 15 template <
typename Scalar>
16 StateMultibodyTpl<Scalar>::StateMultibodyTpl(boost::shared_ptr<pinocchio::ModelTpl<Scalar> > model)
17 : Base(model->nq + model->nv, 2 * model->nv),
19 x0_(VectorXs::Zero(model->nq + model->nv)),
21 x0_.head(nq_) = pinocchio::neutral(*pinocchio_.get());
28 if (model->joints[1].shortname() ==
"JointModelFreeFlyer") {
29 joint_type_ = FreeFlyer;
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;
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();
45 template <
typename Scalar>
46 StateMultibodyTpl<Scalar>::~StateMultibodyTpl() {}
48 template <
typename Scalar>
49 typename MathBaseTpl<Scalar>::VectorXs StateMultibodyTpl<Scalar>::zero()
const {
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());
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_) +
")");
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_) +
")");
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_) +
")");
76 pinocchio::difference(*pinocchio_.get(), x0.head(nq_), x1.head(nq_), dxout.head(nv_));
77 dxout.tail(nv_) = x1.tail(nv_) - x0.tail(nv_);
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_) +
")");
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_) +
")");
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_) +
")");
96 pinocchio::integrate(*pinocchio_.get(), x.head(nq_), dx.head(nv_), xout.head(nq_));
97 xout.tail(nv_) = x.tail(nv_) + dx.tail(nv_);
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_) +
")");
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_) +
")");
114 typedef Eigen::Block<Eigen::Ref<MatrixXs>, -1, 1,
true> NColAlignedVectorBlock;
115 typedef Eigen::Block<Eigen::Ref<MatrixXs> > MatrixBlock;
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_) +
124 NColAlignedVectorBlock dx = Jfirst.template rightCols<1>();
125 MatrixBlock Jdq = Jfirst.bottomLeftCorner(nv_, nv_);
128 pinocchio::dIntegrate(*pinocchio_.get(), x1.head(nq_), dx.topRows(nv_), Jdq, pinocchio::ARG1);
129 updateJdiff(Jdq, Jfirst.topLeftCorner(nv_, nv_),
false);
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_) +
")");
142 NColAlignedVectorBlock dx = Jsecond.template rightCols<1>();
143 MatrixBlock Jdq = Jsecond.bottomLeftCorner(nv_, nv_);
146 pinocchio::dIntegrate(*pinocchio_.get(), x0.head(nq_), dx.topRows(nv_), Jdq, pinocchio::ARG1);
147 updateJdiff(Jdq, Jsecond.topLeftCorner(nv_, nv_));
151 Jsecond.bottomRightCorner(nv_, nv_).diagonal().array() = (Scalar)1;
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_) +
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_) +
")");
167 NColAlignedVectorBlock dx1 = Jfirst.template rightCols<1>();
168 MatrixBlock Jdq1 = Jfirst.bottomLeftCorner(nv_, nv_);
171 pinocchio::dIntegrate(*pinocchio_.get(), x1.head(nq_), dx1.topRows(nv_), Jdq1, pinocchio::ARG1);
172 updateJdiff(Jdq1, Jfirst.topLeftCorner(nv_, nv_),
false);
175 Jfirst.bottomRightCorner(nv_, nv_).diagonal().array() = (Scalar)-1;
178 NColAlignedVectorBlock dx2 = Jsecond.template rightCols<1>();
179 MatrixBlock Jdq2 = Jsecond.bottomLeftCorner(nv_, nv_);
182 pinocchio::dIntegrate(*pinocchio_.get(), x0.head(nq_), dx2.topRows(nv_), Jdq2, pinocchio::ARG1);
183 updateJdiff(Jdq2, Jsecond.topLeftCorner(nv_, nv_));
187 Jsecond.bottomRightCorner(nv_, nv_).diagonal().array() = (Scalar)1;
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_) +
")");
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_) +
")");
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_) +
213 pinocchio::dIntegrate(*pinocchio_.get(), x.head(nq_), dx.head(nv_), Jfirst.topLeftCorner(nv_, nv_),
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_) +
")");
224 pinocchio::dIntegrate(*pinocchio_.get(), x.head(nq_), dx.head(nv_), Jsecond.topLeftCorner(nv_, nv_),
226 Jsecond.bottomRightCorner(nv_, nv_).diagonal().array() = (Scalar)1;
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_) +
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_) +
")");
241 pinocchio::dIntegrate(*pinocchio_.get(), x.head(nq_), dx.head(nv_), Jfirst.topLeftCorner(nv_, nv_),
243 Jfirst.bottomRightCorner(nv_, nv_).diagonal().array() = (Scalar)1;
247 pinocchio::dIntegrate(*pinocchio_.get(), x.head(nq_), dx.head(nv_), Jsecond.topLeftCorner(nv_, nv_),
249 Jsecond.bottomRightCorner(nv_, nv_).diagonal().array() = (Scalar)1;
253 template <
typename Scalar>
254 const boost::shared_ptr<pinocchio::ModelTpl<Scalar> >& StateMultibodyTpl<Scalar>::get_pinocchio()
const {
258 template <
typename Scalar>
259 void StateMultibodyTpl<Scalar>::updateJdiff(
const Eigen::Ref<const MatrixXs>& Jdq, Eigen::Ref<MatrixXs> Jd,
260 bool positive)
const {
262 Jd.diagonal() = Jdq.diagonal();
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();
271 Jd.diagonal() = -Jdq.diagonal();
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();
Definition: action-base.hxx:11