Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/crocoddyl/core/numdiff/control.hxx	Lines:	42	62	67.7 %
Date:	2024-02-13 11:12:33	Branches:	29	124	23.4 %


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

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

namespace crocoddyl {

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

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

template <typename Scalar>
void ControlParametrizationModelNumDiffTpl<Scalar>::calc(
    const boost::shared_ptr<ControlParametrizationDataAbstract>& data,
    const Scalar t, const Eigen::Ref<const VectorXs>& u) const {
  boost::shared_ptr<Data> data_nd = boost::static_pointer_cast<Data>(data);
  model_->calc(data_nd->data_0, t, u);
}

template <typename Scalar>
void ControlParametrizationModelNumDiffTpl<Scalar>::calcDiff(
    const boost::shared_ptr<ControlParametrizationDataAbstract>& data,
    const Scalar t, const Eigen::Ref<const VectorXs>& u) const {
  boost::shared_ptr<Data> data_nd = boost::static_pointer_cast<Data>(data);
  data->w = data_nd->data_0->w;

  data_nd->du.setZero();
  const Scalar uh_jac = e_jac_ * std::max(1., u.norm());
  for (std::size_t i = 0; i < model_->get_nu(); ++i) {
    data_nd->du(i) += uh_jac;
    model_->calc(data_nd->data_u[i], t, u + data_nd->du);
    data->dw_du.col(i) = data_nd->data_u[i]->w - data->w;
    data_nd->du(i) = 0.;
  }
  data->dw_du /= uh_jac;
}

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

template <typename Scalar>
void ControlParametrizationModelNumDiffTpl<Scalar>::params(
    const boost::shared_ptr<ControlParametrizationDataAbstract>& data,
    const Scalar t, const Eigen::Ref<const VectorXs>& w) const {
  model_->params(data, t, w);
}

template <typename Scalar>
void ControlParametrizationModelNumDiffTpl<Scalar>::convertBounds(
    const Eigen::Ref<const VectorXs>& w_lb,
    const Eigen::Ref<const VectorXs>& w_ub, Eigen::Ref<VectorXs> u_lb,
    Eigen::Ref<VectorXs> u_ub) const {
  model_->convertBounds(w_lb, w_ub, u_lb, u_ub);
}

template <typename Scalar>
void ControlParametrizationModelNumDiffTpl<Scalar>::multiplyByJacobian(
    const boost::shared_ptr<ControlParametrizationDataAbstract>& data,
    const Eigen::Ref<const MatrixXs>& A, Eigen::Ref<MatrixXs> out,
    const AssignmentOp op) const {
  assert_pretty(is_a_AssignmentOp(op),
                ("op must be one of the AssignmentOp {settop, addto, rmfrom}"));
  MatrixXs J(nw_, nu_);
  switch (op) {
    case setto:
      out.noalias() = A * data->dw_du;
      break;
    case addto:
      out.noalias() += A * data->dw_du;
      break;
    case rmfrom:
      out.noalias() -= A * data->dw_du;
      break;
    default:
      throw_pretty("Invalid argument: allowed operators: setto, addto, rmfrom");
      break;
  }
}

template <typename Scalar>
void ControlParametrizationModelNumDiffTpl<Scalar>::multiplyJacobianTransposeBy(
    const boost::shared_ptr<ControlParametrizationDataAbstract>& data,
    const Eigen::Ref<const MatrixXs>& A, Eigen::Ref<MatrixXs> out,
    const AssignmentOp op) const {
  assert_pretty(is_a_AssignmentOp(op),
                ("op must be one of the AssignmentOp {settop, addto, rmfrom}"));
  MatrixXs J(nw_, nu_);
  switch (op) {
    case setto:
      out.noalias() = data->dw_du.transpose() * A;
      break;
    case addto:
      out.noalias() += data->dw_du.transpose() * A;
      break;
    case rmfrom:
      out.noalias() -= data->dw_du.transpose() * A;
      break;
    default:
      throw_pretty("Invalid argument: allowed operators: setto, addto, rmfrom");
      break;
  }
}

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

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

template <typename Scalar>
void ControlParametrizationModelNumDiffTpl<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) 2021-2023, LAAS-CNRS, University of Edinburgh, New York
5			// University,
6			// Heriot-Watt University
7			// Max Planck Gesellschaft, University of Trento
8			// Copyright note valid unless otherwise stated in individual files.
9			// All rights reserved.
10			///////////////////////////////////////////////////////////////////////////////
11
12			#include "crocoddyl/core/utils/exception.hpp"
13
14			namespace crocoddyl {
15
16			template <typename Scalar>
17		12	ControlParametrizationModelNumDiffTpl<Scalar>::
18			ControlParametrizationModelNumDiffTpl(boost::shared_ptr<Base> model)
19			: Base(model->get_nw(), model->get_nu()),
20			model_(model),
21		12	e_jac_(std::sqrt(2.0 * std::numeric_limits<Scalar>::epsilon())) {}
22
23			template <typename Scalar>
24		24	ControlParametrizationModelNumDiffTpl<
25		24	Scalar>::~ControlParametrizationModelNumDiffTpl() {}
26
27			template <typename Scalar>
28		12	void ControlParametrizationModelNumDiffTpl<Scalar>::calc(
29			const boost::shared_ptr<ControlParametrizationDataAbstract>& data,
30			const Scalar t, const Eigen::Ref<const VectorXs>& u) const {
31		24	boost::shared_ptr<Data> data_nd = boost::static_pointer_cast<Data>(data);
32	✓✗	12	model_->calc(data_nd->data_0, t, u);
33		12	}
34
35			template <typename Scalar>
36		12	void ControlParametrizationModelNumDiffTpl<Scalar>::calcDiff(
37			const boost::shared_ptr<ControlParametrizationDataAbstract>& data,
38			const Scalar t, const Eigen::Ref<const VectorXs>& u) const {
39		24	boost::shared_ptr<Data> data_nd = boost::static_pointer_cast<Data>(data);
40	✓✗	12	data->w = data_nd->data_0->w;
41
42	✓✗	12	data_nd->du.setZero();
43	✓✗	12	const Scalar uh_jac = e_jac_ * std::max(1., u.norm());
44	✓✓	282	for (std::size_t i = 0; i < model_->get_nu(); ++i) {
45	✓✗	270	data_nd->du(i) += uh_jac;
46	✓✗✓✗ ✓✗	270	model_->calc(data_nd->data_u[i], t, u + data_nd->du);
47	✓✗✓✗ ✓✗	270	data->dw_du.col(i) = data_nd->data_u[i]->w - data->w;
48	✓✗	270	data_nd->du(i) = 0.;
49			}
50	✓✗	12	data->dw_du /= uh_jac;
51		12	}
52
53			template <typename Scalar>
54			boost::shared_ptr<ControlParametrizationDataAbstractTpl<Scalar> >
55		12	ControlParametrizationModelNumDiffTpl<Scalar>::createData() {
56	✓✗	12	return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
57			}
58
59			template <typename Scalar>
60			void ControlParametrizationModelNumDiffTpl<Scalar>::params(
61			const boost::shared_ptr<ControlParametrizationDataAbstract>& data,
62			const Scalar t, const Eigen::Ref<const VectorXs>& w) const {
63			model_->params(data, t, w);
64			}
65
66			template <typename Scalar>
67			void ControlParametrizationModelNumDiffTpl<Scalar>::convertBounds(
68			const Eigen::Ref<const VectorXs>& w_lb,
69			const Eigen::Ref<const VectorXs>& w_ub, Eigen::Ref<VectorXs> u_lb,
70			Eigen::Ref<VectorXs> u_ub) const {
71			model_->convertBounds(w_lb, w_ub, u_lb, u_ub);
72			}
73
74			template <typename Scalar>
75		4	void ControlParametrizationModelNumDiffTpl<Scalar>::multiplyByJacobian(
76			const boost::shared_ptr<ControlParametrizationDataAbstract>& data,
77			const Eigen::Ref<const MatrixXs>& A, Eigen::Ref<MatrixXs> out,
78			const AssignmentOp op) const {
79	✗✓✗✗ ✗✗✗✗ ✗✗	4	assert_pretty(is_a_AssignmentOp(op),
80			("op must be one of the AssignmentOp {settop, addto, rmfrom}"));
81	✓✗	8	MatrixXs J(nw_, nu_);
82	✓✗✗✗	4	switch (op) {
83		4	case setto:
84	✓✗✓✗ ✓✗	4	out.noalias() = A * data->dw_du;
85		4	break;
86			case addto:
87			out.noalias() += A * data->dw_du;
88			break;
89			case rmfrom:
90			out.noalias() -= A * data->dw_du;
91			break;
92			default:
93			throw_pretty("Invalid argument: allowed operators: setto, addto, rmfrom");
94			break;
95			}
96		4	}
97
98			template <typename Scalar>
99		4	void ControlParametrizationModelNumDiffTpl<Scalar>::multiplyJacobianTransposeBy(
100			const boost::shared_ptr<ControlParametrizationDataAbstract>& data,
101			const Eigen::Ref<const MatrixXs>& A, Eigen::Ref<MatrixXs> out,
102			const AssignmentOp op) const {
103	✗✓✗✗ ✗✗✗✗ ✗✗	4	assert_pretty(is_a_AssignmentOp(op),
104			("op must be one of the AssignmentOp {settop, addto, rmfrom}"));
105	✓✗	8	MatrixXs J(nw_, nu_);
106	✓✗✗✗	4	switch (op) {
107		4	case setto:
108	✓✗✓✗ ✓✗✓✗	4	out.noalias() = data->dw_du.transpose() * A;
109		4	break;
110			case addto:
111			out.noalias() += data->dw_du.transpose() * A;
112			break;
113			case rmfrom:
114			out.noalias() -= data->dw_du.transpose() * A;
115			break;
116			default:
117			throw_pretty("Invalid argument: allowed operators: setto, addto, rmfrom");
118			break;
119			}
120		4	}
121
122			template <typename Scalar>
123			const boost::shared_ptr<ControlParametrizationModelAbstractTpl<Scalar> >&
124		294	ControlParametrizationModelNumDiffTpl<Scalar>::get_model() const {
125		294	return model_;
126			}
127
128			template <typename Scalar>
129		12	const Scalar ControlParametrizationModelNumDiffTpl<Scalar>::get_disturbance()
130			const {
131		12	return e_jac_;
132			}
133
134			template <typename Scalar>
135			void ControlParametrizationModelNumDiffTpl<Scalar>::set_disturbance(
136			const Scalar disturbance) {
137			if (disturbance < 0.) {
138			throw_pretty("Invalid argument: "
139			<< "Disturbance constant is positive");
140			}
141			e_jac_ = disturbance;
142			}
143
144			} // namespace crocoddyl