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: tree.cpp
21 * Author: Scott McMillan
22 * Created: 20 March 1997
23 * Summary: GLUT-based example
24 *****************************************************************************/
25
26 #include <GL/glut.h>
27
28 #include <dmTime.h>
29 #include <dmGLMouse.hpp>
30 #include <dmGLPolarCamera_zup.hpp>
31
32 #include <dm.h> // DynaMechs typedefs, globals, etc.
33 #include <dmSystem.hpp> // DynaMechs simulation code.
34 #include <dmArticulation.hpp>
35 #include <dmLink.hpp>
36 #include <dmEnvironment.hpp>
37 #include <dmIntegRK4.hpp>
38
39 #include <dmu.h>
40
41 dmGLMouse *mouse;
42 dmGLPolarCamera_zup *camera;
43 GLfloat view_mat[4][4];
44
45 Float idt;
46 Float sim_time=0.0;
47 Float rtime=0.0;
48 bool paused_flag = true;
49
50 dmSystem *G_robot;
51 dmIntegRK4 *G_integrator;
52
53 dmTimespec tv, last_tv;
54
55 int render_rate;
56 int render_count = 0;
57 int timer_count = 0;
58 int motion_plan_rate; // fixed rate of 100Hz
59 int motion_plan_count = 0; // counter for motion planning updates
60
61 float cmd_direction = 0.0;
62 float cmd_speed = 0.0;
63
64 //======================= control algorithm vars ====================
65 int num_links = 0;
66 dmLink **robot_link;
67
68 int *num_dofs;
69 Float **desired_joint_pos;
70
71 //----------------------------------------------------------------------------
initControl(dmSystem * robot)72 void initControl(dmSystem *robot)
73 {
74 cerr << "initControl():" << endl;
75
76 dmArticulation *articulation = dynamic_cast<dmArticulation*>(robot);
77 num_links = articulation->getNumLinks();
78
79 cerr << " Num links: " << num_links << endl;
80
81 robot_link = new dmLink*[num_links];
82 num_dofs = new int[num_links];
83 desired_joint_pos = new Float*[num_links];
84
85 for (int i=0; i<num_links; i++)
86 {
87 robot_link[i] = articulation->getLink(i);
88 num_dofs[i] = robot_link[i]->getNumDOFs();
89 cerr << " " << dec << i;
90 if (robot_link[i]->getName())
91 {
92 cerr << " name = " << robot_link[i]->getName();
93 }
94 cerr << ": address = " << hex << robot_link[i]
95 << ", dofs = " << num_dofs[i];
96
97 desired_joint_pos[i] = new Float[num_dofs[i]];
98 Float joint_vel[7];
99 robot_link[i]->getState(desired_joint_pos[i], joint_vel);
100
101 if (num_dofs[i] == 1) cerr << ", pos = " << desired_joint_pos[i][0];
102
103 cerr << endl;
104 }
105
106 cout << "Search result 'tree1_30': "
107 << dmuFindObject("tree1_30", articulation) << endl;
108 cout << "Search result 'ref_mem': "
109 << dmuFindObject("ref_mem", articulation) << endl;
110 cout << "Search result 'Branch1': "
111 << dmuFindObject("Branch1", articulation) << endl;
112 cout << "Search result 'link2': "
113 << dmuFindObject("link2", articulation)
114 << endl;
115 cout << "Search result 'link3': "
116 << dmuFindObject("link3", articulation)
117 << endl;
118 }
119
120
121 //----------------------------------------------------------------------------
computeControl(float time)122 void computeControl(float time)
123 {
124 Float joint_input[1];
125 Float joint_pos[1];
126 Float joint_vel[1];
127
128 Float delta_pos = 0.7*sin(2.0*time);
129
130 for (int i=0; i<num_links; i++)
131 {
132 if (num_dofs[i] == 1)
133 {
134 robot_link[i]->getState(joint_pos, joint_vel);
135 joint_input[0] = 75.0*(desired_joint_pos[i][0] +
136 delta_pos - joint_pos[0])
137 - 2.0*joint_vel[0];
138
139 robot_link[i]->setJointInput(joint_input);
140 }
141 }
142 }
143
144
145 //----------------------------------------------------------------------------
myinit(void)146 void myinit (void)
147 {
148 GLfloat light_ambient[] = { 0.0, 0.0, 0.0, 1.0 };
149 GLfloat light_diffuse[] = { 1.0, 1.0, 1.0, 1.0 };
150 GLfloat light_specular[] = { 1.0, 1.0, 1.0, 1.0 };
151 // light_position is NOT default value
152 GLfloat light_position[] = { 1.0, 1.0, 1.0, 0.0 };
153
154 glLightfv (GL_LIGHT0, GL_AMBIENT, light_ambient);
155 glLightfv (GL_LIGHT0, GL_DIFFUSE, light_diffuse);
156 glLightfv (GL_LIGHT0, GL_SPECULAR, light_specular);
157 glLightfv (GL_LIGHT0, GL_POSITION, light_position);
158
159 glEnable (GL_LIGHTING);
160 glEnable (GL_LIGHT0);
161 glDepthFunc(GL_LESS);
162 glEnable(GL_DEPTH_TEST);
163
164 glShadeModel(GL_FLAT);
165 glEnable(GL_CULL_FACE);
166 glCullFace(GL_BACK);
167 }
168
169 //----------------------------------------------------------------------------
display(void)170 void display (void)
171 {
172 glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
173
174 glMatrixMode (GL_MODELVIEW);
175 glPushMatrix ();
176
177 // ===============================================================
178 (dmEnvironment::getEnvironment())->draw();
179
180 glPushAttrib(GL_ALL_ATTRIB_BITS);
181 G_robot->draw();
182 glPopAttrib();
183 // ===============================================================
184
185 glDisable (GL_LIGHTING);
186
187 glBegin(GL_LINES);
188 glColor3f(1.0, 0.0, 0.0);
189 glVertex3f(2.0, 0.0, 0.0);
190 glVertex3f(0.0, 0.0, 0.0);
191 glEnd();
192
193 glBegin(GL_LINES);
194 glColor3f(0.0, 1.0, 0.0);
195 glVertex3f(0.0, 2.0, 0.0);
196 glVertex3f(0.0, 0.0, 0.0);
197 glEnd();
198
199 glBegin(GL_LINES);
200 glColor3f(0.0, 0.0, 1.0);
201 glVertex3f(0.0, 0.0, 2.0);
202 glVertex3f(0.0, 0.0, 0.0);
203 glEnd();
204
205 glEnable (GL_LIGHTING);
206
207 glPopMatrix ();
208
209 glFlush ();
210 glutSwapBuffers();
211 }
212
213
214 //----------------------------------------------------------------------------
myReshape(int w,int h)215 void myReshape(int w, int h)
216 {
217 glViewport (0, 0, w, h);
218 mouse->win_size_x = w;
219 mouse->win_size_y = h;
220
221 camera->setPerspective(45.0, (GLfloat)w/(GLfloat)h, 1.0, 200.0);
222
223 camera->setViewMat(view_mat);
224 camera->applyView();
225 }
226
227
228 //----------------------------------------------------------------------------
processKeyboard(unsigned char key,int,int)229 void processKeyboard(unsigned char key, int, int)
230 {
231 switch (key)
232 {
233 case 27:
234 glutDestroyWindow(glutGetWindow());
235 exit(1);
236 break;
237
238 case 'p':
239 paused_flag = !paused_flag;
240 break;
241 }
242 }
243
244
245
246 //----------------------------------------------------------------------------
processSpecialKeys(int key,int,int)247 void processSpecialKeys(int key, int, int)
248 {
249 switch (key)
250 {
251 case GLUT_KEY_LEFT:
252 cmd_direction += 5.0;
253 if (cmd_direction > 180.0) cmd_direction -= 360.0;
254 break;
255 case GLUT_KEY_RIGHT:
256 cmd_direction -= 5.0;
257 if (cmd_direction < -180.0) cmd_direction += 360.0;
258 break;
259 case GLUT_KEY_UP:
260 cmd_speed += 0.01f;
261 if (cmd_speed > 0.25f) cmd_speed = 0.25f;
262 break;
263 case GLUT_KEY_DOWN:
264 cmd_speed -= 0.01f;
265 if (cmd_speed < 0.0) cmd_speed = 0.0;
266 break;
267 }
268 }
269
270
271 //----------------------------------------------------------------------------
updateSim()272 void updateSim()
273 {
274
275 if (!paused_flag)
276 {
277 for (int i=0; i<render_rate; i++)
278 {
279 computeControl(sim_time);
280 G_integrator->simulate(idt);
281 sim_time += idt;
282 }
283 }
284
285 camera->update(mouse);
286 camera->applyView();
287
288 display();
289
290 // compute render rate
291 timer_count++;
292 dmGetSysTime(&tv);
293 double elapsed_time = ((double) tv.tv_sec - last_tv.tv_sec) +
294 (1.0e-9*((double) tv.tv_nsec - last_tv.tv_nsec));
295
296 if (elapsed_time > 2.5)
297 {
298 rtime += elapsed_time;
299 cerr << "time/real_time: " << sim_time << '/' << rtime
300 << " frame_rate: " << (double) timer_count/elapsed_time << endl;
301
302 timer_count = 0;
303 last_tv.tv_sec = tv.tv_sec;
304 last_tv.tv_nsec = tv.tv_nsec;
305 }
306 }
307
308
309 //----------------------------------------------------------------------------
main(int argc,char ** argv)310 int main(int argc, char** argv)
311 {
312 int i, j;
313
314 glutInit(&argc, argv);
315
316 //=========================
317 char *filename = "tree.cfg";
318 if (argc > 1)
319 {
320 filename = argv[1];
321 }
322
323 glutInitWindowSize(640, 480);
324 glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);
325 glutCreateWindow("DynaMechs Example");
326
327 myinit();
328 mouse = dmGLMouse::dmInitGLMouse();
329
330 for (i=0; i<4; i++)
331 {
332 for (j=0; j<4; j++)
333 {
334 view_mat[i][j] = 0.0;
335 }
336 view_mat[i][i] = 1.0;
337 }
338 view_mat[3][2] = -10.0;
339 camera = new dmGLPolarCamera_zup();
340 camera->setRadius(30.0);
341 camera->setCOI(10.0, 10.0, 5.0);
342 camera->setTranslationScale(0.02f);
343
344 // load robot stuff
345 ifstream cfg_ptr;
346 cfg_ptr.open(filename);
347
348 // Read simulation timing information.
349 readConfigParameterLabel(cfg_ptr,"Integration_Stepsize");
350 cfg_ptr >> idt;
351 if (idt <= 0.0)
352 {
353 cerr << "main error: invalid integration stepsize: " <<idt << endl;;
354 exit(3);
355 }
356 motion_plan_rate = (int) (0.5 + 0.01/idt); // fixed rate of 100Hz
357
358 readConfigParameterLabel(cfg_ptr,"Display_Update_Rate");
359 cfg_ptr >> render_rate;
360 if (render_rate < 1) render_rate = 1;
361
362 // ===========================================================================
363 // Initialize DynaMechs environment - must occur before any linkage systems
364 char env_flname[FILENAME_SIZE];
365 readConfigParameterLabel(cfg_ptr,"Environment_Parameter_File");
366 readFilename(cfg_ptr, env_flname);
367 dmEnvironment *environment = dmuLoadFile_env(env_flname);
368 environment->drawInit();
369 dmEnvironment::setEnvironment(environment);
370
371 // ===========================================================================
372 // Initialize a DynaMechs linkage system
373 char robot_flname[FILENAME_SIZE];
374 readConfigParameterLabel(cfg_ptr,"Robot_Parameter_File");
375 readFilename(cfg_ptr, robot_flname);
376 G_robot = dmuLoadFile_dm(robot_flname);
377
378 //G_robot->initSimVars();
379 G_integrator = new dmIntegRK4();
380 G_integrator->addSystem(G_robot);
381
382 initControl(G_robot);
383
384 glutReshapeFunc(myReshape);
385 glutKeyboardFunc(processKeyboard);
386 glutSpecialFunc(processSpecialKeys);
387 glutDisplayFunc(display);
388 glutIdleFunc(updateSim);
389
390 dmGetSysTime(&last_tv);
391
392 cout << endl;
393 cout << "p - toggles dynamic simulation" << endl;
394 cout << "Use mouse to rotate/move/zoom the camera" << endl << endl;
395
396 glutMainLoop();
397 return 0; /* ANSI C requires main to return int. */
398 }
399