9 #include "contact1d.hpp" 13 template <
typename Scalar>
14 ContactModel1DTpl<Scalar>::ContactModel1DTpl(
15 boost::shared_ptr<StateMultibody> state,
const pinocchio::FrameIndex
id,
16 const Vector3s& xref,
const std::size_t nu,
const Vector2s& gains,
17 const Vector3MaskType& mask,
const pinocchio::ReferenceFrame type)
18 : Base(state, id, Scalar(0.), nu, Vector2s::Zero()),
24 template <
typename Scalar>
25 ContactModel1DTpl<Scalar>::ContactModel1DTpl(
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, Scalar(0.), Vector2s::Zero()),
32 mask_(Vector3MaskType::z),
35 template <
typename Scalar>
36 ContactModel1DTpl<Scalar>::~ContactModel1DTpl() {}
38 template <
typename Scalar>
39 void ContactModel1DTpl<Scalar>::calc(
40 const boost::shared_ptr<crocoddyl::ContactDataAbstractTpl<Scalar>>& data,
41 const Eigen::Ref<const VectorXs>& x) {
42 Data* d =
static_cast<Data*
>(data.get());
43 pinocchio::updateFramePlacement(*state_->get_pinocchio().get(), *d->pinocchio,
45 pinocchio::getFrameJacobian(*state_->get_pinocchio().get(), *d->pinocchio,
46 id_, pinocchio::LOCAL, d->fJf);
48 pinocchio::getFrameClassicalAcceleration(
49 *state_->get_pinocchio().get(), *d->pinocchio, id_, pinocchio::LOCAL)
51 d->vv = d->fJf.template topRows<3>() * x.tail(state_->get_nv());
52 d->vw = d->fJf.template bottomRows<3>() * x.tail(state_->get_nv());
53 d->oRf = d->pinocchio->oMf[id_].rotation();
54 d->Jc.row(0) = d->fJf.row(mask_);
56 if (gains_[0] != 0.) {
57 d->a0_3d_ += gains_[0] * d->oRf.transpose() *
58 (d->pinocchio->oMf[id_].translation() - xref_);
61 if (gains_[1] != 0.) {
62 d->a0_3d_ += gains_[1] * d->vv;
65 d->a0[0] = d->a0_3d_[mask_];
67 if (type_ == pinocchio::LOCAL_WORLD_ALIGNED || type_ == pinocchio::WORLD) {
68 d->a0[0] = (d->oRf * d->a0_3d_)[mask_];
69 d->Jc.row(0) = (d->oRf * d->fJf.template topRows<3>()).row(mask_);
73 template <
typename Scalar>
74 void ContactModel1DTpl<Scalar>::calcDiff(
75 const boost::shared_ptr<crocoddyl::ContactDataAbstractTpl<Scalar>>& data,
76 const Eigen::Ref<const VectorXs>&) {
77 Data* d =
static_cast<Data*
>(data.get());
78 const pinocchio::JointIndex joint =
79 state_->get_pinocchio()->frames[d->frame].parent;
80 pinocchio::getJointAccelerationDerivatives(
81 *state_->get_pinocchio().get(), *d->pinocchio, joint, pinocchio::LOCAL,
82 d->v_partial_dq, d->a_partial_dq, d->a_partial_dv, d->a_partial_da);
83 const std::size_t nv = state_->get_nv();
84 pinocchio::skew(d->vv, d->vv_skew);
85 pinocchio::skew(d->vw, d->vw_skew);
87 d->fXjdv_dq.noalias() = d->fXj * d->v_partial_dq;
88 d->fXjda_dq.noalias() = d->fXj * d->a_partial_dq;
89 d->fXjda_dv.noalias() = d->fXj * d->a_partial_dv;
91 d->da0_dx_3d_.leftCols(nv) = d->fXjda_dq.template topRows<3>();
92 d->da0_dx_3d_.leftCols(nv).noalias() +=
93 d->vw_skew * d->fXjdv_dq.template topRows<3>();
94 d->da0_dx_3d_.leftCols(nv).noalias() -=
95 d->vv_skew * d->fXjdv_dq.template bottomRows<3>();
97 d->da0_dx_3d_.rightCols(nv) = d->fXjda_dv.template topRows<3>();
98 d->da0_dx_3d_.rightCols(nv).noalias() +=
99 d->vw_skew * d->fJf.template topRows<3>();
100 d->da0_dx_3d_.rightCols(nv).noalias() -=
101 d->vv_skew * d->fJf.template bottomRows<3>();
103 if (gains_[0] != 0.) {
105 d->oRf.transpose() * (d->pinocchio->oMf[id_].translation() - xref_),
107 d->da0_dx_3d_.leftCols(nv).noalias() +=
108 gains_[0] * (d->tmp_skew_ * d->fJf.template bottomRows<3>() +
109 d->fJf.template topRows<3>());
112 if (gains_[1] != 0.) {
113 d->da0_dx_3d_.leftCols(nv).noalias() +=
114 gains_[1] * d->fXjdv_dq.template topRows<3>();
115 d->da0_dx_3d_.rightCols(nv).noalias() +=
116 gains_[1] * d->fJf.template topRows<3>();
119 d->da0_dx.row(0) = d->da0_dx_3d_.row(mask_);
121 if (type_ == pinocchio::LOCAL_WORLD_ALIGNED || type_ == pinocchio::WORLD) {
124 d->a0_3d_ = pinocchio::getFrameClassicalAcceleration(
125 *state_->get_pinocchio().get(), *d->pinocchio, id_,
128 if (gains_[0] != 0.) {
129 d->a0_3d_ += gains_[0] * d->oRf.transpose() *
130 (d->pinocchio->oMf[id_].translation() - xref_);
132 if (gains_[1] != 0.) {
133 d->a0_3d_ += gains_[1] * d->vv;
137 pinocchio::skew(d->oRf * d->a0_3d_, d->tmp_skew_);
138 d->da0_dx.leftCols(nv).row(0) =
139 (d->oRf * d->da0_dx_3d_.leftCols(nv) -
140 d->tmp_skew_ * d->oRf * d->fJf.template bottomRows<3>())
142 d->da0_dx.rightCols(nv).row(0) =
143 (d->oRf * d->da0_dx_3d_.rightCols(nv)).row(mask_);
147 template <
typename Scalar>
148 void ContactModel1DTpl<Scalar>::updateForce(
149 const boost::shared_ptr<crocoddyl::ContactDataAbstractTpl<Scalar>>& data,
150 const VectorXs& force) {
151 if (force.size() != 1) {
152 throw_pretty(
"Invalid argument: " 153 <<
"lambda has wrong dimension (it should be 1)");
155 Data* d =
static_cast<Data*
>(data.get());
156 d->oRf = d->pinocchio->oMf[id_].rotation();
157 if (type_ == pinocchio::LOCAL) {
158 data->f.linear() = d->jMf.rotation().col(mask_) * force[0];
159 data->f.angular() = d->jMf.translation().cross(data->f.linear());
161 if (type_ == pinocchio::LOCAL_WORLD_ALIGNED || type_ == pinocchio::WORLD) {
162 data->f = d->jMf.act(pinocchio::ForceTpl<Scalar>(
163 d->oRf.transpose().col(mask_) * force[0], Vector3s::Zero()));
165 pinocchio::skew(d->oRf.transpose().col(mask_) * force[0], d->tmp_skew_);
166 d->drnea_skew_term_ =
167 -d->fJf.topRows(3).transpose() * d->tmp_skew_ * d->fJf.bottomRows(3);
171 template <
typename Scalar>
172 boost::shared_ptr<crocoddyl::ContactDataAbstractTpl<Scalar>>
173 ContactModel1DTpl<Scalar>::createData(pinocchio::DataTpl<Scalar>*
const data) {
174 return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(),
this,
178 template <
typename Scalar>
179 void ContactModel1DTpl<Scalar>::print(std::ostream& os)
const {
180 os <<
"ContactModel1D {frame=" << state_->get_pinocchio()->frames[id_].name
184 template <
typename Scalar>
185 const typename crocoddyl::MathBaseTpl<Scalar>::Vector3s&
186 ContactModel1DTpl<Scalar>::get_reference()
const {
190 template <
typename Scalar>
191 const typename crocoddyl::MathBaseTpl<Scalar>::Vector2s&
192 ContactModel1DTpl<Scalar>::get_gains()
const {
196 template <
typename Scalar>
197 void ContactModel1DTpl<Scalar>::set_reference(
const Vector3s& reference) {
201 template <
typename Scalar>
202 void ContactModel1DTpl<Scalar>::set_type(
const pinocchio::ReferenceFrame type) {
206 template <
typename Scalar>
207 const pinocchio::ReferenceFrame ContactModel1DTpl<Scalar>::get_type()
const {
211 template <
typename Scalar>
212 void ContactModel1DTpl<Scalar>::set_mask(
const Vector3MaskType mask) {
216 template <
typename Scalar>
217 const Vector3MaskType ContactModel1DTpl<Scalar>::get_mask()
const {
Definition: contact-force.hxx:11