9 #include <pinocchio/algorithm/joint-configuration.hpp>
11 #include "crocoddyl/core/utils/exception.hpp"
15 using namespace crocoddyl;
17 template <
typename Scalar>
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()),
24 ntau_(lpf_joint_ids.size()),
25 y0_(
VectorXs::Zero(model->nq + model->nv + lpf_joint_ids.size())) {
38 const std::size_t nq0 = model->joints[1].nq();
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;
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) ");
60 -pinocchio_->effortLimit[model->idx_vs[lpf_joint_ids[i]]];
62 pinocchio_->effortLimit[model->idx_vs[lpf_joint_ids[i]]];
64 Base::update_has_limits();
66 y0_.head(nq_) = pinocchio::neutral(*pinocchio_.get());
67 y0_.tail(nv_ +
ntau_) = VectorXs::Zero(nv_ +
ntau_);
70 template <
typename Scalar>
73 template <
typename Scalar>
78 template <
typename Scalar>
83 template <
typename Scalar>
88 template <
typename Scalar>
93 template <
typename Scalar>
95 VectorXs yrand = VectorXs::Random(ny_);
96 yrand.head(nq_) = pinocchio::randomConfiguration(*pinocchio_.get());
100 template <
typename Scalar>
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_) +
")");
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_) +
")");
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_) +
")");
120 pinocchio::difference(*pinocchio_.get(), y0.head(nq_), y1.head(nq_),
122 dyout.segment(nv_, nv_) = y1.segment(nq_, nv_) - y0.segment(nq_, nv_);
123 dyout.tail(ntau_) = y1.tail(ntau_) - y0.tail(ntau_);
126 template <
typename Scalar>
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_) +
")");
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_) +
")");
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_) +
")");
146 pinocchio::integrate(*pinocchio_.get(),
y.head(nq_), dy.head(nv_),
148 yout.segment(nq_, nv_) =
y.segment(nq_, nv_) + dy.segment(nv_, nv_);
149 yout.tail(ntau_) =
y.tail(ntau_) + dy.tail(ntau_);
152 template <
typename Scalar>
154 const Eigen::Ref<const VectorXs>& y1,
155 Eigen::Ref<MatrixXs> Jfirst,
156 Eigen::Ref<MatrixXs> Jsecond,
157 const Jcomponent firstsecond)
const {
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_) +
")");
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_) +
")");
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_) +
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_) +
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;
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_) +
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_) +
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;
223 template <
typename Scalar>
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 {
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_) +
245 pinocchio::dIntegrate(*pinocchio_.get(),
y.head(nq_), dy.head(nv_),
246 Jfirst.topLeftCorner(nv_, nv_), pinocchio::ARG0,
248 Jfirst.block(nv_, nv_, nv_, nv_).diagonal().array() = (Scalar)1;
249 Jfirst.bottomRightCorner(ntau_, ntau_).diagonal().array() = (Scalar)1;
252 pinocchio::dIntegrate(*pinocchio_.get(),
y.head(nq_), dy.head(nv_),
253 Jfirst.topLeftCorner(nv_, nv_), pinocchio::ARG0,
255 Jfirst.block(nv_, nv_, nv_, nv_).diagonal().array() += (Scalar)1;
256 Jfirst.bottomRightCorner(ntau_, ntau_).diagonal().array() += (Scalar)1;
259 pinocchio::dIntegrate(*pinocchio_.get(),
y.head(nq_), dy.head(nv_),
260 Jfirst.topLeftCorner(nv_, nv_), pinocchio::ARG0,
262 Jfirst.block(nv_, nv_, nv_, nv_).diagonal().array() -= (Scalar)1;
263 Jfirst.bottomRightCorner(ntau_, ntau_).diagonal().array() -= (Scalar)1;
267 "Invalid argument: allowed operators: setto, addto, rmfrom");
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_) +
281 pinocchio::dIntegrate(*pinocchio_.get(),
y.head(nq_), dy.head(nv_),
282 Jsecond.topLeftCorner(nv_, nv_), pinocchio::ARG1,
284 Jsecond.block(nv_, nv_, nv_, nv_).diagonal().array() = (Scalar)1;
285 Jsecond.bottomRightCorner(ntau_, ntau_).diagonal().array() = (Scalar)1;
288 pinocchio::dIntegrate(*pinocchio_.get(),
y.head(nq_), dy.head(nv_),
289 Jsecond.topLeftCorner(nv_, nv_), pinocchio::ARG1,
291 Jsecond.block(nv_, nv_, nv_, nv_).diagonal().array() += (Scalar)1;
292 Jsecond.bottomRightCorner(ntau_, ntau_).diagonal().array() += (Scalar)1;
295 pinocchio::dIntegrate(*pinocchio_.get(),
y.head(nq_), dy.head(nv_),
296 Jsecond.topLeftCorner(nv_, nv_), pinocchio::ARG1,
298 Jsecond.block(nv_, nv_, nv_, nv_).diagonal().array() -= (Scalar)1;
299 Jsecond.bottomRightCorner(ntau_, ntau_).diagonal().array() -= (Scalar)1;
303 "Invalid argument: allowed operators: setto, addto, rmfrom");
309 template <
typename Scalar>
311 const Eigen::Ref<const VectorXs>&
y,
const Eigen::Ref<const VectorXs>& dy,
312 Eigen::Ref<MatrixXs> Jin,
const Jcomponent firstsecond)
const {
314 is_a_Jcomponent(firstsecond),
315 (
"firstsecond must be one of the Jcomponent {both, first, second}"));
317 switch (firstsecond) {
322 pinocchio::dIntegrateTransport(*pinocchio_.get(),
y.head(nq_),
323 dy.head(nv_), Jin.topRows(nv_),
330 pinocchio::dIntegrateTransport(*pinocchio_.get(),
y.head(nq_),
331 dy.head(nv_), Jin.topRows(nv_),
336 "Invalid argument: firstsecond must be either first or second. both "
337 "not supported for this operation.");
342 template <
typename Scalar>
343 const boost::shared_ptr<pinocchio::ModelTpl<Scalar> >&
std::size_t ntau_
Definition: state.hpp:72
const std::size_t & get_ndy() const
Definition: state.hxx:84
StateLPFTpl(boost::shared_ptr< pinocchio::ModelTpl< Scalar > > model, std::vector< int > lpf_joint_ids)
Definition: state.hxx:18
virtual ~StateLPFTpl()
Definition: state.hxx:71
virtual VectorXs zero() const
Definition: state.hxx:89
virtual void diff(const Eigen::Ref< const VectorXs > &y0, const Eigen::Ref< const VectorXs > &y1, Eigen::Ref< VectorXs > dyout) const
Definition: state.hxx:101
virtual VectorXs rand() const
Definition: state.hxx:94
const std::size_t & get_ny() const
Definition: state.hxx:79
StateAbstractTpl< Scalar > Base
Definition: state.hpp:25
virtual void integrate(const Eigen::Ref< const VectorXs > &y, const Eigen::Ref< const VectorXs > &dy, Eigen::Ref< VectorXs > yout) const
Definition: state.hxx:127
std::size_t ny_
Definition: state.hpp:73
MathBase::VectorXs VectorXs
Definition: state.hpp:26
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
std::size_t ndy_
Definition: state.hpp:74
const std::size_t & get_ntau() const
Definition: state.hxx:74
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
const boost::shared_ptr< pinocchio::ModelTpl< Scalar > > & get_pinocchio() const
Definition: state.hxx:344
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
Definition: activation-quad-ref.hpp:19