9 #include "contact1d.hpp" 12 namespace newcontacts {
14 template <
typename Scalar>
15 ContactModel1DTpl<Scalar>::ContactModel1DTpl(
16 boost::shared_ptr<StateMultibody> state,
const pinocchio::FrameIndex
id,
17 const Vector3s& xref,
const std::size_t nu,
const Vector2s& gains,
18 const Vector3MaskType& mask,
const pinocchio::ReferenceFrame type)
19 : Base(state, id, Scalar(0.), nu, Vector2s::Zero()),
25 template <
typename Scalar>
26 ContactModel1DTpl<Scalar>::ContactModel1DTpl(
27 boost::shared_ptr<StateMultibody> state,
const pinocchio::FrameIndex
id,
28 const Vector3s& xref,
const Vector2s& gains,
29 const pinocchio::ReferenceFrame type)
30 : Base(state, id, Scalar(0.), Vector2s::Zero()),
33 mask_(Vector3MaskType::z),
36 template <
typename Scalar>
37 ContactModel1DTpl<Scalar>::~ContactModel1DTpl() {}
39 template <
typename Scalar>
40 void ContactModel1DTpl<Scalar>::calc(
41 const boost::shared_ptr<crocoddyl::ContactDataAbstractTpl<Scalar>>& data,
42 const Eigen::Ref<const VectorXs>& x) {
43 Data* d =
static_cast<Data*
>(data.get());
44 pinocchio::updateFramePlacement(*state_->get_pinocchio().get(), *d->pinocchio,
46 pinocchio::getFrameJacobian(*state_->get_pinocchio().get(), *d->pinocchio,
47 id_, pinocchio::LOCAL, d->fJf);
49 pinocchio::getFrameClassicalAcceleration(
50 *state_->get_pinocchio().get(), *d->pinocchio, id_, pinocchio::LOCAL)
52 d->vv = d->fJf.template topRows<3>() * x.tail(state_->get_nv());
53 d->vw = d->fJf.template bottomRows<3>() * x.tail(state_->get_nv());
54 d->oRf = d->pinocchio->oMf[id_].rotation();
55 d->Jc.row(0) = d->fJf.row(mask_);
57 if (gains_[0] != 0.) {
58 d->a0_3d_ += gains_[0] * d->oRf.transpose() *
59 (d->pinocchio->oMf[id_].translation() - xref_);
62 if (gains_[1] != 0.) {
63 d->a0_3d_ += gains_[1] * d->vv;
66 d->a0[0] = d->a0_3d_[mask_];
68 if (type_ == pinocchio::LOCAL_WORLD_ALIGNED || type_ == pinocchio::WORLD) {
69 d->a0[0] = (d->oRf * d->a0_3d_)[mask_];
70 d->Jc.row(0) = (d->oRf * d->fJf.template topRows<3>()).row(mask_);
74 template <
typename Scalar>
75 void ContactModel1DTpl<Scalar>::calcDiff(
76 const boost::shared_ptr<crocoddyl::ContactDataAbstractTpl<Scalar>>& data,
77 const Eigen::Ref<const VectorXs>&) {
78 Data* d =
static_cast<Data*
>(data.get());
79 const pinocchio::JointIndex joint =
80 state_->get_pinocchio()->frames[d->frame].parent;
81 pinocchio::getJointAccelerationDerivatives(
82 *state_->get_pinocchio().get(), *d->pinocchio, joint, pinocchio::LOCAL,
83 d->v_partial_dq, d->a_partial_dq, d->a_partial_dv, d->a_partial_da);
84 const std::size_t nv = state_->get_nv();
85 pinocchio::skew(d->vv, d->vv_skew);
86 pinocchio::skew(d->vw, d->vw_skew);
88 d->fXjdv_dq.noalias() = d->fXj * d->v_partial_dq;
89 d->fXjda_dq.noalias() = d->fXj * d->a_partial_dq;
90 d->fXjda_dv.noalias() = d->fXj * d->a_partial_dv;
92 d->da0_dx_3d_.leftCols(nv) = d->fXjda_dq.template topRows<3>();
93 d->da0_dx_3d_.leftCols(nv).noalias() +=
94 d->vw_skew * d->fXjdv_dq.template topRows<3>();
95 d->da0_dx_3d_.leftCols(nv).noalias() -=
96 d->vv_skew * d->fXjdv_dq.template bottomRows<3>();
98 d->da0_dx_3d_.rightCols(nv) = d->fXjda_dv.template topRows<3>();
99 d->da0_dx_3d_.rightCols(nv).noalias() +=
100 d->vw_skew * d->fJf.template topRows<3>();
101 d->da0_dx_3d_.rightCols(nv).noalias() -=
102 d->vv_skew * d->fJf.template bottomRows<3>();
104 if (gains_[0] != 0.) {
106 d->oRf.transpose() * (d->pinocchio->oMf[id_].translation() - xref_),
108 d->da0_dx_3d_.leftCols(nv).noalias() +=
109 gains_[0] * (d->tmp_skew_ * d->fJf.template bottomRows<3>() +
110 d->fJf.template topRows<3>());
113 if (gains_[1] != 0.) {
114 d->da0_dx_3d_.leftCols(nv).noalias() +=
115 gains_[1] * d->fXjdv_dq.template topRows<3>();
116 d->da0_dx_3d_.rightCols(nv).noalias() +=
117 gains_[1] * d->fJf.template topRows<3>();
120 d->da0_dx.row(0) = d->da0_dx_3d_.row(mask_);
122 if (type_ == pinocchio::LOCAL_WORLD_ALIGNED || type_ == pinocchio::WORLD) {
125 d->a0_3d_ = pinocchio::getFrameClassicalAcceleration(
126 *state_->get_pinocchio().get(), *d->pinocchio, id_,
129 if (gains_[0] != 0.) {
130 d->a0_3d_ += gains_[0] * d->oRf.transpose() *
131 (d->pinocchio->oMf[id_].translation() - xref_);
133 if (gains_[1] != 0.) {
134 d->a0_3d_ += gains_[1] * d->vv;
138 pinocchio::skew(d->oRf * d->a0_3d_, d->tmp_skew_);
139 d->da0_dx.leftCols(nv).row(0) =
140 (d->oRf * d->da0_dx_3d_.leftCols(nv) -
141 d->tmp_skew_ * d->oRf * d->fJf.template bottomRows<3>())
143 d->da0_dx.rightCols(nv).row(0) =
144 (d->oRf * d->da0_dx_3d_.rightCols(nv)).row(mask_);
148 template <
typename Scalar>
149 void ContactModel1DTpl<Scalar>::updateForce(
150 const boost::shared_ptr<crocoddyl::ContactDataAbstractTpl<Scalar>>& data,
151 const VectorXs& force) {
152 if (force.size() != 1) {
153 throw_pretty(
"Invalid argument: " 154 <<
"lambda has wrong dimension (it should be 1)");
156 Data* d =
static_cast<Data*
>(data.get());
157 d->oRf = d->pinocchio->oMf[id_].rotation();
158 if (type_ == pinocchio::LOCAL) {
159 data->f.linear() = d->jMf.rotation().col(mask_) * force[0];
160 data->f.angular() = d->jMf.translation().cross(data->f.linear());
162 if (type_ == pinocchio::LOCAL_WORLD_ALIGNED || type_ == pinocchio::WORLD) {
163 data->f = d->jMf.act(pinocchio::ForceTpl<Scalar>(
164 d->oRf.transpose().col(mask_) * force[0], Vector3s::Zero()));
166 pinocchio::skew(d->oRf.transpose().col(mask_) * force[0], d->tmp_skew_);
167 d->drnea_skew_term_ =
168 -d->fJf.topRows(3).transpose() * d->tmp_skew_ * d->fJf.bottomRows(3);
172 template <
typename Scalar>
173 boost::shared_ptr<crocoddyl::ContactDataAbstractTpl<Scalar>>
174 ContactModel1DTpl<Scalar>::createData(pinocchio::DataTpl<Scalar>*
const data) {
175 return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(),
this,
179 template <
typename Scalar>
180 void ContactModel1DTpl<Scalar>::print(std::ostream& os)
const {
181 os <<
"ContactModel1D {frame=" << state_->get_pinocchio()->frames[id_].name
185 template <
typename Scalar>
186 const typename crocoddyl::MathBaseTpl<Scalar>::Vector3s&
187 ContactModel1DTpl<Scalar>::get_reference()
const {
191 template <
typename Scalar>
192 const typename crocoddyl::MathBaseTpl<Scalar>::Vector2s&
193 ContactModel1DTpl<Scalar>::get_gains()
const {
197 template <
typename Scalar>
198 void ContactModel1DTpl<Scalar>::set_reference(
const Vector3s& reference) {
202 template <
typename Scalar>
203 void ContactModel1DTpl<Scalar>::set_type(
const pinocchio::ReferenceFrame type) {
207 template <
typename Scalar>
208 const pinocchio::ReferenceFrame ContactModel1DTpl<Scalar>::get_type()
const {
212 template <
typename Scalar>
213 void ContactModel1DTpl<Scalar>::set_mask(
const Vector3MaskType mask) {
217 template <
typename Scalar>
218 const Vector3MaskType ContactModel1DTpl<Scalar>::get_mask()
const {
Definition: contact-force.hxx:11