Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/crocoddyl/multibody/contacts/contact-1d.hxx	Lines:	126	135	93.3 %
Date:	2024-02-13 11:12:33	Branches:	232	465	49.9 %


///////////////////////////////////////////////////////////////////////////////
// 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>
ContactModel1DTpl<Scalar>::ContactModel1DTpl(
    boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
    Scalar xref, const pinocchio::ReferenceFrame type, const Matrix3s& rotation,
    const std::size_t nu, const Vector2s& gains)
    : Base(state, type, 1, nu), xref_(xref), Raxis_(rotation), gains_(gains) {
  id_ = id;
}

template <typename Scalar>
ContactModel1DTpl<Scalar>::ContactModel1DTpl(
    boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
    Scalar xref, const pinocchio::ReferenceFrame type, const Vector2s& gains)
    : Base(state, type, 1), xref_(xref), gains_(gains) {
  id_ = id;
  Raxis_ = Matrix3s::Identity();
}

#pragma GCC diagnostic push  // TODO: Remove once the deprecated FrameXX has
                             // been removed in a future release
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"

template <typename Scalar>
ContactModel1DTpl<Scalar>::ContactModel1DTpl(
    boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
    Scalar xref, const std::size_t nu, const Vector2s& gains)
    : Base(state, pinocchio::ReferenceFrame::LOCAL, 1, nu),
      xref_(xref),
      gains_(gains) {
  id_ = id;
  Raxis_ = Matrix3s::Identity();
  std::cerr << "Deprecated: Use constructor that passes the type of contact, "
               "this assumes is pinocchio::LOCAL."
            << std::endl;
}

template <typename Scalar>
ContactModel1DTpl<Scalar>::ContactModel1DTpl(
    boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
    Scalar xref, const Vector2s& gains)
    : Base(state, pinocchio::ReferenceFrame::LOCAL, 1),
      xref_(xref),
      gains_(gains) {
  id_ = id;
  Raxis_ = Matrix3s::Identity();
  std::cerr << "Deprecated: Use constructor that passes the type of contact, "
               "this assumes is pinocchio::LOCAL."
            << std::endl;
}

#pragma GCC diagnostic pop

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

template <typename Scalar>
void ContactModel1DTpl<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->a0_local =
      pinocchio::getFrameClassicalAcceleration(
          *state_->get_pinocchio().get(), *d->pinocchio, id_, pinocchio::LOCAL)
          .linear();

  const Eigen::Ref<const Matrix3s> oRf = d->pinocchio->oMf[id_].rotation();
  if (gains_[0] != 0.) {
    d->dp = d->pinocchio->oMf[id_].translation() -
            (xref_ * Raxis_ * Vector3s::UnitZ());
    d->dp_local.noalias() = oRf.transpose() * d->dp;
    d->a0_local += gains_[0] * d->dp_local;
  }
  if (gains_[1] != 0.) {
    d->a0_local += gains_[1] * d->v.linear();
  }
  switch (type_) {
    case pinocchio::ReferenceFrame::LOCAL:
      d->Jc.row(0) = (Raxis_ * d->fJf.template topRows<3>()).row(2);
      d->a0[0] = (Raxis_ * d->a0_local)[2];
      break;
    case pinocchio::ReferenceFrame::WORLD:
    case pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED:
      d->Jc.row(0) = (Raxis_ * oRf * d->fJf.template topRows<3>()).row(2);
      d->a0[0] = (Raxis_ * oRf * d->a0_local)[2];
      break;
  }
}

template <typename Scalar>
void ContactModel1DTpl<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->v.linear(), d->vv_skew);
  pinocchio::skew(d->v.angular(), 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_local_dx.leftCols(nv) = d->fXjda_dq.template topRows<3>();
  d->da0_local_dx.leftCols(nv).noalias() +=
      d->vw_skew * d->fXjdv_dq.template topRows<3>();
  d->da0_local_dx.leftCols(nv).noalias() -=
      d->vv_skew * d->fXjdv_dq.template bottomRows<3>();
  d->da0_local_dx.rightCols(nv) = d->fXjda_dv.template topRows<3>();
  d->da0_local_dx.rightCols(nv).noalias() +=
      d->vw_skew * d->fJf.template topRows<3>();
  d->da0_local_dx.rightCols(nv).noalias() -=
      d->vv_skew * d->fJf.template bottomRows<3>();
  const Eigen::Ref<const Matrix3s> oRf = d->pinocchio->oMf[id_].rotation();

  if (gains_[0] != 0.) {
    pinocchio::skew(d->dp_local, d->dp_skew);
    d->da0_local_dx.leftCols(nv).noalias() +=
        gains_[0] * d->dp_skew * d->fJf.template bottomRows<3>();
    d->da0_local_dx.leftCols(nv).noalias() +=
        gains_[0] * d->fJf.template topRows<3>();
  }
  if (gains_[1] != 0.) {
    d->da0_local_dx.leftCols(nv).noalias() +=
        gains_[1] * d->fXjdv_dq.template topRows<3>();
    d->da0_local_dx.rightCols(nv).noalias() +=
        gains_[1] * d->fJf.template topRows<3>();
  }
  switch (type_) {
    case pinocchio::ReferenceFrame::LOCAL:
      d->da0_dx.row(0) = (Raxis_ * d->da0_local_dx).row(2);
      break;
    case pinocchio::ReferenceFrame::WORLD:
    case pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED:
      // Recalculate the constrained accelerations after imposing contact
      // constraints. This is necessary for the forward-dynamics case.
      d->a0_local = pinocchio::getFrameClassicalAcceleration(
                        *state_->get_pinocchio().get(), *d->pinocchio, id_,
                        pinocchio::LOCAL)
                        .linear();
      if (gains_[0] != 0.) {
        d->a0_local += gains_[0] * d->dp_local;
      }
      if (gains_[1] != 0.) {
        d->a0_local += gains_[1] * d->v.linear();
      }
      d->a0[0] = (Raxis_ * oRf * d->a0_local)[2];

      pinocchio::skew((Raxis_ * oRf * d->a0_local).template head<3>(),
                      d->a0_skew);
      d->a0_world_skew.noalias() = d->a0_skew * Raxis_ * oRf;
      d->da0_dx.row(0) = (Raxis_ * oRf * d->da0_local_dx).row(2);
      d->da0_dx.leftCols(nv).row(0) -=
          (d->a0_world_skew * d->fJf.template bottomRows<3>()).row(2);
      break;
  }
}

template <typename Scalar>
void ContactModel1DTpl<Scalar>::updateForce(
    const boost::shared_ptr<ContactDataAbstract>& data, const VectorXs& force) {
  if (force.size() != 1) {
    throw_pretty("Invalid argument: "
                 << "lambda has wrong dimension (it should be 1)");
  }
  Data* d = static_cast<Data*>(data.get());
  const Eigen::Ref<const Matrix3s> R = d->jMf.rotation();
  data->f.linear()[2] = force[0];
  data->f.linear().template head<2>().setZero();
  data->f.angular().setZero();
  switch (type_) {
    case pinocchio::ReferenceFrame::LOCAL:
      data->fext.linear() = (R * Raxis_.transpose()).col(2) * force[0];
      data->fext.angular() = d->jMf.translation().cross(data->fext.linear());
      data->dtau_dq.setZero();
      break;
    case pinocchio::ReferenceFrame::WORLD:
    case pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED:
      const Eigen::Ref<const Matrix3s> oRf = d->pinocchio->oMf[id_].rotation();
      d->f_local.linear().noalias() =
          (oRf.transpose() * Raxis_.transpose()).col(2) * force[0];
      d->f_local.angular().setZero();
      data->fext = data->jMf.act(d->f_local);
      pinocchio::skew(d->f_local.linear(), d->f_skew);
      d->fJf_df.noalias() = d->f_skew * d->fJf.template bottomRows<3>();
      data->dtau_dq.noalias() =
          -d->fJf.template topRows<3>().transpose() * d->fJf_df;
      break;
  }
}

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

template <typename Scalar>
void ContactModel1DTpl<Scalar>::print(std::ostream& os) const {
  os << "ContactModel1D {frame=" << state_->get_pinocchio()->frames[id_].name
     << ", axis=" << (Raxis_ * Vector3s::UnitZ()).transpose() << "}";
}

template <typename Scalar>
const Scalar ContactModel1DTpl<Scalar>::get_reference() const {
  return xref_;
}

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

template <typename Scalar>
const typename MathBaseTpl<Scalar>::Matrix3s&
ContactModel1DTpl<Scalar>::get_axis_rotation() const {
  return Raxis_;
}

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

template <typename Scalar>
void ContactModel1DTpl<Scalar>::set_axis_rotation(const Matrix3s& rotation) {
  Raxis_ = rotation;
}

}  // 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		102	ContactModel1DTpl<Scalar>::ContactModel1DTpl(
14			boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
15			Scalar xref, const pinocchio::ReferenceFrame type, const Matrix3s& rotation,
16			const std::size_t nu, const Vector2s& gains)
17	✓✗✓✗	102	: Base(state, type, 1, nu), xref_(xref), Raxis_(rotation), gains_(gains) {
18		102	id_ = id;
19		102	}
20
21			template <typename Scalar>
22		3	ContactModel1DTpl<Scalar>::ContactModel1DTpl(
23			boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
24			Scalar xref, const pinocchio::ReferenceFrame type, const Vector2s& gains)
25	✓✗✓✗	3	: Base(state, type, 1), xref_(xref), gains_(gains) {
26		3	id_ = id;
27	✓✗✓✗	3	Raxis_ = Matrix3s::Identity();
28		3	}
29
30			#pragma GCC diagnostic push // TODO: Remove once the deprecated FrameXX has
31			// been removed in a future release
32			#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
33
34			template <typename Scalar>
35			ContactModel1DTpl<Scalar>::ContactModel1DTpl(
36			boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
37			Scalar xref, const std::size_t nu, const Vector2s& gains)
38			: Base(state, pinocchio::ReferenceFrame::LOCAL, 1, nu),
39			xref_(xref),
40			gains_(gains) {
41			id_ = id;
42			Raxis_ = Matrix3s::Identity();
43			std::cerr << "Deprecated: Use constructor that passes the type of contact, "
44			"this assumes is pinocchio::LOCAL."
45			<< std::endl;
46			}
47
48			template <typename Scalar>
49			ContactModel1DTpl<Scalar>::ContactModel1DTpl(
50			boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
51			Scalar xref, const Vector2s& gains)
52			: Base(state, pinocchio::ReferenceFrame::LOCAL, 1),
53			xref_(xref),
54			gains_(gains) {
55			id_ = id;
56			Raxis_ = Matrix3s::Identity();
57			std::cerr << "Deprecated: Use constructor that passes the type of contact, "
58			"this assumes is pinocchio::LOCAL."
59			<< std::endl;
60			}
61
62			#pragma GCC diagnostic pop
63
64			template <typename Scalar>
65		214	ContactModel1DTpl<Scalar>::~ContactModel1DTpl() {}
66
67			template <typename Scalar>
68		685	void ContactModel1DTpl<Scalar>::calc(
69			const boost::shared_ptr<ContactDataAbstract>& data,
70			const Eigen::Ref<const VectorXs>&) {
71		685	Data* d = static_cast<Data*>(data.get());
72	✓✗	685	pinocchio::updateFramePlacement(state_->get_pinocchio().get(), d->pinocchio,
73			id_);
74	✓✗	685	pinocchio::getFrameJacobian(state_->get_pinocchio().get(), d->pinocchio,
75		685	id_, pinocchio::LOCAL, d->fJf);
76	✓✗	685	d->v = pinocchio::getFrameVelocity(*state_->get_pinocchio().get(),
77		685	*d->pinocchio, id_);
78
79	✓✗✓✗ ✓✗	685	d->a0_local =
80			pinocchio::getFrameClassicalAcceleration(
81		685	state_->get_pinocchio().get(), d->pinocchio, id_, pinocchio::LOCAL)
82			.linear();
83
84	✓✗✓✗	685	const Eigen::Ref<const Matrix3s> oRf = d->pinocchio->oMf[id_].rotation();
85	✓✗✓✓	685	if (gains_[0] != 0.) {
86	✓✗✓✗ ✓✗✓✗ ✓✗	18	d->dp = d->pinocchio->oMf[id_].translation() -
87	✓✗	9	(xref_ * Raxis_ * Vector3s::UnitZ());
88	✓✗✓✗ ✓✗✓✗	9	d->dp_local.noalias() = oRf.transpose() * d->dp;
89	✓✗✓✗ ✓✗	9	d->a0_local += gains_[0] * d->dp_local;
90			}
91	✓✗✓✗	685	if (gains_[1] != 0.) {
92	✓✗✓✗ ✓✗✓✗	685	d->a0_local += gains_[1] * d->v.linear();
93			}
94	✓✓✗	685	switch (type_) {
95		235	case pinocchio::ReferenceFrame::LOCAL:
96	✓✗✓✗ ✓✗✓✗ ✓✗	235	d->Jc.row(0) = (Raxis_ * d->fJf.template topRows<3>()).row(2);
97	✓✗✓✗ ✓✗	235	d->a0[0] = (Raxis_ * d->a0_local)[2];
98		235	break;
99		450	case pinocchio::ReferenceFrame::WORLD:
100			case pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED:
101	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	450	d->Jc.row(0) = (Raxis_ * oRf * d->fJf.template topRows<3>()).row(2);
102	✓✗✓✗ ✓✗✓✗	450	d->a0[0] = (Raxis_ * oRf * d->a0_local)[2];
103		450	break;
104			}
105		685	}
106
107			template <typename Scalar>
108		35	void ContactModel1DTpl<Scalar>::calcDiff(
109			const boost::shared_ptr<ContactDataAbstract>& data,
110			const Eigen::Ref<const VectorXs>&) {
111		35	Data* d = static_cast<Data*>(data.get());
112		35	const pinocchio::JointIndex joint =
113		35	state_->get_pinocchio()->frames[d->frame].parent;
114	✓✗	35	pinocchio::getJointAccelerationDerivatives(
115		35	state_->get_pinocchio().get(), d->pinocchio, joint, pinocchio::LOCAL,
116		35	d->v_partial_dq, d->a_partial_dq, d->a_partial_dv, d->a_partial_da);
117		35	const std::size_t nv = state_->get_nv();
118	✓✗✓✗	35	pinocchio::skew(d->v.linear(), d->vv_skew);
119	✓✗✓✗	35	pinocchio::skew(d->v.angular(), d->vw_skew);
120	✓✗✓✗ ✓✗	35	d->fXjdv_dq.noalias() = d->fXj * d->v_partial_dq;
121	✓✗✓✗ ✓✗	35	d->fXjda_dq.noalias() = d->fXj * d->a_partial_dq;
122	✓✗✓✗ ✓✗	35	d->fXjda_dv.noalias() = d->fXj * d->a_partial_dv;
123	✓✗✓✗ ✓✗	35	d->da0_local_dx.leftCols(nv) = d->fXjda_dq.template topRows<3>();
124	✓✗✓✗ ✓✗	35	d->da0_local_dx.leftCols(nv).noalias() +=
125	✓✗✓✗	35	d->vw_skew * d->fXjdv_dq.template topRows<3>();
126	✓✗✓✗ ✓✗	35	d->da0_local_dx.leftCols(nv).noalias() -=
127	✓✗✓✗	35	d->vv_skew * d->fXjdv_dq.template bottomRows<3>();
128	✓✗✓✗ ✓✗	35	d->da0_local_dx.rightCols(nv) = d->fXjda_dv.template topRows<3>();
129	✓✗✓✗ ✓✗	35	d->da0_local_dx.rightCols(nv).noalias() +=
130	✓✗✓✗	35	d->vw_skew * d->fJf.template topRows<3>();
131	✓✗✓✗ ✓✗	35	d->da0_local_dx.rightCols(nv).noalias() -=
132	✓✗✓✗	35	d->vv_skew * d->fJf.template bottomRows<3>();
133	✓✗✓✗	35	const Eigen::Ref<const Matrix3s> oRf = d->pinocchio->oMf[id_].rotation();
134
135	✓✗✓✓	35	if (gains_[0] != 0.) {
136	✓✗	3	pinocchio::skew(d->dp_local, d->dp_skew);
137	✓✗✓✗ ✓✗✓✗	3	d->da0_local_dx.leftCols(nv).noalias() +=
138	✓✗✓✗ ✓✗	3	gains_[0] * d->dp_skew * d->fJf.template bottomRows<3>();
139	✓✗✓✗ ✓✗	3	d->da0_local_dx.leftCols(nv).noalias() +=
140	✓✗✓✗ ✓✗	3	gains_[0] * d->fJf.template topRows<3>();
141			}
142	✓✗✓✗	35	if (gains_[1] != 0.) {
143	✓✗✓✗ ✓✗	35	d->da0_local_dx.leftCols(nv).noalias() +=
144	✓✗✓✗ ✓✗	35	gains_[1] * d->fXjdv_dq.template topRows<3>();
145	✓✗✓✗ ✓✗	35	d->da0_local_dx.rightCols(nv).noalias() +=
146	✓✗✓✗ ✓✗	35	gains_[1] * d->fJf.template topRows<3>();
147			}
148	✓✓✗	35	switch (type_) {
149		13	case pinocchio::ReferenceFrame::LOCAL:
150	✓✗✓✗ ✓✗✓✗	13	d->da0_dx.row(0) = (Raxis_ * d->da0_local_dx).row(2);
151		13	break;
152		22	case pinocchio::ReferenceFrame::WORLD:
153			case pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED:
154			// Recalculate the constrained accelerations after imposing contact
155			// constraints. This is necessary for the forward-dynamics case.
156	✓✗✓✗ ✓✗	22	d->a0_local = pinocchio::getFrameClassicalAcceleration(
157		22	state_->get_pinocchio().get(), d->pinocchio, id_,
158			pinocchio::LOCAL)
159			.linear();
160	✓✗✓✓	22	if (gains_[0] != 0.) {
161	✓✗✓✗ ✓✗	2	d->a0_local += gains_[0] * d->dp_local;
162			}
163	✓✗✓✗	22	if (gains_[1] != 0.) {
164	✓✗✓✗ ✓✗✓✗	22	d->a0_local += gains_[1] * d->v.linear();
165			}
166	✓✗✓✗ ✓✗✓✗	22	d->a0[0] = (Raxis_ * oRf * d->a0_local)[2];
167
168	✓✗✓✗ ✓✗	22	pinocchio::skew((Raxis_ * oRf * d->a0_local).template head<3>(),
169	✓✗	22	d->a0_skew);
170	✓✗✓✗ ✓✗✓✗	22	d->a0_world_skew.noalias() = d->a0_skew * Raxis_ * oRf;
171	✓✗✓✗ ✓✗✓✗ ✓✗	22	d->da0_dx.row(0) = (Raxis_ * oRf * d->da0_local_dx).row(2);
172	✓✗✓✗ ✓✗✓✗	22	d->da0_dx.leftCols(nv).row(0) -=
173	✓✗✓✗	22	(d->a0_world_skew * d->fJf.template bottomRows<3>()).row(2);
174		22	break;
175			}
176		35	}
177
178			template <typename Scalar>
179		19	void ContactModel1DTpl<Scalar>::updateForce(
180			const boost::shared_ptr<ContactDataAbstract>& data, const VectorXs& force) {
181	✓✗✗✓	19	if (force.size() != 1) {
182			throw_pretty("Invalid argument: "
183			<< "lambda has wrong dimension (it should be 1)");
184			}
185		19	Data* d = static_cast<Data*>(data.get());
186	✓✗✓✗	19	const Eigen::Ref<const Matrix3s> R = d->jMf.rotation();
187	✓✗✓✗ ✓✗	19	data->f.linear()[2] = force[0];
188	✓✗✓✗ ✓✗	19	data->f.linear().template head<2>().setZero();
189	✓✗✓✗	19	data->f.angular().setZero();
190	✓✓✗	19	switch (type_) {
191		7	case pinocchio::ReferenceFrame::LOCAL:
192	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	7	data->fext.linear() = (R * Raxis_.transpose()).col(2) * force[0];
193	✓✗✓✗ ✓✗✓✗ ✓✗	7	data->fext.angular() = d->jMf.translation().cross(data->fext.linear());
194	✓✗	7	data->dtau_dq.setZero();
195		7	break;
196		12	case pinocchio::ReferenceFrame::WORLD:
197			case pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED:
198	✓✗✓✗	12	const Eigen::Ref<const Matrix3s> oRf = d->pinocchio->oMf[id_].rotation();
199	✓✗✓✗ ✓✗✓✗ ✓✗	12	d->f_local.linear().noalias() =
200	✓✗✓✗ ✓✗✓✗	12	(oRf.transpose() * Raxis_.transpose()).col(2) * force[0];
201	✓✗✓✗	12	d->f_local.angular().setZero();
202	✓✗✓✗	12	data->fext = data->jMf.act(d->f_local);
203	✓✗✓✗	12	pinocchio::skew(d->f_local.linear(), d->f_skew);
204	✓✗✓✗ ✓✗✓✗	12	d->fJf_df.noalias() = d->f_skew * d->fJf.template bottomRows<3>();
205	✓✗✓✗ ✓✗✓✗ ✓✗	12	data->dtau_dq.noalias() =
206	✓✗	12	-d->fJf.template topRows<3>().transpose() * d->fJf_df;
207		12	break;
208			}
209		19	}
210
211			template <typename Scalar>
212			boost::shared_ptr<ContactDataAbstractTpl<Scalar> >
213		735	ContactModel1DTpl<Scalar>::createData(pinocchio::DataTpl<Scalar>* const data) {
214			return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this,
215	✓✗	735	data);
216			}
217
218			template <typename Scalar>
219		15	void ContactModel1DTpl<Scalar>::print(std::ostream& os) const {
220		15	os << "ContactModel1D {frame=" << state_->get_pinocchio()->frames[id_].name
221	✓✗✓✗ ✓✗✓✗	30	<< ", axis=" << (Raxis_ * Vector3s::UnitZ()).transpose() << "}";
222		15	}
223
224			template <typename Scalar>
225			const Scalar ContactModel1DTpl<Scalar>::get_reference() const {
226			return xref_;
227			}
228
229			template <typename Scalar>
230			const typename MathBaseTpl<Scalar>::Vector2s&
231			ContactModel1DTpl<Scalar>::get_gains() const {
232			return gains_;
233			}
234
235			template <typename Scalar>
236			const typename MathBaseTpl<Scalar>::Matrix3s&
237			ContactModel1DTpl<Scalar>::get_axis_rotation() const {
238			return Raxis_;
239			}
240
241			template <typename Scalar>
242			void ContactModel1DTpl<Scalar>::set_reference(const Scalar reference) {
243			xref_ = reference;
244			}
245
246			template <typename Scalar>
247		3	void ContactModel1DTpl<Scalar>::set_axis_rotation(const Matrix3s& rotation) {
248		3	Raxis_ = rotation;
249		3	}
250
251			} // namespace crocoddyl