00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
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
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
00061
00062
00063
00064
00065
00066
00067
00068 error = DynSolver.JntToMass(q, H);
00069 if (error < 0)
00070 return (error);
00071
00072
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
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 }