kinecalc.h
Go to the documentation of this file.
00001 /*
00002  *  P2OS for ROS
00003  *  Copyright (C) 2009
00004  *     David Feil-Seifer, Brian Gerkey, Kasper Stoy, 
00005  *      Richard Vaughan, & Andrew Howard
00006  *
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 Free Software
00020  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00021  *
00022  */
00023 
00024 
00025 #include <stdio.h>
00026 
00027 // Basic vector
00028 typedef struct
00029 {
00030   double x, y, z;
00031 } KineVector;
00032 
00033 // Struct that describes the pose of the end effector
00034 // (result of FK)
00035 typedef struct
00036 {
00037   KineVector p;
00038   KineVector n;
00039   KineVector o;
00040   KineVector a;
00041 } EndEffector;
00042 
00043 class KineCalc
00044 {
00045   public:
00046     KineCalc (void);
00047 
00048     // Kinematics functions
00049     void CalculateFK (const double fromJoints[]);
00050     bool CalculateIK (const EndEffector &fromPosition);
00051 
00052     // Accessor functions
00053     const KineVector& GetP (void) const   { return endEffector.p; }
00054     const KineVector& GetN (void) const   { return endEffector.n; }
00055     const KineVector& GetO (void) const   { return endEffector.o; }
00056     const KineVector& GetA (void) const   { return endEffector.a; }
00057     void SetP (const KineVector &newP)    { endEffector.p.x = newP.x; endEffector.p.y = newP.y; endEffector.p.z = newP.z; }
00058     void SetN (const KineVector &newN)    { endEffector.n.x = newN.x; endEffector.n.y = newN.y; endEffector.n.z = newN.z; }
00059     void SetO (const KineVector &newO)    { endEffector.o.x = newO.x; endEffector.o.y = newO.y; endEffector.o.z = newO.z; }
00060     void SetA (const KineVector &newA)    { endEffector.a.x = newA.x; endEffector.a.y = newA.y; endEffector.a.z = newA.z; }
00061     void SetP (double newPX, double newPY, double newPZ);
00062     void SetN (double newNX, double newNY, double newNZ);
00063     void SetO (double newOX, double newOY, double newOZ);
00064     void SetA (double newAX, double newAY, double newAZ);
00065 
00066     double GetTheta (unsigned int index);
00067     const double* GetThetas (void) const  { return joints; }
00068     void SetTheta (unsigned int index, double newVal);
00069     void SetLinkLengths (double newLink1, double newLink2, double newLink3, double newLink4, double newLink5);
00070     void SetOffset (unsigned int joint, double newOffset);
00071     void SetJointRange (unsigned int joint, double min, double max);
00072 
00073     // Use this to calculate N
00074     KineVector CalculateN (const EndEffector &pose);
00075     KineVector Normalise (const KineVector &vector);
00076 
00077   protected:
00078     void CalcTheta4and5 (double angles[], const EndEffector &fromPosition);
00079     int ChooseSolution (const EndEffector &fromPosition, const double solutions[][5]);
00080     double CalcSolutionError (const double solution[], const EndEffector &fromPosition);
00081     EndEffector CalcFKForJoints (const double angles[]);
00082     bool SolutionInRange (const double angles[]);
00083 
00084     void PrintEndEffector (const EndEffector &endEffector);
00085 
00086     // The 4 vectors that describe the position and orientation of the
00087     // end effector.
00088     // These should be computed when performing forward kinematics
00089     // and are used to provide data to the client.
00090     EndEffector endEffector;
00091 
00092     // The 5 joint angles.
00093     // These are computed when performing inverse kinematics and
00094     // are used for positioning the arm.
00095     double joints[5];
00096     // Joint offsets, as calibrated and supplied in the config file
00097     double jointOffsets[5];
00098     // Joint ranges
00099     double jointMin[5];
00100     double jointMax[5];
00101 
00102     // The link lengths are required for any of the kinematics to be useful.
00103     // I can't see them changing, but just in case better to have the ability to
00104     // specify them in the config file.
00105     double link1, link2, link3, link4, link5;
00106 };


p2os_driver
Author(s): Hunter Allen , David Feil-Seifer , Aris Synodinos , Brian Gerkey, Kasper Stoy, Richard Vaughan, Andrew Howard, Tucker Hermans, ActivMedia Robotics LLC, MobileRobots Inc.
autogenerated on Mon Oct 6 2014 03:12:45