Go to the documentation of this file.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 #include "treeiksolverpos_online.hpp"
00026
00027 namespace KDL {
00028
00029 TreeIkSolverPos_Online::TreeIkSolverPos_Online(const double& nr_of_jnts,
00030 const std::vector<std::string>& endpoints,
00031 const JntArray& q_min,
00032 const JntArray& q_max,
00033 const JntArray& q_dot_max,
00034 const double x_dot_trans_max,
00035 const double x_dot_rot_max,
00036 TreeFkSolverPos& fksolver,
00037 TreeIkSolverVel& iksolver) :
00038 q_min_(nr_of_jnts),
00039 q_max_(nr_of_jnts),
00040 q_dot_max_(nr_of_jnts),
00041 fksolver_(fksolver),
00042 iksolver_(iksolver),
00043 q_dot_(nr_of_jnts)
00044 {
00045 q_min_ = q_min;
00046 q_max_ = q_max;
00047 q_dot_max_ = q_dot_max;
00048 x_dot_trans_max_ = x_dot_trans_max;
00049 x_dot_rot_max_ = x_dot_rot_max;
00050
00051 for (size_t i = 0; i < endpoints.size(); i++)
00052 {
00053 frames_.insert(Frames::value_type(endpoints[i], Frame::Identity()));
00054 delta_twists_.insert(Twists::value_type(endpoints[i], Twist::Zero()));
00055 }
00056 }
00057
00058
00059 TreeIkSolverPos_Online::~TreeIkSolverPos_Online()
00060 {}
00061
00062
00063 double TreeIkSolverPos_Online::CartToJnt(const JntArray& q_in, const Frames& p_in, JntArray& q_out)
00064 {
00065 q_out = q_in;
00066
00067
00068 for(Frames::const_iterator f_des_it=p_in.begin();f_des_it!=p_in.end();++f_des_it)
00069 if(frames_.find(f_des_it->first)==frames_.end())
00070 return -2;
00071
00072 for (Frames::const_iterator f_des_it=p_in.begin();f_des_it!=p_in.end();++f_des_it)
00073 {
00074
00075 Frames::iterator f_it = frames_.find(f_des_it->first);
00076 Twists::iterator delta_twists_it = delta_twists_.find(f_des_it->first);
00077
00078 fksolver_.JntToCart(q_out, f_it->second, f_it->first);
00079 twist_ = diff(f_it->second, f_des_it->second);
00080
00081
00082
00083 enforceCartVelLimits();
00084
00085 delta_twists_it->second = twist_;
00086 }
00087
00088 double res = iksolver_.CartToJnt(q_out, delta_twists_, q_dot_);
00089
00090 if(res<0)
00091 return res;
00092
00093 if(q_out.rows()!=q_min_.rows() || q_out.rows()!=q_max_.rows() || q_out.rows()!= q_dot_max_.rows())
00094 return -1;
00095
00096
00097 enforceJointVelLimits();
00098
00099
00100 Add(q_out, q_dot_, q_out);
00101
00102
00103 for (unsigned int j = 0; j < q_min_.rows(); j++)
00104 {
00105 if (q_out(j) < q_min_(j))
00106 q_out(j) = q_min_(j);
00107 else if (q_out(j) > q_max_(j))
00108 q_out(j) = q_max_(j);
00109 }
00110
00111 return res;
00112 }
00113
00114
00115 void TreeIkSolverPos_Online::enforceJointVelLimits()
00116 {
00117
00118
00119
00120 double rel_os, rel_os_max = 0.0;
00121 bool max_exceeded = false;
00122
00123 for (unsigned int i = 0; i < q_dot_.rows(); i++)
00124 {
00125 if ( q_dot_(i) > q_dot_max_(i) )
00126 {
00127 max_exceeded = true;
00128 rel_os = (q_dot_(i) - q_dot_max_(i)) / q_dot_max_(i);
00129 if ( rel_os > rel_os_max )
00130 {
00131 rel_os_max = rel_os;
00132 }
00133 }
00134 else if ( q_dot_(i) < -q_dot_max_(i) )
00135 {
00136 max_exceeded = true;
00137 rel_os = (-q_dot_(i) - q_dot_max_(i)) / q_dot_max_(i);
00138 if ( rel_os > rel_os_max)
00139 {
00140 rel_os_max = rel_os;
00141 }
00142 }
00143 }
00144
00145
00146 if ( max_exceeded == true )
00147 {
00148 Multiply(q_dot_, ( 1.0 / ( 1.0 + rel_os_max ) ), q_dot_);
00149 }
00150 }
00151
00152
00153 void TreeIkSolverPos_Online::enforceCartVelLimits()
00154 {
00155 double x_dot_trans, x_dot_rot;
00156 x_dot_trans = sqrt( pow(twist_.vel.x(), 2) + pow(twist_.vel.y(), 2) + pow(twist_.vel.z(), 2));
00157 x_dot_rot = sqrt( pow(twist_.rot.x(), 2) + pow(twist_.rot.y(), 2) + pow(twist_.rot.z(), 2));
00158
00159 if ( x_dot_trans > x_dot_trans_max_ || x_dot_rot > x_dot_rot_max_ )
00160 {
00161 if ( x_dot_trans > x_dot_rot )
00162 {
00163 twist_.vel = twist_.vel * ( x_dot_trans_max_ / x_dot_trans );
00164 twist_.rot = twist_.rot * ( x_dot_trans_max_ / x_dot_trans );
00165 }
00166 else if ( x_dot_rot > x_dot_trans )
00167 {
00168 twist_.vel = twist_.vel * ( x_dot_rot_max_ / x_dot_rot );
00169 twist_.rot = twist_.rot * ( x_dot_rot_max_ / x_dot_rot );
00170 }
00171 }
00172 }
00173
00174 }
00175