GCC Code Coverage Report

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2019-2025, 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 <pinocchio/algorithm/frames.hpp>
11			#include <pinocchio/algorithm/kinematics-derivatives.hpp>
12
13			namespace crocoddyl {
14
15			template <typename Scalar>
16		✗	ImpulseModel6DTpl<Scalar>::ImpulseModel6DTpl(
17			std::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
18			const pinocchio::ReferenceFrame type)
19		✗	: Base(state, type, 6) {
20		✗	id_ = id;
21		✗	}
22
23			template <typename Scalar>
24		✗	void ImpulseModel6DTpl<Scalar>::calc(
25			const std::shared_ptr<ImpulseDataAbstract>& data,
26			const Eigen::Ref<const VectorXs>&) {
27		✗	std::shared_ptr<Data> d = std::static_pointer_cast<Data>(data);
28		✗	pinocchio::updateFramePlacement<Scalar>(*state_->get_pinocchio().get(),
29		✗	*d->pinocchio, id_);
30		✗	pinocchio::getFrameJacobian(*state_->get_pinocchio().get(), *d->pinocchio,
31		✗	id_, pinocchio::LOCAL, d->fJf);
32		✗	switch (type_) {
33		✗	case pinocchio::ReferenceFrame::LOCAL:
34		✗	data->Jc = d->fJf;
35		✗	break;
36		✗	case pinocchio::ReferenceFrame::WORLD:
37			case pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED:
38		✗	d->lwaMl.rotation(d->pinocchio->oMf[id_].rotation());
39		✗	data->Jc.noalias() = d->lwaMl.toActionMatrix() * d->fJf;
40		✗	break;
41			}
42		✗	}
43
44			template <typename Scalar>
45		✗	void ImpulseModel6DTpl<Scalar>::calcDiff(
46			const std::shared_ptr<ImpulseDataAbstract>& data,
47			const Eigen::Ref<const VectorXs>&) {
48		✗	std::shared_ptr<Data> d = std::static_pointer_cast<Data>(data);
49		✗	const pinocchio::JointIndex joint =
50		✗	state_->get_pinocchio()->frames[d->frame].parentJoint;
51		✗	pinocchio::getJointVelocityDerivatives(*state_->get_pinocchio().get(),
52		✗	*d->pinocchio, joint, pinocchio::LOCAL,
53		✗	d->v_partial_dq, d->v_partial_dv);
54		✗	d->dv0_local_dq.noalias() = d->fXj * d->v_partial_dq;
55
56		✗	switch (type_) {
57		✗	case pinocchio::ReferenceFrame::LOCAL:
58		✗	data->dv0_dq = d->dv0_local_dq;
59		✗	break;
60		✗	case pinocchio::ReferenceFrame::WORLD:
61			case pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED:
62		✗	const Eigen::Ref<const Matrix3s> oRf = d->pinocchio->oMf[id_].rotation();
63		✗	d->v0 = pinocchio::getFrameVelocity(*state_->get_pinocchio().get(),
64		✗	*d->pinocchio, id_, type_);
65		✗	pinocchio::skew(d->v0.linear(), d->vv_skew);
66		✗	pinocchio::skew(d->v0.angular(), d->vw_skew);
67		✗	d->vv_world_skew.noalias() = d->vv_skew * oRf;
68		✗	d->vw_world_skew.noalias() = d->vw_skew * oRf;
69		✗	data->dv0_dq.noalias() = d->lwaMl.toActionMatrix() * d->dv0_local_dq;
70		✗	d->dv0_dq.template topRows<3>().noalias() -=
71		✗	d->vv_world_skew * d->fJf.template bottomRows<3>();
72		✗	d->dv0_dq.template bottomRows<3>().noalias() -=
73		✗	d->vw_world_skew * d->fJf.template bottomRows<3>();
74		✗	break;
75			}
76		✗	}
77
78			template <typename Scalar>
79		✗	void ImpulseModel6DTpl<Scalar>::updateForce(
80			const std::shared_ptr<ImpulseDataAbstract>& data, const VectorXs& force) {
81		✗	if (force.size() != 6) {
82		✗	throw_pretty(
83			"Invalid argument: " << "lambda has wrong dimension (it should be 6)");
84			}
85		✗	Data* d = static_cast<Data*>(data.get());
86		✗	data->f = pinocchio::ForceTpl<Scalar>(force);
87		✗	switch (type_) {
88		✗	case pinocchio::ReferenceFrame::LOCAL:
89		✗	data->fext = data->jMf.act(data->f);
90		✗	data->dtau_dq.setZero();
91		✗	break;
92		✗	case pinocchio::ReferenceFrame::WORLD:
93			case pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED:
94		✗	d->f_local = d->lwaMl.actInv(data->f);
95		✗	data->fext = data->jMf.act(d->f_local);
96		✗	pinocchio::skew(d->f_local.linear(), d->fv_skew);
97		✗	pinocchio::skew(d->f_local.angular(), d->fw_skew);
98		✗	d->fJf_df.template topRows<3>().noalias() =
99		✗	d->fv_skew * d->fJf.template bottomRows<3>();
100		✗	d->fJf_df.template bottomRows<3>().noalias() =
101		✗	d->fw_skew * d->fJf.template bottomRows<3>();
102		✗	d->dtau_dq.noalias() = -d->fJf.transpose() * d->fJf_df;
103		✗	break;
104			}
105		✗	}
106
107			template <typename Scalar>
108			std::shared_ptr<ImpulseDataAbstractTpl<Scalar> >
109		✗	ImpulseModel6DTpl<Scalar>::createData(pinocchio::DataTpl<Scalar>* const data) {
110		✗	return std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this,
111		✗	data);
112			}
113
114			template <typename Scalar>
115			template <typename NewScalar>
116		✗	ImpulseModel6DTpl<NewScalar> ImpulseModel6DTpl<Scalar>::cast() const {
117			typedef ImpulseModel6DTpl<NewScalar> ReturnType;
118			typedef StateMultibodyTpl<NewScalar> StateType;
119		✗	ReturnType ret(
120		✗	std::make_shared<StateType>(state_->template cast<NewScalar>()), id_,
121		✗	type_);
122		✗	return ret;
123			}
124
125			template <typename Scalar>
126		✗	void ImpulseModel6DTpl<Scalar>::print(std::ostream& os) const {
127		✗	os << "ImpulseModel^D {frame=" << state_->get_pinocchio()->frames[id_].name
128		✗	<< ", type=" << type_ << "}";
129		✗	}
130
131			} // namespace crocoddyl
132