kinecalc.h
Go to the documentation of this file.
1 /*
2  * P2OS for ROS
3  * Copyright (C) 2009
4  * David Feil-Seifer, Brian Gerkey, Kasper Stoy,
5  * Richard Vaughan, & Andrew Howard
6  *
7  *
8  * This program is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2 of the License, or
11  * (at your option) any later version.
12  *
13  * This program is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with this program; if not, write to the Free Software
20  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
21  *
22  */
23 
24 
25 #include <stdio.h>
26 
27 // Basic vector
28 typedef struct
29 {
30  double x, y, z;
31 } KineVector;
32 
33 // Struct that describes the pose of the end effector
34 // (result of FK)
35 typedef struct
36 {
41 } EndEffector;
42 
43 class KineCalc
44 {
45  public:
46  KineCalc (void);
47 
48  // Kinematics functions
49  void CalculateFK (const double fromJoints[]);
50  bool CalculateIK (const EndEffector &fromPosition);
51 
52  // Accessor functions
53  const KineVector& GetP (void) const { return endEffector.p; }
54  const KineVector& GetN (void) const { return endEffector.n; }
55  const KineVector& GetO (void) const { return endEffector.o; }
56  const KineVector& GetA (void) const { return endEffector.a; }
57  void SetP (const KineVector &newP) { endEffector.p.x = newP.x; endEffector.p.y = newP.y; endEffector.p.z = newP.z; }
58  void SetN (const KineVector &newN) { endEffector.n.x = newN.x; endEffector.n.y = newN.y; endEffector.n.z = newN.z; }
59  void SetO (const KineVector &newO) { endEffector.o.x = newO.x; endEffector.o.y = newO.y; endEffector.o.z = newO.z; }
60  void SetA (const KineVector &newA) { endEffector.a.x = newA.x; endEffector.a.y = newA.y; endEffector.a.z = newA.z; }
61  void SetP (double newPX, double newPY, double newPZ);
62  void SetN (double newNX, double newNY, double newNZ);
63  void SetO (double newOX, double newOY, double newOZ);
64  void SetA (double newAX, double newAY, double newAZ);
65 
66  double GetTheta (unsigned int index);
67  const double* GetThetas (void) const { return joints; }
68  void SetTheta (unsigned int index, double newVal);
69  void SetLinkLengths (double newLink1, double newLink2, double newLink3, double newLink4, double newLink5);
70  void SetOffset (unsigned int joint, double newOffset);
71  void SetJointRange (unsigned int joint, double min, double max);
72 
73  // Use this to calculate N
74  KineVector CalculateN (const EndEffector &pose);
75  KineVector Normalise (const KineVector &vector);
76 
77  protected:
78  void CalcTheta4and5 (double angles[], const EndEffector &fromPosition);
79  int ChooseSolution (const EndEffector &fromPosition, const double solutions[][5]);
80  double CalcSolutionError (const double solution[], const EndEffector &fromPosition);
81  EndEffector CalcFKForJoints (const double angles[]);
82  bool SolutionInRange (const double angles[]);
83 
85 
86  // The 4 vectors that describe the position and orientation of the
87  // end effector.
88  // These should be computed when performing forward kinematics
89  // and are used to provide data to the client.
91 
92  // The 5 joint angles.
93  // These are computed when performing inverse kinematics and
94  // are used for positioning the arm.
95  double joints[5];
96  // Joint offsets, as calibrated and supplied in the config file
97  double jointOffsets[5];
98  // Joint ranges
99  double jointMin[5];
100  double jointMax[5];
101 
102  // The link lengths are required for any of the kinematics to be useful.
103  // I can't see them changing, but just in case better to have the ability to
104  // specify them in the config file.
106 };
double jointOffsets[5]
Definition: kinecalc.h:97
const KineVector & GetO(void) const
Definition: kinecalc.h:55
double CalcSolutionError(const double solution[], const EndEffector &fromPosition)
Definition: kinecalc.cc:442
void CalculateFK(const double fromJoints[])
Definition: kinecalc.cc:189
double x
Definition: kinecalc.h:30
double joints[5]
Definition: kinecalc.h:95
const KineVector & GetP(void) const
Definition: kinecalc.h:53
bool CalculateIK(const EndEffector &fromPosition)
Definition: kinecalc.cc:208
const double * GetThetas(void) const
Definition: kinecalc.h:67
void SetO(const KineVector &newO)
Definition: kinecalc.h:59
bool SolutionInRange(const double angles[])
Definition: kinecalc.cc:501
double link1
Definition: kinecalc.h:105
void SetJointRange(unsigned int joint, double min, double max)
Definition: kinecalc.cc:110
void SetOffset(unsigned int joint, double newOffset)
Definition: kinecalc.cc:105
double link4
Definition: kinecalc.h:105
void SetLinkLengths(double newLink1, double newLink2, double newLink3, double newLink4, double newLink5)
Definition: kinecalc.cc:96
double link2
Definition: kinecalc.h:105
void SetTheta(unsigned int index, double newVal)
Definition: kinecalc.cc:91
double jointMax[5]
Definition: kinecalc.h:100
KineVector p
Definition: kinecalc.h:37
void SetN(const KineVector &newN)
Definition: kinecalc.h:58
double GetTheta(unsigned int index)
Definition: kinecalc.cc:86
KineVector n
Definition: kinecalc.h:38
KineVector o
Definition: kinecalc.h:39
KineVector CalculateN(const EndEffector &pose)
Definition: kinecalc.cc:142
double link5
Definition: kinecalc.h:105
const KineVector & GetA(void) const
Definition: kinecalc.h:56
void SetP(const KineVector &newP)
Definition: kinecalc.h:57
double y
Definition: kinecalc.h:30
void PrintEndEffector(const EndEffector &endEffector)
Definition: kinecalc.cc:172
EndEffector endEffector
Definition: kinecalc.h:90
double jointMin[5]
Definition: kinecalc.h:99
EndEffector CalcFKForJoints(const double angles[])
Definition: kinecalc.cc:463
double z
Definition: kinecalc.h:30
void CalcTheta4and5(double angles[], const EndEffector &fromPosition)
Definition: kinecalc.cc:356
int ChooseSolution(const EndEffector &fromPosition, const double solutions[][5])
Definition: kinecalc.cc:409
const KineVector & GetN(void) const
Definition: kinecalc.h:54
KineVector a
Definition: kinecalc.h:40
void SetA(const KineVector &newA)
Definition: kinecalc.h:60
double link3
Definition: kinecalc.h:105
KineCalc(void)
Definition: kinecalc.cc:31
KineVector Normalise(const KineVector &vector)
Definition: kinecalc.cc:122


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 Wed Jun 25 2014 09:37:15