forked from foxis/EasyRobot
-
Notifications
You must be signed in to change notification settings - Fork 0
/
denavithartenberg.go
59 lines (48 loc) · 1.19 KB
/
denavithartenberg.go
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
package dh
import (
"github.com/itohio/EasyRobot/pkg/core/math/mat"
"github.com/itohio/EasyRobot/pkg/core/math/vec"
"github.com/itohio/EasyRobot/pkg/robot/kinematics"
)
type DenavitHartenberg struct {
c []Config
eps float32
maxIterations int
params []float32
pos [7]float32
H0i []mat.Matrix4x4 // TODO replace with Matrix4x4
}
func New(eps float32, maxIterations int, cfg ...Config) kinematics.Kinematics {
return &DenavitHartenberg{
eps: eps,
maxIterations: maxIterations,
c: cfg,
params: make([]float32, len(cfg)),
}
}
func (p *DenavitHartenberg) DOF() int {
return len(p.c)
}
func (p *DenavitHartenberg) Params() vec.Vector {
return p.params[:]
}
func (p *DenavitHartenberg) Effector() vec.Vector {
return p.pos[:]
}
func (p *DenavitHartenberg) Forward() bool {
H := mat.Matrix4x4{}
H.Eye()
p.H0i[0].Eye()
for i, cfg := range p.c {
if cfg.CalculateTransform(p.params[i], &H) {
return false
}
p.H0i[i+1].Mul(p.H0i[i], H)
}
p.H0i[len(p.c)].Col(3, p.pos[:3])
copy(p.pos[3:], p.H0i[len(p.c)].Quaternion().Vector())
return true
}
func (p *DenavitHartenberg) Inverse() bool {
return false
}