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