unicycle.hxx
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2018-2020, LAAS-CNRS, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #include "crocoddyl/core/utils/exception.hpp"
10 
11 namespace crocoddyl {
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.;
16 }
17 
18 template <typename Scalar>
19 ActionModelUnicycleTpl<Scalar>::~ActionModelUnicycleTpl() {}
20 
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()) + ")");
28  }
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_) + ")");
32  }
33 
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;
41 }
42 
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()) + ")");
50  }
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_) + ")");
54  }
55 
56  ActionDataUnicycleTpl<Scalar>* d = static_cast<ActionDataUnicycleTpl<Scalar>*>(data.get());
57 
58  // Cost derivatives
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;
65 
66  // Dynamic derivatives
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_;
71 }
72 
73 template <typename Scalar>
74 boost::shared_ptr<ActionDataAbstractTpl<Scalar> > ActionModelUnicycleTpl<Scalar>::createData() {
75  return boost::make_shared<ActionDataUnicycleTpl<Scalar> >(this);
76 }
77 
78 template <typename Scalar>
79 const typename MathBaseTpl<Scalar>::Vector2s& ActionModelUnicycleTpl<Scalar>::get_cost_weights() const {
80  return cost_weights_;
81 }
82 
83 template <typename Scalar>
84 void ActionModelUnicycleTpl<Scalar>::set_cost_weights(const typename MathBase::Vector2s& weights) {
85  cost_weights_ = weights;
86 }
87 
88 } // namespace crocoddyl
Definition: action-base.hxx:11