9 #include "contact3d.hpp" 12 namespace newcontacts {
14 template <
typename Scalar>
15 ContactModel3DTpl<Scalar>::ContactModel3DTpl(
16 boost::shared_ptr<StateMultibody> state,
const pinocchio::FrameIndex
id,
17 const Vector3s& xref,
const std::size_t nu,
const Vector2s& gains,
18 const pinocchio::ReferenceFrame type)
19 : Base(state, id, Vector3s::Zero(), nu, Vector2s::Zero()),
24 template <
typename Scalar>
25 ContactModel3DTpl<Scalar>::ContactModel3DTpl(
26 boost::shared_ptr<StateMultibody> state,
const pinocchio::FrameIndex
id,
27 const Vector3s& xref,
const Vector2s& gains,
28 const pinocchio::ReferenceFrame type)
29 : Base(state, id, Vector3s::Zero(), Vector2s::Zero()),
34 template <
typename Scalar>
35 ContactModel3DTpl<Scalar>::~ContactModel3DTpl() {}
37 template <
typename Scalar>
38 void ContactModel3DTpl<Scalar>::calc(
39 const boost::shared_ptr<crocoddyl::ContactDataAbstractTpl<Scalar>>& data,
40 const Eigen::Ref<const VectorXs>& x) {
41 Data* d =
static_cast<Data*
>(data.get());
42 pinocchio::updateFramePlacement(*state_->get_pinocchio().get(), *d->pinocchio,
44 pinocchio::getFrameJacobian(*state_->get_pinocchio().get(), *d->pinocchio,
45 id_, pinocchio::LOCAL, d->fJf);
46 d->v = pinocchio::getFrameVelocity(*state_->get_pinocchio().get(),
47 *d->pinocchio, id_, pinocchio::LOCAL);
48 d->vv = d->v.linear();
49 d->vw = d->v.angular();
50 d->Jc = d->fJf.template topRows<3>();
52 pinocchio::getFrameClassicalAcceleration(
53 *state_->get_pinocchio().get(), *d->pinocchio, id_, pinocchio::LOCAL)
55 d->oRf = d->pinocchio->oMf[id_].rotation();
57 if (gains_[0] != 0.) {
58 d->a0_temp_ += gains_[0] * d->oRf.transpose() *
59 (d->pinocchio->oMf[id_].translation() - xref_);
62 if (gains_[1] != 0.) {
63 d->a0_temp_ += gains_[1] * d->vv;
66 if (type_ == pinocchio::LOCAL_WORLD_ALIGNED || type_ == pinocchio::WORLD) {
67 d->a0 = d->oRf * d->a0_temp_;
68 d->Jc = d->oRf * d->fJf.template topRows<3>();
72 template <
typename Scalar>
73 void ContactModel3DTpl<Scalar>::calcDiff(
74 const boost::shared_ptr<crocoddyl::ContactDataAbstractTpl<Scalar>>& data,
75 const Eigen::Ref<const VectorXs>&) {
76 Data* d =
static_cast<Data*
>(data.get());
77 const pinocchio::JointIndex joint =
78 state_->get_pinocchio()->frames[d->frame].parent;
79 pinocchio::getJointAccelerationDerivatives(
80 *state_->get_pinocchio().get(), *d->pinocchio, joint, pinocchio::LOCAL,
81 d->v_partial_dq, d->a_partial_dq, d->a_partial_dv, d->a_partial_da);
82 const std::size_t nv = state_->get_nv();
83 pinocchio::skew(d->vv, d->vv_skew);
84 pinocchio::skew(d->vw, d->vw_skew);
86 d->fXjdv_dq.noalias() = d->fXj * d->v_partial_dq;
87 d->fXjda_dq.noalias() = d->fXj * d->a_partial_dq;
88 d->fXjda_dv.noalias() = d->fXj * d->a_partial_dv;
90 d->da0_dx_temp_.leftCols(nv) = d->fXjda_dq.template topRows<3>();
91 d->da0_dx_temp_.leftCols(nv).noalias() +=
92 d->vw_skew * d->fXjdv_dq.template topRows<3>();
93 d->da0_dx_temp_.leftCols(nv).noalias() -=
94 d->vv_skew * d->fXjdv_dq.template bottomRows<3>();
95 d->da0_dx_temp_.rightCols(nv) = d->fXjda_dv.template topRows<3>();
96 d->da0_dx_temp_.rightCols(nv).noalias() +=
97 d->vw_skew * d->fJf.template topRows<3>();
98 d->da0_dx_temp_.rightCols(nv).noalias() -=
99 d->vv_skew * d->fJf.template bottomRows<3>();
101 if (gains_[0] != 0.) {
103 d->oRf.transpose() * (d->pinocchio->oMf[id_].translation() - xref_),
105 d->da0_dx_temp_.leftCols(nv).noalias() +=
106 gains_[0] * (d->tmp_skew_ * d->fJf.template bottomRows<3>() +
107 d->fJf.template topRows<3>());
110 if (gains_[1] != 0.) {
111 d->da0_dx_temp_.leftCols(nv).noalias() +=
112 gains_[1] * d->fXjdv_dq.template topRows<3>();
113 d->da0_dx_temp_.rightCols(nv).noalias() +=
114 gains_[1] * d->fJf.template topRows<3>();
117 d->da0_dx = d->da0_dx_temp_;
119 if (type_ == pinocchio::LOCAL_WORLD_ALIGNED || type_ == pinocchio::WORLD) {
122 d->a0_temp_ = pinocchio::getFrameClassicalAcceleration(
123 *state_->get_pinocchio().get(), *d->pinocchio, id_,
126 if (gains_[0] != 0.) {
127 d->a0_temp_ += gains_[0] * d->oRf.transpose() *
128 (d->pinocchio->oMf[id_].translation() - xref_);
130 if (gains_[1] != 0.) {
131 d->a0_temp_ += gains_[1] * d->vv;
135 pinocchio::skew(d->oRf * d->a0_temp_, d->tmp_skew_);
136 d->da0_dx.leftCols(nv).noalias() =
137 d->oRf * d->da0_dx_temp_.leftCols(nv) -
138 d->tmp_skew_ * d->oRf * d->fJf.template bottomRows<3>();
139 d->da0_dx.rightCols(nv).noalias() = d->oRf * d->da0_dx_temp_.rightCols(nv);
143 template <
typename Scalar>
144 void ContactModel3DTpl<Scalar>::updateForce(
145 const boost::shared_ptr<crocoddyl::ContactDataAbstractTpl<Scalar>>& data,
146 const VectorXs& force) {
147 if (force.size() != 3) {
148 throw_pretty(
"Invalid argument: " 149 <<
"lambda has wrong dimension (it should be 3)");
151 Data* d =
static_cast<Data*
>(data.get());
152 d->oRf = d->pinocchio->oMf[id_].rotation();
153 if (type_ == pinocchio::LOCAL) {
154 data->f = d->jMf.act(pinocchio::ForceTpl<Scalar>(force, Vector3s::Zero()));
156 if (type_ == pinocchio::LOCAL_WORLD_ALIGNED || type_ == pinocchio::WORLD) {
157 data->f = d->jMf.act(pinocchio::ForceTpl<Scalar>(d->oRf.transpose() * force,
160 pinocchio::skew(d->oRf.transpose() * force, d->tmp_skew_);
161 d->drnea_skew_term_ =
162 -d->fJf.topRows(3).transpose() * d->tmp_skew_ * d->fJf.bottomRows(3);
166 template <
typename Scalar>
167 boost::shared_ptr<crocoddyl::ContactDataAbstractTpl<Scalar>>
168 ContactModel3DTpl<Scalar>::createData(pinocchio::DataTpl<Scalar>*
const data) {
169 return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(),
this,
173 template <
typename Scalar>
174 void ContactModel3DTpl<Scalar>::print(std::ostream& os)
const {
175 os <<
"ContactModel3D {frame=" << state_->get_pinocchio()->frames[id_].name
179 template <
typename Scalar>
180 const typename crocoddyl::MathBaseTpl<Scalar>::Vector3s&
181 ContactModel3DTpl<Scalar>::get_reference()
const {
185 template <
typename Scalar>
186 const typename crocoddyl::MathBaseTpl<Scalar>::Vector2s&
187 ContactModel3DTpl<Scalar>::get_gains()
const {
191 template <
typename Scalar>
192 void ContactModel3DTpl<Scalar>::set_reference(
const Vector3s& reference) {
196 template <
typename Scalar>
197 void ContactModel3DTpl<Scalar>::set_type(
const pinocchio::ReferenceFrame type) {
201 template <
typename Scalar>
202 const pinocchio::ReferenceFrame ContactModel3DTpl<Scalar>::get_type()
const {
Definition: contact-force.hxx:11