-
Notifications
You must be signed in to change notification settings - Fork 110
/
dofbot.go
130 lines (112 loc) · 3.35 KB
/
dofbot.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
// Package yahboom implements a yahboom based gripper.
// code with commands found at http://www.yahboom.net/study/Dofbot-Pi
package yahboom
import (
"context"
// for embedding model file.
_ "embed"
"fmt"
"github.com/edaniels/golog"
"github.com/pkg/errors"
gutils "go.viam.com/utils"
"go.viam.com/rdk/components/arm"
"go.viam.com/rdk/components/arm/yahboom"
"go.viam.com/rdk/components/gripper"
"go.viam.com/rdk/operation"
"go.viam.com/rdk/referenceframe"
"go.viam.com/rdk/resource"
"go.viam.com/rdk/spatialmath"
)
var model = resource.DefaultModelFamily.WithModel("yahboom-dofbot")
// Config is the config for a dofbot gripper.
type Config struct {
Arm string `json:"arm"`
}
// Validate ensures all parts of the config are valid.
func (conf *Config) Validate(path string) ([]string, error) {
var deps []string
if conf.Arm == "" {
return nil, gutils.NewConfigValidationFieldRequiredError(path, "arm")
}
deps = append(deps, conf.Arm)
return deps, nil
}
func init() {
resource.RegisterComponent(gripper.API, model, resource.Registration[gripper.Gripper, *Config]{
Constructor: func(
ctx context.Context,
deps resource.Dependencies,
conf resource.Config,
logger golog.Logger,
) (gripper.Gripper, error) {
return newGripper(deps, conf)
},
})
}
// newGripper instantiates a new Gripper of dofGripper type.
func newGripper(deps resource.Dependencies, conf resource.Config) (gripper.Gripper, error) {
newConf, err := resource.NativeConfig[*Config](conf)
if err != nil {
return nil, err
}
armName := newConf.Arm
if armName == "" {
return nil, errors.New("yahboom-dofbot gripper needs an arm")
}
myArm, err := arm.FromDependencies(deps, armName)
if err != nil {
return nil, err
}
dofArm, ok := myArm.(*yahboom.Dofbot)
if !ok {
return nil, fmt.Errorf("yahboom-dofbot gripper got not a dofbot arm, got %T", myArm)
}
g := &dofGripper{
Named: conf.ResourceName().AsNamed(),
dofArm: dofArm,
geometries: []spatialmath.Geometry{},
}
if conf.Frame != nil && conf.Frame.Geometry != nil {
geometry, err := conf.Frame.Geometry.ParseConfig()
if err != nil {
return nil, err
}
g.geometries = []spatialmath.Geometry{geometry}
}
return g, nil
}
type dofGripper struct {
resource.Named
resource.AlwaysRebuild
resource.TriviallyCloseable
dofArm *yahboom.Dofbot
opMgr operation.SingleOperationManager
geometries []spatialmath.Geometry
}
func (g *dofGripper) Open(ctx context.Context, extra map[string]interface{}) error {
ctx, done := g.opMgr.New(ctx)
defer done()
return g.dofArm.Open(ctx)
}
func (g *dofGripper) Grab(ctx context.Context, extra map[string]interface{}) (bool, error) {
ctx, done := g.opMgr.New(ctx)
defer done()
return g.dofArm.Grab(ctx)
}
func (g *dofGripper) Stop(ctx context.Context, extra map[string]interface{}) error {
// RSDK-388: Implement Stop for gripper
ctx, done := g.opMgr.New(ctx)
defer done()
return g.dofArm.GripperStop(ctx)
}
// IsMoving returns whether the gripper is moving.
func (g *dofGripper) IsMoving(ctx context.Context) (bool, error) {
return g.opMgr.OpRunning(), nil
}
func (g *dofGripper) ModelFrame() referenceframe.Model {
return g.dofArm.ModelFrame()
}
// Geometries returns the geometries associated with the dofGripper.
func (g *dofGripper) Geometries(ctx context.Context, extra map[string]interface{}) ([]spatialmath.Geometry, error) {
return g.geometries, nil
}