Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/crocoddyl/core/numdiff/actuation.hxx	Lines:	75	85	88.2 %
Date:	2024-02-13 11:12:33	Branches:	40	234	17.1 %


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

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

namespace crocoddyl {

template <typename Scalar>
ActuationModelNumDiffTpl<Scalar>::ActuationModelNumDiffTpl(
    boost::shared_ptr<Base> model)
    : Base(model->get_state(), model->get_nu()),
      model_(model),
      e_jac_(std::sqrt(2.0 * std::numeric_limits<Scalar>::epsilon())) {}

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

template <typename Scalar>
void ActuationModelNumDiffTpl<Scalar>::calc(
    const boost::shared_ptr<ActuationDataAbstract>& data,
    const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
  if (static_cast<std::size_t>(x.size()) != model_->get_state()->get_nx()) {
    throw_pretty("Invalid argument: "
                 << "x has wrong dimension (it should be " +
                        std::to_string(model_->get_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());
  model_->calc(d->data_0, x, u);
  data->tau = d->data_0->tau;
}

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

template <typename Scalar>
void ActuationModelNumDiffTpl<Scalar>::calcDiff(
    const boost::shared_ptr<ActuationDataAbstract>& data,
    const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
  if (static_cast<std::size_t>(x.size()) != model_->get_state()->get_nx()) {
    throw_pretty("Invalid argument: "
                 << "x has wrong dimension (it should be " +
                        std::to_string(model_->get_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 VectorXs& tau0 = d->data_0->tau;
  d->du.setZero();

  // Computing the d actuation(x,u) / dx
  model_->get_state()->diff(model_->get_state()->zero(), x, d->dx);
  d->x_norm = d->dx.norm();
  d->dx.setZero();
  d->xh_jac = e_jac_ * std::max(1., d->x_norm);
  for (std::size_t ix = 0; ix < model_->get_state()->get_ndx(); ++ix) {
    d->dx(ix) = d->xh_jac;
    model_->get_state()->integrate(x, d->dx, d->xp);
    model_->calc(d->data_x[ix], d->xp, u);
    d->dtau_dx.col(ix) = (d->data_x[ix]->tau - tau0) / d->xh_jac;
    d->dx(ix) = 0.;
  }

  // Computing the d actuation(x,u) / du
  d->uh_jac = e_jac_ * std::max(1., u.norm());
  for (unsigned iu = 0; iu < model_->get_nu(); ++iu) {
    d->du(iu) = d->uh_jac;
    model_->calc(d->data_u[iu], x, u + d->du);
    d->dtau_du.col(iu) = (d->data_u[iu]->tau - tau0) / d->uh_jac;
    d->du(iu) = 0.;
  }
}

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

  // Computing the d actuation(x,u) / dx
  model_->get_state()->diff(model_->get_state()->zero(), x, d->dx);
  d->x_norm = d->dx.norm();
  d->dx.setZero();
  d->xh_jac = e_jac_ * std::max(1., d->x_norm);
  for (std::size_t ix = 0; ix < model_->get_state()->get_ndx(); ++ix) {
    d->dx(ix) = d->xh_jac;
    model_->get_state()->integrate(x, d->dx, d->xp);
    model_->calc(d->data_x[ix], d->xp);
    d->dtau_dx.col(ix) = (d->data_x[ix]->tau - tau0) / d->xh_jac;
    d->dx(ix) = 0.;
  }
}

template <typename Scalar>
void ActuationModelNumDiffTpl<Scalar>::commands(
    const boost::shared_ptr<ActuationDataAbstract>& data,
    const Eigen::Ref<const VectorXs>& x,
    const Eigen::Ref<const VectorXs>& tau) {
  if (static_cast<std::size_t>(x.size()) != model_->get_state()->get_nx()) {
    throw_pretty("Invalid argument: "
                 << "x has wrong dimension (it should be " +
                        std::to_string(model_->get_state()->get_nx()) + ")");
  }
  if (static_cast<std::size_t>(tau.size()) != model_->get_state()->get_nv()) {
    throw_pretty("Invalid argument: "
                 << "tau has wrong dimension (it should be " +
                        std::to_string(model_->get_state()->get_nv()) + ")");
  }
  Data* d = static_cast<Data*>(data.get());
  model_->commands(d->data_0, x, tau);
  data->u = d->data_0->u;
}

template <typename Scalar>
void ActuationModelNumDiffTpl<Scalar>::torqueTransform(
    const boost::shared_ptr<ActuationDataAbstract>& data,
    const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
  if (static_cast<std::size_t>(x.size()) != model_->get_state()->get_nx()) {
    throw_pretty("Invalid argument: "
                 << "x has wrong dimension (it should be " +
                        std::to_string(model_->get_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());
  model_->torqueTransform(d->data_0, x, u);
  d->Mtau = d->data_0->Mtau;
}

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

template <typename Scalar>
const boost::shared_ptr<ActuationModelAbstractTpl<Scalar> >&
ActuationModelNumDiffTpl<Scalar>::get_model() const {
  return model_;
}

template <typename Scalar>
const Scalar ActuationModelNumDiffTpl<Scalar>::get_disturbance() const {
  return e_jac_;
}

template <typename Scalar>
void ActuationModelNumDiffTpl<Scalar>::set_disturbance(
    const Scalar disturbance) {
  if (disturbance < 0.) {
    throw_pretty("Invalid argument: "
                 << "Disturbance constant is positive");
  }
  e_jac_ = disturbance;
}

}  // namespace crocoddyl


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2019-2024, University of Edinburgh, LAAS-CNRS
5			// Copyright note valid unless otherwise stated in individual files.
6			// All rights reserved.
7			///////////////////////////////////////////////////////////////////////////////
8
9			#include "crocoddyl/core/numdiff/actuation.hpp"
10			#include "crocoddyl/core/utils/exception.hpp"
11
12			namespace crocoddyl {
13
14			template <typename Scalar>
15		72	ActuationModelNumDiffTpl<Scalar>::ActuationModelNumDiffTpl(
16			boost::shared_ptr<Base> model)
17			: Base(model->get_state(), model->get_nu()),
18			model_(model),
19	✓✗	72	e_jac_(std::sqrt(2.0 * std::numeric_limits<Scalar>::epsilon())) {}
20
21			template <typename Scalar>
22		144	ActuationModelNumDiffTpl<Scalar>::~ActuationModelNumDiffTpl() {}
23
24			template <typename Scalar>
25		36	void ActuationModelNumDiffTpl<Scalar>::calc(
26			const boost::shared_ptr<ActuationDataAbstract>& data,
27			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
28	✗✓	36	if (static_cast<std::size_t>(x.size()) != model_->get_state()->get_nx()) {
29			throw_pretty("Invalid argument: "
30			<< "x has wrong dimension (it should be " +
31			std::to_string(model_->get_state()->get_nx()) + ")");
32			}
33	✗✓	36	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		36	Data* d = static_cast<Data*>(data.get());
39		36	model_->calc(d->data_0, x, u);
40		36	data->tau = d->data_0->tau;
41		36	}
42
43			template <typename Scalar>
44		18	void ActuationModelNumDiffTpl<Scalar>::calc(
45			const boost::shared_ptr<ActuationDataAbstract>& data,
46			const Eigen::Ref<const VectorXs>& x) {
47	✗✓	18	if (static_cast<std::size_t>(x.size()) != model_->get_state()->get_nx()) {
48			throw_pretty("Invalid argument: "
49			<< "x has wrong dimension (it should be " +
50			std::to_string(model_->get_state()->get_nx()) + ")");
51			}
52		18	Data* d = static_cast<Data*>(data.get());
53		18	model_->calc(d->data_0, x);
54		18	data->tau = d->data_0->tau;
55		18	}
56
57			template <typename Scalar>
58		36	void ActuationModelNumDiffTpl<Scalar>::calcDiff(
59			const boost::shared_ptr<ActuationDataAbstract>& data,
60			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
61	✗✓	36	if (static_cast<std::size_t>(x.size()) != model_->get_state()->get_nx()) {
62			throw_pretty("Invalid argument: "
63			<< "x has wrong dimension (it should be " +
64			std::to_string(model_->get_state()->get_nx()) + ")");
65			}
66	✗✓	36	if (static_cast<std::size_t>(u.size()) != nu_) {
67			throw_pretty("Invalid argument: "
68			<< "u has wrong dimension (it should be " +
69			std::to_string(nu_) + ")");
70			}
71		36	Data* d = static_cast<Data*>(data.get());
72		36	const VectorXs& tau0 = d->data_0->tau;
73		36	d->du.setZero();
74
75			// Computing the d actuation(x,u) / dx
76	✓✗✓✗ ✓✗	36	model_->get_state()->diff(model_->get_state()->zero(), x, d->dx);
77		36	d->x_norm = d->dx.norm();
78		36	d->dx.setZero();
79		36	d->xh_jac = e_jac_ * std::max(1., d->x_norm);
80	✓✓	1468	for (std::size_t ix = 0; ix < model_->get_state()->get_ndx(); ++ix) {
81		1432	d->dx(ix) = d->xh_jac;
82	✓✗✓✗	1432	model_->get_state()->integrate(x, d->dx, d->xp);
83	✓✗	1432	model_->calc(d->data_x[ix], d->xp, u);
84	✓✗✓✗ ✓✗	1432	d->dtau_dx.col(ix) = (d->data_x[ix]->tau - tau0) / d->xh_jac;
85		1432	d->dx(ix) = 0.;
86			}
87
88			// Computing the d actuation(x,u) / du
89		36	d->uh_jac = e_jac_ * std::max(1., u.norm());
90	✓✓	708	for (unsigned iu = 0; iu < model_->get_nu(); ++iu) {
91		672	d->du(iu) = d->uh_jac;
92	✓✗✓✗	672	model_->calc(d->data_u[iu], x, u + d->du);
93	✓✗✓✗ ✓✗	672	d->dtau_du.col(iu) = (d->data_u[iu]->tau - tau0) / d->uh_jac;
94		672	d->du(iu) = 0.;
95			}
96		36	}
97
98			template <typename Scalar>
99		18	void ActuationModelNumDiffTpl<Scalar>::calcDiff(
100			const boost::shared_ptr<ActuationDataAbstract>& data,
101			const Eigen::Ref<const VectorXs>& x) {
102	✗✓	18	if (static_cast<std::size_t>(x.size()) != model_->get_state()->get_nx()) {
103			throw_pretty("Invalid argument: "
104			<< "x has wrong dimension (it should be " +
105			std::to_string(model_->get_state()->get_nx()) + ")");
106			}
107		18	Data* d = static_cast<Data*>(data.get());
108		18	const VectorXs& tau0 = d->data_0->tau;
109		18	d->dx.setZero();
110
111			// Computing the d actuation(x,u) / dx
112	✓✗✓✗ ✓✗	18	model_->get_state()->diff(model_->get_state()->zero(), x, d->dx);
113		18	d->x_norm = d->dx.norm();
114		18	d->dx.setZero();
115		18	d->xh_jac = e_jac_ * std::max(1., d->x_norm);
116	✓✓	734	for (std::size_t ix = 0; ix < model_->get_state()->get_ndx(); ++ix) {
117		716	d->dx(ix) = d->xh_jac;
118	✓✗✓✗	716	model_->get_state()->integrate(x, d->dx, d->xp);
119		716	model_->calc(d->data_x[ix], d->xp);
120	✓✗✓✗ ✓✗	716	d->dtau_dx.col(ix) = (d->data_x[ix]->tau - tau0) / d->xh_jac;
121		716	d->dx(ix) = 0.;
122			}
123		18	}
124
125			template <typename Scalar>
126		18	void ActuationModelNumDiffTpl<Scalar>::commands(
127			const boost::shared_ptr<ActuationDataAbstract>& data,
128			const Eigen::Ref<const VectorXs>& x,
129			const Eigen::Ref<const VectorXs>& tau) {
130	✗✓	18	if (static_cast<std::size_t>(x.size()) != model_->get_state()->get_nx()) {
131			throw_pretty("Invalid argument: "
132			<< "x has wrong dimension (it should be " +
133			std::to_string(model_->get_state()->get_nx()) + ")");
134			}
135	✗✓	18	if (static_cast<std::size_t>(tau.size()) != model_->get_state()->get_nv()) {
136			throw_pretty("Invalid argument: "
137			<< "tau has wrong dimension (it should be " +
138			std::to_string(model_->get_state()->get_nv()) + ")");
139			}
140		18	Data* d = static_cast<Data*>(data.get());
141		18	model_->commands(d->data_0, x, tau);
142		18	data->u = d->data_0->u;
143		18	}
144
145			template <typename Scalar>
146		18	void ActuationModelNumDiffTpl<Scalar>::torqueTransform(
147			const boost::shared_ptr<ActuationDataAbstract>& data,
148			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
149	✗✓	18	if (static_cast<std::size_t>(x.size()) != model_->get_state()->get_nx()) {
150			throw_pretty("Invalid argument: "
151			<< "x has wrong dimension (it should be " +
152			std::to_string(model_->get_state()->get_nx()) + ")");
153			}
154	✗✓	18	if (static_cast<std::size_t>(u.size()) != nu_) {
155			throw_pretty("Invalid argument: "
156			<< "u has wrong dimension (it should be " +
157			std::to_string(nu_) + ")");
158			}
159		18	Data* d = static_cast<Data*>(data.get());
160		18	model_->torqueTransform(d->data_0, x, u);
161		18	d->Mtau = d->data_0->Mtau;
162		18	}
163
164			template <typename Scalar>
165			boost::shared_ptr<ActuationDataAbstractTpl<Scalar> >
166		72	ActuationModelNumDiffTpl<Scalar>::createData() {
167	✓✗	72	return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
168			}
169
170			template <typename Scalar>
171			const boost::shared_ptr<ActuationModelAbstractTpl<Scalar> >&
172		4640	ActuationModelNumDiffTpl<Scalar>::get_model() const {
173		4640	return model_;
174			}
175
176			template <typename Scalar>
177		54	const Scalar ActuationModelNumDiffTpl<Scalar>::get_disturbance() const {
178		54	return e_jac_;
179			}
180
181			template <typename Scalar>
182			void ActuationModelNumDiffTpl<Scalar>::set_disturbance(
183			const Scalar disturbance) {
184			if (disturbance < 0.) {
185			throw_pretty("Invalid argument: "
186			<< "Disturbance constant is positive");
187			}
188			e_jac_ = disturbance;
189			}
190
191			} // namespace crocoddyl