ImpedanceOutputGenerator.h
Go to the documentation of this file.
00001 #ifndef IMPEDANCETRGETGENERATOR_H
00002 #define IMPEDANCETRGETGENERATOR_H
00003 
00004 #include "RatsMatrix.h"
00005 
00006 struct ImpedanceOutputGenerator
00007 {
00008     // target  : Target pos and rot from SequencePlayer and StateHolder
00009     // current : Current ee pos and rot (IK result)
00010     // output  : Output from ImpedanceOutput which response is characterized by MDK
00011     // index 0 : t+dt. Values in current onExecute.
00012     // index 1 : t. Values in previous onExecute.
00013     // index 2 : t-dt. Values in previous previous onExecute.
00014     hrp::Vector3 target_p0, target_p1, target_p2, current_p1, output_p1, output_p2;
00015     hrp::Matrix33 target_r0, target_r1, target_r2, current_r1, output_r1, output_r2;
00016     double M_p, D_p, K_p;
00017     double M_r, D_r, K_r;
00018     hrp::Matrix33 force_gain, moment_gain;
00019 
00020     ImpedanceOutputGenerator ()
00021     : target_p0(hrp::Vector3::Zero()), target_p1(hrp::Vector3::Zero()), target_p2(hrp::Vector3::Zero()),
00022       current_p1(hrp::Vector3::Zero()),  output_p1(hrp::Vector3::Zero()), output_p2(hrp::Vector3::Zero()),
00023       target_r0(hrp::Matrix33::Identity()), target_r1(hrp::Matrix33::Identity()), target_r2(hrp::Matrix33::Identity()),
00024       current_r1(hrp::Matrix33::Identity()), output_r1(hrp::Matrix33::Identity()), output_r2(hrp::Matrix33::Identity()),
00025       M_p(10), D_p(200), K_p(400), M_r(5), D_r(100), K_r(200),
00026       force_gain(hrp::Matrix33::Identity()), moment_gain(hrp::Matrix33::Identity())
00027     {};
00028     const hrp::Matrix33& getOutputRot () { return output_r1; };
00029     const hrp::Vector3& getOutputPos () { return output_p1; };
00030     void resetPreviousTargetParam ()
00031     {
00032         target_p1 = target_p0;
00033         target_p2 = target_p1;
00034         target_r1 = target_r0;
00035         target_r2 = target_r1;
00036     };
00037     void resetPreviousCurrentParam ()
00038     {
00039         output_p1 = current_p1;
00040         output_p2 = output_p1;
00041         output_r1 = current_r1;
00042         output_r2 = output_r1;
00043     };
00044     void calcTargetVelocityOrg (hrp::Vector3& vel_p, hrp::Vector3& vel_r,
00045                              const hrp::Matrix33& eeR,
00046                              const hrp::Vector3& force_diff, const hrp::Vector3& moment_diff,
00047                              const double _dt, const bool printp = false, const std::string& print_str = "", const std::string& ee_name = "")
00048     {
00049         if ( printp ) {
00050             std::cerr << "[" << print_str << "] impedance calc [" << ee_name << "] (Old version)" << std::endl;
00051             std::cerr << "[" << print_str << "]   cur1 = " << current_p1.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[m]" << std::endl;
00052             std::cerr << "[" << print_str << "]   out1 = " << output_p1.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[m]" << std::endl;
00053             std::cerr << "[" << print_str << "]   out2 = " << output_p2.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[m]" << std::endl;
00054             std::cerr << "[" << print_str << "]   tgt0 = " << target_p0.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[m]" << std::endl;
00055             std::cerr << "[" << print_str << "]   tgt1 = " << target_p1.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[m]" << std::endl;
00056         }
00057 
00058         hrp::Vector3 dif_output_pos, dif_target_pos, vel_target_pos, err_pos;
00059         hrp::Vector3 dif_output_rot, dif_target_rot, vel_target_rot, err_rot;
00060 
00061         dif_output_pos = output_p1 - output_p2;
00062         dif_target_pos = target_p0 - target_p1;
00063         vel_target_pos = target_p0 - current_p1;
00064         err_pos = output_p1 - current_p1;
00065 
00066         rats::difference_rotation(dif_output_rot, output_r2, output_r1);
00067         rats::difference_rotation(dif_target_rot, target_r1, target_r0);
00068         rats::difference_rotation(vel_target_rot, current_r1, target_r0);
00069         rats::difference_rotation(err_rot, current_r1, output_r1);
00070 
00071         if ( printp ) {
00072             std::cerr << "[" << print_str << "]   dif_out_p  = " << dif_output_pos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[m]" << std::endl;
00073             std::cerr << "[" << print_str << "]   dif_tgt_p = " << dif_target_pos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[m]" << std::endl;
00074             std::cerr << "[" << print_str << "]   vel_tgt_p = " << vel_target_pos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[m]" << std::endl;
00075             std::cerr << "[" << print_str << "]   err_pos   = " << err_pos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[m]" << std::endl;
00076             std::cerr << "[" << print_str << "]   dif_out_r  = " << dif_output_rot.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[rad]" << std::endl;
00077             std::cerr << "[" << print_str << "]   dif_tgt_r = " << dif_target_rot.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[rad]" << std::endl;
00078             std::cerr << "[" << print_str << "]   vel_tgt_r = " << vel_target_rot.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[rad]" << std::endl;
00079             std::cerr << "[" << print_str << "]   err_rot   = " << err_rot.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[rad]" << std::endl;
00080         }
00081         vel_p =  ( eeR * (force_gain * (eeR.transpose() * force_diff)) * _dt * _dt
00082                    + M_p * ( dif_output_pos + err_pos )
00083                    + D_p * ( dif_target_pos + err_pos ) * _dt
00084                    + K_p * ( vel_target_pos * _dt * _dt ) ) /
00085             (M_p + (D_p * _dt) + (K_p * _dt * _dt));
00086         vel_r =  ( eeR * (moment_gain * (eeR.transpose() * moment_diff)) * _dt * _dt
00087                    + M_r * ( dif_output_rot + err_rot )
00088                    + D_r * ( dif_target_rot + err_rot ) * _dt
00089                    + K_r * ( vel_target_rot * _dt * _dt  ) ) /
00090             (M_r + (D_r * _dt) + (K_r * _dt * _dt));
00091         // generate smooth motion just after impedance started
00092         if ( printp ) {
00093             std::cerr << "[" << print_str << "]   vel_p  = " << vel_p.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[m]" << std::endl;
00094             std::cerr << "[" << print_str << "]   vel_r  = " << vel_r.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[rad]" << std::endl;
00095         }
00096 
00097         // Spin time-related parameter
00098         output_p2 = output_p1;
00099         output_r2 = output_r1;
00100 
00101         output_p1 = current_p1 + vel_p;
00102         // if ( std::fabs(vel_r.norm() - 0.0) < ::std::numeric_limits<double>::epsilon() ) {
00103         if ( vel_r.norm() != 0.0 ) {
00104             hrp::Matrix33 tmpm;
00105             Eigen::AngleAxis<double> tmpr(vel_r.norm(), vel_r.normalized());
00106             rats::rotm3times(tmpm, tmpr.toRotationMatrix(), current_r1);
00107             output_r1 = tmpm;
00108         } else {
00109             output_r1 = current_r1;
00110         }
00111 
00112         target_p1 = target_p0;
00113         target_r1 = target_r0;
00114     };
00115     void calcTargetVelocity (hrp::Vector3& vel_p, hrp::Vector3& vel_r,
00116                                 const hrp::Matrix33& eeR,
00117                                 const hrp::Vector3& force_diff, const hrp::Vector3& moment_diff,
00118                                 const double _dt, const bool printp = false, const std::string& print_str = "", const std::string& ee_name = "")
00119     {
00120         if ( printp ) {
00121             std::cerr << "[" << print_str << "] impedance calc [" << ee_name << "] (New version)" << std::endl;
00122             std::cerr << "[" << print_str << "]   cur1 = " << current_p1.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[m]" << std::endl;
00123             std::cerr << "[" << print_str << "]   out1 = " << output_p1.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[m]" << std::endl;
00124             std::cerr << "[" << print_str << "]   out2 = " << output_p2.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[m]" << std::endl;
00125             std::cerr << "[" << print_str << "]   tgt0 = " << target_p0.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[m]" << std::endl;
00126             std::cerr << "[" << print_str << "]   tgt1 = " << target_p1.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[m]" << std::endl;
00127             std::cerr << "[" << print_str << "]   tgt2 = " << target_p2.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[m]" << std::endl;
00128         }
00129         hrp::Vector3 dif_pos1, dif_rot1, dif_pos2, dif_rot2, vel_target_pos, vel_target_rot;
00130         dif_pos1 = output_p1 - target_p1;
00131         dif_pos2 = output_p2 - target_p2;
00132         vel_target_pos = target_p0 - current_p1;
00133         rats::difference_rotation(dif_rot1, target_r1, output_r1);
00134         rats::difference_rotation(dif_rot2, target_r2, output_r2);
00135         rats::difference_rotation(vel_target_rot, current_r1, target_r0);
00136         vel_p =  ( eeR * (force_gain * (eeR.transpose() * force_diff)) * _dt * _dt
00137                    + (2 * M_p + D_p * _dt) * dif_pos1
00138                    - M_p * dif_pos2) /
00139             (M_p + (D_p * _dt) + (K_p * _dt * _dt))
00140             + vel_target_pos;
00141         vel_r =  ( eeR * (moment_gain * (eeR.transpose() * moment_diff)) * _dt * _dt
00142                    + (2 * M_r + D_r * _dt) * dif_rot1
00143                    - M_r * dif_rot2) /
00144             (M_r + (D_r * _dt) + (K_r * _dt * _dt))
00145             + vel_target_rot;
00146         // generate smooth motion just after impedance started
00147         if ( printp ) {
00148             std::cerr << "[" << print_str << "]   vel_p  = " << vel_p.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[m]" << std::endl;
00149             std::cerr << "[" << print_str << "]   vel_r  = " << vel_r.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[rad]" << std::endl;
00150         }
00151 
00152         // Spin time-related parameter
00153         output_p2 = output_p1;
00154         output_r2 = output_r1;
00155 
00156         output_p1 = current_p1 + vel_p;
00157         // if ( std::fabs(vel_r.norm() - 0.0) < ::std::numeric_limits<double>::epsilon() ) {
00158         if ( vel_r.norm() != 0.0 ) {
00159             hrp::Matrix33 tmpm;
00160             Eigen::AngleAxis<double> tmpr(vel_r.norm(), vel_r.normalized());
00161             rats::rotm3times(tmpm, tmpr.toRotationMatrix(), current_r1);
00162             output_r1 = tmpm;
00163         } else {
00164             output_r1 = current_r1;
00165         }
00166 
00167         target_p2 = target_p1;
00168         target_r2 = target_r1;
00169         target_p1 = target_p0;
00170         target_r1 = target_r0;
00171     };
00172 };
00173 #endif // IMPEDANCETRGETGENERATOR_H


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed Sep 6 2017 02:35:55