chainfdsolver_recursive_newton_euler.cpp
Go to the documentation of this file.
00001 // Copyright  (C)  2018  Craig Carignan <craigc at ssl dot umd dot edu>
00002 
00003 // Version: 1.0
00004 // Author: Craig Carignan <craigc at ssl dot umd dot edu>
00005 // Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
00006 // URL: http://www.orocos.org/kdl
00007 
00008 // This library is free software; you can redistribute it and/or
00009 // modify it under the terms of the GNU Lesser General Public
00010 // License as published by the Free Software Foundation; either
00011 // version 2.1 of the License, or (at your option) any later version.
00012 
00013 // This library is distributed in the hope that it will be useful,
00014 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00015 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00016 // Lesser General Public License for more details.
00017 
00018 // You should have received a copy of the GNU Lesser General Public
00019 // License along with this library; if not, write to the Free Software
00020 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
00021 
00022 #include "chainfdsolver_recursive_newton_euler.hpp"
00023 #include "utilities/ldl_solver_eigen.hpp"
00024 #include "frames_io.hpp"
00025 #include "kinfam_io.hpp"
00026 
00027 namespace KDL{
00028 
00029     ChainFdSolver_RNE::ChainFdSolver_RNE(const Chain& _chain, Vector _grav):
00030         chain(_chain),
00031         DynSolver(chain, _grav),
00032         IdSolver(chain, _grav),
00033         nj(chain.getNrOfJoints()),
00034         ns(chain.getNrOfSegments()),
00035         H(nj),
00036         Tzeroacc(nj),
00037         H_eig(nj,nj),
00038         Tzeroacc_eig(nj),
00039         L_eig(nj,nj),
00040         D_eig(nj),
00041         r_eig(nj),
00042         acc_eig(nj)
00043     {
00044     }
00045 
00046     void ChainFdSolver_RNE::updateInternalDataStructures() {
00047         nj = chain.getNrOfJoints();
00048         ns = chain.getNrOfSegments();
00049     }
00050 
00051     int ChainFdSolver_RNE::CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &torques, const Wrenches& f_ext, JntArray &q_dotdot)
00052     {
00053         if(nj != chain.getNrOfJoints() || ns != chain.getNrOfSegments())
00054             return (error = E_NOT_UP_TO_DATE);
00055 
00056         //Check sizes of function parameters
00057         if(q.rows()!=nj || q_dot.rows()!=nj || q_dotdot.rows()!=nj || torques.rows()!=nj || f_ext.size()!=ns)
00058             return (error = E_SIZE_MISMATCH);
00059 
00060         // Inverse Dynamics:
00061         //   T = H * qdd + Tcor + Tgrav - J^T * Fext
00062         // Forward Dynamics:
00063         //   1. Call ChainDynParam->JntToMass to get H
00064         //   2. Call ChainIdSolver_RNE->CartToJnt with qdd=0 to get Tcor+Tgrav-J^T*Fext
00065         //   3. Calculate qdd = H^-1 * T where T are applied joint torques minus non-inertial internal torques
00066 
00067         // Calculate Joint Space Inertia Matrix
00068         error = DynSolver.JntToMass(q, H);
00069         if (error < 0)
00070             return (error);
00071 
00072         // Calculate non-inertial internal torques by inputting zero joint acceleration to ID
00073         for(int i=0;i<nj;i++){
00074             q_dotdot(i) = 0.;
00075         }
00076         error = IdSolver.CartToJnt(q, q_dot, q_dotdot, f_ext, Tzeroacc);
00077         if (error < 0)
00078             return (error);
00079 
00080         // Calculate acceleration using inverse symmetric matrix times vector
00081         for(int i=0;i<nj;i++){
00082             Tzeroacc_eig(i) =  (torques(i)-Tzeroacc(i));
00083             for(int j=0;j<nj;j++){
00084                 H_eig(i,j) =  H(i,j);
00085             }
00086         }
00087         ldl_solver_eigen(H_eig, Tzeroacc_eig, L_eig, D_eig, r_eig, acc_eig);
00088         for(int i=0;i<nj;i++){
00089             q_dotdot(i) = acc_eig(i);
00090         }
00091 
00092         return (error = E_NOERROR);
00093     }
00094 
00095     void ChainFdSolver_RNE::RK4Integrator(int& nj, const double& t, double& dt, KDL::JntArray& q, KDL::JntArray& q_dot,
00096                                           KDL::JntArray& torques, KDL::Wrenches& f_ext, KDL::ChainFdSolver_RNE& fdsolver,
00097                                           KDL::JntArray& q_dotdot, KDL::JntArray& dq, KDL::JntArray& dq_dot,
00098                                           KDL::JntArray& q_temp, KDL::JntArray& q_dot_temp)
00099     {
00100         fdsolver.CartToJnt(q, q_dot, torques, f_ext, q_dotdot);
00101         for (int i=0; i<nj; ++i)
00102         {
00103             q_temp(i) = q(i) + q_dot(i)*dt/2.0;
00104             q_dot_temp(i) = q_dot(i) + q_dotdot(i)*dt/2.0;
00105             dq(i) = q_dot(i);
00106             dq_dot(i) = q_dotdot(i);
00107         }
00108         fdsolver.CartToJnt(q_temp, q_dot_temp, torques, f_ext, q_dotdot);
00109         for (int i=0; i<nj; ++i)
00110         {
00111             q_temp(i) = q(i) + q_dot_temp(i)*dt/2.0;
00112             q_dot_temp(i) = q_dot(i) + q_dotdot(i)*dt/2.0;
00113             dq(i) += 2.0*q_dot_temp(i);
00114             dq_dot(i) += 2.0*q_dotdot(i);
00115         }
00116         fdsolver.CartToJnt(q_temp, q_dot_temp, torques, f_ext, q_dotdot);
00117         for (int i=0; i<nj; ++i)
00118         {
00119             q_temp(i) = q(i) + q_dot_temp(i)*dt;
00120             q_dot_temp(i) = q_dot(i) + q_dotdot(i)*dt;
00121             dq(i) += 2.0*q_dot_temp(i);
00122             dq_dot(i) += 2.0*q_dotdot(i);
00123         }
00124         fdsolver.CartToJnt(q_temp, q_dot_temp, torques, f_ext, q_dotdot);
00125         for (int i=0; i<nj; ++i)
00126         {
00127             dq(i) = (dq(i)+q_dot_temp(i))*dt/6.0;
00128             dq_dot(i) = (dq_dot(i)+q_dotdot(i))*dt/6.0;
00129         }
00130         for (int i=0; i<nj; ++i)
00131         {
00132             q(i) += dq(i);
00133             q_dot(i) += dq_dot(i);
00134         }
00135         return;
00136     }
00137 
00138 }//namespace


orocos_kdl
Author(s):
autogenerated on Fri Jun 14 2019 19:33:22