Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/crocoddyl/core/integ-action-base.hxx	Lines:	29	49	59.2 %
Date:	2024-02-13 11:12:33	Branches:	18	78	23.1 %


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

#include <boost/core/demangle.hpp>
#include <iostream>
#include <typeinfo>

#include "crocoddyl/core/controls/poly-zero.hpp"
#include "crocoddyl/core/integ-action-base.hpp"
#include "crocoddyl/core/utils/exception.hpp"

namespace crocoddyl {

template <typename Scalar>
IntegratedActionModelAbstractTpl<Scalar>::IntegratedActionModelAbstractTpl(
    boost::shared_ptr<DifferentialActionModelAbstract> model,
    boost::shared_ptr<ControlParametrizationModelAbstract> control,
    const Scalar time_step, const bool with_cost_residual)
    : Base(model->get_state(), control->get_nu(), model->get_nr(),
           model->get_ng(), model->get_nh()),
      differential_(model),
      control_(control),
      time_step_(time_step),
      with_cost_residual_(with_cost_residual) {
  if (control->get_nw() != model->get_nu()) {
    throw_pretty("Invalid argument: "
                 << "control.nw (" + std::to_string(control->get_nw()) +
                        ") is not equal to model.nu (" +
                        std::to_string(model->get_nu()) + ")");
  }
  init();
}

template <typename Scalar>
IntegratedActionModelAbstractTpl<Scalar>::IntegratedActionModelAbstractTpl(
    boost::shared_ptr<DifferentialActionModelAbstract> model,
    const Scalar time_step, const bool with_cost_residual)
    : Base(model->get_state(), model->get_nu(), model->get_nr(),
           model->get_ng(), model->get_nh()),
      differential_(model),
      control_(
          new ControlParametrizationModelPolyZeroTpl<Scalar>(model->get_nu())),
      time_step_(time_step),
      with_cost_residual_(with_cost_residual) {
  init();
}

template <typename Scalar>
void IntegratedActionModelAbstractTpl<Scalar>::init() {
  time_step2_ = time_step_ * time_step_;
  VectorXs u_lb(nu_), u_ub(nu_);
  control_->convertBounds(differential_->get_u_lb(), differential_->get_u_ub(),
                          u_lb, u_ub);
  Base::set_u_lb(u_lb);
  Base::set_u_ub(u_ub);
  if (time_step_ < Scalar(0.)) {
    time_step_ = Scalar(1e-3);
    time_step2_ = time_step_ * time_step_;
    std::cerr << "Warning: dt should be positive, set to 1e-3" << std::endl;
  }
}

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

template <typename Scalar>
boost::shared_ptr<ActionDataAbstractTpl<Scalar> >
IntegratedActionModelAbstractTpl<Scalar>::createData() {
  if (control_->get_nu() > differential_->get_nu())
    std::cerr << "Warning: It is useless to use an Euler integrator with a "
                 "control parametrization larger than PolyZero"
              << std::endl;
  return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
}

template <typename Scalar>
std::size_t IntegratedActionModelAbstractTpl<Scalar>::get_ng() const {
  return differential_->get_ng();
}

template <typename Scalar>
std::size_t IntegratedActionModelAbstractTpl<Scalar>::get_nh() const {
  return differential_->get_nh();
}

template <typename Scalar>
const typename MathBaseTpl<Scalar>::VectorXs&
IntegratedActionModelAbstractTpl<Scalar>::get_g_lb() const {
  return differential_->get_g_lb();
}

template <typename Scalar>
const typename MathBaseTpl<Scalar>::VectorXs&
IntegratedActionModelAbstractTpl<Scalar>::get_g_ub() const {
  return differential_->get_g_ub();
}

template <typename Scalar>
const boost::shared_ptr<DifferentialActionModelAbstractTpl<Scalar> >&
IntegratedActionModelAbstractTpl<Scalar>::get_differential() const {
  return differential_;
}

template <typename Scalar>
const boost::shared_ptr<ControlParametrizationModelAbstractTpl<Scalar> >&
IntegratedActionModelAbstractTpl<Scalar>::get_control() const {
  return control_;
}

template <typename Scalar>
const Scalar IntegratedActionModelAbstractTpl<Scalar>::get_dt() const {
  return time_step_;
}

template <typename Scalar>
void IntegratedActionModelAbstractTpl<Scalar>::set_dt(const Scalar dt) {
  if (dt < 0.) {
    throw_pretty("Invalid argument: "
                 << "dt has positive value");
  }
  time_step_ = dt;
  time_step2_ = dt * dt;
}

template <typename Scalar>
void IntegratedActionModelAbstractTpl<Scalar>::set_differential(
    boost::shared_ptr<DifferentialActionModelAbstract> model) {
  if (control_->get_nw() != model->get_nu()) {
    throw_pretty("Invalid argument: "
                 << "control.nw (" + std::to_string(control_->get_nw()) +
                        ") is not equal to model.nu (" +
                        std::to_string(model->get_nu()) + ")");
  }

  nr_ = model->get_nr();
  state_ = model->get_state();
  differential_ = model;

  VectorXs p_lb(nu_), p_ub(nu_);
  control_->convertBounds(differential_->get_u_lb(), differential_->get_u_ub(),
                          p_lb, p_ub);
  Base::set_u_lb(p_lb);
  Base::set_u_ub(p_ub);
}

}  // namespace crocoddyl


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2021-2022, LAAS-CNRS, University of Edinburgh, University of
5			// Oxford,
6			// University of Trento, Heriot-Watt University
7			// Copyright note valid unless otherwise stated in individual files.
8			// All rights reserved.
9			///////////////////////////////////////////////////////////////////////////////
10
11			#include <boost/core/demangle.hpp>
12			#include <iostream>
13			#include <typeinfo>
14
15			#include "crocoddyl/core/controls/poly-zero.hpp"
16			#include "crocoddyl/core/integ-action-base.hpp"
17			#include "crocoddyl/core/utils/exception.hpp"
18
19			namespace crocoddyl {
20
21			template <typename Scalar>
22		682	IntegratedActionModelAbstractTpl<Scalar>::IntegratedActionModelAbstractTpl(
23			boost::shared_ptr<DifferentialActionModelAbstract> model,
24			boost::shared_ptr<ControlParametrizationModelAbstract> control,
25			const Scalar time_step, const bool with_cost_residual)
26			: Base(model->get_state(), control->get_nu(), model->get_nr(),
27		1364	model->get_ng(), model->get_nh()),
28			differential_(model),
29			control_(control),
30			time_step_(time_step),
31	✓✗	682	with_cost_residual_(with_cost_residual) {
32	✗✓	682	if (control->get_nw() != model->get_nu()) {
33			throw_pretty("Invalid argument: "
34			<< "control.nw (" + std::to_string(control->get_nw()) +
35			") is not equal to model.nu (" +
36			std::to_string(model->get_nu()) + ")");
37			}
38	✓✗	682	init();
39		682	}
40
41			template <typename Scalar>
42		362	IntegratedActionModelAbstractTpl<Scalar>::IntegratedActionModelAbstractTpl(
43			boost::shared_ptr<DifferentialActionModelAbstract> model,
44			const Scalar time_step, const bool with_cost_residual)
45			: Base(model->get_state(), model->get_nu(), model->get_nr(),
46		724	model->get_ng(), model->get_nh()),
47			differential_(model),
48			control_(
49	✓✗✓✗	362	new ControlParametrizationModelPolyZeroTpl<Scalar>(model->get_nu())),
50			time_step_(time_step),
51	✓✗✓✗	724	with_cost_residual_(with_cost_residual) {
52	✓✗	362	init();
53		362	}
54
55			template <typename Scalar>
56		1044	void IntegratedActionModelAbstractTpl<Scalar>::init() {
57		1044	time_step2_ = time_step_ * time_step_;
58	✓✗✓✗	2088	VectorXs u_lb(nu_), u_ub(nu_);
59	✓✗✓✗ ✓✗✓✗ ✓✗	1044	control_->convertBounds(differential_->get_u_lb(), differential_->get_u_ub(),
60			u_lb, u_ub);
61	✓✗	1044	Base::set_u_lb(u_lb);
62	✓✗	1044	Base::set_u_ub(u_ub);
63	✗✓	1044	if (time_step_ < Scalar(0.)) {
64			time_step_ = Scalar(1e-3);
65			time_step2_ = time_step_ * time_step_;
66			std::cerr << "Warning: dt should be positive, set to 1e-3" << std::endl;
67			}
68		1044	}
69
70			template <typename Scalar>
71		2096	IntegratedActionModelAbstractTpl<Scalar>::~IntegratedActionModelAbstractTpl() {}
72
73			template <typename Scalar>
74			boost::shared_ptr<ActionDataAbstractTpl<Scalar> >
75			IntegratedActionModelAbstractTpl<Scalar>::createData() {
76			if (control_->get_nu() > differential_->get_nu())
77			std::cerr << "Warning: It is useless to use an Euler integrator with a "
78			"control parametrization larger than PolyZero"
79			<< std::endl;
80			return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
81			}
82
83			template <typename Scalar>
84		111638	std::size_t IntegratedActionModelAbstractTpl<Scalar>::get_ng() const {
85		111638	return differential_->get_ng();
86			}
87
88			template <typename Scalar>
89		111638	std::size_t IntegratedActionModelAbstractTpl<Scalar>::get_nh() const {
90		111638	return differential_->get_nh();
91			}
92
93			template <typename Scalar>
94			const typename MathBaseTpl<Scalar>::VectorXs&
95			IntegratedActionModelAbstractTpl<Scalar>::get_g_lb() const {
96			return differential_->get_g_lb();
97			}
98
99			template <typename Scalar>
100			const typename MathBaseTpl<Scalar>::VectorXs&
101			IntegratedActionModelAbstractTpl<Scalar>::get_g_ub() const {
102			return differential_->get_g_ub();
103			}
104
105			template <typename Scalar>
106			const boost::shared_ptr<DifferentialActionModelAbstractTpl<Scalar> >&
107		98136	IntegratedActionModelAbstractTpl<Scalar>::get_differential() const {
108		98136	return differential_;
109			}
110
111			template <typename Scalar>
112			const boost::shared_ptr<ControlParametrizationModelAbstractTpl<Scalar> >&
113		253236	IntegratedActionModelAbstractTpl<Scalar>::get_control() const {
114		253236	return control_;
115			}
116
117			template <typename Scalar>
118			const Scalar IntegratedActionModelAbstractTpl<Scalar>::get_dt() const {
119			return time_step_;
120			}
121
122			template <typename Scalar>
123			void IntegratedActionModelAbstractTpl<Scalar>::set_dt(const Scalar dt) {
124			if (dt < 0.) {
125			throw_pretty("Invalid argument: "
126			<< "dt has positive value");
127			}
128			time_step_ = dt;
129			time_step2_ = dt * dt;
130			}
131
132			template <typename Scalar>
133			void IntegratedActionModelAbstractTpl<Scalar>::set_differential(
134			boost::shared_ptr<DifferentialActionModelAbstract> model) {
135			if (control_->get_nw() != model->get_nu()) {
136			throw_pretty("Invalid argument: "
137			<< "control.nw (" + std::to_string(control_->get_nw()) +
138			") is not equal to model.nu (" +
139			std::to_string(model->get_nu()) + ")");
140			}
141
142			nr_ = model->get_nr();
143			state_ = model->get_state();
144			differential_ = model;
145
146			VectorXs p_lb(nu_), p_ub(nu_);
147			control_->convertBounds(differential_->get_u_lb(), differential_->get_u_ub(),
148			p_lb, p_ub);
149			Base::set_u_lb(p_lb);
150			Base::set_u_ub(p_ub);
151			}
152
153			} // namespace crocoddyl