Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/crocoddyl/multibody/contacts/contact-2d.hxx	Lines:	84	104	80.8 %
Date:	2024-02-13 11:12:33	Branches:	149	342	43.6 %


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

namespace crocoddyl {

template <typename Scalar>
ContactModel2DTpl<Scalar>::ContactModel2DTpl(
    boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
    const Vector2s& xref, const std::size_t nu, const Vector2s& gains)
    : Base(state, pinocchio::ReferenceFrame::LOCAL, 2, nu),
      xref_(xref),
      gains_(gains) {
  id_ = id;
}

template <typename Scalar>
ContactModel2DTpl<Scalar>::ContactModel2DTpl(
    boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
    const Vector2s& xref, const Vector2s& gains)
    : Base(state, pinocchio::ReferenceFrame::LOCAL, 2),
      xref_(xref),
      gains_(gains) {
  id_ = id;
}

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

template <typename Scalar>
void ContactModel2DTpl<Scalar>::calc(
    const boost::shared_ptr<ContactDataAbstract>& data,
    const Eigen::Ref<const VectorXs>&) {
  Data* d = static_cast<Data*>(data.get());
  pinocchio::updateFramePlacement(*state_->get_pinocchio().get(), *d->pinocchio,
                                  id_);
  pinocchio::getFrameJacobian(*state_->get_pinocchio().get(), *d->pinocchio,
                              id_, pinocchio::LOCAL, d->fJf);
  d->v = pinocchio::getFrameVelocity(*state_->get_pinocchio().get(),
                                     *d->pinocchio, id_);
  d->a = pinocchio::getFrameAcceleration(*state_->get_pinocchio().get(),
                                         *d->pinocchio, id_);

  d->Jc.row(0) = d->fJf.row(0);
  d->Jc.row(1) = d->fJf.row(2);

  d->vw = d->v.angular();
  d->vv = d->v.linear();

  d->a0[0] = d->a.linear()[0] + d->vw[1] * d->vv[2] - d->vw[2] * d->vv[1];
  d->a0[1] = d->a.linear()[2] + d->vw[0] * d->vv[1] - d->vw[1] * d->vv[0];

  if (gains_[0] != 0.) {
    d->a0[0] +=
        gains_[0] * (d->pinocchio->oMf[id_].translation()[0] - xref_[0]);
    d->a0[1] +=
        gains_[0] * (d->pinocchio->oMf[id_].translation()[2] - xref_[1]);
  }
  if (gains_[1] != 0.) {
    d->a0[0] += gains_[1] * d->vv[0];
    d->a0[1] += gains_[1] * d->vv[2];
  }
}

template <typename Scalar>
void ContactModel2DTpl<Scalar>::calcDiff(
    const boost::shared_ptr<ContactDataAbstract>& data,
    const Eigen::Ref<const VectorXs>&) {
  Data* d = static_cast<Data*>(data.get());
  const pinocchio::JointIndex joint =
      state_->get_pinocchio()->frames[d->frame].parent;
  pinocchio::getJointAccelerationDerivatives(
      *state_->get_pinocchio().get(), *d->pinocchio, joint, pinocchio::LOCAL,
      d->v_partial_dq, d->a_partial_dq, d->a_partial_dv, d->a_partial_da);
  const std::size_t nv = state_->get_nv();
  pinocchio::skew(d->vv, d->vv_skew);
  pinocchio::skew(d->vw, d->vw_skew);
  d->fXjdv_dq.noalias() = d->fXj * d->v_partial_dq;
  d->fXjda_dq.noalias() = d->fXj * d->a_partial_dq;
  d->fXjda_dv.noalias() = d->fXj * d->a_partial_dv;

  d->da0_dx.leftCols(nv).row(0) = d->fXjda_dq.row(0);
  d->da0_dx.leftCols(nv).row(0).noalias() +=
      d->vw_skew.row(0) * d->fXjdv_dq.template topRows<3>();
  d->da0_dx.leftCols(nv).row(0).noalias() -=
      d->vv_skew.row(0) * d->fXjdv_dq.template bottomRows<3>();

  d->da0_dx.leftCols(nv).row(1) = d->fXjda_dq.row(2);
  d->da0_dx.leftCols(nv).row(1).noalias() +=
      d->vw_skew.row(2) * d->fXjdv_dq.template topRows<3>();
  d->da0_dx.leftCols(nv).row(1).noalias() -=
      d->vv_skew.row(2) * d->fXjdv_dq.template bottomRows<3>();

  d->da0_dx.rightCols(nv).row(0) = d->fXjda_dv.row(0);
  d->da0_dx.rightCols(nv).row(0).noalias() +=
      d->vw_skew.row(0) * d->fJf.template topRows<3>();
  d->da0_dx.rightCols(nv).row(0).noalias() -=
      d->vv_skew.row(0) * d->fJf.template bottomRows<3>();

  d->da0_dx.rightCols(nv).row(1) = d->fXjda_dv.row(2);
  d->da0_dx.rightCols(nv).row(1).noalias() +=
      d->vw_skew.row(2) * d->fJf.template topRows<3>();
  d->da0_dx.rightCols(nv).row(1).noalias() -=
      d->vv_skew.row(2) * d->fJf.template bottomRows<3>();

  if (gains_[0] != 0.) {
    const Eigen::Ref<const Matrix3s> oRf = d->pinocchio->oMf[id_].rotation();
    d->oRf(0, 0) = oRf(0, 0);
    d->oRf(1, 0) = oRf(2, 0);
    d->oRf(0, 1) = oRf(0, 2);
    d->oRf(1, 1) = oRf(2, 2);
    d->da0_dx.leftCols(nv).noalias() += gains_[0] * d->oRf * d->Jc;
  }
  if (gains_[1] != 0.) {
    d->da0_dx.leftCols(nv).row(0).noalias() +=
        gains_[1] * d->fXj.row(0) * d->v_partial_dq;
    d->da0_dx.leftCols(nv).row(1).noalias() +=
        gains_[1] * d->fXj.row(2) * d->v_partial_dq;
    d->da0_dx.rightCols(nv).row(0).noalias() +=
        gains_[1] * d->fXj.row(0) * d->a_partial_da;
    d->da0_dx.rightCols(nv).row(1).noalias() +=
        gains_[1] * d->fXj.row(2) * d->a_partial_da;
  }
}

template <typename Scalar>
void ContactModel2DTpl<Scalar>::updateForce(
    const boost::shared_ptr<ContactDataAbstract>& data, const VectorXs& force) {
  if (force.size() != 2) {
    throw_pretty("Invalid argument: "
                 << "lambda has wrong dimension (it should be 2)");
  }
  Data* d = static_cast<Data*>(data.get());
  const Eigen::Ref<const Matrix3s> R = d->jMf.rotation();
  data->f.linear() = R.col(0) * force[0] + R.col(2) * force[1];
  data->f.angular().setZero();
  data->fext.linear() = R.col(0) * force[0] + R.col(2) * force[1];
  data->fext.angular() = d->jMf.translation().cross(data->fext.linear());
}

template <typename Scalar>
boost::shared_ptr<ContactDataAbstractTpl<Scalar> >
ContactModel2DTpl<Scalar>::createData(pinocchio::DataTpl<Scalar>* const data) {
  return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this,
                                      data);
}

template <typename Scalar>
void ContactModel2DTpl<Scalar>::print(std::ostream& os) const {
  os << "ContactModel2D {frame=" << state_->get_pinocchio()->frames[id_].name
     << "}";
}

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

template <typename Scalar>
const typename MathBaseTpl<Scalar>::Vector2s&
ContactModel2DTpl<Scalar>::get_gains() const {
  return gains_;
}

template <typename Scalar>
void ContactModel2DTpl<Scalar>::set_reference(const Vector2s& reference) {
  xref_ = reference;
}

}  // namespace crocoddyl


Generated by: GCOVR (Version 4.2)

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