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
|
/********************************************************************
* Description: 5axiskins.c
* Trivial kinematics for 3 axis Cartesian machine
*
* Derived from a work by Fred Proctor & Will Shackleford
*
* Author:
* License: GPL Version 2
* System: Linux
*
* Copyright (c) 2007 Chris Radek
*
* Last change:
********************************************************************/
#include "kinematics.h" /* these decls */
#include "posemath.h"
#include "hal.h"
#include "rtapi_math.h"
#define d2r(d) ((d)*PM_PI/180.0)
#define r2d(r) ((r)*180.0/PM_PI)
struct haldata {
hal_float_t *pivot_length;
} *haldata;
static PmCartesian s2r(double r, double t, double p) {
PmCartesian c;
t = d2r(t), p = d2r(p);
c.x = r * sin(p) * cos(t);
c.y = r * sin(p) * sin(t);
c.z = r * cos(p);
return c;
}
int kinematicsForward(const double *joints,
EmcPose * pos,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags)
{
PmCartesian r = s2r(*(haldata->pivot_length) + joints[8], joints[5], 180.0 - joints[4]);
pos->tran.x = joints[0] + r.x;
pos->tran.y = joints[1] + r.y;
pos->tran.z = joints[2] + *(haldata->pivot_length) + r.z;
pos->a = joints[3];
pos->b = joints[4];
pos->c = joints[5];
pos->u = joints[6];
pos->v = joints[7];
pos->w = joints[8];
return 0;
}
int kinematicsInverse(const EmcPose * pos,
double *joints,
const KINEMATICS_INVERSE_FLAGS * iflags,
KINEMATICS_FORWARD_FLAGS * fflags)
{
PmCartesian r = s2r(*(haldata->pivot_length) + pos->w, pos->c, 180.0 - pos->b);
joints[0] = pos->tran.x - r.x;
joints[1] = pos->tran.y - r.y;
joints[2] = pos->tran.z - *(haldata->pivot_length) - r.z;
joints[3] = pos->a;
joints[4] = pos->b;
joints[5] = pos->c;
joints[6] = pos->u;
joints[7] = pos->v;
joints[8] = pos->w;
return 0;
}
/* implemented for these kinematics as giving joints preference */
int kinematicsHome(EmcPose * world,
double *joint,
KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags)
{
*fflags = 0;
*iflags = 0;
return kinematicsForward(joint, world, fflags, iflags);
}
KINEMATICS_TYPE kinematicsType()
{
return KINEMATICS_BOTH;
}
#include "rtapi.h" /* RTAPI realtime OS API */
#include "rtapi_app.h" /* RTAPI realtime module decls */
#include "hal.h"
EXPORT_SYMBOL(kinematicsType);
EXPORT_SYMBOL(kinematicsForward);
EXPORT_SYMBOL(kinematicsInverse);
MODULE_LICENSE("GPL");
int comp_id;
int rtapi_app_main(void) {
int result;
comp_id = hal_init("5axiskins");
if(comp_id < 0) return comp_id;
haldata = hal_malloc(sizeof(struct haldata));
result = hal_pin_float_new("5axiskins.pivot-length", HAL_IO, &(haldata->pivot_length), comp_id);
if(result < 0) goto error;
*(haldata->pivot_length) = 250.0;
hal_ready(comp_id);
return 0;
error:
hal_exit(comp_id);
return result;
}
void rtapi_app_exit(void) { hal_exit(comp_id); }
|
