From 8c6f65caca2de70ead131db1da62131e29b28ada Mon Sep 17 00:00:00 2001 From: Robert Osfield Date: Tue, 14 Oct 2008 15:24:39 +0000 Subject: [PATCH] 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." --- examples/CMakeLists.txt | 1 + examples/osgrobot/CMakeLists.txt | 4 + examples/osgrobot/osgrobot.cpp | 496 +++++++++++++++++++++++++++++++ 3 files changed, 501 insertions(+) create mode 100644 examples/osgrobot/CMakeLists.txt create mode 100644 examples/osgrobot/osgrobot.cpp diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 9eea433a9..adc81da2d 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -28,6 +28,7 @@ IF(DYNAMIC_OPENSCENEGRAPH) ADD_SUBDIRECTORY(osgcompositeviewer) ADD_SUBDIRECTORY(osgcopy) ADD_SUBDIRECTORY(osgcubemap) + ADD_SUBDIRECTORY(osgrobot) ADD_SUBDIRECTORY(osgdelaunay) ADD_SUBDIRECTORY(osgdepthpartition) ADD_SUBDIRECTORY(osgdepthpeeling) diff --git a/examples/osgrobot/CMakeLists.txt b/examples/osgrobot/CMakeLists.txt new file mode 100644 index 000000000..bf7c2bce1 --- /dev/null +++ b/examples/osgrobot/CMakeLists.txt @@ -0,0 +1,4 @@ +SET(TARGET_SRC osgrobot.cpp ) + +#### end var setup ### +SETUP_EXAMPLE(osgrobot) diff --git a/examples/osgrobot/osgrobot.cpp b/examples/osgrobot/osgrobot.cpp new file mode 100644 index 000000000..f7a7c606c --- /dev/null +++ b/examples/osgrobot/osgrobot.cpp @@ -0,0 +1,496 @@ +/* OpenSceneGraph example, osgrobot.cpp. +* +* Permission is hereby granted, free of charge, to any person obtaining a copy +* of this software and associated documentation files (the "Software"), to deal +* in the Software without restriction, including without limitation the rights +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +* copies of the Software, and to permit persons to whom the Software is +* furnished to do so, subject to the following conditions: +* +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +* THE SOFTWARE. +*/ + +/* + 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. + + To rotate the joints use the following table to rotate each respective joint. + + joint key rotation + 1 q + 1 degree + 1 a - 1 degree + 2 w + 1 degree + 2 s - 1 degree + 3 e + 1 degree + 3 d - 1 degree + 4 r + 1 degree + 4 f - 1 degree + 5 t + 1 degree + 5 g - 1 degree + 6 y + 1 degree + 6 h - 1 degree + + In many robotics projects, the x and z axis are only used to define the positions + of the joints of a robotic arm. By not using the y-axis, the mathematics of + inverse kinematics becomes easier (and in some cases possible). All joint rotations + are rotated about the z-axis. If you toggle the variable 'showAxis' to true, an + x (color red), y (color green), and z (color blue) axis will display. Using the + thumb of the right hand to point in the same direction of the z-axis, use the right + hand rule to define a positive angle of rotation. + + The robotic arm in general follows the algorithm + rotate about the x axis, translate along the x-axis, translate along the z axis, + and then rotate about the z-axis. There is 1 exception in the method buildJoint3(...) + where it was necessary to add a 'cheat' to get the joints to align properly. In a + real robotics project such as this, the tube (see buildTube2(...) method) would not + be a part of the mathematics and thus, not have to be drawn and would not require + the cheat. To learn moue about positioning joints using only the x and z axis, + web search 'Denavit-Hartenberg'. wikipedia has comments at the URL... + http://en.wikipedia.org/wiki/Robotics_conventions#Denavit-Hartenberg_link_frame_convention_.28DH.29 + + Lessons learned with osg... + When starting this work I was very new to OSG. When I first placed a cylinder + in a scene and rotated the cylinder about the z and x axis, I noticed that the rotation + is about the barycenter of the object for both axis. I wanted a rotation about the z-axis + to be along the length of the tube (which OSG gives for free -- not always true with + all scenegraphs) while the rotation about the x and y axis about the + beginning (or start position) of the tube. Knowing what I know about barycenters + I positioned the shape along the -height/2 (see example 1 below) + -- believing the offset -height/2 would place the axis of rotation at the start of + the tube. I was certainly wrong about this and found out the equation needs to + be height/2. + + ... + example 1 + joint->addDrawable(new osg::ShapeDrawable(new osg::Capsule(osg::Vec3(0.0f,0.0f,height/2),radius,height),hints)); + ... + + + + author: James Moliere + date written: 10/12/2008 +*/ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + + + +float jointAngle1; +float jointAngle2; +float jointAngle3; +float jointAngle4; +float jointAngle5; +float jointAngle6; +float EndEffector; + +void createAxis(osg::Transform* previousJoint); + +osg::MatrixTransform* joint1; +osg::MatrixTransform* joint2; +osg::MatrixTransform* joint3; +osg::MatrixTransform* joint4; +osg::MatrixTransform* joint5; +osg::MatrixTransform* joint6; + +osg::MatrixTransform* buildJoint1(osg::MatrixTransform* previousJoint); +osg::MatrixTransform* buildJoint2(osg::MatrixTransform* previousJoint); +osg::MatrixTransform* buildTube2(osg::MatrixTransform* previousJoint); +osg::MatrixTransform* buildJoint3(osg::MatrixTransform* previousJoint); +osg::MatrixTransform* buildJoint4(osg::MatrixTransform* previousJoint); +osg::MatrixTransform* buildTube5(osg::MatrixTransform* previousJoint); +osg::MatrixTransform* buildJoint5(osg::MatrixTransform* previousJoint); +osg::MatrixTransform* buildJoint6(osg::MatrixTransform* previousJoint); +osg::MatrixTransform* buildEndEffector(); +osg::TessellationHints* hints = new osg::TessellationHints; + +bool showAxis; + +class KeyboardEventHandler : public osgGA::GUIEventHandler +{ +public: + + KeyboardEventHandler() + {} + + + static void rotate(float angle,osg::MatrixTransform *joint) + { + osg::Matrix zRot; + zRot.makeRotate(angle, 0.0,0.0,1.0); + joint->setMatrix(zRot*joint->getMatrix()); + } + + virtual bool handle(const osgGA::GUIEventAdapter& ea,osgGA::GUIActionAdapter&) + { + switch (ea.getEventType()) + { + case(osgGA::GUIEventAdapter::KEYDOWN): + { + switch (ea.getKey()) + { + case 'q': + rotate(M_PIl/180, joint1); + return true; + case 'a': + rotate(-M_PIl/180, joint1); + return true; + case 'w': + rotate(M_PIl/180, joint2); + return true; + case 's': + rotate(-M_PIl/180, joint2); + return true; + case 'e': + rotate(M_PIl/180, joint3); + return true; + case 'd': + rotate(-M_PIl/180, joint3); + return true; + case 'r': + rotate(M_PIl/180, joint4); + return true; + case 'f': + rotate(-M_PIl/180, joint4); + return true; + case 't': + rotate(M_PIl/180, joint5); + return true; + case 'g': + rotate(-M_PIl/180, joint5); + return true; + case 'y': + rotate(M_PIl/180, joint6); + return true; + case 'h': + rotate(-M_PIl/180, joint6); + return true; + } + } + default: + break; + + } + //return false to allow mouse manipulation + return false; + } +}; + + +osg::Group* createShapes() +{ + osg::Group* group = new osg::Group(); + osg::MatrixTransform* transform =new osg::MatrixTransform(); + group->addChild(transform); + + joint1 = buildJoint1(transform); + joint2 = buildJoint2(joint1); + osg::MatrixTransform* tube2= buildTube2(joint2); + joint3 = buildJoint3(tube2); + joint4 = buildJoint4(joint3); + osg::MatrixTransform *tube5 = buildTube5(joint4); + joint5 = buildJoint5(tube5); + joint6= buildJoint6(joint5); + joint6->addChild( buildEndEffector()); + return group; +} + +osg::MatrixTransform* buildJoint1(osg::MatrixTransform* previousJoint) +{ + osg::MatrixTransform* xTransform =new osg::MatrixTransform(); + previousJoint->addChild(xTransform); + float radius = 6.7640f; + float height = 45.0f; + osg::Geode* joint = new osg::Geode(); + xTransform->addChild(joint); + + //The cylinder's barycenter is the point of rotation. + //For this project, the x and y axis of rotation needs to be at the start of the cylinder and the + //axis of rotation about the z axis needs to be at the center of the cylinder. + // this is where height/2 is used -- to offset x and y axis barycenter. + // note: we did not use -height/2 + joint->addDrawable(new osg::ShapeDrawable(new osg::Cylinder(osg::Vec3(0.0f,0.0f,height/2),radius,height),hints)); + + osg::MatrixTransform *zTransform = new osg::MatrixTransform(); + xTransform->addChild(zTransform); + osg::Matrix zTrans=osg::Matrix::translate(0.0, 0.0, height); + osg::Matrix zRot= osg::Matrix::rotate(jointAngle1, 0.0, 0.0, 1.0); + zTransform->setMatrix(zTrans*zRot); + return zTransform; +} + +osg::MatrixTransform* buildJoint2(osg::MatrixTransform* previousJoint) +{ + if (showAxis) + { + createAxis(previousJoint); + } + double height = 17.6; + double radius = 4.45633; + osg::MatrixTransform* xTransform = new osg::MatrixTransform(); + previousJoint->addChild(xTransform); + osg::Matrix xRot = osg::Matrix::rotate(M_PI_2, 1.0, 0.0, 0.0); + xTransform->setMatrix(xRot); + osg::Geode* joint = new osg::Geode(); + joint->addDrawable(new osg::ShapeDrawable(new osg::Capsule(osg::Vec3(0.0f,0.0f,height/2),radius,height),hints)); + xTransform->addChild(joint); + + osg::MatrixTransform* zTransform = new osg::MatrixTransform(); + osg::Matrix zTrans = osg::Matrix::translate( 0.0, 0.0, height); + osg::Matrix zRot = osg::Matrix::rotate(M_PI_2+jointAngle2, 0.0,0.0,1.0); + zTransform->setMatrix(zTrans*zRot); + xTransform->addChild(zTransform); + return zTransform; +} + +osg::MatrixTransform* buildTube2(osg::MatrixTransform* previousJoint) +{ + if (showAxis) + { + createAxis(previousJoint); + } + double height = 17.6; + double radius = 4.45633; + osg::MatrixTransform* xTransform = new osg::MatrixTransform(); + previousJoint->addChild(xTransform); + osg::Matrix xRot = osg::Matrix::rotate(M_PI_2, 1.0,0.0,0.0); + xTransform->setMatrix(xRot); + osg::Geode* tube3 = new osg::Geode(); + xTransform->addChild(tube3); + tube3->addDrawable(new osg::ShapeDrawable(new osg::Capsule(osg::Vec3(0.0f,0.0f,height/2),radius,height),hints)); + + osg::MatrixTransform* zTransform = new osg::MatrixTransform(); + xTransform->addChild(zTransform); + osg::Matrix zTrans = osg::Matrix::translate(0,0,height); + zTransform->setMatrix(zTrans); + return zTransform; +} + +osg::MatrixTransform* buildJoint3(osg::MatrixTransform* previousJoint) +{ + double height =7.5; + double radius = 4.45633; + osg::Geode* joint = new osg::Geode(); + osg::MatrixTransform* xTransform = new osg::MatrixTransform(); + previousJoint->addChild(xTransform); + osg::Matrix xRot = osg::Matrix::rotate(-M_PI_2, 1.0, 0.0, 0.0); + xTransform->setMatrix(xRot); + if (showAxis) + { + createAxis(xTransform); + } + + osg::MatrixTransform* zCheat = new osg::MatrixTransform(); + osg::Matrix zTransCheat = osg::Matrix::translate(0.0,0.0,-height); + zCheat->setMatrix(zTransCheat); + xTransform->addChild(zCheat); + + osg::ShapeDrawable *shape = new osg::ShapeDrawable(new osg::Capsule(osg::Vec3(0.0f,0.0f,height/2),radius,height),hints); + joint->addDrawable(shape); + zCheat->addChild(joint); + + osg::MatrixTransform* zTransform = new osg::MatrixTransform(); + zCheat->addChild(zTransform); + osg::Matrix zRot = osg::Matrix::rotate((float)jointAngle3, 0.0f, 0.0f, 1.0f); + osg::Matrix zTrans = osg::Matrix::translate(0,0,0); + zTransform->setMatrix(zTrans*zRot); + return zTransform; +} + +osg::MatrixTransform* buildJoint4(osg::MatrixTransform* previousJoint) +{ + double height = 17.5; + double radius = 2.86479; + osg::Geode* tube4 = new osg::Geode(); + tube4->addDrawable(new osg::ShapeDrawable(new osg::Capsule(osg::Vec3(0.0f,0.0f,height/2),radius,height),hints)); + osg::MatrixTransform* xTransform = new osg::MatrixTransform(); + if (showAxis) + { + createAxis(xTransform); + } + previousJoint->addChild(xTransform); + xTransform->addChild(tube4); + osg::Matrix xRot= osg::Matrix::rotate(M_PI_2, 1,0,0); + xTransform->setMatrix(xRot); + height = 7.5; + osg::Matrix zTrans = osg::Matrix::translate(0,0,17.5); + osg::Matrix zRot = osg::Matrix::rotate(jointAngle4-M_PI_2, 0,0,1); + + osg::MatrixTransform* zTransform = new osg::MatrixTransform(); + zTransform->setMatrix(zTrans*zRot); + xTransform->addChild(zTransform); + return zTransform; +} + +osg::MatrixTransform* buildTube5(osg::MatrixTransform* previousJoint) +{ + double height = 7.5; + double radius = 2.86479; + + height = 15.0; + osg::Geode* joint = new osg::Geode(); + osg::ShapeDrawable *shape =new osg::ShapeDrawable(new osg::Cylinder(osg::Vec3(0.0f,0.0f,height/2),radius,height),hints); + joint->addDrawable(shape); + osg::MatrixTransform* xTransform = new osg::MatrixTransform(); + previousJoint->addChild(xTransform); + xTransform->addChild(joint); + + osg::MatrixTransform* zTransform = new osg::MatrixTransform(); + osg::Matrix zTrans = osg::Matrix::translate(0,0,height); + zTransform->setMatrix(zTrans); + xTransform->addChild(zTransform); + return zTransform; +} + +osg::MatrixTransform* buildJoint5(osg::MatrixTransform* previousJoint) +{ + double radius = 2.86479; + double height = 2.86479*2; + osg::Geode* joint = new osg::Geode(); + osg::ShapeDrawable *shape = new osg::ShapeDrawable(new osg::Cylinder(osg::Vec3(0.0f,0.0f,0.0f),radius,height),hints); + joint->addDrawable(shape); + osg::MatrixTransform* xTransform = new osg::MatrixTransform(); + previousJoint->addChild(xTransform); + osg::Matrix xRot= osg::Matrix::rotate(-M_PI_2, 1.0,0,0); + xTransform->setMatrix(xRot); + xTransform->addChild(joint); + if (showAxis) + { + createAxis(xTransform); + } + + osg::MatrixTransform* zTransform = new osg::MatrixTransform(); + osg::Matrix zRot= osg::Matrix::rotate(jointAngle5, 0,0,1); + osg::Matrix zTrans = osg::Matrix::translate(0,0,0); + zTransform->setMatrix(zTrans*zRot); + xTransform->addChild(zTransform); + return zTransform; +} + +osg::MatrixTransform* buildJoint6(osg::MatrixTransform* previousJoint) +{ + double height = 3.0; + double radius = 1.0; + osg::Geode* joint = new osg::Geode(); + joint->addDrawable(new osg::ShapeDrawable(new osg::Cylinder(osg::Vec3(0.0f,0.0f,height/2),radius,height),hints)); + osg::MatrixTransform* xTransform = new osg::MatrixTransform(); + osg::Matrix xRot= osg::Matrix::rotate(M_PI_2, 1.0, 0.0, 0.0); + xTransform->setMatrix(xRot); + xTransform->addChild(joint); + previousJoint->addChild(xTransform); + if (showAxis) + { + createAxis(xTransform); + } + return xTransform; +} + +osg::MatrixTransform* buildEndEffector() +{ + osg::MatrixTransform* mt = new osg::MatrixTransform(); + osg::Matrix m; + double length = 17.0; + m.makeTranslate(0,0,length/2); + mt->setMatrix(m); + osg::Geode *geode_3 = new osg::Geode; + osg::ShapeDrawable *shape1 = new osg::ShapeDrawable(new osg::Box(osg::Vec3(-EndEffector, 0.0f, 0.0f), .5, 1.5f, length), hints); + osg::ShapeDrawable *shape2 = new osg::ShapeDrawable(new osg::Box(osg::Vec3( EndEffector, 0.0f, 0.0f), .5, 1.5f, length), hints); + shape1->setColor(osg::Vec4(0.8f, 0.8f, 0.4f, 1.0f)); + shape2->setColor(osg::Vec4(0.8f, 0.8f, 0.4f, 1.0f)); + geode_3->addDrawable(shape1); + geode_3->addDrawable(shape2); + mt->addChild(geode_3); + return mt; +} + +void createAxis(osg::Transform* previousJoint) +{ + double height = 10.0; + double radius = .5; + + osg::MatrixTransform* zmt = new osg::MatrixTransform(); + + previousJoint->addChild(zmt); + osg::ShapeDrawable *zShape = new osg::ShapeDrawable(new osg::Cylinder(osg::Vec3(0.0f,0.0f,height/2),radius,height),hints); + osg::ShapeDrawable *zCone = new osg::ShapeDrawable(new osg::Cone(osg::Vec3(0.0f,0.0f,1.0),radius+1.0,2.0),hints); + + osg::MatrixTransform* zmtCone = new osg::MatrixTransform(); + osg::Geode *zgCone = new osg::Geode; + + zmtCone->setMatrix( osg::Matrix::translate(0.0,0.0,height)); + previousJoint->addChild(zmtCone); + + zShape->setColor(osg::Vec4(0.0f, 0.0f, 1.0f, 1.0f)); + zCone->setColor(osg::Vec4(0.0f, 0.0f, 1.0f, 1.0f)); + osg::Geode *z = new osg::Geode; + z->addDrawable(zShape); + zgCone->addDrawable(zCone); + zmtCone->addChild(zgCone); + zmt->addChild(z); + + osg::MatrixTransform* mt = new osg::MatrixTransform(); + previousJoint->addChild(mt); + + osg::Matrix xMatrix = osg::Matrix::rotate(-M_PI_2, 0.0, 1.0, 0.0); + mt->setMatrix(xMatrix); + + + osg::ShapeDrawable *xShape = new osg::ShapeDrawable(new osg::Cylinder(osg::Vec3(0.0f,0.0f,height/2),radius,height),hints); + xShape->setColor(osg::Vec4(1.0f, 0.0f, 0.0f, 1.0f)); + osg::Geode *x = new osg::Geode; + x->addDrawable(xShape); + mt->addChild(x); + + + osg::MatrixTransform *yMt = new osg::MatrixTransform(); + previousJoint->addChild(yMt); + osg::Matrix yMatrix = osg::Matrix::rotate(M_PI_2, 1.0, 0.0, 0.0); + yMt->setMatrix(yMatrix); + + osg::ShapeDrawable *yShape = new osg::ShapeDrawable(new osg::Cylinder(osg::Vec3(0.0f,0.0f,height/2),radius,height),hints); + yShape->setColor(osg::Vec4(0.0f, 1.0f, 0.0f, 1.0f)); + osg::Geode *y = new osg::Geode; + y->addDrawable(yShape); + yMt->addChild(y); +} + +int main(int, char **) +{ + hints->setDetailRatio(0.5f); + showAxis = false; + jointAngle1=0.0; + jointAngle2=0.0; + jointAngle3=0.0; + jointAngle4=0.0; + jointAngle5=0.0; + jointAngle6=0.0; + EndEffector=1.0; + + osgViewer::Viewer viewer; + viewer.addEventHandler(new KeyboardEventHandler()); + + // add model to viewer. + viewer.setSceneData( createShapes() ); + return viewer.run(); +}