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