9 #include <pinocchio/algorithm/joint-configuration.hpp> 11 #include "crocoddyl/core/utils/exception.hpp" 12 #include "statelpf.hpp" 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),
22 y0_(VectorXs::Zero(model->nq + model->nv + nu)) {
36 const std::size_t nq0 = model->joints[1].nq();
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();
48 y0_.head(nq_) = pinocchio::neutral(*pinocchio_.get());
49 y0_.tail(nv_ + nu) = VectorXs::Zero(nv_ + nu);
52 template <
typename Scalar>
53 StateLPFTpl<Scalar>::~StateLPFTpl() {}
55 template <
typename Scalar>
56 const std::size_t& StateLPFTpl<Scalar>::get_nw()
const {
60 template <
typename Scalar>
61 const std::size_t& StateLPFTpl<Scalar>::get_ny()
const {
65 template <
typename Scalar>
66 const std::size_t& StateLPFTpl<Scalar>::get_ndy()
const {
70 template <
typename Scalar>
71 typename MathBaseTpl<Scalar>::VectorXs StateLPFTpl<Scalar>::zero()
const {
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());
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_) +
")");
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_) +
")");
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_) +
")");
102 pinocchio::difference(*pinocchio_.get(), y0.head(nq_), y1.head(nq_),
104 dyout.segment(nv_, nv_) = y1.segment(nq_, nv_) - y0.segment(nq_, nv_);
105 dyout.tail(nw_) = y1.tail(nw_) - y0.tail(nw_);
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_) +
")");
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_) +
")");
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_) +
")");
128 pinocchio::integrate(*pinocchio_.get(), y.head(nq_), dy.head(nv_),
130 yout.segment(nq_, nv_) = y.segment(nq_, nv_) + dy.segment(nv_, nv_);
131 yout.tail(nw_) = y.tail(nw_) + dy.tail(nw_);
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 {
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_) +
")");
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_) +
")");
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_) +
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_) +
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;
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_) +
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_) +
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;
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 {
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_) +
227 pinocchio::dIntegrate(*pinocchio_.get(), y.head(nq_), dy.head(nv_),
228 Jfirst.topLeftCorner(nv_, nv_), pinocchio::ARG0,
230 Jfirst.block(nv_, nv_, nv_, nv_).diagonal().array() = (Scalar)1;
231 Jfirst.bottomRightCorner(nw_, nw_).diagonal().array() = (Scalar)1;
234 pinocchio::dIntegrate(*pinocchio_.get(), y.head(nq_), dy.head(nv_),
235 Jfirst.topLeftCorner(nv_, nv_), pinocchio::ARG0,
237 Jfirst.block(nv_, nv_, nv_, nv_).diagonal().array() += (Scalar)1;
238 Jfirst.bottomRightCorner(nw_, nw_).diagonal().array() += (Scalar)1;
241 pinocchio::dIntegrate(*pinocchio_.get(), y.head(nq_), dy.head(nv_),
242 Jfirst.topLeftCorner(nv_, nv_), pinocchio::ARG0,
244 Jfirst.block(nv_, nv_, nv_, nv_).diagonal().array() -= (Scalar)1;
245 Jfirst.bottomRightCorner(nw_, nw_).diagonal().array() -= (Scalar)1;
249 "Invalid argument: allowed operators: setto, addto, rmfrom");
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_) +
263 pinocchio::dIntegrate(*pinocchio_.get(), y.head(nq_), dy.head(nv_),
264 Jsecond.topLeftCorner(nv_, nv_), pinocchio::ARG1,
266 Jsecond.block(nv_, nv_, nv_, nv_).diagonal().array() = (Scalar)1;
267 Jsecond.bottomRightCorner(nw_, nw_).diagonal().array() = (Scalar)1;
270 pinocchio::dIntegrate(*pinocchio_.get(), y.head(nq_), dy.head(nv_),
271 Jsecond.topLeftCorner(nv_, nv_), pinocchio::ARG1,
273 Jsecond.block(nv_, nv_, nv_, nv_).diagonal().array() += (Scalar)1;
274 Jsecond.bottomRightCorner(nw_, nw_).diagonal().array() += (Scalar)1;
277 pinocchio::dIntegrate(*pinocchio_.get(), y.head(nq_), dy.head(nv_),
278 Jsecond.topLeftCorner(nv_, nv_), pinocchio::ARG1,
280 Jsecond.block(nv_, nv_, nv_, nv_).diagonal().array() -= (Scalar)1;
281 Jsecond.bottomRightCorner(nw_, nw_).diagonal().array() -= (Scalar)1;
285 "Invalid argument: allowed operators: setto, addto, rmfrom");
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 {
296 is_a_Jcomponent(firstsecond),
297 (
"firstsecond must be one of the Jcomponent {both, first, second}"));
299 switch (firstsecond) {
304 pinocchio::dIntegrateTransport(*pinocchio_.get(), y.head(nq_),
305 dy.head(nv_), Jin.topRows(nv_),
312 pinocchio::dIntegrateTransport(*pinocchio_.get(), y.head(nq_),
313 dy.head(nv_), Jin.topRows(nv_),
318 "Invalid argument: firstsecond must be either first or second. both " 319 "not supported for this operation.");
324 template <
typename Scalar>
325 const boost::shared_ptr<pinocchio::ModelTpl<Scalar> >&
326 StateLPFTpl<Scalar>::get_pinocchio()
const {
Definition: contact-force.hxx:11