Robotsystem from Robot Group Leipzig: joint.cpp Source File

- Main Page
- Modules
- Namespaces
- Classes
- Files
- Directories
- Examples

- File List
- File Members

# joint.cpp

Go to the documentation of this file.

    00001 /***************************************************************************
    00002  *   Copyright (C) 2005 by Robot Group Leipzig                             *
    00003  *    martius@informatik.uni-leipzig.de                                    *
    00004  *    fhesse@informatik.uni-leipzig.de                                     *
    00005  *    der@informatik.uni-leipzig.de                                        *
    00006  *                                                                         *
    00007  *   This program is free software; you can redistribute it and/or modify  *
    00008  *   it under the terms of the GNU General Public License as published by  *
    00009  *   the Free Software Foundation; either version 2 of the License, or     *
    00010  *   (at your option) any later version.                                   *
    00011  *                                                                         *
    00012  *   This program is distributed in the hope that it will be useful,       *
    00013  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
    00014  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
    00015  *   GNU General Public License for more details.                          *
    00016  *                                                                         *
    00017  *   You should have received a copy of the GNU General Public License     *
    00018  *   along with this program; if not, write to the                         *
    00019  *   Free Software Foundation, Inc.,                                       *
    00020  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
    00021  ***************************************************************************
    00022  *                                                                         *
    00023  *  Different Joint wrappers                                               *
    00024  *                                                                         *
    00025  *   $Log: joint.cpp,v $
    00026  *   Revision 1.1.2.13  2006/03/29 15:05:57  martius
    00027  *   fixed joint
    00028  *
    00029  *   Revision 1.1.2.12  2006/02/01 18:34:03  martius
    00030  *   use Axis type for Joint axis. very important, since otherwise Vec3 * pose is not the right direction vector anymore
    00031  *
    00032  *   Revision 1.1.2.11  2006/01/12 22:18:31  martius
    00033  *   delete slider visual
    00034  *
    00035  *   Revision 1.1.2.10  2006/01/12 14:21:00  martius
    00036  *   drawmode, material
    00037  *
    00038  *   Revision 1.1.2.9  2006/01/11 14:11:28  fhesse
    00039  *   moved anchor up into Joint and introduced getAnchor()
    00040  *
    00041  *   Revision 1.1.2.8  2006/01/11 10:53:36  fhesse
    00042  *   delete visual; in SliderJoint::update() removed
    00043  *
    00044  *   Revision 1.1.2.7  2005/12/22 14:09:18  martius
    00045  *   low quality for joint axes
    00046  *
    00047  *   Revision 1.1.2.6  2005/12/21 15:39:22  martius
    00048  *   OneAxisJoint and TwoAxisJoint as superclasses
    00049  *
    00050  *   Revision 1.1.2.5  2005/12/19 16:34:12  martius
    00051  *   added Ball and Universal joint
    00052  *
    00053  *   Revision 1.1.2.4  2005/12/15 17:03:42  martius
    00054  *   cameramanupulator setPose is working
    00055  *   joints have setter and getter parameters
    00056  *   Primitives are not longer inherited from OSGPrimitive, moreover
    00057  *   they aggregate them
    00058  *
    00059  *   Revision 1.1.2.3  2005/12/14 15:36:45  martius
    00060  *   joints are visible now
    00061  *
    00062  *   Revision 1.1.2.2  2005/12/13 18:11:13  martius
    00063  *   transform primitive added, some joints stuff done, forward declaration
    00064  *
    00065  *   Revision 1.1.2.1  2005/12/12 23:40:42  martius
    00066  *   hinge2 started
    00067  *
    00068  *
    00069  *                                                                 *
    00070  ***************************************************************************/
    00071
    00072 #include <osg/Vec3>
    00073 #include <osg/Matrix>
    00074
    00075 #include "joint.h"
    00076 #include "pos.h"
    00077 #include "mathutils.h"
    00078
    00079
    00080 namespace lpzrobots {
    00081
    00082   using namespace osg;
    00083
    00084   Matrix Joint::anchorAxisPose(const osg::Vec3& anchor, const Axis& axis){
    00085     return rotationMatrixFromAxisZ(axis) * Matrix::translate(anchor);
    00086   }
    00087
    00088  Joint::~Joint(){
    00089    if (joint) dJointDestroy(joint);
    00090  }
    00091
    00092
    00093 /***************************************************************************/
    00094
    00095
    00096   FixedJoint::FixedJoint(Primitive* part1, Primitive* part2)
    00097   : Joint(part1, part2, osg::Vec3(0,0,0)){
    00098   }
    00099
    00100   FixedJoint::~FixedJoint(){}
    00101
    00102     /** initialises (and creates) the joint.
    00103     */
    00104   void FixedJoint::init(const OdeHandle& odeHandle, const OsgHandle& osgHandle,
    00105                       bool withVisual, double visualSize){
    00106     joint = dJointCreateFixed (odeHandle. world,0);
    00107     dJointAttach (joint, part1->getBody(),part2->getBody());
    00108   }
    00109
    00110   void FixedJoint::update(){}
    00111
    00112   void FixedJoint::setParam(int parameter, double value) {
    00113   }
    00114
    00115   double FixedJoint::getParam(int parameter){
    00116     return 0;
    00117   }
    00118
    00119 /***************************************************************************/
    00120
    00121   HingeJoint::HingeJoint(Primitive* part1, Primitive* part2, const osg::Vec3& anchor,
    00122                          const Axis& axis1)
    00123     : OneAxisJoint(part1, part2, anchor, axis1),  visual(0) {
    00124   }
    00125
    00126   HingeJoint::~HingeJoint(){
    00127     if (visual) delete visual;
    00128   }
    00129
    00130   void HingeJoint::init(const OdeHandle& odeHandle, const OsgHandle& osgHandle,
    00131                          bool withVisual, double visualSize){
    00132     joint = dJointCreateHinge (odeHandle. world,0);
    00133     dJointAttach (joint, part1->getBody(),part2->getBody());
    00134     dJointSetHingeAnchor (joint, anchor.x(), anchor.y(), anchor.z());
    00135     dJointSetHingeAxis (joint,  axis1.x(), axis1.y(), axis1.z());
    00136     if(withVisual){
    00137       visual = new OSGCylinder(visualSize/15.0, visualSize);
    00138       visual->init(osgHandle, OSGPrimitive::Low);
    00139       Matrix t = anchorAxisPose(anchor, axis1);
    00140
    00141       visual->setMatrix(t);
    00142     }
    00143   }
    00144
    00145   void HingeJoint::update(){
    00146     if(visual){
    00147       dVector3 v;
    00148       dJointGetHingeAnchor(joint, v);
    00149       anchor.x() = v[0];
    00150       anchor.y() = v[1];
    00151       anchor.z() = v[2];
    00152       dJointGetHingeAxis(joint, v);
    00153       axis1.x() = v[0];
    00154       axis1.y() = v[1];
    00155       axis1.z() = v[2];
    00156       visual->setMatrix(anchorAxisPose(anchor, axis1));
    00157     }
    00158   }
    00159
    00160   void HingeJoint::addTorque(double t){
    00161     dJointAddHingeTorque(joint, t);
    00162   }
    00163
    00164   double HingeJoint::getPosition1(){
    00165     return dJointGetHingeAngle(joint);
    00166   }
    00167
    00168   double HingeJoint::getPosition1Rate(){
    00169     return dJointGetHingeAngleRate(joint);
    00170   }
    00171
    00172   void HingeJoint::setParam(int parameter, double value) {
    00173     dJointSetHingeParam(joint, parameter, value);
    00174   }
    00175
    00176   double HingeJoint::getParam(int parameter){
    00177     return dJointGetHingeParam(joint, parameter);
    00178   }
    00179
    00180 /***************************************************************************/
    00181
    00182   Hinge2Joint::Hinge2Joint(Primitive* part1, Primitive* part2, const osg::Vec3& anchor,
    00183                            const Axis& axis1, const Axis& axis2)
    00184     : TwoAxisJoint(part1, part2, anchor, axis1, axis2), visual(0) {
    00185   }
    00186
    00187   Hinge2Joint::~Hinge2Joint(){
    00188     if (visual) delete visual;
    00189   }
    00190
    00191   void Hinge2Joint::init(const OdeHandle& odeHandle, const OsgHandle& osgHandle,
    00192                          bool withVisual, double visualSize){
    00193     joint = dJointCreateHinge2 (odeHandle. world,0);
    00194     dJointAttach (joint, part1->getBody(),part2->getBody());
    00195     dJointSetHinge2Anchor (joint, anchor.x(), anchor.y(), anchor.z());
    00196     dJointSetHinge2Axis1 (joint,  axis1.x(), axis1.y(), axis1.z());
    00197     dJointSetHinge2Axis2 (joint,  axis2.x(), axis2.y(), axis2.z());
    00198     if(withVisual){
    00199       visual = new OSGCylinder(visualSize/15.0, visualSize);
    00200       visual->init(osgHandle, OSGPrimitive::Low);
    00201       Matrix t = anchorAxisPose(anchor, axis2);
    00202
    00203       visual->setMatrix(t);
    00204     }
    00205   }
    00206
    00207   void Hinge2Joint::update(){
    00208     if(visual){
    00209       dVector3 v;
    00210       dJointGetHinge2Anchor(joint, v);
    00211       anchor.x() = v[0];
    00212       anchor.y() = v[1];
    00213       anchor.z() = v[2];
    00214       dJointGetHinge2Axis2(joint, v);
    00215       axis2.x() = v[0];
    00216       axis2.y() = v[1];
    00217       axis2.z() = v[2];
    00218       visual->setMatrix(anchorAxisPose(anchor, axis2));
    00219
    00220     }
    00221   }
    00222
    00223   /// adds torques to axis 1 and 2
    00224   void Hinge2Joint::addTorques(double t1, double t2){
    00225      dJointAddHinge2Torques(joint, t1, t2);
    00226   }
    00227
    00228   double Hinge2Joint::getPosition1(){
    00229     return dJointGetHinge2Angle1(joint);
    00230   }
    00231
    00232   double Hinge2Joint::getPosition2(){
    00233     fprintf(stderr, "Hinge2Joint::getPosition2() is called, but not supported!\n");
    00234     return 0;
    00235   }
    00236
    00237   double Hinge2Joint::getPosition1Rate(){
    00238     return dJointGetHinge2Angle1Rate(joint);
    00239   }
    00240
    00241   double Hinge2Joint::getPosition2Rate(){
    00242     return dJointGetHinge2Angle2Rate(joint);
    00243   }
    00244
    00245   void Hinge2Joint::setParam(int parameter, double value) {
    00246     dJointSetHinge2Param(joint, parameter, value);
    00247   }
    00248
    00249   double Hinge2Joint::getParam(int parameter){
    00250     return dJointGetHinge2Param(joint, parameter);
    00251   }
    00252
    00253 /***************************************************************************/
    00254
    00255   UniversalJoint::UniversalJoint(Primitive* part1, Primitive* part2, const osg::Vec3& anchor,
    00256                            const Axis& axis1, const Axis& axis2)
    00257     : TwoAxisJoint(part1, part2, anchor, axis1, axis2), visual1(0), visual2(0) {
    00258   }
    00259
    00260   UniversalJoint::~UniversalJoint(){
    00261     if (visual1) delete visual1;
    00262     if (visual2) delete visual2;
    00263   }
    00264
    00265   void UniversalJoint::init(const OdeHandle& odeHandle, const OsgHandle& osgHandle,
    00266                          bool withVisual, double visualSize){
    00267     joint = dJointCreateUniversal (odeHandle. world,0);
    00268     dJointAttach (joint, part1->getBody(),part2->getBody());
    00269     dJointSetUniversalAnchor (joint, anchor.x(), anchor.y(), anchor.z());
    00270     dJointSetUniversalAxis1 (joint,  axis1.x(), axis1.y(), axis1.z());
    00271     dJointSetUniversalAxis2 (joint,  axis2.x(), axis2.y(), axis2.z());
    00272     if(withVisual){
    00273       visual1 = new OSGCylinder(visualSize/15.0, visualSize);
    00274       visual1->init(osgHandle, OSGPrimitive::Low);
    00275       Matrix t = anchorAxisPose(anchor, axis1);
    00276       visual1->setMatrix(t);
    00277       visual2 = new OSGCylinder(visualSize/15.0, visualSize);
    00278       visual2->init(osgHandle, OSGPrimitive::Low);
    00279       t = anchorAxisPose(anchor, axis2);
    00280       visual2->setMatrix(t);
    00281     }
    00282   }
    00283
    00284   void UniversalJoint::update(){
    00285     if(visual1 && visual2){
    00286       dVector3 v;
    00287       dJointGetUniversalAnchor(joint, v);
    00288       anchor.x() = v[0];
    00289       anchor.y() = v[1];
    00290       anchor.z() = v[2];
    00291       dJointGetUniversalAxis1(joint, v);
    00292       axis1.x() = v[0];
    00293       axis1.y() = v[1];
    00294       axis1.z() = v[2];
    00295       dJointGetUniversalAxis2(joint, v);
    00296       axis2.x() = v[0];
    00297       axis2.y() = v[1];
    00298       axis2.z() = v[2];
    00299       visual1->setMatrix(anchorAxisPose(anchor, axis1));
    00300       visual2->setMatrix(anchorAxisPose(anchor, axis2));
    00301     }
    00302   }
    00303
    00304   /// adds torques to axis 1 and 2
    00305   void UniversalJoint::addTorques(double t1, double t2){
    00306     dJointAddUniversalTorques(joint, t1,t2);
    00307   }
    00308
    00309   double UniversalJoint::getPosition1(){
    00310     return dJointGetUniversalAngle1(joint);
    00311   }
    00312
    00313   double UniversalJoint::getPosition2(){
    00314     return dJointGetUniversalAngle2(joint);
    00315   }
    00316
    00317   double UniversalJoint::getPosition1Rate(){
    00318     return dJointGetUniversalAngle1Rate(joint);
    00319   }
    00320
    00321   double UniversalJoint::getPosition2Rate(){
    00322     return dJointGetUniversalAngle2Rate(joint);
    00323   }
    00324
    00325   void UniversalJoint::setParam(int parameter, double value) {
    00326     dJointSetUniversalParam(joint, parameter, value);
    00327   }
    00328
    00329   double UniversalJoint::getParam(int parameter){
    00330     return dJointGetUniversalParam(joint, parameter);
    00331   }
    00332
    00333 /***************************************************************************/
    00334
    00335   BallJoint::BallJoint(Primitive* part1, Primitive* part2, const osg::Vec3& anchor)
    00336     : Joint(part1, part2, anchor), visual(0){
    00337
    00338   }
    00339
    00340   BallJoint::~BallJoint() {
    00341     if (visual) delete visual;
    00342   }
    00343
    00344   void BallJoint::init(const OdeHandle& odeHandle, const OsgHandle& osgHandle,
    00345                        bool withVisual, double visualSize){
    00346     joint = dJointCreateBall(odeHandle. world,0);
    00347     dJointAttach (joint, part1->getBody(),part2->getBody());
    00348     dJointSetBallAnchor (joint, anchor.x(), anchor.y(), anchor.z());
    00349     if(withVisual){
    00350       visual = new OSGSphere(visualSize);
    00351       visual->init(osgHandle, OSGPrimitive::Low);
    00352       visual->setMatrix(osg::Matrix::translate(anchor));
    00353     }
    00354   }
    00355
    00356   void BallJoint::update(){
    00357     if(visual){
    00358       dVector3 v;
    00359       dJointGetBallAnchor(joint, v);
    00360       anchor.x() = v[0];
    00361       anchor.y() = v[1];
    00362       anchor.z() = v[2];
    00363       visual->setMatrix(osg::Matrix::translate(anchor));
    00364     }
    00365   }
    00366
    00367   // Ball and Socket has no parameter
    00368   void BallJoint::setParam(int parameter, double value) { }
    00369
    00370   double BallJoint::getParam(int parameter){
    00371     return 0; // Ball and Socket has no parameter
    00372   }
    00373
    00374
    00375 /***************************************************************************/
    00376
    00377   SliderJoint::SliderJoint(Primitive* part1, Primitive* part2, const osg::Vec3& anchor,
    00378                          const Axis& axis1)
    00379     : OneAxisJoint(part1, part2, anchor, axis1), visual(0) {
    00380   }
    00381
    00382   SliderJoint::~SliderJoint(){
    00383     if (visual) delete visual;
    00384   }
    00385
    00386   void SliderJoint::init(const OdeHandle& odeHandle, const OsgHandle& osgHandle,
    00387                          bool withVisual, double visualSize){
    00388     this->osgHandle= osgHandle;
    00389     this->visualSize = visualSize;
    00390
    00391     joint = dJointCreateSlider (odeHandle. world,0);
    00392     dJointAttach (joint, part1->getBody(),part2->getBody());
    00393     osg::Vec3 p1 = part1->getPosition();
    00394     osg::Vec3 p2 = part2->getPosition();
    00395     anchor = (p1+p2)*0.5;
    00396     dJointSetSliderAxis (joint,  axis1.x(), axis1.y(), axis1.z());
    00397     if(withVisual){
    00398       double len = getPosition1();
    00399       visual = new OSGCylinder(visualSize/10, len+visualSize);
    00400       visual->init(osgHandle, OSGPrimitive::Low);
    00401       Matrix t = anchorAxisPose(anchor, axis1);
    00402
    00403       visual->setMatrix(t);
    00404     }
    00405   }
    00406
    00407   void SliderJoint::update(){
    00408     if(visual){
    00409       osg::Vec3 p1 = part1->getPosition();
    00410       osg::Vec3 p2 = part2->getPosition();
    00411       anchor = (p1+p2)*0.5;
    00412       dVector3 v;
    00413       dJointGetSliderAxis(joint, v);
    00414       axis1.x() = v[0];
    00415       axis1.y() = v[1];
    00416       axis1.z() = v[2];
    00417
    00418       double len = getPosition1();
    00419       delete visual;
    00420       visual = new OSGCylinder(visualSize/10, len+visualSize);
    00421       visual->init(osgHandle, OSGPrimitive::Low);
    00422       Matrix t = anchorAxisPose(anchor, axis1);
    00423       visual->setMatrix(t);
    00424     }
    00425   }
    00426
    00427   void SliderJoint::addForce(double t){
    00428     dJointAddSliderForce(joint, t);
    00429   }
    00430
    00431   double SliderJoint::getPosition1(){
    00432     return dJointGetSliderPosition(joint);
    00433   }
    00434
    00435   double SliderJoint::getPosition1Rate(){
    00436     return dJointGetSliderPositionRate(joint);
    00437   }
    00438
    00439   void SliderJoint::setParam(int parameter, double value) {
    00440     dJointSetSliderParam(joint, parameter, value);
    00441   }
    00442
    00443   double SliderJoint::getParam(int parameter){
    00444     return dJointGetSliderParam(joint, parameter);
    00445   }
    00446
    00447
    00448 }