1 |
|
|
/////////////////////////////////////////////////////////////////////////////// |
2 |
|
|
// BSD 3-Clause License |
3 |
|
|
// |
4 |
|
|
// Copyright (C) 2021, LAAS-CNRS, University of Edinburgh |
5 |
|
|
// Copyright note valid unless otherwise stated in individual files. |
6 |
|
|
// All rights reserved. |
7 |
|
|
/////////////////////////////////////////////////////////////////////////////// |
8 |
|
|
|
9 |
|
|
#include <pinocchio/algorithm/center-of-mass-derivatives.hpp> |
10 |
|
|
#include <pinocchio/algorithm/center-of-mass.hpp> |
11 |
|
|
|
12 |
|
|
#include "crocoddyl/multibody/residuals/impulse-com.hpp" |
13 |
|
|
|
14 |
|
|
namespace crocoddyl { |
15 |
|
|
|
16 |
|
|
template <typename Scalar> |
17 |
|
8 |
ResidualModelImpulseCoMTpl<Scalar>::ResidualModelImpulseCoMTpl( |
18 |
|
|
boost::shared_ptr<StateMultibody> state) |
19 |
|
|
: Base(state, 3, 0, true, true, false), |
20 |
✓✗ |
8 |
pin_model_(state->get_pinocchio()) {} |
21 |
|
|
|
22 |
|
|
template <typename Scalar> |
23 |
|
16 |
ResidualModelImpulseCoMTpl<Scalar>::~ResidualModelImpulseCoMTpl() {} |
24 |
|
|
|
25 |
|
|
template <typename Scalar> |
26 |
|
2016 |
void ResidualModelImpulseCoMTpl<Scalar>::calc( |
27 |
|
|
const boost::shared_ptr<ResidualDataAbstract>& data, |
28 |
|
|
const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>&) { |
29 |
|
|
// Compute the residual residual give the reference CoM position |
30 |
|
2016 |
Data* d = static_cast<Data*>(data.get()); |
31 |
|
2016 |
const std::size_t nq = state_->get_nq(); |
32 |
|
2016 |
const std::size_t nv = state_->get_nv(); |
33 |
✓✗ |
2016 |
const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q = |
34 |
|
|
x.head(nq); |
35 |
✓✗ |
2016 |
const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v = |
36 |
|
|
x.tail(nv); |
37 |
|
|
|
38 |
✓✗ |
2016 |
pinocchio::centerOfMass(*pin_model_.get(), d->pinocchio_internal, q, |
39 |
✓✗ |
2016 |
d->impulses->vnext - v); |
40 |
✓✗ |
2016 |
data->r = d->pinocchio_internal.vcom[0]; |
41 |
|
2016 |
} |
42 |
|
|
|
43 |
|
|
template <typename Scalar> |
44 |
|
26 |
void ResidualModelImpulseCoMTpl<Scalar>::calcDiff( |
45 |
|
|
const boost::shared_ptr<ResidualDataAbstract>& data, |
46 |
|
|
const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) { |
47 |
|
26 |
Data* d = static_cast<Data*>(data.get()); |
48 |
|
|
|
49 |
|
|
// Compute the derivatives of the frame placement |
50 |
|
26 |
const std::size_t nv = state_->get_nv(); |
51 |
|
26 |
const std::size_t ndx = state_->get_ndx(); |
52 |
|
26 |
pinocchio::getCenterOfMassVelocityDerivatives( |
53 |
|
26 |
*pin_model_.get(), d->pinocchio_internal, d->dvc_dq); |
54 |
|
26 |
pinocchio::jacobianCenterOfMass(*pin_model_.get(), d->pinocchio_internal, |
55 |
|
|
false); |
56 |
✓✗ |
26 |
d->ddv_dv = d->impulses->dvnext_dx.rightCols(ndx - nv); |
57 |
✓✗✓✗
✓✗ |
26 |
d->ddv_dv.diagonal().array() -= 1; |
58 |
✓✗ |
26 |
data->Rx.leftCols(nv) = d->dvc_dq; |
59 |
✓✗✓✗
✓✗ |
26 |
data->Rx.leftCols(nv).noalias() += |
60 |
✓✗ |
26 |
d->pinocchio_internal.Jcom * d->impulses->dvnext_dx.leftCols(nv); |
61 |
✓✗✓✗
✓✗ |
26 |
data->Rx.rightCols(ndx - nv).noalias() = |
62 |
|
26 |
d->pinocchio_internal.Jcom * d->ddv_dv; |
63 |
|
26 |
} |
64 |
|
|
|
65 |
|
|
template <typename Scalar> |
66 |
|
|
boost::shared_ptr<ResidualDataAbstractTpl<Scalar> > |
67 |
|
1080 |
ResidualModelImpulseCoMTpl<Scalar>::createData( |
68 |
|
|
DataCollectorAbstract* const data) { |
69 |
|
|
return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this, |
70 |
✓✗ |
1080 |
data); |
71 |
|
|
} |
72 |
|
|
|
73 |
|
|
template <typename Scalar> |
74 |
|
|
void ResidualModelImpulseCoMTpl<Scalar>::print(std::ostream& os) const { |
75 |
|
|
os << "ResidualModelImpulseCoM"; |
76 |
|
|
} |
77 |
|
|
|
78 |
|
|
} // namespace crocoddyl |