00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
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
00092
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
00112
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
00140
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:
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:
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:
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
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
00232
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:
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:
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:
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 }