9 #include "crocoddyl/core/utils/exception.hpp" 12 template <
typename Scalar>
13 ActionModelUnicycleTpl<Scalar>::ActionModelUnicycleTpl()
14 : ActionModelAbstractTpl<Scalar>(boost::make_shared<StateVectorTpl<Scalar> >(3), 2, 5), dt_(0.1) {
15 cost_weights_ << 10., 1.;
18 template <
typename Scalar>
19 ActionModelUnicycleTpl<Scalar>::~ActionModelUnicycleTpl() {}
21 template <
typename Scalar>
22 void ActionModelUnicycleTpl<Scalar>::calc(
const boost::shared_ptr<ActionDataAbstractTpl<Scalar> >& data,
23 const Eigen::Ref<const typename MathBase::VectorXs>& x,
24 const Eigen::Ref<const typename MathBase::VectorXs>& u) {
25 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
26 throw_pretty(
"Invalid argument: " 27 <<
"x has wrong dimension (it should be " + std::to_string(state_->get_nx()) +
")");
29 if (static_cast<std::size_t>(u.size()) != nu_) {
30 throw_pretty(
"Invalid argument: " 31 <<
"u has wrong dimension (it should be " + std::to_string(nu_) +
")");
34 ActionDataUnicycleTpl<Scalar>* d =
static_cast<ActionDataUnicycleTpl<Scalar>*
>(data.get());
35 const Scalar& c = cos(x[2]);
36 const Scalar& s = sin(x[2]);
37 d->xnext << x[0] + c * u[0] * dt_, x[1] + s * u[0] * dt_, x[2] + u[1] * dt_;
38 d->r.template head<3>() = cost_weights_[0] * x;
39 d->r.template tail<2>() = cost_weights_[1] * u;
40 d->cost = 0.5 * d->r.transpose() * d->r;
43 template <
typename Scalar>
44 void ActionModelUnicycleTpl<Scalar>::calcDiff(
const boost::shared_ptr<ActionDataAbstractTpl<Scalar> >& data,
45 const Eigen::Ref<const typename MathBase::VectorXs>& x,
46 const Eigen::Ref<const typename MathBase::VectorXs>& u) {
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()) +
")");
51 if (static_cast<std::size_t>(u.size()) != nu_) {
52 throw_pretty(
"Invalid argument: " 53 <<
"u has wrong dimension (it should be " + std::to_string(nu_) +
")");
56 ActionDataUnicycleTpl<Scalar>* d =
static_cast<ActionDataUnicycleTpl<Scalar>*
>(data.get());
59 const Scalar& w_x = cost_weights_[0] * cost_weights_[0];
60 const Scalar& w_u = cost_weights_[1] * cost_weights_[1];
61 d->Lx = x.cwiseProduct(MathBase::VectorXs::Constant(state_->get_nx(), w_x));
62 d->Lu = u.cwiseProduct(MathBase::VectorXs::Constant(nu_, w_u));
63 d->Lxx.diagonal() << w_x, w_x, w_x;
64 d->Luu.diagonal() << w_u, w_u;
67 const Scalar& c = cos(x[2]);
68 const Scalar& s = sin(x[2]);
69 d->Fx << 1., 0., -s * u[0] * dt_, 0., 1., c * u[0] * dt_, 0., 0., 1.;
70 d->Fu << c * dt_, 0., s * dt_, 0., 0., dt_;
73 template <
typename Scalar>
74 boost::shared_ptr<ActionDataAbstractTpl<Scalar> > ActionModelUnicycleTpl<Scalar>::createData() {
75 return boost::make_shared<ActionDataUnicycleTpl<Scalar> >(
this);
78 template <
typename Scalar>
79 const typename MathBaseTpl<Scalar>::Vector2s& ActionModelUnicycleTpl<Scalar>::get_cost_weights()
const {
83 template <
typename Scalar>
84 void ActionModelUnicycleTpl<Scalar>::set_cost_weights(
const typename MathBase::Vector2s& weights) {
85 cost_weights_ = weights;
Definition: action-base.hxx:11