Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/crocoddyl/core/actions/unicycle.hxx	Lines:	58	76	76.3 %
Date:	2024-02-13 11:12:33	Branches:	59	224	26.3 %


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2022, LAAS-CNRS, University of Edinburgh,
//                          Heriot-Watt University
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#include "crocoddyl/core/utils/exception.hpp"

namespace crocoddyl {
template <typename Scalar>
ActionModelUnicycleTpl<Scalar>::ActionModelUnicycleTpl()
    : ActionModelAbstractTpl<Scalar>(
          boost::make_shared<StateVectorTpl<Scalar> >(3), 2, 5),
      dt_(Scalar(0.1)) {
  cost_weights_ << Scalar(10.), Scalar(1.);
}

template <typename Scalar>
ActionModelUnicycleTpl<Scalar>::~ActionModelUnicycleTpl() {}

template <typename Scalar>
void ActionModelUnicycleTpl<Scalar>::calc(
    const boost::shared_ptr<ActionDataAbstractTpl<Scalar> >& data,
    const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
  if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
    throw_pretty("Invalid argument: "
                 << "x has wrong dimension (it should be " +
                        std::to_string(state_->get_nx()) + ")");
  }
  if (static_cast<std::size_t>(u.size()) != nu_) {
    throw_pretty("Invalid argument: "
                 << "u has wrong dimension (it should be " +
                        std::to_string(nu_) + ")");
  }
  Data* d = static_cast<Data*>(data.get());

  const Scalar c = cos(x[2]);
  const Scalar s = sin(x[2]);
  d->xnext << x[0] + c * u[0] * dt_, x[1] + s * u[0] * dt_, x[2] + u[1] * dt_;
  d->r.template head<3>() = cost_weights_[0] * x;
  d->r.template tail<2>() = cost_weights_[1] * u;
  d->cost = Scalar(0.5) * d->r.dot(d->r);
}

template <typename Scalar>
void ActionModelUnicycleTpl<Scalar>::calc(
    const boost::shared_ptr<ActionDataAbstractTpl<Scalar> >& data,
    const Eigen::Ref<const VectorXs>& x) {
  if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
    throw_pretty("Invalid argument: "
                 << "x has wrong dimension (it should be " +
                        std::to_string(state_->get_nx()) + ")");
  }
  Data* d = static_cast<Data*>(data.get());

  d->xnext = x;
  d->r.template head<3>() = cost_weights_[0] * x;
  d->r.template tail<2>().setZero();
  d->cost = Scalar(0.5) * d->r.template head<3>().dot(d->r.template head<3>());
}

template <typename Scalar>
void ActionModelUnicycleTpl<Scalar>::calcDiff(
    const boost::shared_ptr<ActionDataAbstractTpl<Scalar> >& data,
    const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
  if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
    throw_pretty("Invalid argument: "
                 << "x has wrong dimension (it should be " +
                        std::to_string(state_->get_nx()) + ")");
  }
  if (static_cast<std::size_t>(u.size()) != nu_) {
    throw_pretty("Invalid argument: "
                 << "u has wrong dimension (it should be " +
                        std::to_string(nu_) + ")");
  }
  Data* d = static_cast<Data*>(data.get());

  const Scalar c = cos(x[2]);
  const Scalar s = sin(x[2]);
  const Scalar w_x = cost_weights_[0] * cost_weights_[0];
  const Scalar w_u = cost_weights_[1] * cost_weights_[1];
  d->Lx = x * w_x;
  d->Lu = u * w_u;
  d->Lxx.diagonal().setConstant(w_x);
  d->Luu.diagonal().setConstant(w_u);
  d->Fx(0, 2) = -s * u[0] * dt_;
  d->Fx(1, 2) = c * u[0] * dt_;
  d->Fu(0, 0) = c * dt_;
  d->Fu(1, 0) = s * dt_;
  d->Fu(2, 1) = dt_;
}

template <typename Scalar>
void ActionModelUnicycleTpl<Scalar>::calcDiff(
    const boost::shared_ptr<ActionDataAbstractTpl<Scalar> >& data,
    const Eigen::Ref<const VectorXs>& x) {
  if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
    throw_pretty("Invalid argument: "
                 << "x has wrong dimension (it should be " +
                        std::to_string(state_->get_nx()) + ")");
  }
  Data* d = static_cast<Data*>(data.get());

  const Scalar w_x = cost_weights_[0] * cost_weights_[0];
  d->Lx = x * w_x;
  d->Lxx.diagonal().setConstant(w_x);
}

template <typename Scalar>
boost::shared_ptr<ActionDataAbstractTpl<Scalar> >
ActionModelUnicycleTpl<Scalar>::createData() {
  return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
}

template <typename Scalar>
bool ActionModelUnicycleTpl<Scalar>::checkData(
    const boost::shared_ptr<ActionDataAbstract>& data) {
  boost::shared_ptr<Data> d = boost::dynamic_pointer_cast<Data>(data);
  if (d != NULL) {
    return true;
  } else {
    return false;
  }
}

template <typename Scalar>
void ActionModelUnicycleTpl<Scalar>::print(std::ostream& os) const {
  os << "ActionModelUnicycle {dt=" << dt_ << "}";
}

template <typename Scalar>
const typename MathBaseTpl<Scalar>::Vector2s&
ActionModelUnicycleTpl<Scalar>::get_cost_weights() const {
  return cost_weights_;
}

template <typename Scalar>
void ActionModelUnicycleTpl<Scalar>::set_cost_weights(
    const typename MathBase::Vector2s& weights) {
  cost_weights_ = weights;
}

template <typename Scalar>
Scalar ActionModelUnicycleTpl<Scalar>::get_dt() const {
  return dt_;
}

template <typename Scalar>
void ActionModelUnicycleTpl<Scalar>::set_dt(const Scalar dt) {
  if (dt <= 0)
    throw_pretty("Invalid argument: dt should be strictly positive.");
  dt_ = dt;
}

}  // namespace crocoddyl


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2019-2022, LAAS-CNRS, University of Edinburgh,
5			// Heriot-Watt University
6			// Copyright note valid unless otherwise stated in individual files.
7			// All rights reserved.
8			///////////////////////////////////////////////////////////////////////////////
9
10			#include "crocoddyl/core/utils/exception.hpp"
11
12			namespace crocoddyl {
13			template <typename Scalar>
14		36	ActionModelUnicycleTpl<Scalar>::ActionModelUnicycleTpl()
15			: ActionModelAbstractTpl<Scalar>(
16			boost::make_shared<StateVectorTpl<Scalar> >(3), 2, 5),
17	✓✗✓✗ ✓✗	36	dt_(Scalar(0.1)) {
18	✓✗✓✗	36	cost_weights_ << Scalar(10.), Scalar(1.);
19		36	}
20
21			template <typename Scalar>
22		76	ActionModelUnicycleTpl<Scalar>::~ActionModelUnicycleTpl() {}
23
24			template <typename Scalar>
25		2125	void ActionModelUnicycleTpl<Scalar>::calc(
26			const boost::shared_ptr<ActionDataAbstractTpl<Scalar> >& data,
27			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
28	✗✓	2125	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
29			throw_pretty("Invalid argument: "
30			<< "x has wrong dimension (it should be " +
31			std::to_string(state_->get_nx()) + ")");
32			}
33	✗✓	2125	if (static_cast<std::size_t>(u.size()) != nu_) {
34			throw_pretty("Invalid argument: "
35			<< "u has wrong dimension (it should be " +
36			std::to_string(nu_) + ")");
37			}
38		2125	Data* d = static_cast<Data*>(data.get());
39
40		2125	const Scalar c = cos(x[2]);
41		2125	const Scalar s = sin(x[2]);
42	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	2125	d->xnext << x[0] + c * u[0] * dt_, x[1] + s * u[0] * dt_, x[2] + u[1] * dt_;
43	✓✗✓✗	2125	d->r.template head<3>() = cost_weights_[0] * x;
44	✓✗✓✗	2125	d->r.template tail<2>() = cost_weights_[1] * u;
45		2125	d->cost = Scalar(0.5) * d->r.dot(d->r);
46		2125	}
47
48			template <typename Scalar>
49		187	void ActionModelUnicycleTpl<Scalar>::calc(
50			const boost::shared_ptr<ActionDataAbstractTpl<Scalar> >& data,
51			const Eigen::Ref<const VectorXs>& x) {
52	✗✓	187	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
53			throw_pretty("Invalid argument: "
54			<< "x has wrong dimension (it should be " +
55			std::to_string(state_->get_nx()) + ")");
56			}
57		187	Data* d = static_cast<Data*>(data.get());
58
59		187	d->xnext = x;
60	✓✗✓✗	187	d->r.template head<3>() = cost_weights_[0] * x;
61	✓✗	187	d->r.template tail<2>().setZero();
62	✓✗✓✗	187	d->cost = Scalar(0.5) * d->r.template head<3>().dot(d->r.template head<3>());
63		187	}
64
65			template <typename Scalar>
66		1245	void ActionModelUnicycleTpl<Scalar>::calcDiff(
67			const boost::shared_ptr<ActionDataAbstractTpl<Scalar> >& data,
68			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
69	✓✗✗✓	1245	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
70			throw_pretty("Invalid argument: "
71			<< "x has wrong dimension (it should be " +
72			std::to_string(state_->get_nx()) + ")");
73			}
74	✓✗✗✓	1245	if (static_cast<std::size_t>(u.size()) != nu_) {
75			throw_pretty("Invalid argument: "
76			<< "u has wrong dimension (it should be " +
77			std::to_string(nu_) + ")");
78			}
79		1245	Data* d = static_cast<Data*>(data.get());
80
81	✓✗	1245	const Scalar c = cos(x[2]);
82	✓✗	1245	const Scalar s = sin(x[2]);
83	✓✗✓✗	1245	const Scalar w_x = cost_weights_[0] * cost_weights_[0];
84	✓✗✓✗	1245	const Scalar w_u = cost_weights_[1] * cost_weights_[1];
85	✓✗✓✗	1245	d->Lx = x * w_x;
86	✓✗✓✗	1245	d->Lu = u * w_u;
87	✓✗✓✗	1245	d->Lxx.diagonal().setConstant(w_x);
88	✓✗✓✗	1245	d->Luu.diagonal().setConstant(w_u);
89	✓✗✓✗	1245	d->Fx(0, 2) = -s * u[0] * dt_;
90	✓✗✓✗	1245	d->Fx(1, 2) = c * u[0] * dt_;
91	✓✗	1245	d->Fu(0, 0) = c * dt_;
92	✓✗	1245	d->Fu(1, 0) = s * dt_;
93	✓✗	1245	d->Fu(2, 1) = dt_;
94		1245	}
95
96			template <typename Scalar>
97		113	void ActionModelUnicycleTpl<Scalar>::calcDiff(
98			const boost::shared_ptr<ActionDataAbstractTpl<Scalar> >& data,
99			const Eigen::Ref<const VectorXs>& x) {
100	✓✗✗✓	113	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
101			throw_pretty("Invalid argument: "
102			<< "x has wrong dimension (it should be " +
103			std::to_string(state_->get_nx()) + ")");
104			}
105		113	Data* d = static_cast<Data*>(data.get());
106
107	✓✗✓✗	113	const Scalar w_x = cost_weights_[0] * cost_weights_[0];
108	✓✗✓✗	113	d->Lx = x * w_x;
109	✓✗✓✗	113	d->Lxx.diagonal().setConstant(w_x);
110		113	}
111
112			template <typename Scalar>
113			boost::shared_ptr<ActionDataAbstractTpl<Scalar> >
114		983	ActionModelUnicycleTpl<Scalar>::createData() {
115	✓✗	983	return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
116			}
117
118			template <typename Scalar>
119		64	bool ActionModelUnicycleTpl<Scalar>::checkData(
120			const boost::shared_ptr<ActionDataAbstract>& data) {
121		128	boost::shared_ptr<Data> d = boost::dynamic_pointer_cast<Data>(data);
122	✓✗	64	if (d != NULL) {
123		64	return true;
124			} else {
125			return false;
126			}
127			}
128
129			template <typename Scalar>
130		22	void ActionModelUnicycleTpl<Scalar>::print(std::ostream& os) const {
131		22	os << "ActionModelUnicycle {dt=" << dt_ << "}";
132		22	}
133
134			template <typename Scalar>
135			const typename MathBaseTpl<Scalar>::Vector2s&
136			ActionModelUnicycleTpl<Scalar>::get_cost_weights() const {
137			return cost_weights_;
138			}
139
140			template <typename Scalar>
141			void ActionModelUnicycleTpl<Scalar>::set_cost_weights(
142			const typename MathBase::Vector2s& weights) {
143			cost_weights_ = weights;
144			}
145
146			template <typename Scalar>
147			Scalar ActionModelUnicycleTpl<Scalar>::get_dt() const {
148			return dt_;
149			}
150
151			template <typename Scalar>
152			void ActionModelUnicycleTpl<Scalar>::set_dt(const Scalar dt) {
153			if (dt <= 0)
154			throw_pretty("Invalid argument: dt should be strictly positive.");
155			dt_ = dt;
156			}
157
158			} // namespace crocoddyl