9 #include "contact3d.hpp"
13 template <
typename Scalar>
14 ContactModel3DTpl<Scalar>::ContactModel3DTpl(
15 boost::shared_ptr<StateMultibody> state,
const pinocchio::FrameIndex
id,
16 const Vector3s& xref,
const std::size_t nu,
const Vector2s& gains,
17 const pinocchio::ReferenceFrame type)
18 : Base(state, id, Vector3s::Zero(), nu, Vector2s::Zero()),
23 template <
typename Scalar>
24 ContactModel3DTpl<Scalar>::ContactModel3DTpl(
25 boost::shared_ptr<StateMultibody> state,
const pinocchio::FrameIndex
id,
26 const Vector3s& xref,
const Vector2s& gains,
27 const pinocchio::ReferenceFrame type)
28 : Base(state, id, Vector3s::Zero(), Vector2s::Zero()),
33 template <
typename Scalar>
34 ContactModel3DTpl<Scalar>::~ContactModel3DTpl() {}
36 template <
typename Scalar>
37 void ContactModel3DTpl<Scalar>::calc(
38 const boost::shared_ptr<crocoddyl::ContactDataAbstractTpl<Scalar>>& data,
39 const Eigen::Ref<const VectorXs>& x) {
40 Data* d =
static_cast<Data*
>(data.get());
41 pinocchio::updateFramePlacement(*state_->get_pinocchio().get(), *d->pinocchio,
43 pinocchio::getFrameJacobian(*state_->get_pinocchio().get(), *d->pinocchio,
44 id_, pinocchio::LOCAL, d->fJf);
45 d->v = pinocchio::getFrameVelocity(*state_->get_pinocchio().get(),
46 *d->pinocchio, id_, pinocchio::LOCAL);
47 d->vv = d->v.linear();
48 d->vw = d->v.angular();
49 d->Jc = d->fJf.template topRows<3>();
51 pinocchio::getFrameClassicalAcceleration(
52 *state_->get_pinocchio().get(), *d->pinocchio, id_, pinocchio::LOCAL)
54 d->oRf = d->pinocchio->oMf[id_].rotation();
56 if (gains_[0] != 0.) {
57 d->a0_temp_ += gains_[0] * d->oRf.transpose() *
58 (d->pinocchio->oMf[id_].translation() - xref_);
61 if (gains_[1] != 0.) {
62 d->a0_temp_ += gains_[1] * d->vv;
65 if (type_ == pinocchio::LOCAL_WORLD_ALIGNED || type_ == pinocchio::WORLD) {
66 d->a0 = d->oRf * d->a0_temp_;
67 d->Jc = d->oRf * d->fJf.template topRows<3>();
71 template <
typename Scalar>
72 void ContactModel3DTpl<Scalar>::calcDiff(
73 const boost::shared_ptr<crocoddyl::ContactDataAbstractTpl<Scalar>>& data,
74 const Eigen::Ref<const VectorXs>&) {
75 Data* d =
static_cast<Data*
>(data.get());
76 const pinocchio::JointIndex joint =
77 state_->get_pinocchio()->frames[d->frame].parent;
78 pinocchio::getJointAccelerationDerivatives(
79 *state_->get_pinocchio().get(), *d->pinocchio, joint, pinocchio::LOCAL,
80 d->v_partial_dq, d->a_partial_dq, d->a_partial_dv, d->a_partial_da);
81 const std::size_t nv = state_->get_nv();
82 pinocchio::skew(d->vv, d->vv_skew);
83 pinocchio::skew(d->vw, d->vw_skew);
85 d->fXjdv_dq.noalias() = d->fXj * d->v_partial_dq;
86 d->fXjda_dq.noalias() = d->fXj * d->a_partial_dq;
87 d->fXjda_dv.noalias() = d->fXj * d->a_partial_dv;
89 d->da0_dx_temp_.leftCols(nv) = d->fXjda_dq.template topRows<3>();
90 d->da0_dx_temp_.leftCols(nv).noalias() +=
91 d->vw_skew * d->fXjdv_dq.template topRows<3>();
92 d->da0_dx_temp_.leftCols(nv).noalias() -=
93 d->vv_skew * d->fXjdv_dq.template bottomRows<3>();
94 d->da0_dx_temp_.rightCols(nv) = d->fXjda_dv.template topRows<3>();
95 d->da0_dx_temp_.rightCols(nv).noalias() +=
96 d->vw_skew * d->fJf.template topRows<3>();
97 d->da0_dx_temp_.rightCols(nv).noalias() -=
98 d->vv_skew * d->fJf.template bottomRows<3>();
100 if (gains_[0] != 0.) {
102 d->oRf.transpose() * (d->pinocchio->oMf[id_].translation() - xref_),
104 d->da0_dx_temp_.leftCols(nv).noalias() +=
105 gains_[0] * (d->tmp_skew_ * d->fJf.template bottomRows<3>() +
106 d->fJf.template topRows<3>());
109 if (gains_[1] != 0.) {
110 d->da0_dx_temp_.leftCols(nv).noalias() +=
111 gains_[1] * d->fXjdv_dq.template topRows<3>();
112 d->da0_dx_temp_.rightCols(nv).noalias() +=
113 gains_[1] * d->fJf.template topRows<3>();
116 d->da0_dx = d->da0_dx_temp_;
118 if (type_ == pinocchio::LOCAL_WORLD_ALIGNED || type_ == pinocchio::WORLD) {
121 d->a0_temp_ = pinocchio::getFrameClassicalAcceleration(
122 *state_->get_pinocchio().get(), *d->pinocchio, id_,
125 if (gains_[0] != 0.) {
126 d->a0_temp_ += gains_[0] * d->oRf.transpose() *
127 (d->pinocchio->oMf[id_].translation() - xref_);
129 if (gains_[1] != 0.) {
130 d->a0_temp_ += gains_[1] * d->vv;
134 pinocchio::skew(d->oRf * d->a0_temp_, d->tmp_skew_);
135 d->da0_dx.leftCols(nv).noalias() =
136 d->oRf * d->da0_dx_temp_.leftCols(nv) -
137 d->tmp_skew_ * d->oRf * d->fJf.template bottomRows<3>();
138 d->da0_dx.rightCols(nv).noalias() = d->oRf * d->da0_dx_temp_.rightCols(nv);
142 template <
typename Scalar>
143 void ContactModel3DTpl<Scalar>::updateForce(
144 const boost::shared_ptr<crocoddyl::ContactDataAbstractTpl<Scalar>>& data,
145 const VectorXs& force) {
146 if (force.size() != 3) {
147 throw_pretty(
"Invalid argument: "
148 <<
"lambda has wrong dimension (it should be 3)");
150 Data* d =
static_cast<Data*
>(data.get());
151 d->oRf = d->pinocchio->oMf[id_].rotation();
152 if (type_ == pinocchio::LOCAL) {
153 data->f = d->jMf.act(pinocchio::ForceTpl<Scalar>(force, Vector3s::Zero()));
155 if (type_ == pinocchio::LOCAL_WORLD_ALIGNED || type_ == pinocchio::WORLD) {
156 data->f = d->jMf.act(pinocchio::ForceTpl<Scalar>(d->oRf.transpose() * force,
159 pinocchio::skew(d->oRf.transpose() * force, d->tmp_skew_);
160 d->drnea_skew_term_ =
161 -d->fJf.topRows(3).transpose() * d->tmp_skew_ * d->fJf.bottomRows(3);
165 template <
typename Scalar>
166 boost::shared_ptr<crocoddyl::ContactDataAbstractTpl<Scalar>>
167 ContactModel3DTpl<Scalar>::createData(pinocchio::DataTpl<Scalar>*
const data) {
168 return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(),
this,
172 template <
typename Scalar>
173 void ContactModel3DTpl<Scalar>::print(std::ostream& os)
const {
174 os <<
"ContactModel3D {frame=" << state_->get_pinocchio()->frames[id_].name
178 template <
typename Scalar>
179 const typename crocoddyl::MathBaseTpl<Scalar>::Vector3s&
180 ContactModel3DTpl<Scalar>::get_reference()
const {
184 template <
typename Scalar>
185 const typename crocoddyl::MathBaseTpl<Scalar>::Vector2s&
186 ContactModel3DTpl<Scalar>::get_gains()
const {
190 template <
typename Scalar>
191 void ContactModel3DTpl<Scalar>::set_reference(
const Vector3s& reference) {
195 template <
typename Scalar>
196 void ContactModel3DTpl<Scalar>::set_type(
const pinocchio::ReferenceFrame type) {
200 template <
typename Scalar>
201 const pinocchio::ReferenceFrame ContactModel3DTpl<Scalar>::get_type()
const {
Definition: contact-force.hxx:11