495 lines
18 KiB
C++
495 lines
18 KiB
C++
/* 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 hierarchy 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 <osg/Geode>
|
|
#include <osg/ShapeDrawable>
|
|
#include <osg/Material>
|
|
#include <osg/Texture2D>
|
|
|
|
#include <osgViewer/Viewer>
|
|
#include <iostream>
|
|
#include <osgDB/ReadFile>
|
|
#include <osg/Math>
|
|
#include <osg/Matrixd>
|
|
#include <osg/MatrixTransform>
|
|
#include <osg/Vec3d>
|
|
#include <iostream>
|
|
#include <osg/PositionAttitudeTransform>
|
|
|
|
|
|
|
|
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(osg::PI/180, joint1);
|
|
return true;
|
|
case 'a':
|
|
rotate(-osg::PI/180, joint1);
|
|
return true;
|
|
case 'w':
|
|
rotate(osg::PI/180, joint2);
|
|
return true;
|
|
case 's':
|
|
rotate(-osg::PI/180, joint2);
|
|
return true;
|
|
case 'e':
|
|
rotate(osg::PI/180, joint3);
|
|
return true;
|
|
case 'd':
|
|
rotate(-osg::PI/180, joint3);
|
|
return true;
|
|
case 'r':
|
|
rotate(osg::PI/180, joint4);
|
|
return true;
|
|
case 'f':
|
|
rotate(-osg::PI/180, joint4);
|
|
return true;
|
|
case 't':
|
|
rotate(osg::PI/180, joint5);
|
|
return true;
|
|
case 'g':
|
|
rotate(-osg::PI/180, joint5);
|
|
return true;
|
|
case 'y':
|
|
rotate(osg::PI/180, joint6);
|
|
return true;
|
|
case 'h':
|
|
rotate(-osg::PI/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);
|
|
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(osg::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(osg::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(osg::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(-osg::PI_2, 1.0, 0.0, 0.0);
|
|
xTransform->setMatrix(xRot);
|
|
|
|
|
|
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);
|
|
if (showAxis)
|
|
{
|
|
createAxis(zTransform);
|
|
}
|
|
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(osg::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-osg::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)
|
|
{
|
|
if (showAxis)
|
|
{
|
|
createAxis(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(-osg::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(osg::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 = 12.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(-osg::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(osg::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 = true;
|
|
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();
|
|
}
|