/
kinematics.h
215 lines (180 loc) · 9.47 KB
/
kinematics.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
/********************************************************************
* Description: kinematics.h
*
* Derived from a work by Fred Proctor & Will Shackleford
*
* Author:
* License: GPL Version 2
* System: Linux
*
* Copyright (c) 2004 All rights reserved.
*
* Last change:
********************************************************************/
#ifndef KINEMATICS_H
#define KINEMATICS_H
#include "emcpos.h" /* EmcPose */
#include "rtapi_bool.h"
/*
The type of kinematics used.
KINEMATICS_IDENTITY means that the joints and world coordinates are the
same, as for slideway machines (XYZ milling machines). The EMC will allow
changing from joint to world mode and vice versa. Also, the EMC will set
the actual world position to be the actual joint positions (not commanded)
by calling the forward kinematics each trajectory cycle.
KINEMATICS_FORWARD_ONLY means that only the forward kinematics exist.
Since the EMC requires at least the inverse kinematics, this should simply
terminate the EMC.
KINEMATICS_INVERSE_ONLY means that only the inverse kinematics exist.
The forwards won't be called, and the EMC will only allow changing from
joint to world mode at the home position.
KINEMATICS_BOTH means that both the forward and inverse kins are defined.
Like KINEMATICS_IDENTITY, the EMC will allow changing between world and
joint modes. However, the kins are assumed to be somewhat expensive
computationally, and the forwards won't be called at the trajectory rate
to compute actual world coordinates from actual joint values.
*/
typedef enum {
KINEMATICS_IDENTITY = 1,/* forward=inverse, both well-behaved */
KINEMATICS_FORWARD_ONLY,/* forward but no inverse */
KINEMATICS_INVERSE_ONLY,/* inverse but no forward */
KINEMATICS_BOTH /* forward and inverse both */
} KINEMATICS_TYPE;
/* the forward flags are passed to the forward kinematics so that they
can resolve ambiguities in the world coordinates for a given joint set,
e.g., for hexpods, this would be platform-below-base, platform-above-base.
The flags are also passed to the inverse kinematics and are set by them,
which is how they are changed from their initial value. For example, for
hexapods you could do a coordinated move that brings the platform up from
below the base to above the base. The forward flags would be set to
indicate this. */
typedef unsigned long int KINEMATICS_FORWARD_FLAGS;
/* the inverse flags are passed to the inverse kinematics so that they
can resolve ambiguities in the joint angles for a given world coordinate,
e.g., for robots, this would be elbow-up, elbow-down, etc.
The flags are also passed to the forward kinematics and are set by them,
which is how they are changed from their initial value. For example, for
robots you could do a joint move that brings the elbow from a down
configuration to an up configuration. The inverse flags would be set to
indicate this. */
typedef unsigned long int KINEMATICS_INVERSE_FLAGS;
/* the forward kinematics take joint values and determine world coordinates,
given forward kinematics flags to resolve any ambiguities. The inverse
flags are set to indicate their value appropriate to the joint values
passed in. */
extern int kinematicsForward(const double *joint,
struct EmcPose * world,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags);
/* the inverse kinematics take world coordinates and determine joint values,
given the inverse kinematics flags to resolve any ambiguities. The forward
flags are set to indicate their value appropriate to the world coordinates
passed in. */
extern int kinematicsInverse(const struct EmcPose * world,
double *joint,
const KINEMATICS_INVERSE_FLAGS * iflags,
KINEMATICS_FORWARD_FLAGS * fflags);
/* the home kinematics function sets all its arguments to their proper
values at the known home position. When called, these should be set,
when known, to initial values, e.g., from an INI file. If the home
kinematics can accept arbitrary starting points, these initial values
should be used.
*/
extern int kinematicsHome(struct EmcPose * world,
double *joint,
KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags);
extern KINEMATICS_TYPE kinematicsType(void);
/* parameters for use with switchkins.c */
typedef struct kinematics_parms {
char* sparm; // module string parameter passed to kins
char* kinsname; // must agree with module(file) name
char* halprefix; // for hal pin hames
char* required_coordinates;
int max_joints;
int allow_duplicates;
int fwd_iterates_mask; // identify kins types that use iterative
// forward kinematics (typ: genhex)
// bitmask: 0x0 none
// bitmask: 0x1 bit0: switchkins_type==0
// bitmask: 0x2 bit1: switchkins_type==1
// bitmask: 0x4 bit2: switchkins_type==2
int gui_kinstype; // may be reqd for parallel kins with vismach
// to select switchkins_type for gui pins
} kparms;
/* map letters in a coordinates string to joint numbers
** sequentially. Axis indices are 0:x,1:y,...,etc
** Example: coordinates=XYZYAC
** Result: axis_idx_for_jno[0] = 0 ==> X
** axis_idx_for_jno[1] = 1 ==> Y
** axis_idx_for_jno[2] = 2 ==> Z
** axis_idx_for_jno[3] = 1 ==> Y (duplicate allowed)
** axis_idx_for_jno[4] = 1 ==> A
** axis_idx_for_jno[5] = 1 ==> C
*/
extern int map_coordinates_to_jnumbers(const char *coordinates,
const int max_joints,
const int allow_duplicates,
int axis_idx_for_jno[]);
extern int mapped_joints_to_position(const int max_joints,
const double* joints,
EmcPose* pose);
extern int position_to_mapped_joints(const int max_joints,
const EmcPose* pos,
double* joints);
extern int identityKinematicsSetup(const int comp_id,
const char* coordinates,
kparms* ksetup_parms);
extern int identityKinematicsForward(const double *joint,
struct EmcPose * world,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags);
extern int identityKinematicsInverse(const struct EmcPose * world,
double *joint,
const KINEMATICS_INVERSE_FLAGS * iflags,
KINEMATICS_FORWARD_FLAGS * fflags);
extern int kinematicsSwitchable(void);
extern int kinematicsSwitch(int switchkins_type);
//NOTE: switchable kinematics may require Interp::Synch
// before/after invoking kinematicsSwitch()
// A convenient command to synch is: M66 E0 L0
#define KINS_NOT_SWITCHABLE \
extern int kinematicsSwitchable() {return 0;} \
extern int kinematicsSwitch(int switchkins_type) {return 0;} \
EXPORT_SYMBOL(kinematicsSwitchable); \
EXPORT_SYMBOL(kinematicsSwitch);
// support for template for user-defined switchkins_type==2
extern int userkKinematicsSetup(const int comp_id,
const char* coordinates,
kparms* ksetup_parms);
extern int userkKinematicsForward(const double *joint,
struct EmcPose * world,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags);
extern int userkKinematicsInverse(const struct EmcPose * world,
double *joint,
const KINEMATICS_INVERSE_FLAGS * iflags,
KINEMATICS_FORWARD_FLAGS * fflags);
#endif
//*********************************************************************
// xyzac,xyzbc;
extern int trtKinematicsSetup(const int comp_id,
const char* coordinates,
kparms* ksetup_parms);
extern int xyzacKinematicsForward(const double *joints,
EmcPose * pos,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags);
extern int xyzacKinematicsInverse(const EmcPose * pos,
double *joints,
const KINEMATICS_INVERSE_FLAGS * iflags,
KINEMATICS_FORWARD_FLAGS * fflags);
extern int xyzbcKinematicsForward(const double *joints,
EmcPose * pos,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags);
extern int xyzbcKinematicsInverse(const EmcPose * pos,
double *joints,
const KINEMATICS_INVERSE_FLAGS * iflags,
KINEMATICS_FORWARD_FLAGS * fflags);
//*********************************************************************