9 #include "crocoddyl/core/utils/exception.hpp" 13 template <
typename Scalar>
14 ActionModelAbstractTpl<Scalar>::ActionModelAbstractTpl(boost::shared_ptr<StateAbstractTpl<Scalar> > state,
15 const std::size_t& nu,
const std::size_t& nr)
19 unone_(MathBase::VectorXs::Zero(nu)),
20 u_lb_(MathBase::VectorXs::Constant(nu, -std::numeric_limits<Scalar>::infinity())),
21 u_ub_(MathBase::VectorXs::Constant(nu, std::numeric_limits<Scalar>::infinity())),
22 has_control_limits_(false) {}
24 template <
typename Scalar>
25 ActionModelAbstractTpl<Scalar>::~ActionModelAbstractTpl() {}
27 template <
typename Scalar>
28 void ActionModelAbstractTpl<Scalar>::calc(
const boost::shared_ptr<ActionDataAbstractTpl<Scalar> >& data,
29 const Eigen::Ref<const VectorXs>& x) {
30 calc(data, x, unone_);
33 template <
typename Scalar>
34 void ActionModelAbstractTpl<Scalar>::calcDiff(
const boost::shared_ptr<ActionDataAbstractTpl<Scalar> >& data,
35 const Eigen::Ref<const VectorXs>& x) {
36 calcDiff(data, x, unone_);
39 template <
typename Scalar>
40 void ActionModelAbstractTpl<Scalar>::quasiStatic(
const boost::shared_ptr<ActionDataAbstractTpl<Scalar> >& data,
41 Eigen::Ref<VectorXs> u,
const Eigen::Ref<const VectorXs>& x,
42 const std::size_t& maxiter,
const Scalar& tol) {
43 if (static_cast<std::size_t>(u.size()) != nu_) {
44 throw_pretty(
"Invalid argument: " 45 <<
"u has wrong dimension (it should be " + std::to_string(nu_) +
")");
47 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
48 throw_pretty(
"Invalid argument: " 49 <<
"x has wrong dimension (it should be " + std::to_string(state_->get_nx()) +
")");
52 const std::size_t& ndx = state_->get_ndx();
53 VectorXs dx = VectorXs::Zero(ndx);
57 VectorXs du = VectorXs::Zero(nu_);
58 for (std::size_t i = 0; i < maxiter; ++i) {
61 state_->diff(x, data->xnext, dx);
62 du = -pseudoInverse(data->Fu) * data->Fx * dx;
64 if (du.norm() <= tol) {
71 template <
typename Scalar>
72 typename MathBaseTpl<Scalar>::VectorXs ActionModelAbstractTpl<Scalar>::quasiStatic_x(
73 const boost::shared_ptr<ActionDataAbstract>& data,
const VectorXs& x,
const std::size_t& maxiter,
77 quasiStatic(data, u, x, maxiter, tol);
81 template <
typename Scalar>
82 boost::shared_ptr<ActionDataAbstractTpl<Scalar> > ActionModelAbstractTpl<Scalar>::createData() {
83 return boost::make_shared<ActionDataAbstractTpl<Scalar> >(
this);
86 template <
typename Scalar>
87 const std::size_t& ActionModelAbstractTpl<Scalar>::get_nu()
const {
91 template <
typename Scalar>
92 const std::size_t& ActionModelAbstractTpl<Scalar>::get_nr()
const {
96 template <
typename Scalar>
97 const boost::shared_ptr<StateAbstractTpl<Scalar> >& ActionModelAbstractTpl<Scalar>::get_state()
const {
101 template <
typename Scalar>
102 const typename MathBaseTpl<Scalar>::VectorXs& ActionModelAbstractTpl<Scalar>::get_u_lb()
const {
106 template <
typename Scalar>
107 const typename MathBaseTpl<Scalar>::VectorXs& ActionModelAbstractTpl<Scalar>::get_u_ub()
const {
111 template <
typename Scalar>
112 bool const& ActionModelAbstractTpl<Scalar>::get_has_control_limits()
const {
113 return has_control_limits_;
116 template <
typename Scalar>
117 void ActionModelAbstractTpl<Scalar>::set_u_lb(
const VectorXs& u_lb) {
118 if (static_cast<std::size_t>(u_lb.size()) != nu_) {
119 throw_pretty(
"Invalid argument: " 120 <<
"lower bound has wrong dimension (it should be " + std::to_string(nu_) +
")");
123 update_has_control_limits();
126 template <
typename Scalar>
127 void ActionModelAbstractTpl<Scalar>::set_u_ub(
const VectorXs& u_ub) {
128 if (static_cast<std::size_t>(u_ub.size()) != nu_) {
129 throw_pretty(
"Invalid argument: " 130 <<
"upper bound has wrong dimension (it should be " + std::to_string(nu_) +
")");
133 update_has_control_limits();
136 template <
typename Scalar>
137 void ActionModelAbstractTpl<Scalar>::update_has_control_limits() {
138 has_control_limits_ = isfinite(u_lb_.array()).any() && isfinite(u_ub_.array()).any();
Definition: action-base.hxx:11