-
Notifications
You must be signed in to change notification settings - Fork 110
/
model_json.go
150 lines (128 loc) · 4.11 KB
/
model_json.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
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
package referenceframe
import (
"encoding/json"
"os"
"github.com/pkg/errors"
)
// ModelConfig represents all supported fields in a kinematics JSON file.
type ModelConfig struct {
Name string `json:"name"`
KinParamType string `json:"kinematic_param_type,omitempty"`
Links []LinkConfig `json:"links,omitempty"`
Joints []JointConfig `json:"joints,omitempty"`
DHParams []DHParamConfig `json:"dhParams,omitempty"`
}
// ParseConfig converts the ModelConfig struct into a full Model with the name modelName.
func (cfg *ModelConfig) ParseConfig(modelName string) (Model, error) {
var err error
if modelName == "" {
modelName = cfg.Name
}
model := NewSimpleModel(modelName)
model.modelConfig = cfg
transforms := map[string]Frame{}
// Make a map of parents for each element for post-process, to allow items to be processed out of order
parentMap := map[string]string{}
switch cfg.KinParamType {
case "SVA", "":
for _, link := range cfg.Links {
if link.ID == World {
return nil, errors.New("reserved word: cannot name a link 'world'")
}
}
for _, joint := range cfg.Joints {
if joint.ID == World {
return nil, errors.New("reserved word: cannot name a joint 'world'")
}
}
for _, link := range cfg.Links {
lif, err := link.ParseConfig()
if err != nil {
return nil, err
}
parentMap[link.ID] = link.Parent
transforms[link.ID], err = lif.ToStaticFrame(link.ID)
if err != nil {
return nil, err
}
}
// Now we add all of the transforms. Will eventually support: "cylindrical|fixed|helical|prismatic|revolute|spherical"
for _, joint := range cfg.Joints {
parentMap[joint.ID] = joint.Parent
transforms[joint.ID], err = joint.ToFrame()
if err != nil {
return nil, err
}
}
case "DH":
for _, dh := range cfg.DHParams {
rFrame, lFrame, err := dh.ToDHFrames()
if err != nil {
return nil, err
}
// Joint part of DH param
jointID := dh.ID + "_j"
parentMap[jointID] = dh.Parent
transforms[jointID] = rFrame
// Link part of DH param
linkID := dh.ID
parentMap[linkID] = jointID
transforms[dh.ID] = lFrame
}
default:
return nil, errors.Errorf("unsupported param type: %s, supported params are SVA and DH", cfg.KinParamType)
}
// Determine which transforms have no children
parents := map[string]Frame{}
// First create a copy of the map
for id, trans := range transforms {
parents[id] = trans
}
// Now remove all parents
for _, trans := range transforms {
delete(parents, parentMap[trans.Name()])
}
if len(parents) > 1 {
return nil, errors.New("more than one end effector not supported")
}
if len(parents) < 1 {
return nil, errors.New("need at least one end effector")
}
var eename string
// TODO(pl): is there a better way to do all this? Annoying to iterate over a map three times. Maybe if we
// implement Child() as well as Parent()?
for id := range parents {
eename = id
}
// Create an ordered list of transforms
model.OrdTransforms, err = sortTransforms(transforms, parentMap, eename, World)
if err != nil {
return nil, err
}
return model, nil
}
// ParseModelJSONFile will read a given file and then parse the contained JSON data.
func ParseModelJSONFile(filename, modelName string) (Model, error) {
//nolint:gosec
jsonData, err := os.ReadFile(filename)
if err != nil {
return nil, errors.Wrap(err, "failed to read json file")
}
return UnmarshalModelJSON(jsonData, modelName)
}
// ErrNoModelInformation is used when there is no model information.
var ErrNoModelInformation = errors.New("no model information")
// UnmarshalModelJSON will parse the given JSON data into a kinematics model. modelName sets the name of the model,
// will use the name from the JSON if string is empty.
func UnmarshalModelJSON(jsonData []byte, modelName string) (Model, error) {
m := &ModelConfig{}
// empty data probably means that the robot component has no model information
if len(jsonData) == 0 {
return nil, ErrNoModelInformation
}
err := json.Unmarshal(jsonData, m)
if err != nil {
return nil, errors.Wrap(err, "failed to unmarshal json file")
}
return m.ParseConfig(modelName)
}