root/OpenSceneGraph/trunk/examples/osgrobot/osgrobot.cpp @ 13347

Revision 12292, 17.6 kB (checked in by robert, 4 years ago)

Ran svn propset -R svn:eol-style native . on the OpenSceneGraph

  • Property svn:eol-style set to native
Line 
1/* OpenSceneGraph example, osgrobot.cpp.
2*
3*  Permission is hereby granted, free of charge, to any person obtaining a copy
4*  of this software and associated documentation files (the "Software"), to deal
5*  in the Software without restriction, including without limitation the rights
6*  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
7*  copies of the Software, and to permit persons to whom the Software is
8*  furnished to do so, subject to the following conditions:
9*
10*  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
11*  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
12*  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
13*  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
14*  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
15*  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
16*  THE SOFTWARE.
17*/
18
19/*
20    The code below is to show how a heirarchy of objects can be made within a scenegraph.
21    In other words, how there can be a parent/child relationship between objects such
22    that when a parent is rotated or translated, the children move is respect to it's
23    parent movement.  A robotic arm is used in this example because this is what I'm
24    using OSG for.
25 
26    To rotate the joints use the following table to rotate each respective joint.
27 
28    joint    key        rotation
29    1        q        + 1 degree
30    1        a        - 1 degree
31    2        w        + 1 degree
32    2        s        - 1 degree
33    3        e        + 1 degree
34    3        d        - 1 degree
35    4        r        + 1 degree
36    4        f        - 1 degree
37    5        t        + 1 degree
38    5        g        - 1 degree
39    6        y        + 1 degree
40    6        h        - 1 degree
41 
42    In many robotics projects, the x and z axis are only used to define the positions
43    of the joints of a robotic arm.  By not using the y-axis, the mathematics of
44    inverse kinematics becomes easier (and in some cases possible).  All joint rotations
45    are rotated about the z-axis.  If you toggle the variable 'showAxis' to true, an
46    x (color red), y (color green), and z (color blue) axis will display.  Using the
47    thumb of the right hand to point in the same direction of the z-axis, use the right
48    hand rule to define a positive angle of rotation.
49 
50    The robotic arm in general follows the algorithm
51    rotate about the x axis, translate along the x-axis, translate along the z axis,
52    and then rotate about the z-axis. There is 1 exception in the method buildJoint3(...)
53    where it was necessary to add a 'cheat' to get the joints to align properly.  In a
54    real robotics project such as this, the tube (see buildTube2(...) method) would not
55    be a part of the mathematics and thus, not have to be drawn and would not require
56    the cheat.  To learn moue about positioning joints using only the x and z axis,
57    web search 'Denavit-Hartenberg'.  wikipedia has comments at the URL...
58    http://en.wikipedia.org/wiki/Robotics_conventions#Denavit-Hartenberg_link_frame_convention_.28DH.29
59 
60    Lessons learned with osg...
61    When starting this work I was very new to OSG.  When I first placed a cylinder
62    in a scene and rotated the cylinder about the z and x axis, I noticed that the rotation
63    is about the barycenter of the object for both axis.  I wanted a rotation about the z-axis
64    to be along the length of the tube (which OSG gives for free -- not always true with
65    all scenegraphs) while the rotation about the x and y axis about the
66    beginning (or start position) of the tube.  Knowing what I know about barycenters
67    I positioned the shape along the -height/2 (see example 1 below)
68    -- believing the offset -height/2 would place the axis of rotation at the start of
69    the tube.  I was certainly wrong about this and found out the equation needs to
70    be height/2.
71 
72    ...
73    example 1
74    joint->addDrawable(new osg::ShapeDrawable(new osg::Capsule(osg::Vec3(0.0f,0.0f,height/2),radius,height),hints));
75    ...
76   
77 
78 
79    author: James Moliere
80    date written: 10/12/2008
81*/
82
83#include <osg/Geode>
84#include <osg/ShapeDrawable>
85#include <osg/Material>
86#include <osg/Texture2D>
87
88#include <osgViewer/Viewer>
89#include <iostream>
90#include <osgDB/ReadFile>
91#include <osg/Math>
92#include <osg/Matrixd>
93#include <osg/MatrixTransform>
94#include <osg/Vec3d>
95#include <iostream>
96#include <osg/PositionAttitudeTransform>
97
98
99
100float jointAngle1;
101float jointAngle2;
102float jointAngle3;
103float jointAngle4;
104float jointAngle5;
105float jointAngle6;
106float EndEffector;
107
108void createAxis(osg::Transform* previousJoint);
109
110osg::MatrixTransform* joint1;
111osg::MatrixTransform* joint2;
112osg::MatrixTransform* joint3;
113osg::MatrixTransform* joint4;
114osg::MatrixTransform* joint5;
115osg::MatrixTransform* joint6;
116
117osg::MatrixTransform* buildJoint1(osg::MatrixTransform* previousJoint);
118osg::MatrixTransform* buildJoint2(osg::MatrixTransform* previousJoint);
119osg::MatrixTransform* buildTube2(osg::MatrixTransform* previousJoint);
120osg::MatrixTransform* buildJoint3(osg::MatrixTransform* previousJoint);
121osg::MatrixTransform* buildJoint4(osg::MatrixTransform* previousJoint);
122osg::MatrixTransform* buildTube5(osg::MatrixTransform* previousJoint);
123osg::MatrixTransform* buildJoint5(osg::MatrixTransform* previousJoint);
124osg::MatrixTransform* buildJoint6(osg::MatrixTransform* previousJoint);
125osg::MatrixTransform* buildEndEffector();
126osg::TessellationHints* hints = new osg::TessellationHints;
127
128bool showAxis;
129
130class KeyboardEventHandler : public osgGA::GUIEventHandler
131{
132public:
133
134    KeyboardEventHandler()
135    {}
136
137
138    static void rotate(float angle,osg::MatrixTransform *joint)
139    {
140        osg::Matrix zRot;
141        zRot.makeRotate(angle, 0.0,0.0,1.0);
142        joint->setMatrix(zRot*joint->getMatrix());
143    }
144
145    virtual bool handle(const osgGA::GUIEventAdapter& ea,osgGA::GUIActionAdapter&)
146    {
147        switch (ea.getEventType())
148        {
149        case(osgGA::GUIEventAdapter::KEYDOWN):
150            {
151                switch (ea.getKey())
152                {
153                case 'q':
154                    rotate(osg::PI/180, joint1);
155                    return true;
156                case 'a':
157                    rotate(-osg::PI/180, joint1);
158                    return true;
159                case 'w':
160                    rotate(osg::PI/180, joint2);
161                    return true;
162                case 's':
163                    rotate(-osg::PI/180, joint2);
164                    return true;
165                case 'e':
166                    rotate(osg::PI/180, joint3);
167                    return true;
168                case 'd':
169                    rotate(-osg::PI/180, joint3);
170                    return true;
171                case 'r':
172                    rotate(osg::PI/180, joint4);
173                    return true;
174                case 'f':
175                    rotate(-osg::PI/180, joint4);
176                    return true;
177                case 't':
178                    rotate(osg::PI/180, joint5);
179                    return true;
180                case 'g':
181                    rotate(-osg::PI/180, joint5);
182                    return true;
183                case 'y':
184                    rotate(osg::PI/180, joint6);
185                    return true;
186                case 'h':
187                    rotate(-osg::PI/180, joint6);
188                    return true;
189                }
190            }
191        default:
192            break;
193
194        }
195        //return false to allow mouse manipulation
196        return false;
197    }
198};
199
200
201osg::Group* createShapes()
202{
203    osg::Group* group = new osg::Group();
204    osg::MatrixTransform* transform =new osg::MatrixTransform();
205    group->addChild(transform);
206
207    joint1 = buildJoint1(transform);
208    joint2 = buildJoint2(joint1);
209    osg::MatrixTransform* tube2= buildTube2(joint2);
210    joint3 = buildJoint3(tube2);
211    joint4 = buildJoint4(joint3);
212    osg::MatrixTransform *tube5 = buildTube5(joint4);
213    joint5 = buildJoint5(tube5);
214    joint6= buildJoint6(joint5);
215    joint6->addChild( buildEndEffector());
216    return group;
217}
218
219osg::MatrixTransform* buildJoint1(osg::MatrixTransform* previousJoint)
220{
221    osg::MatrixTransform* xTransform =new osg::MatrixTransform();
222    previousJoint->addChild(xTransform);
223    float radius = 6.7640f;
224    float height = 45.0f;
225    osg::Geode* joint = new osg::Geode();
226    xTransform->addChild(joint);
227    joint->addDrawable(new osg::ShapeDrawable(new osg::Cylinder(osg::Vec3(0.0f,0.0f,height/2),radius,height),hints));
228
229    osg::MatrixTransform *zTransform = new osg::MatrixTransform();
230    xTransform->addChild(zTransform);
231    osg::Matrix zTrans=osg::Matrix::translate(0.0, 0.0, height);
232    osg::Matrix zRot= osg::Matrix::rotate(jointAngle1, 0.0, 0.0, 1.0);
233    zTransform->setMatrix(zTrans*zRot);
234    return zTransform;
235}
236
237osg::MatrixTransform* buildJoint2(osg::MatrixTransform* previousJoint)
238{
239    if (showAxis)
240    {
241        createAxis(previousJoint);
242    }
243    double height = 17.6;
244    double radius = 4.45633;
245    osg::MatrixTransform* xTransform = new osg::MatrixTransform();
246    previousJoint->addChild(xTransform);
247    osg::Matrix xRot = osg::Matrix::rotate(osg::PI_2, 1.0, 0.0, 0.0);
248    xTransform->setMatrix(xRot);
249    osg::Geode* joint = new osg::Geode();
250    joint->addDrawable(new osg::ShapeDrawable(new osg::Capsule(osg::Vec3(0.0f,0.0f,height/2),radius,height),hints));
251    xTransform->addChild(joint);
252
253    osg::MatrixTransform* zTransform = new osg::MatrixTransform();
254    osg::Matrix zTrans = osg::Matrix::translate( 0.0, 0.0, height);
255    osg::Matrix zRot = osg::Matrix::rotate(osg::PI_2+jointAngle2, 0.0,0.0,1.0);
256    zTransform->setMatrix(zTrans*zRot);
257    xTransform->addChild(zTransform);
258    return zTransform;
259}
260
261osg::MatrixTransform* buildTube2(osg::MatrixTransform* previousJoint)
262{
263    if (showAxis)
264    {
265        createAxis(previousJoint);
266    }
267    double height = 17.6;
268    double radius = 4.45633;
269    osg::MatrixTransform* xTransform = new osg::MatrixTransform();
270    previousJoint->addChild(xTransform);
271    osg::Matrix xRot = osg::Matrix::rotate(osg::PI_2, 1.0,0.0,0.0);
272    xTransform->setMatrix(xRot);
273    osg::Geode* tube3 = new osg::Geode();
274    xTransform->addChild(tube3);
275    tube3->addDrawable(new osg::ShapeDrawable(new osg::Capsule(osg::Vec3(0.0f,0.0f,height/2),radius,height),hints));
276
277    osg::MatrixTransform* zTransform = new osg::MatrixTransform();
278    xTransform->addChild(zTransform);
279    osg::Matrix zTrans = osg::Matrix::translate(0,0,height);
280    zTransform->setMatrix(zTrans);
281    return zTransform;
282}
283
284osg::MatrixTransform* buildJoint3(osg::MatrixTransform* previousJoint)
285{
286    double height =7.5;
287    double radius = 4.45633;
288    osg::Geode* joint = new osg::Geode();
289    osg::MatrixTransform* xTransform = new osg::MatrixTransform();
290    previousJoint->addChild(xTransform);
291    osg::Matrix xRot = osg::Matrix::rotate(-osg::PI_2, 1.0, 0.0, 0.0);
292    xTransform->setMatrix(xRot);
293
294
295    osg::MatrixTransform* zCheat = new osg::MatrixTransform();
296    osg::Matrix zTransCheat = osg::Matrix::translate(0.0,0.0,-height);
297    zCheat->setMatrix(zTransCheat);
298    xTransform->addChild(zCheat);
299   
300    osg::ShapeDrawable *shape = new osg::ShapeDrawable(new osg::Capsule(osg::Vec3(0.0f,0.0f,height/2),radius,height),hints);
301    joint->addDrawable(shape);
302    zCheat->addChild(joint);
303
304    osg::MatrixTransform* zTransform = new osg::MatrixTransform();
305    zCheat->addChild(zTransform);
306    osg::Matrix zRot = osg::Matrix::rotate((float)jointAngle3, 0.0f, 0.0f, 1.0f);
307    osg::Matrix zTrans = osg::Matrix::translate(0,0,0);
308    zTransform->setMatrix(zTrans*zRot);
309    if (showAxis)
310    {
311        createAxis(zTransform);
312    }
313    return zTransform;
314}
315osg::MatrixTransform* buildJoint4(osg::MatrixTransform* previousJoint)
316{
317    double height = 17.5;
318    double radius = 2.86479;
319    osg::Geode* tube4 = new osg::Geode();
320    tube4->addDrawable(new osg::ShapeDrawable(new osg::Capsule(osg::Vec3(0.0f,0.0f,height/2),radius,height),hints));
321    osg::MatrixTransform* xTransform = new osg::MatrixTransform();
322    //if (showAxis)
323    //{
324        //createAxis(xTransform);
325    //}
326    previousJoint->addChild(xTransform);
327    xTransform->addChild(tube4);
328    osg::Matrix xRot= osg::Matrix::rotate(osg::PI_2, 1,0,0);
329    xTransform->setMatrix(xRot);
330    height = 7.5;
331    osg::Matrix zTrans = osg::Matrix::translate(0,0,17.5);
332    osg::Matrix zRot = osg::Matrix::rotate(jointAngle4-osg::PI_2, 0,0,1);
333
334    osg::MatrixTransform* zTransform = new osg::MatrixTransform();
335    zTransform->setMatrix(zTrans*zRot);
336    xTransform->addChild(zTransform);
337    return zTransform;
338}
339
340
341osg::MatrixTransform* buildTube5(osg::MatrixTransform* previousJoint)
342{
343    if (showAxis)
344    {
345        createAxis(previousJoint);
346    }
347    double height = 7.5;
348    double radius = 2.86479;
349
350    height = 15.0;
351    osg::Geode* joint = new osg::Geode();
352    osg::ShapeDrawable *shape =new osg::ShapeDrawable(new osg::Cylinder(osg::Vec3(0.0f,0.0f,height/2),radius,height),hints);
353    joint->addDrawable(shape);
354    osg::MatrixTransform* xTransform = new osg::MatrixTransform();
355    previousJoint->addChild(xTransform);
356    xTransform->addChild(joint);
357
358    osg::MatrixTransform* zTransform = new osg::MatrixTransform();
359    osg::Matrix zTrans = osg::Matrix::translate(0,0,height);
360    zTransform->setMatrix(zTrans);
361    xTransform->addChild(zTransform);
362    return zTransform;
363}
364
365osg::MatrixTransform* buildJoint5(osg::MatrixTransform* previousJoint)
366{
367    double radius = 2.86479;
368    double height = 2.86479*2;
369    osg::Geode* joint = new osg::Geode();
370    osg::ShapeDrawable *shape = new osg::ShapeDrawable(new osg::Cylinder(osg::Vec3(0.0f,0.0f,0.0f),radius,height),hints);
371    joint->addDrawable(shape);
372    osg::MatrixTransform* xTransform = new osg::MatrixTransform();
373    previousJoint->addChild(xTransform);
374    osg::Matrix xRot= osg::Matrix::rotate(-osg::PI_2, 1.0,0,0);
375    xTransform->setMatrix(xRot);
376    xTransform->addChild(joint);
377    if (showAxis)
378    {
379        createAxis(xTransform);
380    }
381
382    osg::MatrixTransform* zTransform = new osg::MatrixTransform();
383    osg::Matrix zRot= osg::Matrix::rotate(jointAngle5, 0,0,1);
384    osg::Matrix zTrans = osg::Matrix::translate(0,0,0);
385    zTransform->setMatrix(zTrans*zRot);
386    xTransform->addChild(zTransform);
387    return zTransform;
388}
389
390osg::MatrixTransform* buildJoint6(osg::MatrixTransform* previousJoint)
391{
392    double height = 3.0;
393    double radius = 1.0;
394    osg::Geode* joint = new osg::Geode();
395    joint->addDrawable(new osg::ShapeDrawable(new osg::Cylinder(osg::Vec3(0.0f,0.0f,height/2),radius,height),hints));
396    osg::MatrixTransform* xTransform = new osg::MatrixTransform();
397    osg::Matrix xRot= osg::Matrix::rotate(osg::PI_2, 1.0, 0.0, 0.0);
398    xTransform->setMatrix(xRot);
399    xTransform->addChild(joint);
400    previousJoint->addChild(xTransform);
401    if (showAxis)
402    {
403        createAxis(xTransform);
404    }
405    return xTransform;
406}
407
408osg::MatrixTransform* buildEndEffector()
409{
410    osg::MatrixTransform* mt = new osg::MatrixTransform();
411    osg::Matrix m;
412    double length = 17.0;
413    m.makeTranslate(0,0,length/2);
414    mt->setMatrix(m);
415    osg::Geode *geode_3 = new osg::Geode;
416    osg::ShapeDrawable *shape1 = new osg::ShapeDrawable(new osg::Box(osg::Vec3(-EndEffector, 0.0f, 0.0f), .5, 1.5f, length), hints);
417    osg::ShapeDrawable *shape2 = new osg::ShapeDrawable(new osg::Box(osg::Vec3( EndEffector, 0.0f, 0.0f), .5, 1.5f, length), hints);
418    shape1->setColor(osg::Vec4(0.8f, 0.8f, 0.4f, 1.0f));
419    shape2->setColor(osg::Vec4(0.8f, 0.8f, 0.4f, 1.0f));
420    geode_3->addDrawable(shape1);
421    geode_3->addDrawable(shape2);
422    mt->addChild(geode_3);
423    return mt;
424}
425
426void createAxis(osg::Transform* previousJoint)
427{
428    double height = 12.0;
429    double radius = .5;
430
431    osg::MatrixTransform* zmt = new osg::MatrixTransform();
432
433    previousJoint->addChild(zmt);
434    osg::ShapeDrawable *zShape = new osg::ShapeDrawable(new osg::Cylinder(osg::Vec3(0.0f,0.0f,height/2),radius,height),hints);
435    osg::ShapeDrawable *zCone = new osg::ShapeDrawable(new osg::Cone(osg::Vec3(0.0f,0.0f,1.0),radius+1.0,2.0),hints);
436
437    osg::MatrixTransform* zmtCone = new osg::MatrixTransform();
438    osg::Geode *zgCone = new osg::Geode;
439
440    zmtCone->setMatrix( osg::Matrix::translate(0.0,0.0,height));
441    previousJoint->addChild(zmtCone);
442
443    zShape->setColor(osg::Vec4(0.0f, 0.0f, 1.0f, 1.0f));
444    zCone->setColor(osg::Vec4(0.0f, 0.0f, 1.0f, 1.0f));
445    osg::Geode *z = new osg::Geode;
446    z->addDrawable(zShape);
447    zgCone->addDrawable(zCone);
448    zmtCone->addChild(zgCone);
449    zmt->addChild(z);
450
451    osg::MatrixTransform* mt = new osg::MatrixTransform();
452    previousJoint->addChild(mt);
453
454    osg::Matrix xMatrix = osg::Matrix::rotate(-osg::PI_2, 0.0, 1.0, 0.0);
455    mt->setMatrix(xMatrix);
456
457
458    osg::ShapeDrawable *xShape = new osg::ShapeDrawable(new osg::Cylinder(osg::Vec3(0.0f,0.0f,height/2),radius,height),hints);
459    xShape->setColor(osg::Vec4(1.0f, 0.0f, 0.0f, 1.0f));
460    osg::Geode *x = new osg::Geode;
461    x->addDrawable(xShape);
462    mt->addChild(x);
463
464
465    osg::MatrixTransform *yMt = new osg::MatrixTransform();
466    previousJoint->addChild(yMt);
467    osg::Matrix yMatrix = osg::Matrix::rotate(osg::PI_2, 1.0, 0.0, 0.0);
468    yMt->setMatrix(yMatrix);
469
470    osg::ShapeDrawable *yShape = new osg::ShapeDrawable(new osg::Cylinder(osg::Vec3(0.0f,0.0f,height/2),radius,height),hints);
471    yShape->setColor(osg::Vec4(0.0f, 1.0f, 0.0f, 1.0f));
472    osg::Geode *y = new osg::Geode;
473    y->addDrawable(yShape);
474    yMt->addChild(y);
475}
476
477int main(int, char **)
478{
479    hints->setDetailRatio(0.5f);
480    showAxis = true;
481    jointAngle1=0.0;
482    jointAngle2=0.0;
483    jointAngle3=0.0;
484    jointAngle4=0.0;
485    jointAngle5=0.0;
486    jointAngle6=0.0;
487    EndEffector=1.0;
488    osgViewer::Viewer viewer;
489    viewer.addEventHandler(new KeyboardEventHandler());
490
491    // add model to viewer.
492    viewer.setSceneData( createShapes() );
493    return viewer.run();
494}
Note: See TracBrowser for help on using the browser.