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