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

Revision 9029, 17.8 kB (checked in by robert, 5 years ago)

From James Moliere,
" The code below is to show how a heirarchy of objects can be made within a scenegraph.

In other words, how there can be a parent/child relationship between objects such
that when a parent is rotated or translated, the children move is respect to it's
parent movement. A robotic arm is used in this example because this is what I'm
using OSG for."

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(M_PIl/180, joint1);
155                    return true;
156                case 'a':
157                    rotate(-M_PIl/180, joint1);
158                    return true;
159                case 'w':
160                    rotate(M_PIl/180, joint2);
161                    return true;
162                case 's':
163                    rotate(-M_PIl/180, joint2);
164                    return true;
165                case 'e':
166                    rotate(M_PIl/180, joint3);
167                    return true;
168                case 'd':
169                    rotate(-M_PIl/180, joint3);
170                    return true;
171                case 'r':
172                    rotate(M_PIl/180, joint4);
173                    return true;
174                case 'f':
175                    rotate(-M_PIl/180, joint4);
176                    return true;
177                case 't':
178                    rotate(M_PIl/180, joint5);
179                    return true;
180                case 'g':
181                    rotate(-M_PIl/180, joint5);
182                    return true;
183                case 'y':
184                    rotate(M_PIl/180, joint6);
185                    return true;
186                case 'h':
187                    rotate(-M_PIl/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
228    //The cylinder's barycenter is the point of rotation.
229    //For this project, the x and y axis of rotation needs to be at the start of the cylinder and the
230    //axis of rotation about the z axis needs to be at the center of the cylinder.
231    //  this is where height/2 is used -- to offset x and y axis barycenter.
232    // note: we did not use -height/2
233    joint->addDrawable(new osg::ShapeDrawable(new osg::Cylinder(osg::Vec3(0.0f,0.0f,height/2),radius,height),hints));
234
235    osg::MatrixTransform *zTransform = new osg::MatrixTransform();
236    xTransform->addChild(zTransform);
237    osg::Matrix zTrans=osg::Matrix::translate(0.0, 0.0, height);
238    osg::Matrix zRot= osg::Matrix::rotate(jointAngle1, 0.0, 0.0, 1.0);
239    zTransform->setMatrix(zTrans*zRot);
240    return zTransform;
241}
242
243osg::MatrixTransform* buildJoint2(osg::MatrixTransform* previousJoint)
244{
245    if (showAxis)
246    {
247        createAxis(previousJoint);
248    }
249    double height = 17.6;
250    double radius = 4.45633;
251    osg::MatrixTransform* xTransform = new osg::MatrixTransform();
252    previousJoint->addChild(xTransform);
253    osg::Matrix xRot = osg::Matrix::rotate(M_PI_2, 1.0, 0.0, 0.0);
254    xTransform->setMatrix(xRot);
255    osg::Geode* joint = new osg::Geode();
256    joint->addDrawable(new osg::ShapeDrawable(new osg::Capsule(osg::Vec3(0.0f,0.0f,height/2),radius,height),hints));
257    xTransform->addChild(joint);
258
259    osg::MatrixTransform* zTransform = new osg::MatrixTransform();
260    osg::Matrix zTrans = osg::Matrix::translate( 0.0, 0.0, height);
261    osg::Matrix zRot = osg::Matrix::rotate(M_PI_2+jointAngle2, 0.0,0.0,1.0);
262    zTransform->setMatrix(zTrans*zRot);
263    xTransform->addChild(zTransform);
264    return zTransform;
265}
266
267osg::MatrixTransform* buildTube2(osg::MatrixTransform* previousJoint)
268{
269    if (showAxis)
270    {
271        createAxis(previousJoint);
272    }
273    double height = 17.6;
274    double radius = 4.45633;
275    osg::MatrixTransform* xTransform = new osg::MatrixTransform();
276    previousJoint->addChild(xTransform);
277    osg::Matrix xRot = osg::Matrix::rotate(M_PI_2, 1.0,0.0,0.0);
278    xTransform->setMatrix(xRot);
279    osg::Geode* tube3 = new osg::Geode();
280    xTransform->addChild(tube3);
281    tube3->addDrawable(new osg::ShapeDrawable(new osg::Capsule(osg::Vec3(0.0f,0.0f,height/2),radius,height),hints));
282
283    osg::MatrixTransform* zTransform = new osg::MatrixTransform();
284    xTransform->addChild(zTransform);
285    osg::Matrix zTrans = osg::Matrix::translate(0,0,height);
286    zTransform->setMatrix(zTrans);
287    return zTransform;
288}
289
290osg::MatrixTransform* buildJoint3(osg::MatrixTransform* previousJoint)
291{
292    double height =7.5;
293    double radius = 4.45633;
294    osg::Geode* joint = new osg::Geode();
295    osg::MatrixTransform* xTransform = new osg::MatrixTransform();
296    previousJoint->addChild(xTransform);
297    osg::Matrix xRot = osg::Matrix::rotate(-M_PI_2, 1.0, 0.0, 0.0);
298    xTransform->setMatrix(xRot);
299    if (showAxis)
300    {
301        createAxis(xTransform);
302    }
303
304    osg::MatrixTransform* zCheat = new osg::MatrixTransform();
305    osg::Matrix zTransCheat = osg::Matrix::translate(0.0,0.0,-height);
306    zCheat->setMatrix(zTransCheat);
307    xTransform->addChild(zCheat);
308
309    osg::ShapeDrawable *shape = new osg::ShapeDrawable(new osg::Capsule(osg::Vec3(0.0f,0.0f,height/2),radius,height),hints);
310    joint->addDrawable(shape);
311    zCheat->addChild(joint);
312
313    osg::MatrixTransform* zTransform = new osg::MatrixTransform();
314    zCheat->addChild(zTransform);
315    osg::Matrix zRot = osg::Matrix::rotate((float)jointAngle3, 0.0f, 0.0f, 1.0f);
316    osg::Matrix zTrans = osg::Matrix::translate(0,0,0);
317    zTransform->setMatrix(zTrans*zRot);
318    return zTransform;
319}
320
321osg::MatrixTransform* buildJoint4(osg::MatrixTransform* previousJoint)
322{
323    double height = 17.5;
324    double radius = 2.86479;
325    osg::Geode* tube4 = new osg::Geode();
326    tube4->addDrawable(new osg::ShapeDrawable(new osg::Capsule(osg::Vec3(0.0f,0.0f,height/2),radius,height),hints));
327    osg::MatrixTransform* xTransform = new osg::MatrixTransform();
328    if (showAxis)
329    {
330        createAxis(xTransform);
331    }
332    previousJoint->addChild(xTransform);
333    xTransform->addChild(tube4);
334    osg::Matrix xRot= osg::Matrix::rotate(M_PI_2, 1,0,0);
335    xTransform->setMatrix(xRot);
336    height = 7.5;
337    osg::Matrix zTrans = osg::Matrix::translate(0,0,17.5);
338    osg::Matrix zRot = osg::Matrix::rotate(jointAngle4-M_PI_2, 0,0,1);
339
340    osg::MatrixTransform* zTransform = new osg::MatrixTransform();
341    zTransform->setMatrix(zTrans*zRot);
342    xTransform->addChild(zTransform);
343    return zTransform;
344}
345
346osg::MatrixTransform* buildTube5(osg::MatrixTransform* previousJoint)
347{
348    double height = 7.5;
349    double radius = 2.86479;
350
351    height = 15.0;
352    osg::Geode* joint = new osg::Geode();
353    osg::ShapeDrawable *shape =new osg::ShapeDrawable(new osg::Cylinder(osg::Vec3(0.0f,0.0f,height/2),radius,height),hints);
354    joint->addDrawable(shape);
355    osg::MatrixTransform* xTransform = new osg::MatrixTransform();
356    previousJoint->addChild(xTransform);
357    xTransform->addChild(joint);
358
359    osg::MatrixTransform* zTransform = new osg::MatrixTransform();
360    osg::Matrix zTrans = osg::Matrix::translate(0,0,height);
361    zTransform->setMatrix(zTrans);
362    xTransform->addChild(zTransform);
363    return zTransform;
364}
365
366osg::MatrixTransform* buildJoint5(osg::MatrixTransform* previousJoint)
367{
368    double radius = 2.86479;
369    double height = 2.86479*2;
370    osg::Geode* joint = new osg::Geode();
371    osg::ShapeDrawable *shape = new osg::ShapeDrawable(new osg::Cylinder(osg::Vec3(0.0f,0.0f,0.0f),radius,height),hints);
372    joint->addDrawable(shape);
373    osg::MatrixTransform* xTransform = new osg::MatrixTransform();
374    previousJoint->addChild(xTransform);
375    osg::Matrix xRot= osg::Matrix::rotate(-M_PI_2, 1.0,0,0);
376    xTransform->setMatrix(xRot);
377    xTransform->addChild(joint);
378    if (showAxis)
379    {
380        createAxis(xTransform);
381    }
382
383    osg::MatrixTransform* zTransform = new osg::MatrixTransform();
384    osg::Matrix zRot= osg::Matrix::rotate(jointAngle5, 0,0,1);
385    osg::Matrix zTrans = osg::Matrix::translate(0,0,0);
386    zTransform->setMatrix(zTrans*zRot);
387    xTransform->addChild(zTransform);
388    return zTransform;
389}
390
391osg::MatrixTransform* buildJoint6(osg::MatrixTransform* previousJoint)
392{
393    double height = 3.0;
394    double radius = 1.0;
395    osg::Geode* joint = new osg::Geode();
396    joint->addDrawable(new osg::ShapeDrawable(new osg::Cylinder(osg::Vec3(0.0f,0.0f,height/2),radius,height),hints));
397    osg::MatrixTransform* xTransform = new osg::MatrixTransform();
398    osg::Matrix xRot= osg::Matrix::rotate(M_PI_2, 1.0, 0.0, 0.0);
399    xTransform->setMatrix(xRot);
400    xTransform->addChild(joint);
401    previousJoint->addChild(xTransform);
402    if (showAxis)
403    {
404        createAxis(xTransform);
405    }
406    return xTransform;
407}
408
409osg::MatrixTransform* buildEndEffector()
410{
411    osg::MatrixTransform* mt = new osg::MatrixTransform();
412    osg::Matrix m;
413    double length = 17.0;
414    m.makeTranslate(0,0,length/2);
415    mt->setMatrix(m);
416    osg::Geode *geode_3 = new osg::Geode;
417    osg::ShapeDrawable *shape1 = new osg::ShapeDrawable(new osg::Box(osg::Vec3(-EndEffector, 0.0f, 0.0f), .5, 1.5f, length), hints);
418    osg::ShapeDrawable *shape2 = new osg::ShapeDrawable(new osg::Box(osg::Vec3( EndEffector, 0.0f, 0.0f), .5, 1.5f, length), hints);
419    shape1->setColor(osg::Vec4(0.8f, 0.8f, 0.4f, 1.0f));
420    shape2->setColor(osg::Vec4(0.8f, 0.8f, 0.4f, 1.0f));
421    geode_3->addDrawable(shape1);
422    geode_3->addDrawable(shape2);
423    mt->addChild(geode_3);
424    return mt;
425}
426
427void createAxis(osg::Transform* previousJoint)
428{
429    double height = 10.0;
430    double radius = .5;
431
432    osg::MatrixTransform* zmt = new osg::MatrixTransform();
433
434    previousJoint->addChild(zmt);
435    osg::ShapeDrawable *zShape = new osg::ShapeDrawable(new osg::Cylinder(osg::Vec3(0.0f,0.0f,height/2),radius,height),hints);
436    osg::ShapeDrawable *zCone = new osg::ShapeDrawable(new osg::Cone(osg::Vec3(0.0f,0.0f,1.0),radius+1.0,2.0),hints);
437
438    osg::MatrixTransform* zmtCone = new osg::MatrixTransform();
439    osg::Geode *zgCone = new osg::Geode;
440
441    zmtCone->setMatrix( osg::Matrix::translate(0.0,0.0,height));
442    previousJoint->addChild(zmtCone);
443
444    zShape->setColor(osg::Vec4(0.0f, 0.0f, 1.0f, 1.0f));
445    zCone->setColor(osg::Vec4(0.0f, 0.0f, 1.0f, 1.0f));
446    osg::Geode *z = new osg::Geode;
447    z->addDrawable(zShape);
448    zgCone->addDrawable(zCone);
449    zmtCone->addChild(zgCone);
450    zmt->addChild(z);
451
452    osg::MatrixTransform* mt = new osg::MatrixTransform();
453    previousJoint->addChild(mt);
454
455    osg::Matrix xMatrix = osg::Matrix::rotate(-M_PI_2, 0.0, 1.0, 0.0);
456    mt->setMatrix(xMatrix);
457
458
459    osg::ShapeDrawable *xShape = new osg::ShapeDrawable(new osg::Cylinder(osg::Vec3(0.0f,0.0f,height/2),radius,height),hints);
460    xShape->setColor(osg::Vec4(1.0f, 0.0f, 0.0f, 1.0f));
461    osg::Geode *x = new osg::Geode;
462    x->addDrawable(xShape);
463    mt->addChild(x);
464
465
466    osg::MatrixTransform *yMt = new osg::MatrixTransform();
467    previousJoint->addChild(yMt);
468    osg::Matrix yMatrix = osg::Matrix::rotate(M_PI_2, 1.0, 0.0, 0.0);
469    yMt->setMatrix(yMatrix);
470
471    osg::ShapeDrawable *yShape = new osg::ShapeDrawable(new osg::Cylinder(osg::Vec3(0.0f,0.0f,height/2),radius,height),hints);
472    yShape->setColor(osg::Vec4(0.0f, 1.0f, 0.0f, 1.0f));
473    osg::Geode *y = new osg::Geode;
474    y->addDrawable(yShape);
475    yMt->addChild(y);
476}
477
478int main(int, char **)
479{
480    hints->setDetailRatio(0.5f);
481    showAxis = false;
482    jointAngle1=0.0;
483    jointAngle2=0.0;
484    jointAngle3=0.0;
485    jointAngle4=0.0;
486    jointAngle5=0.0;
487    jointAngle6=0.0;
488    EndEffector=1.0;
489
490    osgViewer::Viewer viewer;
491    viewer.addEventHandler(new KeyboardEventHandler());
492
493    // add model to viewer.
494    viewer.setSceneData( createShapes() );
495    return viewer.run();
496}
Note: See TracBrowser for help on using the browser.