Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/crocoddyl/multibody/residuals/state.hxx	Lines:	57	70	81.4 %
Date:	2024-02-13 11:12:33	Branches:	42	180	23.3 %


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2022-2023, 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"
#include "crocoddyl/multibody/residuals/state.hpp"

namespace crocoddyl {

template <typename Scalar>
ResidualModelStateTpl<Scalar>::ResidualModelStateTpl(
    boost::shared_ptr<typename Base::StateAbstract> state, const VectorXs& xref,
    const std::size_t nu)
    : Base(state, state->get_ndx(), nu, true, true, false), xref_(xref) {
  if (static_cast<std::size_t>(xref_.size()) != state_->get_nx()) {
    throw_pretty("Invalid argument: "
                 << "xref has wrong dimension (it should be " +
                        std::to_string(state_->get_nx()) + ")");
  }
  // Define the pinocchio model for the multibody state case
  const boost::shared_ptr<StateMultibody>& s =
      boost::dynamic_pointer_cast<StateMultibody>(state);
  if (s) {
    pin_model_ = s->get_pinocchio();
  }
}

template <typename Scalar>
ResidualModelStateTpl<Scalar>::ResidualModelStateTpl(
    boost::shared_ptr<typename Base::StateAbstract> state, const VectorXs& xref)
    : Base(state, state->get_ndx(), true, true, false), xref_(xref) {
  if (static_cast<std::size_t>(xref_.size()) != state_->get_nx()) {
    throw_pretty("Invalid argument: "
                 << "xref has wrong dimension (it should be " +
                        std::to_string(state_->get_nx()) + ")");
  }
  // Define the pinocchio model for the multibody state case
  const boost::shared_ptr<StateMultibody>& s =
      boost::dynamic_pointer_cast<StateMultibody>(state);
  if (s) {
    pin_model_ = s->get_pinocchio();
  }
}

template <typename Scalar>
ResidualModelStateTpl<Scalar>::ResidualModelStateTpl(
    boost::shared_ptr<typename Base::StateAbstract> state, const std::size_t nu)
    : Base(state, state->get_ndx(), nu, true, true, false),
      xref_(state->zero()) {
  // Define the pinocchio model for the multibody state case
  const boost::shared_ptr<StateMultibody>& s =
      boost::dynamic_pointer_cast<StateMultibody>(state);
  if (s) {
    pin_model_ = s->get_pinocchio();
  }
}

template <typename Scalar>
ResidualModelStateTpl<Scalar>::ResidualModelStateTpl(
    boost::shared_ptr<typename Base::StateAbstract> state)
    : Base(state, state->get_ndx(), true, true, false), xref_(state->zero()) {
  // Define the pinocchio model for the multibody state case
  const boost::shared_ptr<StateMultibody>& s =
      boost::dynamic_pointer_cast<StateMultibody>(state);
  if (s) {
    pin_model_ = s->get_pinocchio();
  }
}

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

template <typename Scalar>
void ResidualModelStateTpl<Scalar>::calc(
    const boost::shared_ptr<ResidualDataAbstract>& data,
    const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>&) {
  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()) + ")");
  }

  state_->diff(xref_, x, data->r);
}

template <typename Scalar>
void ResidualModelStateTpl<Scalar>::calcDiff(
    const boost::shared_ptr<ResidualDataAbstract>& data,
    const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>&) {
  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()) + ")");
  }

  state_->Jdiff(xref_, x, data->Rx, data->Rx, second);
}

template <typename Scalar>
void ResidualModelStateTpl<Scalar>::calcCostDiff(
    const boost::shared_ptr<CostDataAbstract>& cdata,
    const boost::shared_ptr<ResidualDataAbstract>& rdata,
    const boost::shared_ptr<ActivationDataAbstract>& adata, const bool) {
  const std::size_t nv = state_->get_nv();
  if (pin_model_) {
    typedef Eigen::Block<MatrixXs> MatrixBlock;
    for (pinocchio::JointIndex i = 1;
         i < (pinocchio::JointIndex)pin_model_->njoints; ++i) {
      const MatrixBlock& RxBlock =
          rdata->Rx.block(pin_model_->idx_vs[i], pin_model_->idx_vs[i],
                          pin_model_->nvs[i], pin_model_->nvs[i]);
      cdata->Lx.segment(pin_model_->idx_vs[i], pin_model_->nvs[i]).noalias() =
          RxBlock.transpose() *
          adata->Ar.segment(pin_model_->idx_vs[i], pin_model_->nvs[i]);
      cdata->Lxx
          .block(pin_model_->idx_vs[i], pin_model_->idx_vs[i],
                 pin_model_->nvs[i], pin_model_->nvs[i])
          .noalias() = RxBlock.transpose() *
                       adata->Arr.diagonal()
                           .segment(pin_model_->idx_vs[i], pin_model_->nvs[i])
                           .asDiagonal() *
                       RxBlock;
    }
    cdata->Lx.tail(nv) = adata->Ar.tail(nv);
    cdata->Lxx.diagonal().tail(nv) = adata->Arr.diagonal().tail(nv);
  } else {
    cdata->Lx = adata->Ar;
    cdata->Lxx.diagonal() = adata->Arr.diagonal();
  }
}

template <typename Scalar>
void ResidualModelStateTpl<Scalar>::print(std::ostream& os) const {
  os << "ResidualModelState";
}

template <typename Scalar>
const typename MathBaseTpl<Scalar>::VectorXs&
ResidualModelStateTpl<Scalar>::get_reference() const {
  return xref_;
}

template <typename Scalar>
void ResidualModelStateTpl<Scalar>::set_reference(const VectorXs& reference) {
  if (static_cast<std::size_t>(reference.size()) != state_->get_nx()) {
    throw_pretty(
        "Invalid argument: "
        << "the state reference has wrong dimension (" << reference.size()
        << " provided - it should be " + std::to_string(state_->get_nx()) + ")")
  }
  xref_ = reference;
}

}  // namespace crocoddyl


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2022-2023, 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			#include "crocoddyl/multibody/residuals/state.hpp"
12
13			namespace crocoddyl {
14
15			template <typename Scalar>
16		1487	ResidualModelStateTpl<Scalar>::ResidualModelStateTpl(
17			boost::shared_ptr<typename Base::StateAbstract> state, const VectorXs& xref,
18			const std::size_t nu)
19	✓✗✓✗	1487	: Base(state, state->get_ndx(), nu, true, true, false), xref_(xref) {
20	✓✗✗✓	1487	if (static_cast<std::size_t>(xref_.size()) != state_->get_nx()) {
21			throw_pretty("Invalid argument: "
22			<< "xref has wrong dimension (it should be " +
23			std::to_string(state_->get_nx()) + ")");
24			}
25			// Define the pinocchio model for the multibody state case
26		2974	const boost::shared_ptr<StateMultibody>& s =
27			boost::dynamic_pointer_cast<StateMultibody>(state);
28	✓✗	1487	if (s) {
29		1487	pin_model_ = s->get_pinocchio();
30			}
31		1487	}
32
33			template <typename Scalar>
34			ResidualModelStateTpl<Scalar>::ResidualModelStateTpl(
35			boost::shared_ptr<typename Base::StateAbstract> state, const VectorXs& xref)
36			: Base(state, state->get_ndx(), true, true, false), xref_(xref) {
37			if (static_cast<std::size_t>(xref_.size()) != state_->get_nx()) {
38			throw_pretty("Invalid argument: "
39			<< "xref has wrong dimension (it should be " +
40			std::to_string(state_->get_nx()) + ")");
41			}
42			// Define the pinocchio model for the multibody state case
43			const boost::shared_ptr<StateMultibody>& s =
44			boost::dynamic_pointer_cast<StateMultibody>(state);
45			if (s) {
46			pin_model_ = s->get_pinocchio();
47			}
48			}
49
50			template <typename Scalar>
51		3	ResidualModelStateTpl<Scalar>::ResidualModelStateTpl(
52			boost::shared_ptr<typename Base::StateAbstract> state, const std::size_t nu)
53			: Base(state, state->get_ndx(), nu, true, true, false),
54	✓✗✓✗	3	xref_(state->zero()) {
55			// Define the pinocchio model for the multibody state case
56		6	const boost::shared_ptr<StateMultibody>& s =
57			boost::dynamic_pointer_cast<StateMultibody>(state);
58	✓✗	3	if (s) {
59		3	pin_model_ = s->get_pinocchio();
60			}
61		3	}
62
63			template <typename Scalar>
64		10	ResidualModelStateTpl<Scalar>::ResidualModelStateTpl(
65			boost::shared_ptr<typename Base::StateAbstract> state)
66	✓✗✓✗	10	: Base(state, state->get_ndx(), true, true, false), xref_(state->zero()) {
67			// Define the pinocchio model for the multibody state case
68		20	const boost::shared_ptr<StateMultibody>& s =
69			boost::dynamic_pointer_cast<StateMultibody>(state);
70	✓✗	10	if (s) {
71		10	pin_model_ = s->get_pinocchio();
72			}
73		10	}
74
75			template <typename Scalar>
76		3004	ResidualModelStateTpl<Scalar>::~ResidualModelStateTpl() {}
77
78			template <typename Scalar>
79		86859	void ResidualModelStateTpl<Scalar>::calc(
80			const boost::shared_ptr<ResidualDataAbstract>& data,
81			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>&) {
82	✗✓	86859	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
83			throw_pretty("Invalid argument: "
84			<< "x has wrong dimension (it should be " +
85			std::to_string(state_->get_nx()) + ")");
86			}
87
88	✓✗✓✗	86859	state_->diff(xref_, x, data->r);
89		86859	}
90
91			template <typename Scalar>
92		14106	void ResidualModelStateTpl<Scalar>::calcDiff(
93			const boost::shared_ptr<ResidualDataAbstract>& data,
94			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>&) {
95	✗✓	14106	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
96			throw_pretty("Invalid argument: "
97			<< "x has wrong dimension (it should be " +
98			std::to_string(state_->get_nx()) + ")");
99			}
100
101	✓✗✓✗ ✓✗	14106	state_->Jdiff(xref_, x, data->Rx, data->Rx, second);
102		14106	}
103
104			template <typename Scalar>
105		14018	void ResidualModelStateTpl<Scalar>::calcCostDiff(
106			const boost::shared_ptr<CostDataAbstract>& cdata,
107			const boost::shared_ptr<ResidualDataAbstract>& rdata,
108			const boost::shared_ptr<ActivationDataAbstract>& adata, const bool) {
109		14018	const std::size_t nv = state_->get_nv();
110	✓✗	14018	if (pin_model_) {
111			typedef Eigen::Block<MatrixXs> MatrixBlock;
112		14018	for (pinocchio::JointIndex i = 1;
113	✓✓	192986	i < (pinocchio::JointIndex)pin_model_->njoints; ++i) {
114	✓✗	178968	const MatrixBlock& RxBlock =
115		357936	rdata->Rx.block(pin_model_->idx_vs[i], pin_model_->idx_vs[i],
116		178968	pin_model_->nvs[i], pin_model_->nvs[i]);
117	✓✗✓✗ ✓✗✓✗ ✓✗	357936	cdata->Lx.segment(pin_model_->idx_vs[i], pin_model_->nvs[i]).noalias() =
118	✓✗	178968	RxBlock.transpose() *
119		178968	adata->Ar.segment(pin_model_->idx_vs[i], pin_model_->nvs[i]);
120		178968	cdata->Lxx
121		357936	.block(pin_model_->idx_vs[i], pin_model_->idx_vs[i],
122		357936	pin_model_->nvs[i], pin_model_->nvs[i])
123	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗	894840	.noalias() = RxBlock.transpose() *
124		178968	adata->Arr.diagonal()
125		178968	.segment(pin_model_->idx_vs[i], pin_model_->nvs[i])
126			.asDiagonal() *
127			RxBlock;
128			}
129	✓✗✓✗	14018	cdata->Lx.tail(nv) = adata->Ar.tail(nv);
130	✓✗✓✗ ✓✗	14018	cdata->Lxx.diagonal().tail(nv) = adata->Arr.diagonal().tail(nv);
131			} else {
132			cdata->Lx = adata->Ar;
133			cdata->Lxx.diagonal() = adata->Arr.diagonal();
134			}
135		14018	}
136
137			template <typename Scalar>
138		63	void ResidualModelStateTpl<Scalar>::print(std::ostream& os) const {
139		63	os << "ResidualModelState";
140		63	}
141
142			template <typename Scalar>
143			const typename MathBaseTpl<Scalar>::VectorXs&
144		1	ResidualModelStateTpl<Scalar>::get_reference() const {
145		1	return xref_;
146			}
147
148			template <typename Scalar>
149		1	void ResidualModelStateTpl<Scalar>::set_reference(const VectorXs& reference) {
150	✗✓	1	if (static_cast<std::size_t>(reference.size()) != state_->get_nx()) {
151			throw_pretty(
152			"Invalid argument: "
153			<< "the state reference has wrong dimension (" << reference.size()
154			<< " provided - it should be " + std::to_string(state_->get_nx()) + ")")
155			}
156		1	xref_ = reference;
157		1	}
158
159			} // namespace crocoddyl