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
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
|
/*****************************************************************
* Description: scarakins.c
* Kinematics for scara typed robots
* Set the params using HAL to fit your robot
*
* Derived from a work by Sagar Behere
*
* Author: Sagar Behere
* License: GPL Version 2
* System: Linux
*
* Copyright (c) 2003 All rights reserved.
*
* Last change:
*******************************************************************
*/
#include "posemath.h"
#include "rtapi_math.h"
#include "kinematics.h" /* decls for kinematicsForward, etc. */
#include "rtapi.h" /* RTAPI realtime OS API */
#include "rtapi_app.h" /* RTAPI realtime module decls */
#include "hal.h"
struct scara_data {
hal_float_t *d1, *d2, *d3, *d4, *d5, *d6;
} *haldata = 0;
/* key dimensions
joint[0] = Entire arm rotates around a vertical axis at its inner end
which is attached to the earth. A value of zero means the
inner arm is pointing along the X axis.
D1 = Vertical distance from the ground plane to the center of the inner
arm.
D2 = Horizontal distance between joint[0] axis and joint[1] axis, ie.
the length of the inner arm.
joint[1] = Outer arm rotates around a vertical axis at its inner end
which is attached to the outer end of the inner arm. A
value of zero means the outer arm is parallel to the
inner arm (and extending outward).
D3 = Vertical distance from the center of the inner arm to the center
of the outer arm. May be positive or negative depending
on the structure of the robot.
joint[2] = End effector slides along a vertical axis at the outer end
of the outer arm. A value of zero means the end effector
is at the same height as the center of the outer arm, and
positive values mean downward movement.
D4 = Horizontal distance between joint[1] axis and joint[2] axis, ie.
the length of the outer arm
joint[3] = End effector rotates around the same vertical axis that it
slides along. A value of zero means that the tooltip (if
offset from the axis) is pointing in the same direction
as the centerline of the outer arm.
D5 = Vertical distance from the end effector to the tooltip. Positive
means the tooltip is lower than the end effector, and is
the normal case.
D6 = Horizontal distance from the centerline of the end effector (and
the joints 2 and 3 axis) and the tooltip. Zero means the
tooltip is on the centerline. Non-zero values should be
positive, if negative they introduce a 180 degree offset
on the value of joint[3].
*/
#define D1 (*(haldata->d1))
#define D2 (*(haldata->d2))
#define D3 (*(haldata->d3))
#define D4 (*(haldata->d4))
#define D5 (*(haldata->d5))
#define D6 (*(haldata->d6))
/* joint[0], joint[1] and joint[3] are in degrees and joint[2] is in length units */
int kinematicsForward(const double * joint,
EmcPose * world,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags)
{
double a0, a1, a3;
double x, y, z, c;
/* convert joint angles to radians for sin() and cos() */
a0 = joint[0] * ( PM_PI / 180 );
a1 = joint[1] * ( PM_PI / 180 );
a3 = joint[3] * ( PM_PI / 180 );
/* convert angles into world coords */
a1 = a1 + a0;
a3 = a3 + a1;
x = D2*cos(a0) + D4*cos(a1) + D6*cos(a3);
y = D2*sin(a0) + D4*sin(a1) + D6*sin(a3);
z = D1 + D3 - joint[2] - D5;
c = a3;
*iflags = 0;
if (joint[1] < 90)
*iflags = 1;
world->tran.x = x;
world->tran.y = y;
world->tran.z = z;
world->c = c * 180 / PM_PI;
world->a = joint[4];
world->b = joint[5];
return (0);
}
int kinematicsInverse(const EmcPose * world,
double * joint,
const KINEMATICS_INVERSE_FLAGS * iflags,
KINEMATICS_FORWARD_FLAGS * fflags)
{
double a3;
double q0, q1;
double xt, yt, rsq, cc;
double x, y, z, c;
x = world->tran.x;
y = world->tran.y;
z = world->tran.z;
c = world->c;
/* convert degrees to radians */
a3 = c * ( PM_PI / 180 );
/* center of end effector (correct for D6) */
xt = x - D6*cos(a3);
yt = y - D6*sin(a3);
/* horizontal distance (squared) from end effector centerline
to main column centerline */
rsq = xt*xt + yt*yt;
/* joint 1 angle needed to make arm length match sqrt(rsq) */
cc = (rsq - D2*D2 - D4*D4) / (2*D2*D4);
if(cc < -1) cc = -1;
if(cc > 1) cc = 1;
q1 = acos(cc);
if (*iflags)
q1 = -q1;
/* angle to end effector */
q0 = atan2(yt, xt);
/* end effector coords in inner arm coord system */
xt = D2 + D4*cos(q1);
yt = D4*sin(q1);
/* inner arm angle */
q0 = q0 - atan2(yt, xt);
/* q0 and q1 are still in radians. convert them to degrees */
q0 = q0 * (180 / PM_PI);
q1 = q1 * (180 / PM_PI);
joint[0] = q0;
joint[1] = q1;
joint[2] = D1 + D3 - D5 - z;
joint[3] = c - ( q0 + q1);
joint[4] = world->a;
joint[5] = world->b;
*fflags = 0;
return (0);
}
int kinematicsHome(EmcPose * world,
double * joint,
KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags)
{
/* use joints, set world */
return kinematicsForward(joint, world, fflags, iflags);
}
KINEMATICS_TYPE kinematicsType()
{
return KINEMATICS_BOTH;
}
#define DEFAULT_D1 490
#define DEFAULT_D2 340
#define DEFAULT_D3 50
#define DEFAULT_D4 250
#define DEFAULT_D5 50
#define DEFAULT_D6 50
EXPORT_SYMBOL(kinematicsType);
EXPORT_SYMBOL(kinematicsForward);
EXPORT_SYMBOL(kinematicsInverse);
EXPORT_SYMBOL(kinematicsHome);
int comp_id;
int rtapi_app_main(void) {
int res=0;
comp_id = hal_init("scarakins");
if (comp_id < 0) return comp_id;
haldata = hal_malloc(sizeof(*haldata));
if (!haldata) goto error;
if((res = hal_pin_float_new("scarakins.D1", HAL_IO, &(haldata->d1), comp_id)) < 0) goto error;
if((res = hal_pin_float_new("scarakins.D2", HAL_IO, &(haldata->d2), comp_id)) < 0) goto error;
if((res = hal_pin_float_new("scarakins.D3", HAL_IO, &(haldata->d3), comp_id)) < 0) goto error;
if((res = hal_pin_float_new("scarakins.D4", HAL_IO, &(haldata->d4), comp_id)) < 0) goto error;
if((res = hal_pin_float_new("scarakins.D5", HAL_IO, &(haldata->d5), comp_id)) < 0) goto error;
if((res = hal_pin_float_new("scarakins.D6", HAL_IO, &(haldata->d6), comp_id)) < 0) goto error;
D1 = DEFAULT_D1;
D2 = DEFAULT_D2;
D3 = DEFAULT_D3;
D4 = DEFAULT_D4;
D5 = DEFAULT_D5;
D6 = DEFAULT_D6;
hal_ready(comp_id);
return 0;
error:
hal_exit(comp_id);
return res;
}
void rtapi_app_exit(void) { hal_exit(comp_id); }
|
