Robot Simulator of the Robotics Group for Self-Organization of Control: joint.h Source File

# joint.h

Go to the documentation of this file.

    00001 /***************************************************************************
    00002  *   Copyright (C) 2005-2011 LpzRobots development team                    *
    00003  *    Georg Martius  <georg dot martius at web dot de>                     *
    00004  *    Frank Guettler <guettler at informatik dot uni-leipzig dot de        *
    00005  *    Frank Hesse    <frank at nld dot ds dot mpg dot de>                  *
    00006  *    Ralf Der       <ralfder at mis dot mpg dot de>                       *
    00007  *                                                                         *
    00008  *   This program is free software; you can redistribute it and/or modify  *
    00009  *   it under the terms of the GNU General Public License as published by  *
    00010  *   the Free Software Foundation; either version 2 of the License, or     *
    00011  *   (at your option) any later version.                                   *
    00012  *                                                                         *
    00013  *   This program is distributed in the hope that it will be useful,       *
    00014  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
    00015  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
    00016  *   GNU General Public License for more details.                          *
    00017  *                                                                         *
    00018  *   You should have received a copy of the GNU General Public License     *
    00019  *   along with this program; if not, write to the                         *
    00020  *   Free Software Foundation, Inc.,                                       *
    00021  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
    00022  *                                                                         *
    00023  ***************************************************************************/
    00024 #ifndef __JOINT_H
    00025 #define __JOINT_H
    00026
    00027 #include <assert.h>
    00028 #include <list>
    00029
    00030 #include "primitive.h"
    00031 #include "osgforwarddecl.h"
    00032 #include "axis.h"
    00033 #include "osghandle.h"
    00034 #include "odehandle.h"
    00035
    00036 namespace lpzrobots {
    00037
    00038
    00039   /***************************************************************************/
    00040
    00041   class Joint {
    00042   public:
    00043     Joint(Primitive* part1, Primitive* part2, const osg::Vec3& anchor)
    00044       : joint(0), part1(part1), part2(part2), anchor(anchor) {
    00045       assert(part1 && part2);
    00046     }
    00047     virtual ~Joint();
    00048     /** initialises (and creates) the joint. If visual is true then the joints is
    00049         also drawn. visualSize is the size of the visual representation.
    00050         If ignoreColl is true then the pair of connected parts is ignored
    00051         at collision handling.
    00052         The member variable odeHandle is set to the given handle.
    00053         (To be overloaded, but this init should be called always from children!)
    00054     */
    00055     virtual void init(const OdeHandle& odeHandle, const OsgHandle& osgHandle,
    00056                       bool withVisual = true, double visualSize = 0.2,
    00057                       bool ignoreColl = true);
    00058
    00059     /// should syncronise the Ode stuff and the OSG notes (if any)
    00060     virtual void update() = 0;
    00061     /// sets the ODE joint parameter (see ODE manual)
    00062     virtual void setParam(int parameter, double value) = 0;
    00063     /// return the ODE joint parameter (see ODE manual)
    00064     virtual double getParam(int parameter) const = 0;
    00065
    00066     dJointID getJoint() const  { return joint; }
    00067     const Primitive* getPart1() const { return part1; }
    00068     Primitive* getPart1() { return part1; }
    00069     const Primitive* getPart2() const { return part2; }
    00070     Primitive* getPart2() { return part2; }
    00071     const osg::Vec3 getAnchor() const { return anchor; }
    00072
    00073     /// returns the number of Axes
    00074     virtual int getNumberAxes() const = 0;
    00075     /// returns the positions of all Axes
    00076     virtual std::list<double> getPositions() const { return std::list<double>(); }
    00077     /// returns the position rates of all Axes
    00078     virtual std::list<double> getPositionRates() const { return std::list<double>(); }
    00079     /// stores the positions of all Axes into sensorarray and returns the number of written entries
    00080     virtual int getPositions(double* sensorarray) const { return 0; }
    00081     /** stores the position rates of all Axes into sensorarray and
    00082         returns the number of written entries
    00083      */
    00084     virtual int getPositionRates(double* sensorarray) const { return 0; }
    00085
    00086     static osg::Matrix anchorAxisPose(const osg::Vec3& anchor, const Axis& axis);
    00087   protected:
    00088     dJointID joint;
    00089     Primitive* part1;
    00090     Primitive* part2;
    00091     osg::Vec3 anchor;
    00092   public:
    00093     OdeHandle odeHandle;
    00094   };
    00095
    00096   class OneAxisJoint : public Joint {
    00097   public:
    00098     OneAxisJoint(Primitive* part1, Primitive* part2, const osg::Vec3& anchor, const Axis axis1)
    00099       : Joint(part1, part2, anchor), axis1(axis1) {}
    00100     virtual Axis getAxis1() const { return axis1; };
    00101
    00102     virtual double getPosition1() const = 0;
    00103     virtual double getPosition1Rate() const = 0;
    00104     virtual void addForce1(double force) = 0;
    00105
    00106     virtual int getNumberAxes() const { return 1;};
    00107     virtual std::list<double> getPositions() const;
    00108     virtual std::list<double> getPositionRates() const;
    00109     virtual int getPositions(double* sensorarray) const;
    00110     virtual int getPositionRates(double* sensorarray) const;
    00111   protected:
    00112     Axis axis1;
    00113   };
    00114
    00115   class TwoAxisJoint : public OneAxisJoint {
    00116   public:
    00117     TwoAxisJoint(Primitive* part1, Primitive* part2, const osg::Vec3& anchor, const Axis axis1,
    00118                  const Axis axis2 )
    00119       : OneAxisJoint(part1, part2, anchor, axis1), axis2(axis2) {}
    00120     virtual Axis getAxis2() const { return axis2; };
    00121
    00122     virtual double getPosition2() const = 0;
    00123     virtual double getPosition2Rate() const = 0;
    00124     virtual void addForce2(double force) = 0;
    00125     void addForces(double force1,double force2){
    00126       addForce1(force1); addForce2(force2);
    00127     }
    00128
    00129     virtual int getNumberAxes() const { return 2;};
    00130     virtual std::list<double> getPositions() const;
    00131     virtual std::list<double> getPositionRates() const;
    00132     virtual int getPositions(double* sensorarray) const;
    00133     virtual int getPositionRates(double* sensorarray) const;
    00134
    00135   protected:
    00136     Axis  axis2;
    00137   };
    00138
    00139   /***************************************************************************/
    00140
    00141   class FixedJoint : public Joint {
    00142   public:
    00143     /** the anchor is only required for drawing.
    00144         If not provided than part1 position is used
    00145     */
    00146     FixedJoint(Primitive* part1, Primitive* part2,
    00147                const osg::Vec3& anchor = osg::Vec3(0,0,0));
    00148
    00149     virtual ~FixedJoint();
    00150
    00151     /** initialises (and creates) the joint.
    00152     */
    00153     virtual void init(const OdeHandle& odeHandle, const OsgHandle& osgHandle,
    00154                       bool withVisual = true, double visualSize = 0.2,
    00155                       bool ignoreColl = true);
    00156
    00157     virtual void update();
    00158     virtual void setParam(int parameter, double value);
    00159     virtual double getParam(int parameter) const;
    00160
    00161     virtual int getNumberAxes() const { return 0; }
    00162   protected:
    00163     OSGPrimitive* visual;
    00164   };
    00165
    00166
    00167   /***************************************************************************/
    00168
    00169   class HingeJoint : public OneAxisJoint {
    00170   public:
    00171     HingeJoint(Primitive* part1, Primitive* part2, const osg::Vec3& anchor,
    00172                 const Axis& axis1);
    00173
    00174     virtual ~HingeJoint();
    00175
    00176     /** initialises (and creates) the joint. If visual is true then the axis of the joints is
    00177         also drawn as a slim cylinder. visualSize is the length of the cylinder.
    00178     */
    00179     virtual void init(const OdeHandle& odeHandle, const OsgHandle& osgHandle,
    00180                       bool withVisual = true, double visualSize = 0.2,
    00181                       bool ignoreColl = true);
    00182
    00183     virtual void update();
    00184
    00185     virtual void addForce1(double t);
    00186     virtual double getPosition1() const;
    00187     virtual double getPosition1Rate() const;
    00188     virtual void setParam(int parameter, double value);
    00189     virtual double getParam(int parameter) const;
    00190
    00191   protected:
    00192     OSGPrimitive* visual;
    00193   };
    00194
    00195   /***************************************************************************/
    00196
    00197   class Hinge2Joint : public TwoAxisJoint {
    00198   public:
    00199     Hinge2Joint(Primitive* part1, Primitive* part2, const osg::Vec3& anchor,
    00200                 const Axis& axis1, const Axis& axis2);
    00201
    00202     virtual ~Hinge2Joint();
    00203
    00204     /** initialises (and creates) the joint. If visual is true then axis2 of the joints is
    00205         also drawn as a slim cylinder. visualSize is the length of the cylinder.
    00206     */
    00207     virtual void init(const OdeHandle& odeHandle, const OsgHandle& osgHandle,
    00208                       bool withVisual = true, double visualSize = 0.2,
    00209                       bool ignoreColl = true);
    00210
    00211     virtual void update();
    00212
    00213     /// adds torques to axis 1 and 2
    00214     virtual void addForce1(double t1);
    00215     virtual void addForce2(double t2);
    00216     virtual double getPosition1() const;
    00217     virtual double getPosition2() const; /// This is not supported by the joint!
    00218     virtual double getPosition1Rate() const;
    00219     virtual double getPosition2Rate() const;
    00220     virtual void setParam(int parameter, double value);
    00221     virtual double getParam(int parameter) const;
    00222
    00223   protected:
    00224     OSGPrimitive* visual;
    00225   };
    00226
    00227   /***************************************************************************/
    00228
    00229   class UniversalJoint : public TwoAxisJoint {
    00230   public:
    00231     UniversalJoint(Primitive* part1, Primitive* part2, const osg::Vec3& anchor,
    00232                 const Axis& axis1, const Axis& axis2);
    00233
    00234     virtual ~UniversalJoint();
    00235
    00236     /** initialises (and creates) the joint. If visual is true then axix1 and axis2 of the joints is
    00237         also drawn as a slim cylinder. visualSize is the length of the cylinder.
    00238     */
    00239     virtual void init(const OdeHandle& odeHandle, const OsgHandle& osgHandle,
    00240                       bool withVisual = true, double visualSize = 0.2,
    00241                       bool ignoreColl = true);
    00242
    00243     virtual void update();
    00244
    00245     /// adds torques to axis 1 and 2
    00246     virtual void addForce1(double t1);
    00247     virtual void addForce2(double t2);
    00248     virtual double getPosition1() const;
    00249     virtual double getPosition2() const;
    00250     virtual double getPosition1Rate() const;
    00251     virtual double getPosition2Rate() const;
    00252
    00253     virtual void setParam(int parameter, double value);
    00254     virtual double getParam(int parameter) const;
    00255
    00256   protected:
    00257     OSGPrimitive* visual1;
    00258     OSGPrimitive* visual2;
    00259   };
    00260
    00261   /***************************************************************************/
    00262
    00263   class BallJoint : public Joint {
    00264   public:
    00265     BallJoint(Primitive* part1, Primitive* part2, const osg::Vec3& anchor);
    00266
    00267     virtual ~BallJoint();
    00268
    00269     /** initialises (and creates) the joint.
    00270         If visual is true then ball is drawn as a sphere with radius of visualSize.
    00271     */
    00272     virtual void init(const OdeHandle& odeHandle, const OsgHandle& osgHandle,
    00273                       bool withVisual = true, double visualSize = 0.2,
    00274                       bool ignoreColl = true);
    00275
    00276     virtual void update();
    00277
    00278     virtual int getNumberAxes() const { return 0; }
    00279     // Ball and Socket has no parameter
    00280     virtual void setParam(int parameter, double value);
    00281     virtual double getParam(int parameter) const;
    00282
    00283   protected:
    00284     OSGPrimitive* visual;
    00285   };
    00286
    00287   /***************************************************************************/
    00288
    00289   class SliderJoint : public OneAxisJoint {
    00290   public:
    00291     SliderJoint(Primitive* part1, Primitive* part2, const osg::Vec3& anchor,
    00292                 const Axis& axis1);
    00293
    00294     virtual ~SliderJoint();
    00295
    00296     /** initialises (and creates) the joint. If visual is true then the axis of the joints is
    00297         also drawn as a slim cylinder. VisualSize is added to the lenght of the slider and is used
    00298         for the length of the cylinder. The radius is visualSize/10
    00299     */
    00300     virtual void init(const OdeHandle& odeHandle, const OsgHandle& osgHandle,
    00301                       bool withVisual = true, double visualSize = 0.1,
    00302                       bool ignoreColl = true);
    00303
    00304     virtual void update();
    00305
    00306     virtual void addForce1(double t);
    00307     virtual double getPosition1() const;
    00308     virtual double getPosition1Rate() const;
    00309     virtual void setParam(int parameter, double value);
    00310     virtual double getParam(int parameter) const;
    00311
    00312   protected:
    00313     OSGPrimitive* visual;
    00314     double visualSize;
    00315     OsgHandle osgHandle;
    00316   };
    00317
    00318   /***************************************************************************/
    00319
    00320
    00321
    00322 }
    00323
    00324 #endif