pinocchio_gravity_compensation_dynamics_solver_derivatives.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2020, University of Oxford
3 // All rights reserved.
4 //
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are met:
7 //
8 // * Redistributions of source code must retain the above copyright notice,
9 // this list of conditions and the following disclaimer.
10 // * Redistributions in binary form must reproduce the above copyright
11 // notice, this list of conditions and the following disclaimer in the
12 // documentation and/or other materials provided with the distribution.
13 // * Neither the name of nor the names of its contributors may be used to
14 // endorse or promote products derived from this software without specific
15 // prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27 // POSSIBILITY OF SUCH DAMAGE.
28 //
29 
30 #include <exotica_pinocchio_dynamics_solver/pinocchio_gravity_compensation_dynamics_solver.h>
31 
32 #include <pinocchio/algorithm/cholesky.hpp>
33 #include <pinocchio/algorithm/compute-all-terms.hpp>
34 #include <pinocchio/algorithm/joint-configuration.hpp>
35 #include <pinocchio/algorithm/rnea-derivatives.hpp>
36 #include <pinocchio/algorithm/rnea.hpp>
37 
38 REGISTER_DYNAMICS_SOLVER_TYPE("PinocchioDynamicsSolverWithGravityCompensation", exotica::PinocchioDynamicsSolverWithGravityCompensation)
39 
40 namespace exotica
41 {
42 void PinocchioDynamicsSolverWithGravityCompensation::ComputeDerivatives(const StateVector& x, const ControlVector& u)
43 {
44  Eigen::VectorBlock<const Eigen::VectorXd> q = x.head(num_positions_);
45  Eigen::VectorBlock<const Eigen::VectorXd> v = x.tail(num_velocities_);
46 
47  // Obtain torque to compensate gravity and dynamic effects (Coriolis)
48  u_nle_ = pinocchio::nonLinearEffects(model_, *pinocchio_data_.get(), q, v);
49 
50  // Commanded torque is u_nle_ + u
51  u_command_.noalias() = u_nle_ + u;
52 
53  pinocchio_data_->Minv.setZero();
54  pinocchio::computeAllTerms(model_, *pinocchio_data_.get(), q, v);
55  pinocchio::cholesky::decompose(model_, *pinocchio_data_.get());
56  pinocchio::cholesky::computeMinv(model_, *pinocchio_data_.get(), pinocchio_data_->Minv);
57 
58  // du_command_dq_
59  a_.noalias() = pinocchio_data_->Minv * u_command_;
60  pinocchio::computeRNEADerivatives(model_, *pinocchio_data_.get(), q, v, a_);
61  du_command_dq_.noalias() = pinocchio_data_->Minv * pinocchio_data_->dtau_dq;
62 
63  // du_nle_dq_
64  a_.noalias() = pinocchio_data_->Minv * u_nle_;
65  pinocchio::computeRNEADerivatives(model_, *pinocchio_data_.get(), q, v, a_);
66  du_nle_dq_.noalias() = pinocchio_data_->Minv * pinocchio_data_->dtau_dq;
67 
68  // du_dq_
69  fx_.block(num_velocities_, 0, num_velocities_, num_velocities_).noalias() = du_nle_dq_ - du_command_dq_;
70 
71  // Since dtau_du=Identity, the partial derivative of fu is directly Minv.
72  fu_.bottomRightCorner(num_velocities_, num_velocities_) = pinocchio_data_->Minv;
73 
74  Eigen::Block<Eigen::MatrixXd> da_dx = fx_.block(num_velocities_, 0, num_velocities_, get_num_state_derivative());
75  Eigen::Block<Eigen::MatrixXd> da_du = fu_.block(num_velocities_, 0, num_velocities_, num_controls_);
76 
77  switch (integrator_)
78  {
79  // Forward Euler (RK1)
80  case Integrator::RK1:
81  {
82  Fx_.topRows(num_velocities_).setZero();
83  Fx_.bottomRows(num_velocities_).noalias() = dt_ * da_dx;
84  Fx_.topRightCorner(num_velocities_, num_velocities_).diagonal().array() += dt_;
85  pinocchio::dIntegrateTransport(model_, x.head(num_positions_), x.tail(num_velocities_), Fx_.topRows(num_velocities_), pinocchio::ARG1);
86  pinocchio::dIntegrate(model_, x.head(num_positions_), x.tail(num_velocities_), Fx_.topLeftCorner(num_velocities_, num_velocities_), pinocchio::ARG0, pinocchio::ADDTO);
87  Fx_.bottomRightCorner(num_velocities_, num_velocities_).diagonal().array() += 1.0;
88 
89  Fu_.topRows(num_velocities_).setZero();
90  Fu_.bottomRows(num_velocities_).noalias() = dt_ * da_du;
91  pinocchio::dIntegrateTransport(model_, x.head(num_positions_), x.tail(num_velocities_), Fu_.topRows(num_velocities_), pinocchio::ARG1);
92  }
93  break;
94  // Semi-implicit Euler
95  case Integrator::SymplecticEuler:
96  {
97  a_.noalias() = pinocchio_data_->Minv * u_command_;
98  Eigen::VectorXd dx_v = dt_ * x.tail(num_velocities_) + dt_ * dt_ * a_;
99 
100  Fx_.topRows(num_velocities_).noalias() = dt_ * dt_ * da_dx;
101  Fx_.bottomRows(num_velocities_).noalias() = dt_ * da_dx;
102  Fx_.topRightCorner(num_velocities_, num_velocities_).diagonal().array() += dt_;
103  pinocchio::dIntegrateTransport(model_, x.head(num_positions_), dx_v, Fx_.topRows(num_velocities_), pinocchio::ARG1);
104  pinocchio::dIntegrate(model_, x.head(num_positions_), dx_v, Fx_.topLeftCorner(num_velocities_, num_velocities_), pinocchio::ARG0, pinocchio::ADDTO);
105  Fx_.bottomRightCorner(num_velocities_, num_velocities_).diagonal().array() += 1.0;
106 
107  Fu_.topRows(num_velocities_).noalias() = dt_ * dt_ * da_du;
108  Fu_.bottomRows(num_velocities_).noalias() = dt_ * da_du;
109  pinocchio::dIntegrateTransport(model_, x.head(num_positions_), dx_v, Fu_.topRows(num_velocities_), pinocchio::ARG1);
110  }
111  break;
112  default:
113  ThrowPretty("Not implemented!");
114  };
115 }
116 
117 Eigen::MatrixXd PinocchioDynamicsSolverWithGravityCompensation::fx(const StateVector& x, const ControlVector& u)
118 {
119  // This causes redundant computations but this method is no longer called in solvers.
120  ComputeDerivatives(x, u);
121 
122  /*Eigen::VectorBlock<const Eigen::VectorXd> q = x.head(num_positions_);
123  Eigen::VectorBlock<const Eigen::VectorXd> v = x.tail(num_velocities_);
124 
125  // Obtain torque to compensate gravity and dynamic effects (Coriolis)
126  u_nle_ = pinocchio::nonLinearEffects(model_, *pinocchio_data_.get(), q, v);
127 
128  // Commanded torque is u_nle_ + u
129  u_command_.noalias() = u_nle_ + u;
130 
131  pinocchio_data_->Minv.setZero();
132  pinocchio::computeAllTerms(model_, *pinocchio_data_.get(), q, v);
133  pinocchio::cholesky::decompose(model_, *pinocchio_data_.get());
134  pinocchio::cholesky::computeMinv(model_, *pinocchio_data_.get(), pinocchio_data_->Minv);
135 
136  // du_command_dq_
137  a_.noalias() = pinocchio_data_->Minv * u_command_;
138  pinocchio::computeRNEADerivatives(model_, *pinocchio_data_.get(), q, v, a_);
139  du_command_dq_.noalias() = pinocchio_data_->Minv * pinocchio_data_->dtau_dq;
140 
141  // du_nle_dq_
142  a_.noalias() = pinocchio_data_->Minv * u_nle_;
143  pinocchio::computeRNEADerivatives(model_, *pinocchio_data_.get(), q, v, a_);
144  du_nle_dq_.noalias() = pinocchio_data_->Minv * pinocchio_data_->dtau_dq;
145 
146  // du_dq_
147  fx_.block(num_velocities_, 0, num_velocities_, num_velocities_).noalias() = du_nle_dq_ - du_command_dq_;*/
148 
149  return fx_;
150 }
151 
152 Eigen::MatrixXd PinocchioDynamicsSolverWithGravityCompensation::fu(const StateVector& x, const ControlVector& u)
153 {
154  // This causes redundant computations but this method is no longer called in solvers.
155  ComputeDerivatives(x, u);
156 
157  /*Eigen::VectorBlock<const Eigen::VectorXd> q = x.head(num_positions_);
158  Eigen::VectorBlock<const Eigen::VectorXd> v = x.tail(num_velocities_);
159 
160  // Obtain torque to compensate gravity and dynamic effects (Coriolis)
161  u_nle_ = pinocchio::nonLinearEffects(model_, *pinocchio_data_.get(), q, v);
162 
163  // Commanded torque is u_nle_ + u
164  u_command_.noalias() = u_nle_ + u;
165 
166  // Since dtau_du=Identity, the partial derivative of fu is directly Minv.
167  Eigen::Block<Eigen::MatrixXd> Minv = fu_.bottomRightCorner(num_velocities_, num_velocities_);
168 
169  pinocchio::computeAllTerms(model_, *pinocchio_data_.get(), q, v);
170  pinocchio::cholesky::decompose(model_, *pinocchio_data_.get());
171  pinocchio::cholesky::computeMinv(model_, *pinocchio_data_.get(), Minv);*/
172 
173  return fu_;
174 }
175 
176 } // namespace exotica
pinocchio_gravity_compensation_dynamics_solver.h
pinocchio::computeAllTerms
void computeAllTerms(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v)
q
q
pinocchio::cholesky::computeMinv
Mat & computeMinv(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< Mat > &Minv)
REGISTER_DYNAMICS_SOLVER_TYPE
#define REGISTER_DYNAMICS_SOLVER_TYPE(TYPE, DERIV)
pinocchio::cholesky::decompose
const DataTpl< Scalar, Options, JointCollectionTpl >::MatrixXs & decompose(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
pinocchio::dIntegrateTransport
void dIntegrateTransport(const LieGroupGenericTpl< LieGroupCollection > &lg, const Eigen::MatrixBase< Config_t > &q, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< JacobianIn_t > &J_in, const Eigen::MatrixBase< JacobianOut_t > &J_out, const ArgumentPosition arg)
ThrowPretty
#define ThrowPretty(m)
v
v
pinocchio::computeRNEADerivatives
void computeRNEADerivatives(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &a, const Eigen::MatrixBase< MatrixType1 > &rnea_partial_dq, const Eigen::MatrixBase< MatrixType2 > &rnea_partial_dv, const Eigen::MatrixBase< MatrixType3 > &rnea_partial_da)
pinocchio::dIntegrate
void dIntegrate(const LieGroupGenericTpl< LieGroupCollection > &lg, const Eigen::MatrixBase< Config_t > &q, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< JacobianOut_t > &J, const ArgumentPosition arg, const AssignmentOperatorType op=SETTO)
exotica::AbstractDynamicsSolver::ControlVector
Eigen::Matrix< T, NU, 1 > ControlVector
pinocchio::nonLinearEffects
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & nonLinearEffects(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v)
exotica::AbstractDynamicsSolver::StateVector
Eigen::Matrix< T, NX, 1 > StateVector

exotica_pinocchio_dynamics_solver
Author(s):
autogenerated on Sat Apr 10 2021 02:35:54