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 using namespace std;
00032
00033 #ifdef use_namespace
00034 namespace ROBOOP {
00035 using namespace NEWMAT;
00036 #endif
00037
00038 static const char rcsid[] __UNUSED__ = "$Id: dynamics_sim.cpp,v 1.4 2007/11/11 23:57:24 ejt Exp $";
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