dynamics_sim.cpp
Go to the documentation of this file.
1 /*
2 Copyright (C) 2002-2004 Etienne Lachance
3 
4 This library is free software; you can redistribute it and/or modify
5 it under the terms of the GNU Lesser General Public License as
6 published by the Free Software Foundation; either version 2.1 of the
7 License, or (at your option) any later version.
8 
9 This library is distributed in the hope that it will be useful,
10 but WITHOUT ANY WARRANTY; without even the implied warranty of
11 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 GNU Lesser General Public License for more details.
13 
14 You should have received a copy of the GNU Lesser General Public
15 License along with this library; if not, write to the Free Software
16 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
17 
18 
19 Report problems and direct all questions to:
20 
21 email: etienne.lachance@polymtl.ca or richard.gourdeau@polymtl.ca
22 
23 -------------------------------------------------------------------------------
24 Revision_history:
25 
26 2005/06/10: Etienne Lachance
27  -The desired joint acceleration was missing in the computed torque method.
28 
29 2006/05/19: Richard Gourdeau
30  -Fix set_q, setq_p bug in xdot (reported by Philip Gruebele)
31 -------------------------------------------------------------------------------
32 */
33 
39 static const char rcsid[] = "$Id: dynamics_sim.cpp,v 1.6 2006/05/19 21:05:57 gourdeau Exp $";
41 
42 #include "dynamics_sim.h"
43 #include "robot.h"
44 
45 #ifdef use_namespace
46 namespace ROBOOP {
47  using namespace NEWMAT;
48 #endif
49 
51 
52 
58 {
59  ndof = 1;
60  dof_fix = 1;
61  if(robot)
62  {
63  ndof = robot->get_dof();
64  dof_fix = ndof + robot->get_fix();
65  }
66 
67  q = ColumnVector(ndof);
68  q = 0;
69  qp = ColumnVector(ndof);
70  qp = 0;
72  qpp = 0;
73  qd = ColumnVector(ndof);
74  qd = 0.0;
76  qpd = 0;
78  qppd = 0;
80  tau = 0.0;
81  pd = ColumnVector(3); pd = 0;
82  ppd = ColumnVector(3);
83  ppd = 0;
84  pppd = ColumnVector(3);
85  pppd = 0;
86  wd = ColumnVector(3);
87  wd = 0;
88  wpd = ColumnVector(3);
89  wpd = 0;
90  nsteps = 50;
91  to = 0;
92  tf = 0.1;
93  dt = ((tf-to)/nsteps)/4.0;
94  tf_cont = 1;
95 
96  first_pass_Kutta = true;
97  instance = this;
98 }
99 
105 {
106  return instance;
107 }
108 
116 {
117  ndof = 1;
118  dof_fix = 1;
119  robot = robot_;
120  if(robot)
121  {
122  ndof = robot->get_dof();
123  dof_fix = ndof + robot->get_fix();
124  }
125  q = ColumnVector(ndof);
126  q = 0;
127  qp = ColumnVector(ndof);
128  qp = 0;
129  qpp = ColumnVector(ndof);
130  qpp = 0;
131  qd = ColumnVector(ndof);
132  qd = 0.0;
133  qpd = ColumnVector(ndof);
134  qpd = 0;
136  qppd = 0;
137  tau = ColumnVector(ndof);
138  tau = 0.0;
139 
140  first_pass_Kutta = true;
141 }
142 
143 void Dynamics::set_time_frame(const int nsteps_)
145 {
146  nsteps = nsteps_;
147 }
148 
149 void Dynamics::set_final_time(const double tf)
151 {
152  tf_cont = tf;
153 }
154 
157 {
158  first_pass_Kutta = true;
159 }
160 
163 {
164  control = control_;
165  if( (ndof != control.get_dof()) && (control.type != NONE) )
166  {
167  control.type = NONE;
168  cerr << "Dynamics::set_controller: mismatch degree of freedom between robot and controller." << endl;
169  return -1;
170  }
171  return 0;
172 }
173 
174 short Dynamics::set_trajectory(const Trajectory_Select & path_select_)
176 {
177  path_select = path_select_;
178 
180  {
181  control.type = NONE;
183  cerr << "Dynamics::set_trajectory: controller space and trajectory space are not compatible.\n"
184  << " Both should be in joint space or in cartesian space." << endl;
185  return -1;
186  }
187  return 0;
188 }
189 
201 {
202  ColumnVector qs(2*ndof);
203 
205  {
206  if(path_select.path.p_pdot(0.0, qd, qpd))
207  cerr << "Dynamics::set_robot_on_first_point_of_spines: invalid joint space path." << endl;
208  else
209  {
211  robot->set_q(qd);
212  qs.SubMatrix(1,ndof,1,1) = qd;
213  qs.SubMatrix(ndof+1,2*ndof,1,1) = 0;
214  qs.Release();
215  return qs;
216  }
217  }
218  else if(path_select.type == CARTESIAN_SPACE) // && quaternion active
219  {
220  if( (path_select.path.p_pdot_pddot(0.0, pd, ppd, pppd) == 0) &&
221  (path_select.path_quat.quat_w(0.0, quatd, wd) == 0) )
222  {
223  Matrix Tobj(4,4); Tobj = 0;
224  Tobj.SubMatrix(1,3,1,3) = quatd.R();
225  Tobj.SubMatrix(1,3,4,4) = pd;
226  Tobj(4,4) = 1;
227  bool converge;
228  robot->inv_kin(Tobj, 0, converge);
229 
230  if(converge)
231  {
233  q = robot->get_q();
234  qs.SubMatrix(1,ndof,1,1) = q;
235  qs.SubMatrix(ndof+1,2*ndof,1,1) = 0;
236 
237  qs.Release();
238  return qs;
239  }
240  }
241  else
242  cerr << "Dynamics::set_robot_on_first_point_of_spines: invalid cartesian path." << endl;
243  }
244 
245  q = robot->get_q();
246  qs.SubMatrix(1,ndof,1,1) = q;
247  qs.SubMatrix(ndof+1,2*ndof,1,1) = 0;
248  qs.Release();
249  return qs;
250 }
251 
260 {
261  q = x.SubMatrix(1,ndof,1,1); // joint position from state vecteur
262  qp = x.SubMatrix(ndof+1,2*ndof,1,1);// " " velocity " "
263  robot->set_q(q);
264  robot->set_qp(qp);
265 
266  if(robot)
267  {
268  switch (control.type)
269  {
270  case PD:
272  {
274  {
275  xd = qp & qpp;
276  xd.Release();
277  return xd;
278  }
279  }
280  else if(path_select.type == CARTESIAN_SPACE)
281  cerr << "Dynamics::xdot: Cartesian path can not be used with CTM controller." << endl;
282 
284 
285  break;
286 
287  case CTM:
289  {
291  {
292  xd = qp & qpp;
293  xd.Release();
294  return xd;
295  }
296  }
297  else if(path_select.type == CARTESIAN_SPACE)
298  cerr << "Dynamics::xdot: Cartesian path can not be used with CTM controller." << endl;
299 
301 
302  break;
303 
304  case RRA:
306  {
309  {
310  xd = qp & qpp;
311  xd.Release();
312  return xd;
313  }
314  }
315  else if(path_select.type == JOINT_SPACE)
316  cerr << "Dynamics::xdot: Joint Space path can not be used with RRA controller." << endl;
317 
319  break;
320  default:
321  tau = 0;
322  }
323  qpp = robot->acceleration(q, qp, tau);
324  }
325 
326  plot();
327 
328  xd = qp & qpp; // state derivative
329 
330  xd.Release();
331  return xd;
332 }
333 
336 {
337  if(first_pass_Kutta)
338  {
339  h = (tf-to)/nsteps;
340  h2 = h/2.0;
341  time = to;
343  first_pass_Kutta = false;
344  return;
345  }
346 
347  k1 = xdot(x) * h;
348  time += h2;
349  k2 = xdot(x+k1*0.5)*h;
350  k3 = xdot(x+k2*0.5)*h;
351  time += h2;
352  k4 = xdot(x+k3)*h;
353  x = x + (k1*0.5+ k2 + k3 + k4*0.5)*(1.0/3.0);
354 }
355 
358 {
360  h = (tf_cont - to)/nsteps;
361  h2 = h/2.0;
362  time = to;
363 
364  for (int i = 1; i <= nsteps; i++) {
365  k1 = xdot(x) * h;
366  k2 = xdot(x+k1*0.5)*h;
367  k3 = xdot(x+k2*0.5)*h;
368  k4 = xdot(x+k3)*h;
369  x = x + (k1*0.5+ k2 + k3 + k4*0.5)*(1.0/3.0);
370  time += h;
371  }
372 }
373 
374 #ifdef use_namespace
375 }
376 #endif
377 
378 
379 
380 
381 
382 
383 
384 
385 
386 
387 
ReturnMatrix R() const
Rotation matrix from a unit quaternion.
Definition: quaternion.cpp:458
double to
Initial simulation time.
Definition: dynamics_sim.h:85
short type
Cartesian or joint space.
Definition: trajectory.h:173
Matrix x
Stated vector obtain in Runge Kutta functions.
Definition: dynamics_sim.h:92
void Release()
Definition: newmat.h:514
double get_final_time()
Definition: trajectory.h:131
double tf
Final time used in Runge_Kutta4_Real_time.
Definition: dynamics_sim.h:85
ColumnVector qpd
Desired joints velocities.
Definition: dynamics_sim.h:98
ReturnMatrix xdot(const Matrix &xin)
Obtain state derivative.
ColumnVector qpp
Joints accelerations.
Definition: dynamics_sim.h:98
int get_dof() const
Return dof.
Definition: robot.h:240
ColumnVector pd
Desired end effector cartesian position.
Definition: dynamics_sim.h:98
void Runge_Kutta4()
Runge Kutta solver.
ReturnMatrix torque_cmd(Robot_basic &robot, const ColumnVector &pdpp, const ColumnVector &pdp, const ColumnVector &pd, const ColumnVector &wdp, const ColumnVector &wd, const Quaternion &qd, const short link_pc, const Real dt)
Output torque.
Definition: controller.cpp:471
Quaternion quatd
Desired orientation express by a quaternion.
Definition: dynamics_sim.h:110
double dt
Time frame.
Definition: dynamics_sim.h:85
virtual void plot()
Virtual plot functions.
Definition: dynamics_sim.h:76
int nsteps
Numbers of iterations between.
Definition: dynamics_sim.h:82
Robots class definitions.
double tf_cont
Final time used in Runge_Kutta4.
Definition: dynamics_sim.h:85
double h2
Runge Kutta temporary variable.
Definition: dynamics_sim.h:85
ReturnMatrix torque_cmd(Robot_basic &robot, const ColumnVector &qd, const ColumnVector &qpd, const ColumnVector &qppd)
Output torque.
Definition: controller.cpp:537
Matrix k3
Runge Kutta temporary variable.
Definition: dynamics_sim.h:92
Robot_basic * robot
Pointer on Robot_basic class.
Definition: dynamics_sim.h:113
ColumnVector qppd
Desired joints accelerations.
Definition: dynamics_sim.h:98
Header file for Dynamics definitions.
Dynamics(Robot_basic *robot_)
Constructor.
static Dynamics * Instance()
A pointer to Dynamics instance. Pointer is 0 if there is no instance (logic done in Constructor)...
Trajectory class selection.
Definition: trajectory.h:164
ReturnMatrix torque_cmd(Robot_basic &robot, const ColumnVector &qd, const ColumnVector &qpd)
Output torque.
Definition: controller.cpp:632
Proportional_Derivative pd
Spl_path path
Spl_path instance.
Definition: trajectory.h:174
Matrix k2
Runge Kutta temporary variable.
Definition: dynamics_sim.h:92
#define JOINT_SPACE
Definition: trajectory.h:109
void set_dof(Robot_basic *robot_)
Set the degree of freedom.
double time
Time during simulation.
Definition: dynamics_sim.h:85
ColumnVector wpd
Desired end effector cartesian angular acceleration.
Definition: dynamics_sim.h:98
short quat_w(const Real t, Quaternion &s, ColumnVector &w)
Quaternion interpollation and angular velocity.
Definition: trajectory.cpp:575
Trajectory_Select path_select
Instance of Trajectory_Select class.
Definition: dynamics_sim.h:112
ReturnMatrix acceleration(const ColumnVector &q, const ColumnVector &qp, const ColumnVector &tau)
Joints acceleration without contact force.
Definition: dynamics.cpp:100
double h
Runge Kutta temporary variable.
Definition: dynamics_sim.h:85
Spl_Quaternion path_quat
Spl_Quaternion instance.
Definition: trajectory.h:175
int ndof
Degree of freedom.
Definition: dynamics_sim.h:82
void set_time_frame(const int nsteps)
Set the number of iterations.
Computed_torque_method ctm
void set_q(const ColumnVector &q)
Set the joint position vector.
Definition: robot.cpp:1137
Virtual base robot class.
Definition: robot.h:216
ColumnVector q
Joints positions.
Definition: dynamics_sim.h:98
Matrix xd
Statd vector derivative obtaint in xdot function.
Definition: dynamics_sim.h:92
int get_fix() const
Return fix.
Definition: robot.h:243
ColumnVector tau
Controller output torque.
Definition: dynamics_sim.h:98
The usual rectangular matrix.
Definition: newmat.h:625
Resolved_acc rra
short p_pdot_pddot(const Real time, ColumnVector &p, ColumnVector &pdot, ColumnVector &pdotdot)
Position, velocity and acceleration vector at time t.
Definition: trajectory.cpp:386
short set_trajectory(const Trajectory_Select &x)
Set the path_select variable from the Trajectory_Select reference.
#define CARTESIAN_SPACE
Definition: trajectory.h:110
void set_final_time(const double tf)
Set the file time.
#define NONE
ReturnMatrix set_robot_on_first_point_of_splines()
Set the robot on first point of trajectory.
ColumnVector qp
Joints velocities.
Definition: dynamics_sim.h:98
Matrix k4
Runge Kutta temporary variable.
Definition: dynamics_sim.h:92
bool first_pass_Kutta
First time in all Runge_Kutta4 functions.
Definition: dynamics_sim.h:81
Select controller class.
ColumnVector ppd
Desired end effector cartesian velocity.
Definition: dynamics_sim.h:98
void set_qp(const ColumnVector &qp)
Set the joint velocity vector.
Definition: robot.cpp:1176
#define RRA
GetSubMatrix SubMatrix(int fr, int lr, int fc, int lc) const
Definition: newmat.h:2146
short space_type
JOINT_SPACE or CARTESIAN_SPACE.
void reset_time()
Set the simulation time to the inital time.
int get_dof()
Return the degree of freedom.
virtual ReturnMatrix inv_kin(const Matrix &Tobj, const int mj=0)
Numerical inverse kinematics. See inv_kin(const Matrix & Tobj, const int mj, const int endlink...
Definition: invkine.cpp:83
ColumnVector wd
Desired end effector cartesian angular velocity.
Definition: dynamics_sim.h:98
ColumnVector pppd
Desired end effector cartesian acceleration.
Definition: dynamics_sim.h:98
static const char rcsid[]
RCS/CVS version.
short p_pdot(const Real time, ColumnVector &p, ColumnVector &pdot)
Position and velocity vector at time t.
Definition: trajectory.cpp:369
short type
Type of controller: PD, CTM,...
Column vector.
Definition: newmat.h:1008
Matrix k1
Runge Kutta temporary variable.
Definition: dynamics_sim.h:92
void Runge_Kutta4_Real_time()
Runge Kutta solver for real time.
Control_Select control
Instance of Control_Select class.
Definition: dynamics_sim.h:111
#define PD
int dof_fix
Degree of freedom + virtual link.
Definition: dynamics_sim.h:82
short set_controller(const Control_Select &x)
Set the control variable from the Control_Select reference.
Dynamics simulation handling class.
Definition: dynamics_sim.h:60
Real get_q(const int i) const
Definition: robot.h:235
Robot robot
Definition: demo.cpp:227
#define CTM
ColumnVector qd
Desired joints positions.
Definition: dynamics_sim.h:98
static Dynamics * instance
Static pointer on Dynamics class.
Definition: dynamics_sim.h:115


kni
Author(s): Martin Günther
autogenerated on Fri Jan 3 2020 04:01:16