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
|
/********************************************************************
* genserkins.c employing switchkins.[ch]
* License: GPL Version 2
*
* NOTEs:
* 1) specify all kparms items
* 2) specify 3 KS,KF,KI functions (setup,forward,inverse)
*/
/********************************************************************
TEST: switchable kinematics: identity or genserkins
1) genser kinematics provided by genserfuncs.c (shared with genserkins.c
2) uses same pin names as genserkins
3) tesing mm configs: increase GO_REAL_EPSILON from 1e-7 to 1e-6
NOTE:
a) requires *exactly* 6 joints
b) identity assignments can use any of xyzabcuvw
but should agree with [TRAJ]COORDINATES
and may be confusing
www refs:
frame-larger-than:
https://www.mail-archive.com/emc-developers@lists.sourceforge.net/msg03790.html
angles:
https://www.mail-archive.com/emc-developers@lists.sourceforge.net/msg15285.html
*/
//----------------------------------------------------------------------
// genserKinematicsInverse() is 5104 with buster amd64 gcc 8.3.0-6
//#pragma GCC diagnostic error "-Wframe-larger-than=6000"
#pragma GCC diagnostic warning "-Wframe-larger-than=6000"
#include "rtapi.h"
#include <rtapi_string.h>
#include "genserkins.h"
#include "motion.h"
#include "switchkins.h"
//-7 is system defined -3 ok, -4 ok, -5 ok,-6 ok (mm system)
#undef GO_REAL_EPSILON
#define GO_REAL_EPSILON (1e-6)
//*********************************************************************
int switchkinsSetup(kparms* kp,
KS* kset0, KS* kset1, KS* kset2,
KF* kfwd0, KF* kfwd1, KF* kfwd2,
KI* kinv0, KI* kinv1, KI* kinv2
)
{
kp->kinsname = "genserkins"; // !!! must agree with filename
kp->halprefix = "genserkins"; // hal pin names
kp->required_coordinates = "xyzabcuvw"; // u,v,w are joints 6,7,8
kp->max_joints = strlen(kp->required_coordinates);
kp->allow_duplicates = 0;
*kset0 = genserKinematicsSetup;
*kfwd0 = genserKinematicsForward;
*kinv0 = genserKinematicsInverse;
*kset1 = identityKinematicsSetup;
*kfwd1 = identityKinematicsForward;
*kinv1 = identityKinematicsInverse;
*kset2 = userkKinematicsSetup;
*kfwd2 = userkKinematicsForward;
*kinv2 = userkKinematicsInverse;
return 0;
}
|
