single_rigid_body_dynamics.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/models/single_rigid_body_dynamics.h>
00031 #include <towr/variables/cartesian_dimensions.h>
00032 
00033 namespace towr {
00034 
00035 // some Eigen helper functions
00036 static Eigen::Matrix3d BuildInertiaTensor( double Ixx, double Iyy, double Izz,
00037                                            double Ixy, double Ixz, double Iyz)
00038 {
00039   Eigen::Matrix3d I;
00040   I <<  Ixx, -Ixy, -Ixz,
00041        -Ixy,  Iyy, -Iyz,
00042        -Ixz, -Iyz,  Izz;
00043   return I;
00044 }
00045 
00046 // builds a cross product matrix out of "in", so in x v = X(in)*v
00047 SingleRigidBodyDynamics::Jac
00048 Cross(const Eigen::Vector3d& in)
00049 {
00050   SingleRigidBodyDynamics::Jac out(3,3);
00051 
00052   out.coeffRef(0,1) = -in(2); out.coeffRef(0,2) =  in(1);
00053   out.coeffRef(1,0) =  in(2); out.coeffRef(1,2) = -in(0);
00054   out.coeffRef(2,0) = -in(1); out.coeffRef(2,1) =  in(0);
00055 
00056   return out;
00057 }
00058 
00059 SingleRigidBodyDynamics::SingleRigidBodyDynamics (double mass,
00060                                   double Ixx, double Iyy, double Izz,
00061                                   double Ixy, double Ixz, double Iyz,
00062                                   int ee_count)
00063    : SingleRigidBodyDynamics(mass,
00064                      BuildInertiaTensor(Ixx, Iyy, Izz, Ixy, Ixz, Iyz),
00065                      ee_count)
00066 {
00067 }
00068 
00069 SingleRigidBodyDynamics::SingleRigidBodyDynamics (double mass, const Eigen::Matrix3d& inertia_b,
00070                                   int ee_count)
00071     :DynamicModel(mass, ee_count)
00072 {
00073   I_b = inertia_b.sparseView();
00074 }
00075 
00076 SingleRigidBodyDynamics::BaseAcc
00077 SingleRigidBodyDynamics::GetDynamicViolation () const
00078 {
00079   // https://en.wikipedia.org/wiki/Newton%E2%80%93Euler_equations
00080 
00081   Vector3d f_sum, tau_sum;
00082   f_sum.setZero(); tau_sum.setZero();
00083 
00084   for (int ee=0; ee<ee_pos_.size(); ++ee) {
00085     Vector3d f = ee_force_.at(ee);
00086     tau_sum += f.cross(com_pos_ - ee_pos_.at(ee));
00087     f_sum   += f;
00088   }
00089 
00090   // express inertia matrix in world frame based on current body orientation
00091   Jac I_w = w_R_b_.sparseView() * I_b * w_R_b_.transpose().sparseView();
00092 
00093   BaseAcc acc;
00094   acc.segment(AX, k3D) = I_w*omega_dot_
00095                          + Cross(omega_)*(I_w*omega_)
00096                          - tau_sum;
00097   acc.segment(LX, k3D) = m()*com_acc_
00098                          - f_sum
00099                          - Vector3d(0.0, 0.0, -m()*g()); // gravity force
00100   return acc;
00101 }
00102 
00103 SingleRigidBodyDynamics::Jac
00104 SingleRigidBodyDynamics::GetJacobianWrtBaseLin (const Jac& jac_pos_base_lin,
00105                                         const Jac& jac_acc_base_lin) const
00106 {
00107   // build the com jacobian
00108   int n = jac_pos_base_lin.cols();
00109 
00110   Jac jac_tau_sum(k3D, n);
00111   for (const Vector3d& f : ee_force_) {
00112     Jac jac_tau = Cross(f)*jac_pos_base_lin;
00113     jac_tau_sum += jac_tau;
00114   }
00115 
00116   Jac jac(k6D, n);
00117   jac.middleRows(AX, k3D) = -jac_tau_sum;
00118   jac.middleRows(LX, k3D) = m()*jac_acc_base_lin;
00119 
00120   return jac;
00121 }
00122 
00123 SingleRigidBodyDynamics::Jac
00124 SingleRigidBodyDynamics::GetJacobianWrtBaseAng (const EulerConverter& base_euler,
00125                                         double t) const
00126 {
00127   Jac I_w = w_R_b_.sparseView() * I_b * w_R_b_.transpose().sparseView();
00128 
00129   // Derivative of R*I_b*R^T * wd
00130   // 1st term of product rule (derivative of R)
00131   Vector3d v11 = I_b*w_R_b_.transpose()*omega_dot_;
00132   Jac jac11 = base_euler.DerivOfRotVecMult(t, v11, false);
00133 
00134   // 2nd term of product rule (derivative of R^T)
00135   Jac jac12 = w_R_b_.sparseView()*I_b*base_euler.DerivOfRotVecMult(t, omega_dot_, true);
00136 
00137   // 3rd term of product rule (derivative of wd)
00138   Jac jac_ang_acc = base_euler.GetDerivOfAngAccWrtEulerNodes(t);
00139   Jac jac13 = I_w * jac_ang_acc;
00140   Jac jac1 = jac11 + jac12 + jac13;
00141 
00142 
00143   // Derivative of w x Iw
00144   // w x d_dn(R*I_b*R^T*w) -(I*w x d_dnw)
00145   // right derivative same as above, just with velocity instead acceleration
00146   Vector3d v21 = I_b*w_R_b_.transpose()*omega_;
00147   Jac jac21 = base_euler.DerivOfRotVecMult(t, v21, false);
00148 
00149   // 2nd term of product rule (derivative of R^T)
00150   Jac jac22 = w_R_b_.sparseView()*I_b*base_euler.DerivOfRotVecMult(t, omega_, true);
00151 
00152   // 3rd term of product rule (derivative of omega)
00153   Jac jac_ang_vel = base_euler.GetDerivOfAngVelWrtEulerNodes(t);
00154   Jac jac23 = I_w * jac_ang_vel;
00155 
00156   Jac jac2 = Cross(omega_)*(jac21+jac22+jac23) - Cross(I_w*omega_)*jac_ang_vel;
00157 
00158 
00159   // Combine the two to get sensitivity to I_w*w + w x (I_w*w)
00160   int n = jac_ang_vel.cols();
00161   Jac jac(k6D, n);
00162   jac.middleRows(AX, k3D) = jac1 + jac2;
00163 
00164   return jac;
00165 }
00166 
00167 SingleRigidBodyDynamics::Jac
00168 SingleRigidBodyDynamics::GetJacobianWrtForce (const Jac& jac_force, EE ee) const
00169 {
00170   Vector3d r = com_pos_ - ee_pos_.at(ee);
00171   Jac jac_tau = -Cross(r)*jac_force;
00172 
00173   int n = jac_force.cols();
00174   Jac jac(k6D, n);
00175   jac.middleRows(AX, k3D) = -jac_tau;
00176   jac.middleRows(LX, k3D) = -jac_force;
00177 
00178   return jac;
00179 }
00180 
00181 SingleRigidBodyDynamics::Jac
00182 SingleRigidBodyDynamics::GetJacobianWrtEEPos (const Jac& jac_ee_pos, EE ee) const
00183 {
00184   Vector3d f = ee_force_.at(ee);
00185   Jac jac_tau = Cross(f)*(-jac_ee_pos);
00186 
00187   Jac jac(k6D, jac_tau.cols());
00188   jac.middleRows(AX, k3D) = -jac_tau;
00189 
00190   // linear dynamics don't depend on endeffector position.
00191   return jac;
00192 }
00193 
00194 } /* namespace towr */


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