control_select.cppGo 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
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include "control_select.h"
00038
00039 #ifdef use_namespace
00040 namespace ROBOOP {
00041 using namespace NEWMAT;
00042 #endif
00043
00044 static const char rcsid[] __UNUSED__ = "$Id: control_select.cpp,v 1.4 2005/07/26 03:22:09 ejt Exp $";
00045
00046 Control_Select::Control_Select()
00047
00048 {
00049 type = NONE;
00050 space_type = NONE;
00051 dof = 0;
00052 }
00053
00054 Control_Select::Control_Select(const string & filename)
00055
00056
00057
00058
00059 {
00060 set_control(filename);
00061 }
00062
00063 Control_Select::Control_Select(const Control_Select & x)
00064
00065 {
00066 type = x.type;
00067 space_type = x.space_type;
00068 dof = x.dof;
00069 pd = x.pd;
00070 ctm = x.ctm;
00071 rra = x.rra;
00072 impedance = x.impedance;
00073 }
00074
00075 Control_Select & Control_Select::operator=(const Control_Select & x)
00076
00077 {
00078 type = x.type;
00079 space_type = x.space_type;
00080 dof = x.dof;
00081 pd = x.pd;
00082 ctm = x.ctm;
00083 rra = x.rra;
00084 impedance = x.impedance;
00085
00086 return *this;
00087 }
00088
00089 int Control_Select::get_dof()
00090
00091 {
00092 return dof;
00093 }
00094
00095 void Control_Select::set_control(const string & filename)
00096
00097 {
00098 Config conf(filename);
00099 conf.read_conf();
00100
00101 conf.select_string(CONTROLLER, "type", ControllerName);
00102
00103 if (ControllerName == PROPORTIONAL_DERIVATIVE)
00104 {
00105 type = PD;
00106 space_type = JOINT_SPACE;
00107 }
00108 else if(ControllerName == COMPUTED_TORQUE_METHOD)
00109 {
00110 type = CTM;
00111 space_type = JOINT_SPACE;
00112 }
00113 else if(ControllerName == RESOLVED_RATE_ACCELERATION)
00114 {
00115 type = RRA;
00116 space_type = CARTESIAN_SPACE;
00117 }
00118 else if(ControllerName == IMPEDANCE)
00119 {
00120 type = IMP;
00121 space_type = CARTESIAN_SPACE;
00122 }
00123 else
00124 {
00125 ControllerName = "";
00126 type = 0;
00127 space_type = 0;
00128 }
00129
00130 conf.select_int(CONTROLLER, "dof", dof);
00131
00132 switch (type) {
00133 case PD:
00134 {
00135 pd = Proportional_Derivative(dof);
00136 DiagonalMatrix Kp(dof), Kd(dof);
00137 for(int i = 1; i <= dof; i++)
00138 {
00139 #ifdef __WATCOMC__
00140 ostrstream Kp_ostr, Kd_ostr;
00141 Kp_ostr << "Kp_" << i;
00142 string temp = Kp_ostr.str();
00143 temp[Kp_ostr.pcount()] = 0;
00144 conf.select_real("GAINS", temp.c_str(), Kp(i));
00145 Kd_ostr << "Kd_" << i;
00146 temp = Kd_ostr.str();
00147 temp[Kd_ostr.pcount()] = 0;
00148 conf.select_real("GAINS", temp.c_str(), Kd(i));
00149 #else
00150 ostringstream Kp_ostr, Kd_ostr;
00151 Kp_ostr << "Kp_" << i;
00152 conf.select_real("GAINS", Kp_ostr.str(), Kp(i));
00153 Kd_ostr << "Kd_" << i;
00154 conf.select_real("GAINS", Kd_ostr.str(), Kd(i));
00155 #endif
00156 }
00157 pd.set_Kp(Kp);
00158 pd.set_Kd(Kd);
00159 }
00160 break;
00161
00162 case CTM:
00163 {
00164 ctm = Computed_torque_method(dof);
00165 DiagonalMatrix Kp(dof), Kd(dof);
00166 for(int i = 1; i <= dof; i++)
00167 {
00168 #ifdef __WATCOMC__
00169 ostrstream Kp_ostr, Kd_ostr;
00170 Kp_ostr << "Kp_" << i;
00171 string temp = Kp_ostr.str();
00172 temp[Kp_ostr.pcount()] = 0;
00173 conf.select_real("GAINS", temp.c_str(), Kp(i));
00174 Kd_ostr << "Kd_" << i;
00175 temp = Kd_ostr.str();
00176 temp[Kd_ostr.pcount()] = 0;
00177 conf.select_real("GAINS", temp.c_str(), Kd(i));
00178 #else
00179 ostringstream Kp_ostr, Kd_ostr;
00180 Kp_ostr << "Kp_" << i;
00181 conf.select_real("GAINS", Kp_ostr.str(), Kp(i));
00182 Kd_ostr << "Kd_" << i;
00183 conf.select_real("GAINS", Kd_ostr.str(), Kd(i));
00184 #endif
00185 }
00186 ctm.set_Kp(Kp);
00187 ctm.set_Kd(Kd);
00188 }
00189 break;
00190
00191 case RRA:
00192 {
00193 rra = Resolved_acc(dof);
00194 Real Kvp, Kpp, Kvo, Kpo;
00195 conf.select_real("GAINS", "Kvp", Kvp);
00196 conf.select_real("GAINS", "Kpp", Kpp);
00197 conf.select_real("GAINS", "Kvo", Kvo);
00198 conf.select_real("GAINS", "Kpo", Kpo);
00199 rra.set_Kvp( Kvp );
00200 rra.set_Kpp( Kpp );
00201 rra.set_Kvo( Kvo );
00202 rra.set_Kpo( Kpo );
00203 }
00204 break;
00205
00206 case IMP:
00207 {
00208 }
00209 break;
00210
00211 default:
00212 break;
00213 }
00214 }
00215
00216 #ifdef use_namespace
00217 }
00218 #endif
00219
|