euler_converter.cc
Go to the documentation of this file.
00001 /******************************************************************************
00002 Copyright (c) 2018, Alexander W. Winkler. All rights reserved.
00003 
00004 Redistribution and use in source and binary forms, with or without
00005 modification, are permitted provided that the following conditions are met:
00006 
00007 * Redistributions of source code must retain the above copyright notice, this
00008   list of conditions and the following disclaimer.
00009 
00010 * Redistributions in binary form must reproduce the above copyright notice,
00011   this list of conditions and the following disclaimer in the documentation
00012   and/or other materials provided with the distribution.
00013 
00014 * Neither the name of the copyright holder nor the names of its
00015   contributors may be used to endorse or promote products derived from
00016   this software without specific prior written permission.
00017 
00018 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00019 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00020 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00021 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
00022 FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
00023 DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
00024 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00025 CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
00026 OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
00027 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00028 ******************************************************************************/
00029 
00030 #include <towr/variables/euler_converter.h>
00031 
00032 #include <cassert>
00033 #include <cmath>
00034 
00035 namespace towr {
00036 
00037 
00038 EulerConverter::EulerConverter (const NodeSpline::Ptr& euler)
00039 {
00040   euler_ = euler;
00041   jac_wrt_nodes_structure_ = Jacobian(k3D, euler->GetNodeVariablesCount());
00042 }
00043 
00044 Eigen::Quaterniond
00045 EulerConverter::GetQuaternionBaseToWorld (double t) const
00046 {
00047   State ori = euler_->GetPoint(t);
00048   return GetQuaternionBaseToWorld(ori.p());
00049 }
00050 
00051 Eigen::Quaterniond
00052 EulerConverter::GetQuaternionBaseToWorld (const EulerAngles& pos)
00053 {
00054   Eigen::Matrix3d R_WB = GetRotationMatrixBaseToWorld(pos);
00055   return Eigen::Quaterniond(R_WB);
00056 }
00057 
00058 Eigen::Vector3d
00059 EulerConverter::GetAngularVelocityInWorld (double t) const
00060 {
00061   State ori = euler_->GetPoint(t);
00062   return GetAngularVelocityInWorld(ori.p(), ori.v());
00063 }
00064 
00065 Eigen::Vector3d
00066 EulerConverter::GetAngularVelocityInWorld (const EulerAngles& pos,
00067                                            const EulerRates& vel)
00068 {
00069   return GetM(pos)*vel;
00070 }
00071 
00072 Eigen::Vector3d
00073 EulerConverter::GetAngularAccelerationInWorld (double t) const
00074 {
00075   State ori = euler_->GetPoint(t);
00076   return GetAngularAccelerationInWorld(ori);
00077 }
00078 
00079 Eigen::Vector3d
00080 EulerConverter::GetAngularAccelerationInWorld (State ori)
00081 {
00082   return GetMdot(ori.p(), ori.v())*ori.v() + GetM(ori.p())*ori.a();
00083 }
00084 
00085 EulerConverter::Jacobian
00086 EulerConverter::GetDerivOfAngVelWrtEulerNodes(double t) const
00087 {
00088   Jacobian jac = jac_wrt_nodes_structure_;
00089 
00090   State ori = euler_->GetPoint(t);
00091   // convert to sparse, but also regard 0.0 as non-zero element, because
00092   // could turn nonzero during the course of the program
00093   JacobianRow vel = ori.v().transpose().sparseView(1.0, -1.0);
00094   Jacobian dVel_du  = euler_->GetJacobianWrtNodes(t, kVel);
00095 
00096   for (auto dim : {X,Y,Z}) {
00097     Jacobian dM_du = GetDerivMwrtNodes(t,dim);
00098     jac.row(dim) = vel*dM_du + GetM(ori.p()).row(dim)*dVel_du;
00099   }
00100 
00101   return jac;
00102 }
00103 
00104 EulerConverter::Jacobian
00105 EulerConverter::GetDerivOfAngAccWrtEulerNodes (double t) const
00106 {
00107   Jacobian jac = jac_wrt_nodes_structure_;
00108 
00109 
00110   State ori = euler_->GetPoint(t);
00111   // convert to sparse, but also regard 0.0 as non-zero element, because
00112   // could turn nonzero during the course of the program
00113   JacobianRow vel = ori.v().transpose().sparseView(1.0, -1.0);
00114   JacobianRow acc = ori.a().transpose().sparseView(1.0, -1.0);
00115 
00116   Jacobian dVel_du  = euler_->GetJacobianWrtNodes(t, kVel);
00117   Jacobian dAcc_du  = euler_->GetJacobianWrtNodes(t, kAcc);
00118 
00119 
00120   for (auto dim : {X,Y,Z}) {
00121     Jacobian dMdot_du = GetDerivMdotwrtNodes(t,dim);
00122     Jacobian dM_du    = GetDerivMwrtNodes(t,dim);
00123 
00124     jac.row(dim) = vel                               * dMdot_du
00125                    + GetMdot(ori.p(), ori.v()).row(dim)* dVel_du
00126                    + acc                             * dM_du
00127                    + GetM(ori.p()).row(dim)           * dAcc_du;
00128   }
00129 
00130   return jac;
00131 }
00132 
00133 EulerConverter::MatrixSXd
00134 EulerConverter::GetM (const EulerAngles& xyz)
00135 {
00136   double z = xyz(Z);
00137   double y = xyz(Y);
00138 
00139   // Euler ZYX rates to angular velocity
00140   // http://docs.leggedrobotics.com/kindr/cheatsheet_latest.pdf
00141   Jacobian M(k3D, k3D);
00142 
00143        /* - */           M.coeffRef(0,Y) = -sin(z);  M.coeffRef(0,X) =  cos(y)*cos(z);
00144        /* - */           M.coeffRef(1,Y) =  cos(z);  M.coeffRef(1,X) =  cos(y)*sin(z);
00145   M.coeffRef(2,Z) = 1.0;          /* - */            M.coeffRef(2,X) =  -sin(y);
00146 
00147   return M;
00148 }
00149 
00150 EulerConverter::MatrixSXd
00151 EulerConverter::GetMdot (const EulerAngles& xyz,
00152                          const EulerRates& xyz_d)
00153 {
00154   double z  = xyz(Z);
00155   double zd = xyz_d(Z);
00156   double y  = xyz(Y);
00157   double yd = xyz_d(Y);
00158 
00159   Jacobian Mdot(k3D, k3D);
00160 
00161   Mdot.coeffRef(0,Y) = -cos(z)*zd; Mdot.coeffRef(0,X) = -cos(z)*sin(y)*yd - cos(y)*sin(z)*zd;
00162   Mdot.coeffRef(1,Y) = -sin(z)*zd; Mdot.coeffRef(1,X) =  cos(y)*cos(z)*zd - sin(y)*sin(z)*yd;
00163               /* - */              Mdot.coeffRef(2,X) = -cos(y)*yd;
00164 
00165  return Mdot;
00166 }
00167 
00168 EulerConverter::Jacobian
00169 EulerConverter::GetDerivMwrtNodes (double t, Dim3D ang_acc_dim) const
00170 {
00171   State ori = euler_->GetPoint(t);
00172 
00173   double z = ori.p()(Z);
00174   double y = ori.p()(Y);
00175   JacobianRow jac_z = GetJac(t, kPos, Z);
00176   JacobianRow jac_y = GetJac(t, kPos, Y);
00177 
00178   Jacobian jac = jac_wrt_nodes_structure_;
00179 
00180   switch (ang_acc_dim) {
00181     case X: // basically derivative of top row (3 elements) of matrix M
00182       jac.row(Y) = -cos(z)*jac_z;
00183       jac.row(X) = -cos(z)*sin(y)*jac_y - cos(y)*sin(z)*jac_z;
00184       break;
00185     case Y: // middle row of M
00186       jac.row(Y) = -sin(z)*jac_z;
00187       jac.row(X) = cos(y)*cos(z)*jac_z - sin(y)*sin(z)*jac_y;
00188       break;
00189     case Z: // bottom row of M
00190       jac.row(X) = -cos(y)*jac_y;
00191       break;
00192     default:
00193       assert(false);
00194       break;
00195   }
00196 
00197   return jac;
00198 }
00199 
00200 EulerConverter::MatrixSXd
00201 EulerConverter::GetRotationMatrixBaseToWorld (double t) const
00202 {
00203   State ori = euler_->GetPoint(t);
00204   return GetRotationMatrixBaseToWorld(ori.p());
00205 }
00206 
00207 EulerConverter::MatrixSXd
00208 EulerConverter::GetRotationMatrixBaseToWorld (const EulerAngles& xyz)
00209 {
00210   double x = xyz(X);
00211   double y = xyz(Y);
00212   double z = xyz(Z);
00213 
00214   Eigen::Matrix3d M;
00215   //  http://docs.leggedrobotics.com/kindr/cheatsheet_latest.pdf (Euler ZYX)
00216   M << cos(y)*cos(z), cos(z)*sin(x)*sin(y) - cos(x)*sin(z), sin(x)*sin(z) + cos(x)*cos(z)*sin(y),
00217        cos(y)*sin(z), cos(x)*cos(z) + sin(x)*sin(y)*sin(z), cos(x)*sin(y)*sin(z) - cos(z)*sin(x),
00218              -sin(y),                        cos(y)*sin(x),                        cos(x)*cos(y);
00219 
00220   return M.sparseView(1.0, -1.0);
00221 }
00222 
00223 EulerConverter::Jacobian
00224 EulerConverter::DerivOfRotVecMult (double t, const Vector3d& v, bool inverse) const
00225 {
00226   JacRowMatrix Rd = GetDerivativeOfRotationMatrixWrtNodes(t);
00227   Jacobian jac = jac_wrt_nodes_structure_;
00228 
00229   for (int row : {X,Y,Z}) {
00230     for (int col : {X, Y, Z}) {
00231       // since for every rotation matrix R^(-1) = R^T, just swap rows and
00232       // columns for calculation of derivative of inverse rotation matrix
00233       JacobianRow jac_row = inverse? Rd.at(col).at(row) : Rd.at(row).at(col);
00234       jac.row(row) += v(col)*jac_row;
00235     }
00236   }
00237 
00238   return jac;
00239 }
00240 
00241 EulerConverter::JacRowMatrix
00242 EulerConverter::GetDerivativeOfRotationMatrixWrtNodes (double t) const
00243 {
00244   JacRowMatrix jac;
00245 
00246   State ori = euler_->GetPoint(t);
00247   double x = ori.p()(X);
00248   double y = ori.p()(Y);
00249   double z = ori.p()(Z);
00250 
00251   JacobianRow jac_x = GetJac(t, kPos, X);
00252   JacobianRow jac_y = GetJac(t, kPos, Y);
00253   JacobianRow jac_z = GetJac(t, kPos, Z);
00254 
00255   jac.at(X).at(X) = -cos(z)*sin(y)*jac_y - cos(y)*sin(z)*jac_z;
00256   jac.at(X).at(Y) =  sin(x)*sin(z)*jac_x - cos(x)*cos(z)*jac_z - sin(x)*sin(y)*sin(z)*jac_z + cos(x)*cos(z)*sin(y)*jac_x + cos(y)*cos(z)*sin(x)*jac_y;
00257   jac.at(X).at(Z) =  cos(x)*sin(z)*jac_x + cos(z)*sin(x)*jac_z - cos(z)*sin(x)*sin(y)*jac_x - cos(x)*sin(y)*sin(z)*jac_z + cos(x)*cos(y)*cos(z)*jac_y;
00258 
00259   jac.at(Y).at(X) = cos(y)*cos(z)*jac_z - sin(y)*sin(z)*jac_y;
00260   jac.at(Y).at(Y) = cos(x)*sin(y)*sin(z)*jac_x - cos(x)*sin(z)*jac_z - cos(z)*sin(x)*jac_x + cos(y)*sin(x)*sin(z)*jac_y + cos(z)*sin(x)*sin(y)*jac_z;
00261   jac.at(Y).at(Z) = sin(x)*sin(z)*jac_z - cos(x)*cos(z)*jac_x - sin(x)*sin(y)*sin(z)*jac_x + cos(x)*cos(y)*sin(z)*jac_y + cos(x)*cos(z)*sin(y)*jac_z;
00262 
00263   jac.at(Z).at(X) = -cos(y)*jac_y;
00264   jac.at(Z).at(Y) =  cos(x)*cos(y)*jac_x - sin(x)*sin(y)*jac_y;
00265   jac.at(Z).at(Z) = -cos(y)*sin(x)*jac_x - cos(x)*sin(y)*jac_y;
00266 
00267   return jac;
00268 }
00269 
00270 EulerConverter::Jacobian
00271 EulerConverter::GetDerivMdotwrtNodes (double t, Dim3D ang_acc_dim) const
00272 {
00273   State ori = euler_->GetPoint(t);
00274 
00275   double z  = ori.p()(Z);
00276   double zd = ori.v()(Z);
00277   double y  = ori.p()(Y);
00278   double yd = ori.v()(Y);
00279 
00280   JacobianRow jac_z  = GetJac(t, kPos, Z);
00281   JacobianRow jac_y  = GetJac(t, kPos, Y);
00282   JacobianRow jac_zd = GetJac(t, kVel, Z);
00283   JacobianRow jac_yd = GetJac(t, kVel, Y);
00284 
00285   Jacobian jac = jac_wrt_nodes_structure_;
00286   switch (ang_acc_dim) {
00287     case X: // derivative of top row (3 elements) of matrix M-dot
00288       jac.row(Y) = sin(z)*zd*jac_z - cos(z)*jac_zd;
00289       jac.row(X) = sin(y)*sin(z)*yd*jac_z - cos(y)*sin(z)*jac_zd - cos(y)*cos(z)*yd*jac_y - cos(y)*cos(z)*zd*jac_z - cos(z)*sin(y)*jac_yd + sin(y)*sin(z)*jac_y*zd;
00290       break;
00291     case Y: // middle row of M
00292       jac.row(Y) = - sin(z)*jac_zd - cos(z)*zd*jac_z;
00293       jac.row(X) = cos(y)*cos(z)*jac_zd - sin(y)*sin(z)*jac_yd - cos(y)*sin(z)*yd*jac_y - cos(z)*sin(y)*yd*jac_z - cos(z)*sin(y)*jac_y*zd - cos(y)*sin(z)*zd*jac_z;
00294       break;
00295     case Z: // bottom Row of M
00296       jac.row(X) = sin(y)*yd*jac_y - cos(y)*jac_yd;
00297       break;
00298     default:
00299       assert(false);
00300       break;
00301   }
00302 
00303   return jac;
00304 }
00305 
00306 EulerConverter::JacobianRow
00307 EulerConverter::GetJac (double t, Dx deriv, Dim3D dim) const
00308 {
00309   return euler_->GetJacobianWrtNodes(t, deriv).row(dim);
00310 }
00311 
00312 } /* namespace towr */


towr
Author(s): Alexander W. Winkler
autogenerated on Mon Apr 15 2019 02:42:32