GCC Code Coverage Report

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2020-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			namespace crocoddyl {
11
12			template <typename Scalar>
13		✗	ContactModel2DTpl<Scalar>::ContactModel2DTpl(
14			std::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
15			const Vector2s& xref, const std::size_t nu, const Vector2s& gains)
16			: Base(state, pinocchio::ReferenceFrame::LOCAL, 2, nu),
17		✗	xref_(xref),
18		✗	gains_(gains) {
19		✗	id_ = id;
20		✗	}
21
22			template <typename Scalar>
23		✗	ContactModel2DTpl<Scalar>::ContactModel2DTpl(
24			std::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
25			const Vector2s& xref, const Vector2s& gains)
26			: Base(state, pinocchio::ReferenceFrame::LOCAL, 2),
27		✗	xref_(xref),
28		✗	gains_(gains) {
29		✗	id_ = id;
30		✗	}
31
32			template <typename Scalar>
33		✗	void ContactModel2DTpl<Scalar>::calc(
34			const std::shared_ptr<ContactDataAbstract>& data,
35			const Eigen::Ref<const VectorXs>&) {
36		✗	Data* d = static_cast<Data*>(data.get());
37		✗	pinocchio::updateFramePlacement(*state_->get_pinocchio().get(), *d->pinocchio,
38			id_);
39		✗	pinocchio::getFrameJacobian(*state_->get_pinocchio().get(), *d->pinocchio,
40		✗	id_, pinocchio::LOCAL, d->fJf);
41		✗	d->v = pinocchio::getFrameVelocity(*state_->get_pinocchio().get(),
42		✗	*d->pinocchio, id_);
43		✗	d->a = pinocchio::getFrameAcceleration(*state_->get_pinocchio().get(),
44		✗	*d->pinocchio, id_);
45
46		✗	d->Jc.row(0) = d->fJf.row(0);
47		✗	d->Jc.row(1) = d->fJf.row(2);
48
49		✗	d->vw = d->v.angular();
50		✗	d->vv = d->v.linear();
51
52		✗	d->a0[0] = d->a.linear()[0] + d->vw[1] * d->vv[2] - d->vw[2] * d->vv[1];
53		✗	d->a0[1] = d->a.linear()[2] + d->vw[0] * d->vv[1] - d->vw[1] * d->vv[0];
54
55		✗	if (gains_[0] != 0.) {
56		✗	d->a0[0] +=
57		✗	gains_[0] * (d->pinocchio->oMf[id_].translation()[0] - xref_[0]);
58		✗	d->a0[1] +=
59		✗	gains_[0] * (d->pinocchio->oMf[id_].translation()[2] - xref_[1]);
60			}
61		✗	if (gains_[1] != 0.) {
62		✗	d->a0[0] += gains_[1] * d->vv[0];
63		✗	d->a0[1] += gains_[1] * d->vv[2];
64			}
65		✗	}
66
67			template <typename Scalar>
68		✗	void ContactModel2DTpl<Scalar>::calcDiff(
69			const std::shared_ptr<ContactDataAbstract>& data,
70			const Eigen::Ref<const VectorXs>&) {
71		✗	Data* d = static_cast<Data*>(data.get());
72		✗	const pinocchio::JointIndex joint =
73		✗	state_->get_pinocchio()->frames[d->frame].parentJoint;
74		✗	pinocchio::getJointAccelerationDerivatives(
75		✗	*state_->get_pinocchio().get(), *d->pinocchio, joint, pinocchio::LOCAL,
76		✗	d->v_partial_dq, d->a_partial_dq, d->a_partial_dv, d->a_partial_da);
77		✗	const std::size_t nv = state_->get_nv();
78		✗	pinocchio::skew(d->vv, d->vv_skew);
79		✗	pinocchio::skew(d->vw, d->vw_skew);
80		✗	d->fXjdv_dq.noalias() = d->fXj * d->v_partial_dq;
81		✗	d->fXjda_dq.noalias() = d->fXj * d->a_partial_dq;
82		✗	d->fXjda_dv.noalias() = d->fXj * d->a_partial_dv;
83
84		✗	d->da0_dx.leftCols(nv).row(0) = d->fXjda_dq.row(0);
85		✗	d->da0_dx.leftCols(nv).row(0).noalias() +=
86		✗	d->vw_skew.row(0) * d->fXjdv_dq.template topRows<3>();
87		✗	d->da0_dx.leftCols(nv).row(0).noalias() -=
88		✗	d->vv_skew.row(0) * d->fXjdv_dq.template bottomRows<3>();
89
90		✗	d->da0_dx.leftCols(nv).row(1) = d->fXjda_dq.row(2);
91		✗	d->da0_dx.leftCols(nv).row(1).noalias() +=
92		✗	d->vw_skew.row(2) * d->fXjdv_dq.template topRows<3>();
93		✗	d->da0_dx.leftCols(nv).row(1).noalias() -=
94		✗	d->vv_skew.row(2) * d->fXjdv_dq.template bottomRows<3>();
95
96		✗	d->da0_dx.rightCols(nv).row(0) = d->fXjda_dv.row(0);
97		✗	d->da0_dx.rightCols(nv).row(0).noalias() +=
98		✗	d->vw_skew.row(0) * d->fJf.template topRows<3>();
99		✗	d->da0_dx.rightCols(nv).row(0).noalias() -=
100		✗	d->vv_skew.row(0) * d->fJf.template bottomRows<3>();
101
102		✗	d->da0_dx.rightCols(nv).row(1) = d->fXjda_dv.row(2);
103		✗	d->da0_dx.rightCols(nv).row(1).noalias() +=
104		✗	d->vw_skew.row(2) * d->fJf.template topRows<3>();
105		✗	d->da0_dx.rightCols(nv).row(1).noalias() -=
106		✗	d->vv_skew.row(2) * d->fJf.template bottomRows<3>();
107
108		✗	if (gains_[0] != 0.) {
109		✗	const Eigen::Ref<const Matrix3s> oRf = d->pinocchio->oMf[id_].rotation();
110		✗	d->oRf(0, 0) = oRf(0, 0);
111		✗	d->oRf(1, 0) = oRf(2, 0);
112		✗	d->oRf(0, 1) = oRf(0, 2);
113		✗	d->oRf(1, 1) = oRf(2, 2);
114		✗	d->da0_dx.leftCols(nv).noalias() += gains_[0] * d->oRf * d->Jc;
115			}
116		✗	if (gains_[1] != 0.) {
117		✗	d->da0_dx.leftCols(nv).row(0).noalias() +=
118		✗	gains_[1] * d->fXj.row(0) * d->v_partial_dq;
119		✗	d->da0_dx.leftCols(nv).row(1).noalias() +=
120		✗	gains_[1] * d->fXj.row(2) * d->v_partial_dq;
121		✗	d->da0_dx.rightCols(nv).row(0).noalias() +=
122		✗	gains_[1] * d->fXj.row(0) * d->a_partial_da;
123		✗	d->da0_dx.rightCols(nv).row(1).noalias() +=
124		✗	gains_[1] * d->fXj.row(2) * d->a_partial_da;
125			}
126		✗	}
127
128			template <typename Scalar>
129		✗	void ContactModel2DTpl<Scalar>::updateForce(
130			const std::shared_ptr<ContactDataAbstract>& data, const VectorXs& force) {
131		✗	if (force.size() != 2) {
132		✗	throw_pretty(
133			"Invalid argument: " << "lambda has wrong dimension (it should be 2)");
134			}
135		✗	Data* d = static_cast<Data*>(data.get());
136		✗	const Eigen::Ref<const Matrix3s> R = d->jMf.rotation();
137		✗	data->f.linear() = R.col(0) * force[0] + R.col(2) * force[1];
138		✗	data->f.angular().setZero();
139		✗	data->fext.linear() = R.col(0) * force[0] + R.col(2) * force[1];
140		✗	data->fext.angular() = d->jMf.translation().cross(data->fext.linear());
141		✗	}
142
143			template <typename Scalar>
144			std::shared_ptr<ContactDataAbstractTpl<Scalar> >
145		✗	ContactModel2DTpl<Scalar>::createData(pinocchio::DataTpl<Scalar>* const data) {
146		✗	return std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this,
147		✗	data);
148			}
149
150			template <typename Scalar>
151			template <typename NewScalar>
152		✗	ContactModel2DTpl<NewScalar> ContactModel2DTpl<Scalar>::cast() const {
153			typedef ContactModel2DTpl<NewScalar> ReturnType;
154			typedef StateMultibodyTpl<NewScalar> StateType;
155		✗	ReturnType ret(
156		✗	std::make_shared<StateType>(state_->template cast<NewScalar>()), id_,
157		✗	xref_.template cast<NewScalar>(), nu_, gains_.template cast<NewScalar>());
158		✗	return ret;
159			}
160
161			template <typename Scalar>
162		✗	void ContactModel2DTpl<Scalar>::print(std::ostream& os) const {
163		✗	os << "ContactModel2D {frame=" << state_->get_pinocchio()->frames[id_].name
164		✗	<< "}";
165		✗	}
166
167			template <typename Scalar>
168			const typename MathBaseTpl<Scalar>::Vector2s&
169		✗	ContactModel2DTpl<Scalar>::get_reference() const {
170		✗	return xref_;
171			}
172
173			template <typename Scalar>
174			const typename MathBaseTpl<Scalar>::Vector2s&
175		✗	ContactModel2DTpl<Scalar>::get_gains() const {
176		✗	return gains_;
177			}
178
179			template <typename Scalar>
180		✗	void ContactModel2DTpl<Scalar>::set_reference(const Vector2s& reference) {
181		✗	xref_ = reference;
182		✗	}
183
184			} // namespace crocoddyl
185