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