1 /*****************************************************************************
2 * DynaMechs: A Multibody Dynamic Simulation Library
3 *
4 * Copyright (C) 1994-2001 Scott McMillan All Rights Reserved.
5 *
6 * This library is free software; you can redistribute it and/or
7 * modify it under the terms of the GNU General Public License
8 * as published by the Free Software Foundation; either version 2
9 * of the License, or (at your option) any later version.
10 *
11 * This library is distributed in the hope that it will be useful,
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
14 * General Public License for more details.
15 *
16 * You should have received a copy of the GNU Library General Public
17 * License along with this library; if not, write to the Free
18 * Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
19 *****************************************************************************
20 * File: aquarobot.cpp
21 * Author: Scott McMillan
22 * Created: 20 March 1997
23 * Summary:
24 *****************************************************************************/
25
26 #include <dm.h> // DynaMechs typedefs, globals, etc.
27 #include <dmu.h>
28 #include <dmGL.h>
29
30 #include <GL/glut.h>
31
32 #include <dmTime.h>
33 #include <dmGLMouse.hpp>
34 #include <dmGLPolarCamera_zup.hpp>
35
36 #include <dmSystem.hpp> // DynaMechs simulation code.
37 #include <dmArticulation.hpp>
38 #include <dmLink.hpp>
39 #include <dmIntegRK4.hpp>
40 #include <dmEnvironment.hpp>
41 #include <dmMDHLink.hpp>
42 #include <dmMobileBaseLink.hpp>
43
44
45 // ---------------------- Gait algorithm stuff -------------------------
46 #include <GaitAlgorithm.h> // Kenji Suzuki's gait algorithm
47 #include <Robot.hpp>
48
49 const float MAX_SPEED = 0.25;
50
51 Robot new_robot;
52 Float desiredJointPos[6][3];
53 float motion_command[3] = {0.0, 0.0, 0.0}; // v_x, v_y, omega_z
54
55 int motion_plan_rate; // fixed rate of 100Hz
56 int motion_plan_count = 0; // counter for motion planning updates
57
58 float cmd_direction = 0.0;
59 float cmd_speed = 0.0;
60 // ----------------------------------------------------------------------
61
62 const float RADTODEG = (float)(180.0/M_PI); // M_PI is defined in math.h
63 const float DEGTORAD = (float)(M_PI/180.0);
64
65 dmGLMouse *mouse;
66 dmGLPolarCamera_zup *camera;
67 GLfloat view_mat[4][4];
68
69 Float idt;
70 Float sim_time=0.0;
71 Float rtime=0.0;
72 bool paused_flag = true;
73
74 dmArticulation *G_robot;
75 dmIntegRK4 *G_integrator;
76
77 dmTimespec tv, last_tv;
78
79 int render_rate;
80 int render_count = 0;
81 int timer_count = 0;
82
83 //-------------------- control interface stuff ---------------------
84 dmMobileBaseLink *ref_member;
85
86 int num_arts = 6;
87 int num_links[6] = {3, 3, 3, 3, 3, 3};
88 dmMDHLink *robot_link[6][3];
89 Float desired_joint_pos[6][3];
90
91 //----------------------------------------------------------------------------
initAquaControl(dmArticulation * robot)92 void initAquaControl(dmArticulation *robot)
93 {
94 int i,j;
95
96 cerr << "initAquaControl():" << endl;
97
98 ref_member = dynamic_cast<dmMobileBaseLink*>(dmuFindObject("refmember",
99 robot));
100
101 for (j=0; j<num_arts; j++)
102 {
103 for (i=0; i<num_links[j]; i++)
104 {
105 char label[8];
106 sprintf(label, "%d%d", j, i);
107 robot_link[j][i] = dynamic_cast<dmMDHLink*>(dmuFindObject(label,
108 robot));
109 }
110 }
111 }
112
113
114 /*****************************************************************************
115 * Function : motor_control *
116 * Purpose : Compute the motor voltages from PD servo loops. *
117 * Inputs : Desired and actual joint positions, and actual joint velocities*
118 * Outputs : Motor voltages (jointInput). *
119 *****************************************************************************/
120
121 #define DAMPING_CONSTANT 0.0
122 #define SPRING_CONSTANT 1500.0
123 #define MAX_VOLTAGE 75.0
124
computeAquaControl(Float jointPosDesired[6][3])125 void computeAquaControl(Float jointPosDesired[6][3])
126 {
127 register int i, k;
128 Float joint_input[1];
129 Float joint_pos[1];
130 Float joint_vel[1];
131
132 // leg joint controllers
133 for (k=0; k<6; k++)
134 {
135 for (i=0; i<3; i++)
136 {
137 robot_link[k][i]->getState(joint_pos, joint_vel);
138
139 joint_input[0] = -DAMPING_CONSTANT*joint_vel[0] -
140 SPRING_CONSTANT*(joint_pos[0] - jointPosDesired[k][i]);
141
142 //cerr << jointPosDesired[k][i] << ' ';
143
144 if (joint_input[0] > MAX_VOLTAGE)
145 joint_input[0] = MAX_VOLTAGE;
146 else if (joint_input[0] < -MAX_VOLTAGE)
147 joint_input[0] = -MAX_VOLTAGE;
148
149 robot_link[k][i]->setJointInput(joint_input);
150 }
151 //cerr << endl;
152 }
153 //cerr << endl;
154 }
155
156
157 //----------------------------------------------------------------------------
myinit(void)158 void myinit (void)
159 {
160 GLfloat light_ambient[] = { 0.0, 0.0, 0.0, 1.0 };
161 GLfloat light_diffuse[] = { 1.0, 1.0, 1.0, 1.0 };
162 GLfloat light_specular[] = { 1.0, 1.0, 1.0, 1.0 };
163 // light_position is NOT default value
164 GLfloat light_position[] = { 1.0, 1.0, 1.0, 0.0 };
165
166 glLightfv (GL_LIGHT0, GL_AMBIENT, light_ambient);
167 glLightfv (GL_LIGHT0, GL_DIFFUSE, light_diffuse);
168 glLightfv (GL_LIGHT0, GL_SPECULAR, light_specular);
169 glLightfv (GL_LIGHT0, GL_POSITION, light_position);
170
171 glEnable (GL_LIGHTING);
172 glEnable (GL_LIGHT0);
173 glDepthFunc(GL_LESS);
174 glEnable(GL_DEPTH_TEST);
175
176 glShadeModel(GL_FLAT);
177 glEnable(GL_CULL_FACE);
178 glCullFace(GL_BACK);
179 }
180
181 //----------------------------------------------------------------------------
182 Float q[7];
183 SpatialVector dummy;
184
display(void)185 void display (void)
186 {
187 glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
188
189 glMatrixMode (GL_MODELVIEW);
190 glPushMatrix ();
191
192 // ===============================================================
193 (dmEnvironment::getEnvironment())->draw();
194
195 glPushAttrib(GL_ALL_ATTRIB_BITS);
196 G_robot->draw();
197 glPopAttrib();
198 // ===============================================================
199
200 glDisable (GL_LIGHTING);
201
202 glBegin(GL_LINES);
203 glColor3f(1.0, 0.0, 0.0);
204 glVertex3f(2.0, 0.0, 0.0);
205 glVertex3f(0.0, 0.0, 0.0);
206 glEnd();
207
208 glBegin(GL_LINES);
209 glColor3f(0.0, 1.0, 0.0);
210 glVertex3f(0.0, 2.0, 0.0);
211 glVertex3f(0.0, 0.0, 0.0);
212 glEnd();
213
214 glBegin(GL_LINES);
215 glColor3f(0.0, 0.0, 1.0);
216 glVertex3f(0.0, 0.0, 2.0);
217 glVertex3f(0.0, 0.0, 0.0);
218 glEnd();
219
220 // output a speed/direction command vector
221 ref_member->getState(q, dummy);
222
223 glPushMatrix();
224 {
225 glTranslatef(q[4], q[5], q[6]);
226
227 float len = sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2]);
228 if (len > 1.0e-6)
229 {
230 float angle = 2.0*atan2(len, q[3]);
231 glRotatef(angle*RADTODEG, q[0]/len, q[1]/len, q[2]/len);
232 }
233
234 glBegin(GL_LINES);
235 glVertex3f(0.0, 0.0, 0.8f);
236 glVertex3f(cos(cmd_direction/RADTODEG)*cmd_speed*3.0,
237 sin(cmd_direction/RADTODEG)*cmd_speed*3.0,
238 0.8f);
239 glEnd();
240 }
241 glPopMatrix();
242
243 glEnable (GL_LIGHTING);
244
245 glPopMatrix ();
246
247 glFlush ();
248 glutSwapBuffers();
249 }
250
251 //----------------------------------------------------------------------------
252 // Summary:
253 // Parameters:
254 // Returns:
255 //----------------------------------------------------------------------------
myReshape(int w,int h)256 void myReshape(int w, int h)
257 {
258 glViewport (0, 0, w, h);
259 mouse->win_size_x = w;
260 mouse->win_size_y = h;
261
262 camera->setPerspective(45.0, (GLfloat)w/(GLfloat)h, 1.0, 200.0);
263
264 camera->setViewMat(view_mat);
265 camera->applyView();
266 }
267
268 //----------------------------------------------------------------------------
269 // Summary:
270 // Parameters:
271 // Returns:
272 //----------------------------------------------------------------------------
processKeyboard(unsigned char key,int,int)273 void processKeyboard(unsigned char key, int, int)
274 {
275 switch (key)
276 {
277 case 27:
278 glutDestroyWindow(glutGetWindow());
279 exit(1);
280 break;
281
282 case 'p':
283 paused_flag = !paused_flag;
284 break;
285 }
286 }
287
288
289 //----------------------------------------------------------------------------
290 // Summary:
291 // Parameters:
292 // Returns:
293 //----------------------------------------------------------------------------
processSpecialKeys(int key,int,int)294 void processSpecialKeys(int key, int, int)
295 {
296 switch (key)
297 {
298 case GLUT_KEY_LEFT:
299 cmd_direction += 5.0;
300 if (cmd_direction > 180.0) cmd_direction -= 360.0;
301 break;
302 case GLUT_KEY_RIGHT:
303 cmd_direction -= 5.0;
304 if (cmd_direction < -180.0) cmd_direction += 360.0;
305 break;
306 case GLUT_KEY_UP:
307 cmd_speed += 0.01f;
308 if (cmd_speed > MAX_SPEED) cmd_speed = MAX_SPEED;
309 break;
310 case GLUT_KEY_DOWN:
311 cmd_speed -= 0.01f;
312 if (cmd_speed < 0.0) cmd_speed = 0.0;
313 break;
314 }
315 }
316
317
318 //===========================================================================
319 // FUNCTION: void interface_Gait2DynaMechs(Robot &robot)
320 // PURPOSE: interface between "triple" array(for Dyanamics)
321 // and "Robot" class(for gait planning).
322 // AUTHOR: Kenji Suzuki.
323 // DATE: October 18, 1993
324 // COMMENT:
325 //===========================================================================
326
interface_Gait2DynaMechs(Robot & robot,EulerAngles refPose,CartesianVector refPos,Float jointPos[6][3])327 void interface_Gait2DynaMechs(Robot &robot,
328 EulerAngles refPose, CartesianVector refPos,
329 Float jointPos[6][3])
330 {
331 refPose[2] = robot.body.W_body_pos.azimuth;
332 refPose[1] = robot.body.W_body_pos.elevation;
333 refPose[0] = robot.body.W_body_pos.roll;
334
335 refPos[0] = robot.body.W_body_pos.x/100.0;
336 refPos[1] = robot.body.W_body_pos.y/100.0;
337 refPos[2] = robot.body.W_body_pos.z/100.0;
338
339 // LEG1
340 jointPos[0][0] = robot.leg1.link1.joint_angle_pos;
341 jointPos[0][1] = robot.leg1.link2.joint_angle_pos;
342 jointPos[0][2] = robot.leg1.link3.joint_angle_pos;
343 // jointPos[0][3][2] = -M_PI/2.0 - jointPos[0][1][0] - jointPos[0][2][0];
344
345 // LEG2
346 jointPos[1][0] = robot.leg2.link1.joint_angle_pos;
347 jointPos[1][1] = robot.leg2.link2.joint_angle_pos;
348 jointPos[1][2] = robot.leg2.link3.joint_angle_pos;
349 // jointPos[1][3][2] = -M_PI/2.0 - jointPos[1][1][0] - jointPos[1][2][0];
350
351 // LEG3
352 jointPos[2][0] = robot.leg3.link1.joint_angle_pos;
353 jointPos[2][1] = robot.leg3.link2.joint_angle_pos;
354 jointPos[2][2] = robot.leg3.link3.joint_angle_pos;
355 // jointPos[2][3][2] = -M_PI/2.0 - jointPos[2][1][0] - jointPos[2][2][0];
356
357 // LEG4
358 jointPos[3][0] = robot.leg4.link1.joint_angle_pos;
359 jointPos[3][1] = robot.leg4.link2.joint_angle_pos;
360 jointPos[3][2] = robot.leg4.link3.joint_angle_pos;
361 // jointPos[3][3][2] = -M_PI/2.0 - jointPos[3][1][0] - jointPos[3][2][0];
362
363 // LEG5
364 jointPos[4][0] = robot.leg5.link1.joint_angle_pos;
365 jointPos[4][1] = robot.leg5.link2.joint_angle_pos;
366 jointPos[4][2] = robot.leg5.link3.joint_angle_pos;
367 // jointPos[4][3][2] = -M_PI/2.0 - jointPos[4][1][0] - jointPos[4][2][0];
368
369 // LEG6
370 jointPos[5][0] = robot.leg6.link1.joint_angle_pos;
371 jointPos[5][1] = robot.leg6.link2.joint_angle_pos;
372 jointPos[5][2] = robot.leg6.link3.joint_angle_pos;
373 // jointPos[5][3][2] = -M_PI/2.0 - jointPos[5][1][0] - jointPos[5][2][0];
374 }
375
376 //----------------------------------------------------------------------------
updateSim()377 void updateSim()
378 {
379 if (!paused_flag)
380 {
381 for (int i=0; i<render_rate; i++)
382 {
383 // Kenji Suzuki/Kan Yoneda's motion planning algorithm
384 // for dynamic sim.
385 // Can run more often than rendering and less often than
386 // the dynamic simulation.
387 if (motion_plan_count%motion_plan_rate == 0)
388 {
389 // Note motion_planning requires cm/sec velocity commands
390 // commands are run through a simple low pass filter.
391 motion_command[0] = cos(cmd_direction*DEGTORAD)*cmd_speed*100.0;
392 motion_command[1] = sin(cmd_direction*DEGTORAD)*cmd_speed*100.0;
393 motion_command[2] = 0.0;
394
395 // Gait algorithm
396 gait_algorithm(sim_time, idt*motion_plan_rate,
397 new_robot, motion_command);
398
399 // Interface between gait algorithm and DynaMechs. This
400 // returns desiredJointPos with the next set of desired
401 // joint positions.
402 static CartesianVector refPos;
403 static EulerAngles refPose;
404 interface_Gait2DynaMechs(new_robot, refPose, refPos,
405 desiredJointPos);
406
407 motion_plan_count = 0;
408 }
409
410 computeAquaControl(desiredJointPos);
411
412 G_integrator->simulate(idt);
413
414 sim_time += idt;
415 motion_plan_count++;
416 }
417 }
418
419 camera->update(mouse);
420 camera->applyView();
421
422 display();
423
424 // compute render rate
425 timer_count++;
426 dmGetSysTime(&tv);
427 double elapsed_time = ((double) tv.tv_sec - last_tv.tv_sec) +
428 (1.0e-9*((double) tv.tv_nsec - last_tv.tv_nsec));
429
430 if (elapsed_time > 2.5)
431 {
432 rtime += elapsed_time;
433 cerr << "time/real_time: " << sim_time << '/' << rtime
434 << " frame_rate: " << (double) timer_count/elapsed_time << endl;
435
436 timer_count = 0;
437 last_tv.tv_sec = tv.tv_sec;
438 last_tv.tv_nsec = tv.tv_nsec;
439 }
440 }
441
442 //----------------------------------------------------------------------------
443 // Summary:
444 // Parameters:
445 // Returns:
446 //----------------------------------------------------------------------------
main(int argc,char ** argv)447 int main(int argc, char** argv)
448 {
449 int i, j;
450
451 glutInit(&argc, argv);
452
453 //=========================
454 char *filename = "simulation.cfg";
455 if (argc > 1)
456 {
457 filename = argv[1];
458 }
459
460 glutInitWindowSize(640,480);
461 glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);
462 glutCreateWindow("DynaMechs Example");
463
464 myinit();
465 mouse = dmGLMouse::dmInitGLMouse();
466
467 for (i=0; i<4; i++)
468 {
469 for (j=0; j<4; j++)
470 {
471 view_mat[i][j] = 0.0;
472 }
473 view_mat[i][i] = 1.0;
474 }
475 //view_mat[3][2] = -10.0;
476 camera = new dmGLPolarCamera_zup();
477 camera->setRadius(8.0);
478 camera->setCOI(8.0, 10.0, 2.0);
479 camera->setTranslationScale(0.02f);
480
481 // load robot stuff
482 ifstream cfg_ptr;
483 cfg_ptr.open(filename);
484
485 // Read simulation timing information.
486 readConfigParameterLabel(cfg_ptr,"Integration_Stepsize");
487 cfg_ptr >> idt;
488 if (idt <= 0.0)
489 {
490 cerr << "main error: invalid integration stepsize: " << idt << endl;
491 exit(3);
492 }
493 motion_plan_rate = (int) (0.5 + 0.01/idt); // fixed rate of 100Hz
494
495 readConfigParameterLabel(cfg_ptr,"Display_Update_Rate");
496 cfg_ptr >> render_rate;
497 if (render_rate < 1) render_rate = 1;
498
499 // ===========================================================================
500 // Initialize DynaMechs environment - must occur before any linkage systems
501 char env_flname[FILENAME_SIZE];
502 readConfigParameterLabel(cfg_ptr,"Environment_Parameter_File");
503 readFilename(cfg_ptr, env_flname);
504 dmEnvironment *environment = dmuLoadFile_env(env_flname);
505 environment->drawInit();
506 dmEnvironment::setEnvironment(environment);
507
508 // ===========================================================================
509 // Initialize a DynaMechs linkage system
510 char robot_flname[FILENAME_SIZE];
511 readConfigParameterLabel(cfg_ptr,"Robot_Parameter_File");
512 readFilename(cfg_ptr, robot_flname);
513 G_robot = dynamic_cast<dmArticulation*>(dmuLoadFile_dm(robot_flname));
514
515 G_integrator = new dmIntegRK4();
516 G_integrator->addSystem(G_robot);
517
518 initAquaControl(G_robot);
519
520 glutReshapeFunc(myReshape);
521 glutKeyboardFunc(processKeyboard);
522 glutSpecialFunc(processSpecialKeys);
523 glutDisplayFunc(display);
524 glutIdleFunc(updateSim);
525
526 dmGetSysTime(&last_tv);
527
528 cout << endl;
529 cout << " p - toggles dynamic simulation" << endl;
530 cout << " UP/DOWN ARROW - increases/decreases velocity command" << endl;
531 cout << "RIGHT/LEFT ARROW - changes heading command" << endl << endl;
532
533 glutMainLoop();
534 return 0; /* ANSI C requires main to return int. */
535 }
536