dynamics_sim.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 #include "dynamics_sim.h"
00030
00031 #ifdef use_namespace
00032 namespace ROBOOP {
00033 using namespace NEWMAT;
00034 #endif
00035
00036 static const char rcsid[] __UNUSED__ = "$Id: dynamics_sim.cpp,v 1.3 2005/07/26 03:22:09 ejt Exp $";
00037
00038 Dynamics *Dynamics::instance = 0;
00039
00040
00041
00042
00043
00044
00045 Dynamics::Dynamics(Robot_basic *robot_): robot(robot_)
00046 {
00047 ndof = 1;
00048 dof_fix = 1;
00049 if(robot)
00050 {
00051 ndof = robot->get_dof();
00052 dof_fix = ndof + robot->get_fix();
00053 }
00054
00055 q = ColumnVector(ndof);
00056 q = 0;
00057 qp = ColumnVector(ndof);
00058 qp = 0;
00059 qpp = ColumnVector(ndof);
00060 qpp = 0;
00061 qd = ColumnVector(ndof);
00062 qd = 0.0;
00063 qpd = ColumnVector(ndof);
00064 qpd = 0;
00065 tau = ColumnVector(ndof);
00066 tau = 0.0;
00067 pd = ColumnVector(3); pd = 0;
00068 ppd = ColumnVector(3);
00069 ppd = 0;
00070 pppd = ColumnVector(3);
00071 pppd = 0;
00072 wd = ColumnVector(3);
00073 wd = 0;
00074 wpd = ColumnVector(3);
00075 wpd = 0;
00076 nsteps = 50;
00077 to = 0;
00078 tf = 0.1;
00079 dt = ((tf-to)/nsteps)/4.0;
00080 tf_cont = 1;
00081
00082 first_pass_Kutta = true;
00083 instance = this;
00084 }
00085
00086 Dynamics *Dynamics::Instance()
00087
00088
00089
00090
00091 {
00092 return instance;
00093 }
00094
00095 void Dynamics::set_dof(Robot_basic *robot_)
00096
00097
00098
00099
00100
00101
00102 {
00103 ndof = 1;
00104 dof_fix = 1;
00105 robot = robot_;
00106 if(robot)
00107 {
00108 ndof = robot->get_dof();
00109 dof_fix = ndof + robot->get_fix();
00110 }
00111 q = ColumnVector(ndof);
00112 q = 0;
00113 qp = ColumnVector(ndof);
00114 qp = 0;
00115 qpp = ColumnVector(ndof);
00116 qpp = 0;
00117 qd = ColumnVector(ndof);
00118 qd = 0.0;
00119 qpd = ColumnVector(ndof);
00120 qpd = 0;
00121 tau = ColumnVector(ndof);
00122 tau = 0.0;
00123
00124 first_pass_Kutta = true;
00125 }
00126
00127 void Dynamics::set_time_frame(const int nsteps_)
00128
00129 {
00130 nsteps = nsteps_;
00131 }
00132
00133 void Dynamics::set_final_time(const double tf)
00134
00135 {
00136 tf_cont = tf;
00137 }
00138
00139 void Dynamics::reset_time()
00140
00141 {
00142 first_pass_Kutta = true;
00143 }
00144
00145 short Dynamics::set_controller(const Control_Select & control_)
00146
00147 {
00148 control = control_;
00149 if(ndof != control.get_dof())
00150 {
00151 control.type = NONE;
00152 cerr << "Dynamics::set_controller: mismatch degree of freedom between robot and controller." << endl;
00153 return -1;
00154 }
00155 return 0;
00156 }
00157
00158 short Dynamics::set_trajectory(const Trajectory_Select & path_select_)
00159
00160 {
00161 path_select = path_select_;
00162
00163 if (control.space_type != path_select.type)
00164 {
00165 control.type = NONE;
00166 path_select.type = NONE;
00167 cerr << "Dynamics::set_trajectory: controller space and trajectory space are not compatible.\n"
00168 << " Both should be in joint space or in cartesian space." << endl;
00169 return -1;
00170 }
00171 return 0;
00172 }
00173
00174 ReturnMatrix Dynamics::set_robot_on_first_point_of_splines()
00175
00176
00177
00178
00179
00180
00181
00182
00183
00184
00185 {
00186 ColumnVector qs(2*ndof);
00187
00188 if(path_select.type == JOINT_SPACE)
00189 {
00190 if(path_select.path.p_pdot(0.0, qd, qpd))
00191 cerr << "Dynamics::set_robot_on_first_point_of_spines: invalid joint space path." << endl;
00192 else
00193 {
00194 tf_cont = path_select.path.get_final_time();
00195 robot->set_q(qd);
00196 qs.SubMatrix(1,ndof,1,1) = qd;
00197 qs.SubMatrix(ndof+1,2*ndof,1,1) = 0;
00198 qs.Release();
00199 return qs;
00200 }
00201 }
00202 else if(path_select.type == CARTESIAN_SPACE)
00203 {
00204 if( (path_select.path.p_pdot_pddot(0.0, pd, ppd, pppd) == 0) &&
00205 (path_select.path_quat.quat_w(0.0, quatd, wd) == 0) )
00206 {
00207 Matrix Tobj(4,4); Tobj = 0;
00208 Tobj.SubMatrix(1,3,1,3) = quatd.R();
00209 Tobj.SubMatrix(1,3,4,4) = pd;
00210 Tobj(4,4) = 1;
00211 bool converge;
00212 robot->inv_kin(Tobj, 0, converge);
00213
00214 if(converge)
00215 {
00216 tf_cont = path_select.path.get_final_time();
00217 q = robot->get_q();
00218 qs.SubMatrix(1,ndof,1,1) = q;
00219 qs.SubMatrix(ndof+1,2*ndof,1,1) = 0;
00220
00221 qs.Release();
00222 return qs;
00223 }
00224 }
00225 else
00226 cerr << "Dynamics::set_robot_on_first_point_of_spines: invalid cartesian path." << endl;
00227 }
00228
00229 q = robot->get_q();
00230 qs.SubMatrix(1,ndof,1,1) = q;
00231 qs.SubMatrix(ndof+1,2*ndof,1,1) = 0;
00232 qs.Release();
00233 return qs;
00234 }
00235
00236 ReturnMatrix Dynamics::xdot(const Matrix & x)
00237
00238
00239
00240
00241
00242
00243
00244 {
00245 q = x.SubMatrix(1,ndof,1,1);
00246 qp = x.SubMatrix(ndof+1,2*ndof,1,1);
00247
00248 if(robot)
00249 {
00250 switch (control.type)
00251 {
00252 case PD:
00253 if(path_select.type == JOINT_SPACE)
00254 {
00255 if(path_select.path.p_pdot(time, qd, qpd))
00256 {
00257 xd = qp & qpp;
00258 xd.Release();
00259 return xd;
00260 }
00261 }
00262 else if(path_select.type == CARTESIAN_SPACE)
00263 cerr << "Dynamics::xdot: Cartesian path can not be used with CTM controller." << endl;
00264
00265 tau = control.pd.torque_cmd(*robot, qd, qpd);
00266
00267 break;
00268
00269 case CTM:
00270 if(path_select.type == JOINT_SPACE)
00271 {
00272 if(path_select.path.p_pdot(time, qd, qpd))
00273 {
00274 xd = qp & qpp;
00275 xd.Release();
00276 return xd;
00277 }
00278 }
00279 else if(path_select.type == CARTESIAN_SPACE)
00280 cerr << "Dynamics::xdot: Cartesian path can not be used with CTM controller." << endl;
00281
00282 tau = control.ctm.torque_cmd(*robot, qd, qpd);
00283
00284 break;
00285
00286 case RRA:
00287 if(path_select.type == CARTESIAN_SPACE)
00288 {
00289 if (path_select.path.p_pdot_pddot(time, pd, ppd, pppd) ||
00290 path_select.path_quat.quat_w(time, quatd, wd))
00291 {
00292 xd = qp & qpp;
00293 xd.Release();
00294 return xd;
00295 }
00296 }
00297 else if(path_select.type == JOINT_SPACE)
00298 cerr << "Dynamics::xdot: Joint Space path can not be used with RRA controller." << endl;
00299
00300 tau = control.rra.torque_cmd(*robot, pppd, ppd, pd, wpd, wd, quatd, dof_fix, dt);
00301 break;
00302 default:
00303 tau = 0;
00304 }
00305 qpp = robot->acceleration(q, qp, tau);
00306 }
00307
00308 plot();
00309
00310 xd = qp & qpp;
00311
00312 xd.Release();
00313 return xd;
00314 }
00315
00316 void Dynamics::Runge_Kutta4_Real_time()
00317
00318 {
00319 if(first_pass_Kutta)
00320 {
00321 h = (tf-to)/nsteps;
00322 h2 = h/2.0;
00323 time = to;
00324 x = set_robot_on_first_point_of_splines();
00325 first_pass_Kutta = false;
00326 return;
00327 }
00328
00329 k1 = xdot(x) * h;
00330 time += h2;
00331 k2 = xdot(x+k1*0.5)*h;
00332 k3 = xdot(x+k2*0.5)*h;
00333 time += h2;
00334 k4 = xdot(x+k3)*h;
00335 x = x + (k1*0.5+ k2 + k3 + k4*0.5)*(1.0/3.0);
00336 }
00337
00338 void Dynamics::Runge_Kutta4()
00339
00340 {
00341 x = set_robot_on_first_point_of_splines();
00342 h = (tf_cont - to)/nsteps;
00343 h2 = h/2.0;
00344 time = to;
00345
00346 for (int i = 1; i <= nsteps; i++) {
00347 k1 = xdot(x) * h;
00348 k2 = xdot(x+k1*0.5)*h;
00349 k3 = xdot(x+k2*0.5)*h;
00350 k4 = xdot(x+k3)*h;
00351 x = x + (k1*0.5+ k2 + k3 + k4*0.5)*(1.0/3.0);
00352 time += h;
00353 }
00354 }
00355
00356 #ifdef use_namespace
00357 }
00358 #endif
00359
00360
00361
00362
00363
00364
00365
00366
00367
00368
00369
|