Skip to content

Commit 5c10f4e

Browse files
raybjorkraybjork
authored
RSDK-4120 add MoveThroughJointPositions to Arm interface (viamrobotics#4508)
1 parent 45a795a commit 5c10f4e

File tree

14 files changed

+303
-110
lines changed

14 files changed

+303
-110
lines changed

‎components/arm/arm.go

+20-23
Original file line numberDiff line numberDiff line change
@@ -75,17 +75,26 @@ func Named(name string) resource.Name {
7575
//
7676
// MoveToJointPositions example:
7777
//
78-
// // Assumes you have imported "go.viam.com/api/component/arm/v1" as `componentpb`
7978
// myArm, err := arm.FromRobot(machine, "my_arm")
8079
//
81-
// // Declare an array of values with your desired rotational value for each joint on the arm.
82-
// degrees := []float64{4.0, 5.0, 6.0}
80+
// // Declare an array of values with your desired rotational value (in radians) for each joint on the arm.
81+
// inputs := referenceframe.FloatsToInputs([]float64{0, math.Pi/2, math.Pi})
8382
//
84-
// // Declare a new JointPositions with these values.
85-
// jointPos := &componentpb.JointPositions{Values: degrees}
83+
// // Move each joint of the arm to the positions specified in the above slice
84+
// err = myArm.MoveToJointPositions(context.Background(), inputs, nil)
8685
//
87-
// // Move each joint of the arm to the position these values specify.
88-
// err = myArm.MoveToJointPositions(context.Background(), jointPos, nil)
86+
// MoveToJointPositions example:
87+
//
88+
// myArm, err := arm.FromRobot(machine, "my_arm")
89+
//
90+
// // Declare a 2D array of values with your desired rotational value (in radians) for each joint on the arm.
91+
// inputs := [][]referenceframe.Input{
92+
// referenceframe.FloatsToInputs([]float64{0, math.Pi/2, math.Pi})
93+
// referenceframe.FloatsToInputs([]float64{0, 0, 0})
94+
// }
95+
//
96+
// // Move each joint of the arm through the positions in the slice defined above
97+
// err = myArm.MoveThroughJointPositions(context.Background(), inputs, nil, nil)
8998
//
9099
// JointPositions example:
91100
//
@@ -113,6 +122,10 @@ type Arm interface {
113122
// This will block until done or a new operation cancels this one.
114123
MoveToJointPositions(ctx context.Context, positions []referenceframe.Input, extra map[string]interface{}) error
115124

125+
// MoveThroughJointPositions moves the arm's joints through the given positions in the order they are specified.
126+
// This will block until done or a new operation cancels this one.
127+
MoveThroughJointPositions(ctx context.Context, positions [][]referenceframe.Input, options *MoveOptions, extra map[string]any) error
128+
116129
// JointPositions returns the current joint positions of the arm.
117130
JointPositions(ctx context.Context, extra map[string]interface{}) ([]referenceframe.Input, error)
118131
}
@@ -159,22 +172,6 @@ func CreateStatus(ctx context.Context, a Arm) (*pb.Status, error) {
159172
return &pb.Status{EndPosition: endPosition, JointPositions: jointPositions, IsMoving: isMoving}, nil
160173
}
161174

162-
// GoToWaypoints will visit in turn each of the joint position waypoints generated by a motion planner.
163-
func GoToWaypoints(ctx context.Context, a Arm, waypoints [][]referenceframe.Input) error {
164-
for _, waypoint := range waypoints {
165-
err := ctx.Err() // make sure we haven't been cancelled
166-
if err != nil {
167-
return err
168-
}
169-
170-
err = a.GoToInputs(ctx, waypoint)
171-
if err != nil {
172-
return err
173-
}
174-
}
175-
return nil
176-
}
177-
178175
// CheckDesiredJointPositions validates that the desired joint positions either bring the joint back
179176
// in bounds or do not move the joint more out of bounds.
180177
func CheckDesiredJointPositions(ctx context.Context, a Arm, desiredInputs []referenceframe.Input) error {

‎components/arm/client.go

+34-10
Original file line numberDiff line numberDiff line change
@@ -94,9 +94,6 @@ func (c *client) MoveToJointPositions(ctx context.Context, positions []reference
9494
if err != nil {
9595
return err
9696
}
97-
if positions == nil {
98-
c.logger.Warnf("%s MoveToJointPositions: position parameter is nil", c.name)
99-
}
10097
jp, err := referenceframe.JointPositionsFromInputs(c.model, positions)
10198
if err != nil {
10299
return err
@@ -109,6 +106,39 @@ func (c *client) MoveToJointPositions(ctx context.Context, positions []reference
109106
return err
110107
}
111108

109+
func (c *client) MoveThroughJointPositions(
110+
ctx context.Context,
111+
positions [][]referenceframe.Input,
112+
options *MoveOptions,
113+
extra map[string]interface{},
114+
) error {
115+
ext, err := protoutils.StructToStructPb(extra)
116+
if err != nil {
117+
return err
118+
}
119+
if positions == nil {
120+
c.logger.Warnf("%s MoveThroughJointPositions: position argument is nil", c.name)
121+
}
122+
allJPs := make([]*pb.JointPositions, 0, len(positions))
123+
for _, position := range positions {
124+
jp, err := referenceframe.JointPositionsFromInputs(c.model, position)
125+
if err != nil {
126+
return err
127+
}
128+
allJPs = append(allJPs, jp)
129+
}
130+
req := &pb.MoveThroughJointPositionsRequest{
131+
Name: c.name,
132+
Positions: allJPs,
133+
Extra: ext,
134+
}
135+
if options != nil {
136+
req.Options = options.toProtobuf()
137+
}
138+
_, err = c.client.MoveThroughJointPositions(ctx, req)
139+
return err
140+
}
141+
112142
func (c *client) JointPositions(ctx context.Context, extra map[string]interface{}) ([]referenceframe.Input, error) {
113143
ext, err := protoutils.StructToStructPb(extra)
114144
if err != nil {
@@ -145,13 +175,7 @@ func (c *client) CurrentInputs(ctx context.Context) ([]referenceframe.Input, err
145175
}
146176

147177
func (c *client) GoToInputs(ctx context.Context, inputSteps ...[]referenceframe.Input) error {
148-
// TODO: switch this over call MoveThroughJointPositions when that API is ready
149-
for _, goal := range inputSteps {
150-
if err := c.MoveToJointPositions(ctx, goal, nil); err != nil {
151-
return err
152-
}
153-
}
154-
return nil
178+
return c.MoveThroughJointPositions(ctx, inputSteps, nil, nil)
155179
}
156180

157181
func (c *client) DoCommand(ctx context.Context, cmd map[string]interface{}) (map[string]interface{}, error) {

‎components/arm/client_test.go

+28-7
Original file line numberDiff line numberDiff line change
@@ -32,12 +32,14 @@ func TestClient(t *testing.T) {
3232
var (
3333
capArmPos spatialmath.Pose
3434
capArmJointPos []referenceframe.Input
35+
moveOptions arm.MoveOptions
3536
extraOptions map[string]interface{}
3637
)
3738

3839
pos1 := spatialmath.NewPoseFromPoint(r3.Vector{X: 1, Y: 2, Z: 3})
3940
jointPos1 := []referenceframe.Input{{1.}, {2.}, {3.}}
4041
expectedGeometries := []spatialmath.Geometry{spatialmath.NewPoint(r3.Vector{1, 2, 3}, "")}
42+
expectedMoveOptions := arm.MoveOptions{MaxVelRads: 1, MaxAccRads: 2}
4143
injectArm := &inject.Arm{}
4244
injectArm.EndPositionFunc = func(ctx context.Context, extra map[string]interface{}) (spatialmath.Pose, error) {
4345
extraOptions = extra
@@ -52,12 +54,22 @@ func TestClient(t *testing.T) {
5254
extraOptions = extra
5355
return nil
5456
}
55-
5657
injectArm.MoveToJointPositionsFunc = func(ctx context.Context, jp []referenceframe.Input, extra map[string]interface{}) error {
5758
capArmJointPos = jp
5859
extraOptions = extra
5960
return nil
6061
}
62+
injectArm.MoveThroughJointPositionsFunc = func(
63+
ctx context.Context,
64+
positions [][]referenceframe.Input,
65+
options *arm.MoveOptions,
66+
extra map[string]interface{},
67+
) error {
68+
capArmJointPos = positions[len(positions)-1]
69+
moveOptions = *options
70+
extraOptions = extra
71+
return nil
72+
}
6173
injectArm.StopFunc = func(ctx context.Context, extra map[string]interface{}) error {
6274
extraOptions = extra
6375
return errStopUnimplemented
@@ -82,7 +94,6 @@ func TestClient(t *testing.T) {
8294
capArmPos = ap
8395
return nil
8496
}
85-
8697
injectArm2.MoveToJointPositionsFunc = func(ctx context.Context, jp []referenceframe.Input, extra map[string]interface{}) error {
8798
capArmJointPos = jp
8899
return nil
@@ -94,11 +105,10 @@ func TestClient(t *testing.T) {
94105
return nil
95106
}
96107

97-
armSvc, err := resource.NewAPIResourceCollection(
98-
arm.API, map[resource.Name]arm.Arm{
99-
arm.Named(testArmName): injectArm,
100-
arm.Named(testArmName2): injectArm2,
101-
})
108+
armSvc, err := resource.NewAPIResourceCollection(arm.API, map[resource.Name]arm.Arm{
109+
arm.Named(testArmName): injectArm,
110+
arm.Named(testArmName2): injectArm2,
111+
})
102112
test.That(t, err, test.ShouldBeNil)
103113
resourceAPI, ok, err := resource.LookupAPIRegistration[arm.Arm](arm.API)
104114
test.That(t, err, test.ShouldBeNil)
@@ -164,6 +174,17 @@ func TestClient(t *testing.T) {
164174
test.That(t, capArmJointPos, test.ShouldResemble, jointPos2)
165175
test.That(t, extraOptions, test.ShouldResemble, map[string]interface{}{"foo": "MoveToJointPositions"})
166176

177+
err = arm1Client.MoveThroughJointPositions(
178+
context.Background(),
179+
[][]referenceframe.Input{jointPos2, jointPos1},
180+
&expectedMoveOptions,
181+
map[string]interface{}{"foo": "MoveThroughJointPositions"},
182+
)
183+
test.That(t, err, test.ShouldBeNil)
184+
test.That(t, capArmJointPos, test.ShouldResemble, jointPos1)
185+
test.That(t, moveOptions, test.ShouldResemble, expectedMoveOptions)
186+
test.That(t, extraOptions, test.ShouldResemble, map[string]interface{}{"foo": "MoveThroughJointPositions"})
187+
167188
err = arm1Client.Stop(context.Background(), map[string]interface{}{"foo": "Stop"})
168189
test.That(t, err, test.ShouldNotBeNil)
169190
test.That(t, err.Error(), test.ShouldContainSubstring, errStopUnimplemented.Error())

‎components/arm/eva/eva.go

+19-10
Original file line numberDiff line numberDiff line change
@@ -177,6 +177,24 @@ func (e *eva) MoveToJointPositions(ctx context.Context, newPositions []reference
177177
return e.doMoveJoints(ctx, radians)
178178
}
179179

180+
func (e *eva) MoveThroughJointPositions(
181+
ctx context.Context,
182+
positions [][]referenceframe.Input,
183+
_ *arm.MoveOptions,
184+
_ map[string]interface{},
185+
) error {
186+
for _, goal := range positions {
187+
if err := arm.CheckDesiredJointPositions(ctx, e, goal); err != nil {
188+
return err
189+
}
190+
err := e.MoveToJointPositions(ctx, goal, nil)
191+
if err != nil {
192+
return err
193+
}
194+
}
195+
return nil
196+
}
197+
180198
func (e *eva) doMoveJoints(ctx context.Context, joints []float64) error {
181199
if err := e.apiLock(ctx); err != nil {
182200
return err
@@ -384,16 +402,7 @@ func (e *eva) CurrentInputs(ctx context.Context) ([]referenceframe.Input, error)
384402
}
385403

386404
func (e *eva) GoToInputs(ctx context.Context, inputSteps ...[]referenceframe.Input) error {
387-
for _, goal := range inputSteps {
388-
if err := arm.CheckDesiredJointPositions(ctx, e, goal); err != nil {
389-
return err
390-
}
391-
err := e.MoveToJointPositions(ctx, goal, nil)
392-
if err != nil {
393-
return err
394-
}
395-
}
396-
return nil
405+
return e.MoveThroughJointPositions(ctx, inputSteps, nil, nil)
397406
}
398407

399408
func (e *eva) Close(ctx context.Context) error {

‎components/arm/fake/fake.go

+16-6
Original file line numberDiff line numberDiff line change
@@ -188,6 +188,21 @@ func (a *Arm) MoveToJointPositions(ctx context.Context, joints []referenceframe.
188188
return nil
189189
}
190190

191+
// MoveThroughJointPositions moves the fake arm through the given inputs.
192+
func (a *Arm) MoveThroughJointPositions(
193+
ctx context.Context,
194+
positions [][]referenceframe.Input,
195+
_ *arm.MoveOptions,
196+
_ map[string]interface{},
197+
) error {
198+
for _, goal := range positions {
199+
if err := a.MoveToJointPositions(ctx, goal, nil); err != nil {
200+
return err
201+
}
202+
}
203+
return nil
204+
}
205+
191206
// JointPositions returns joints.
192207
func (a *Arm) JointPositions(ctx context.Context, extra map[string]interface{}) ([]referenceframe.Input, error) {
193208
a.mu.RLock()
@@ -214,12 +229,7 @@ func (a *Arm) CurrentInputs(ctx context.Context) ([]referenceframe.Input, error)
214229

215230
// GoToInputs moves the fake arm to the given inputs.
216231
func (a *Arm) GoToInputs(ctx context.Context, inputSteps ...[]referenceframe.Input) error {
217-
for _, goal := range inputSteps {
218-
if err := a.MoveToJointPositions(ctx, goal, nil); err != nil {
219-
return err
220-
}
221-
}
222-
return nil
232+
return a.MoveThroughJointPositions(ctx, inputSteps, nil, nil)
223233
}
224234

225235
// Close does nothing.

‎components/arm/pb_helpers.go

+34
Original file line numberDiff line numberDiff line change
@@ -4,11 +4,45 @@ import (
44
"fmt"
55

66
commonpb "go.viam.com/api/common/v1"
7+
pb "go.viam.com/api/component/arm/v1"
78

89
"go.viam.com/rdk/referenceframe"
910
"go.viam.com/rdk/referenceframe/urdf"
11+
"go.viam.com/rdk/utils"
1012
)
1113

14+
// MoveOptions define parameters to be obeyed during arm movement.
15+
type MoveOptions struct {
16+
MaxVelRads, MaxAccRads float64
17+
}
18+
19+
func moveOptionsFromProtobuf(protobuf *pb.MoveOptions) *MoveOptions {
20+
if protobuf == nil {
21+
protobuf = &pb.MoveOptions{}
22+
}
23+
24+
var vel, acc float64
25+
if protobuf.MaxVelDegsPerSec != nil {
26+
vel = *protobuf.MaxVelDegsPerSec
27+
}
28+
if protobuf.MaxAccDegsPerSec2 != nil {
29+
acc = *protobuf.MaxAccDegsPerSec2
30+
}
31+
return &MoveOptions{
32+
MaxVelRads: utils.DegToRad(vel),
33+
MaxAccRads: utils.DegToRad(acc),
34+
}
35+
}
36+
37+
func (opts *MoveOptions) toProtobuf() *pb.MoveOptions {
38+
vel := utils.RadToDeg(opts.MaxVelRads)
39+
acc := utils.RadToDeg(opts.MaxAccRads)
40+
return &pb.MoveOptions{
41+
MaxVelDegsPerSec: &vel,
42+
MaxAccDegsPerSec2: &acc,
43+
}
44+
}
45+
1246
func parseKinematicsResponse(name string, resp *commonpb.GetKinematicsResponse) (referenceframe.Model, error) {
1347
format := resp.GetFormat()
1448
data := resp.GetKinematicsData()

‎components/arm/server.go

+22
Original file line numberDiff line numberDiff line change
@@ -103,6 +103,28 @@ func (s *serviceServer) MoveToJointPositions(
103103
return &pb.MoveToJointPositionsResponse{}, arm.MoveToJointPositions(ctx, inputs, req.Extra.AsMap())
104104
}
105105

106+
// MoveThroughJointPositions moves an arm of the underlying robot through the requested joint positions.
107+
func (s *serviceServer) MoveThroughJointPositions(
108+
ctx context.Context,
109+
req *pb.MoveThroughJointPositionsRequest,
110+
) (*pb.MoveThroughJointPositionsResponse, error) {
111+
operation.CancelOtherWithLabel(ctx, req.Name)
112+
arm, err := s.coll.Resource(req.Name)
113+
if err != nil {
114+
return nil, err
115+
}
116+
allInputs := make([][]referenceframe.Input, 0, len(req.Positions))
117+
for _, position := range req.Positions {
118+
inputs, err := referenceframe.InputsFromJointPositions(arm.ModelFrame(), position)
119+
if err != nil {
120+
return nil, err
121+
}
122+
allInputs = append(allInputs, inputs)
123+
}
124+
err = arm.MoveThroughJointPositions(ctx, allInputs, moveOptionsFromProtobuf(req.Options), req.Extra.AsMap())
125+
return &pb.MoveThroughJointPositionsResponse{}, err
126+
}
127+
106128
// Stop stops the arm specified.
107129
func (s *serviceServer) Stop(ctx context.Context, req *pb.StopRequest) (*pb.StopResponse, error) {
108130
operation.CancelOtherWithLabel(ctx, req.Name)

0 commit comments

Comments
 (0)