RSDK-1653 Fix arm behavior when OOB #1822
Conversation
viambot
added
the
safe to test
viambot
added
safe to test
|
Can you add some details about how you tested this change? For example, #1788 (comment) |
components/arm/arm.go
Outdated
| r.mu.RLock() | ||
| defer r.mu.RUnlock() | ||
| return r.actual.MoveToJointPositions(ctx, positionDegs, extra) | ||
| } | ||
|
|
||
| // checkDesiredJointPositions validates that the desired joint positions either bring the joint back | ||
| // in bounds or do not move the joint more out of bounds. | ||
| func (r *reconfigurableArm) checkDesiredJointPositions(ctx context.Context, desiredJoints []float64) error { |
There was a problem hiding this comment.
[major] In general we don't ever want to be modifying the reconfigurable arm itself, and this method along with the other functionality introduced should work on the generic arm.Arm type
There was a problem hiding this comment.
Agreed. Nick, take a look at the Move() and Plan() functions at the end of arm.go; there are arm-generic Plan and Move functions there, that each individual arm driver calls for MoveToPosition. We probably want something similar to that for joint movement.
viambot
added
viam-org
motionplan/kinematic_test.go
Outdated
| @@ -88,8 +88,11 @@ func TestForwardKinematics(t *testing.T) { | |||
| // Test out of bounds. Note that ComputePosition will return nil on OOB. | |||
| newPos = []float64{-45, 0, 0, 0, 0, 999} | |||
| pos, err = ComputePosition(m, &pb.JointPositions{Values: newPos}) | |||
| test.That(t, pos, test.ShouldBeNil) | |||
| test.That(t, err, test.ShouldNotBeNil) | |||
| checkOrien := &spatial.R4AA{Theta: -2.48798057005674, RX: 0.23071941493324336, RY: -0.7100813450467474, RZ: 0.6652465971273088} | |||
There was a problem hiding this comment.
[nit] Seems like this could more closely match the form of what is taking place on L79-86.
| @@ -24,7 +25,7 @@ func ComputePosition(model referenceframe.Frame, joints *pb.JointPositions) (spa | |||
| } | |||
|
|
|||
| pose, err := model.Transform(model.InputFromProtobuf(joints)) | |||
| if err != nil { | |||
| if err != nil && !strings.Contains(err.Error(), referenceframe.OOBErrString) { | |||
There was a problem hiding this comment.
[major, q] So ComputePosition has been modified to NOT return nil if we have an OOB error. Is this what we want to be doing? It feels wrong that we could be computing a position with known bad inputs and returning that pose. There may also be some value in continuing with this change, but I would like us to think about the implications here.
There was a problem hiding this comment.
Deleted the last comment because I realized that it was not right. You are correct good callout, we should probably be floating the OOBError higher in the stack because now its being squashed in frame.go
There was a problem hiding this comment.
Agreed; this should live elsewhere.
The current defined behavior for Transform on a Frame is that if OOB, it will return an Error, and the OOB pose.
However, this is not a method on a Frame, but a public method. We shouldn't be changing the behavior here, since it's public. We should re-examine whether we want to use a public method at the point in question, and if so, we should create a new one that explicitly will return OOB positions.
There was a problem hiding this comment.
part of the ticket description states:
This should be updated such that if an arm is currently in an out-of-bounds state, it is still able to report its present state
When looking that the implementation of EndPosition I see that it calls motionplan.ComputePosition.
Hence, if we error on ComputePosition we error on EndPosition.
Example here:
https://github.com/viamrobotics/rdk/blob/main/components/arm/universalrobots/ur.go#L322
There was a problem hiding this comment.
need to do some thinking abt this.. 🤔
There was a problem hiding this comment.
ok after a night's rest I am back with some new thoughts.
to-reiterate, part of the ticket description states:
This should be updated such that if an arm is currently in an out-of-bounds state, it is still able to report its present state
Hence, when in an OOB state, arm.EndPosition and arm.JointPositions should still return the desired information.
There may also be some value in continuing with this change, but I would like us to think about the implications here.
The addition of !strings.Contains(err.Error(), referenceframe.OOBErrString) to the conditional inside motionplan/kinematic.go/ComputePosition() makes it such that components/arm/arm.go/CreateStatus() does not return nil, err when in an OOB state.
If we do return nil, err, this means that the arm card on the UI is blank.
Hence, to rescue an OOB arm we must rely on the use of the SDK.
In my opinion, the loss of the arm UI card is detrimental to the user experience for this specific case. However, this can be fixed through submitting a ticket to fleet mgmt and having a more in depth discussion about how to shape the UI when OOB..
You are correct good callout, we should probably be floating the OOBError higher in the stack because now its being squashed in frame.go
We shouldn't be changing the behavior here, since it's public. We should re-examine whether we want to use a public method at the point in question, and if so, we should create a new one that explicitly will return OOB positions.
I agree that we should float the OOBError higher in the stack. Because we "shouldn't be changing the behavior here, since it's public", I propose new public method inside motionplan/kinematic.go called ComputeOOBPositions(). Then, ComputePositions() would error on OOB, while ComputeOOBPositions() would not error on OOB.
This means that all arm driver implementations of EndPosition() could be changed from:
return motionplan.ComputePosition(e.model, joints)
to
return motionplan.ComputeOOBPosition(e.model, joints)
In turn, all arm driver implementations of MoveToPosition() could be changed from:
model := e.ModelFrame()
joints, err := e.JointPositions(ctx, nil)
if err != nil {
return err
}
// check that joint positions are not out of bounds
if _, err = model.Transform(model.InputFromProtobuf(joints)); err != nil {
return err
}
to
joints, err := e.JointPositions(ctx, nil)
if err != nil {
return err
}
if _, err = motionplan.ComputePosition(e.model, joints); err != nil {
return err
}
This way, the error would be surfaced in CreateStatus.
I am sure there are multiple other solutions, so I welcome feedback to what I have proposed.
There was a problem hiding this comment.
This is great. ComputeOOBPosition() is the correct path to go down here. Thus ComputePosition() maintains its current behavior, and a user must be explicit about the call if they want OOB to be allowable.
I have one concern here: if we implement the above as-specified, while the arm card will now be valid, if a joint is indeed OOB, then the user needs to know that that is the case, and be told exactly what to do to fix it, since position moves will be failing.
Can we update the error statement to specify exactly which joint is OOB, in addition to the current "val needs to be within bounds [min, max]"?
motionplan/kinematic_test.go
Outdated
| @@ -88,8 +88,11 @@ func TestForwardKinematics(t *testing.T) { | |||
| // Test out of bounds. Note that ComputePosition will return nil on OOB. | |||
There was a problem hiding this comment.
[nit] Pending what we decide on the ComputePosition change, we would want this comment to be updated.
components/arm/arm.go
Outdated
|
|
||
| // checkDesiredJointPositions validates that the desired joint positions either bring the joint back | ||
| // in bounds or do not move the joint more out of bounds. | ||
| func checkDesiredJointPositions(ctx context.Context, a Arm, desiredJoints []float64) error { |
There was a problem hiding this comment.
Each individual arm driver should call this on MoveToJointPositions (and it should be removed from reconfigurableArm
Otterverse
added
safe to test
|
question: I am unsure how |
…individual arm driver
viambot
added
safe to test
viambot
added
safe to test
viambot
added
safe to test
viambot
added
safe to test
components/arm/arm.go
Outdated
| // check that joint positions are not out of bounds | ||
| _, err = motionplan.ComputePosition(model, joints) | ||
| if err != nil && strings.Contains(err.Error(), referenceframe.OOBErrString) { | ||
| err = errors.New("cartesian movements are not allowed when arm joints are out of bounds") |
There was a problem hiding this comment.
Can we concatenate the returned err from above with this string?
There was a problem hiding this comment.
you got it. now the error statement is of form:
cartesian movements are not allowed when arm joints are out of bounds - joint A input out of bounds, input B needs to be within range [C D]
spieswl
dismissed
raybjork’s stale review
Hey Ray, pulling you off since you're OoO for today and we don't want to bug you.
viambot
added
safe to test
| @@ -60,6 +62,10 @@ const ( | |||
| SubtypeName = resource.SubtypeName("arm") | |||
| ) | |||
|
|
|||
| // MTPoob is a string that all MoveToPosition errors should contain if the method is called | |||
| // and there are joints which are out of bounds. | |||
| const MTPoob = "cartesian movements are not allowed when arm joints are out of bounds" | |||
There was a problem hiding this comment.
what do we think of MTPoob as the name for this error?
Open to feedback!
There was a problem hiding this comment.
Personally I think the name MTPoob is hilarious and we should keep it :)
components/arm/arm.go
Outdated
| // check that joint positions are not out of bounds | ||
| _, err = motionplan.ComputePosition(model, joints) | ||
| if err != nil && strings.Contains(err.Error(), referenceframe.OOBErrString) { | ||
| return errors.New(strings.Join( |
There was a problem hiding this comment.
Rather than importing the strings lib and using join, this can just be:
return errors.New(MTPoob + ": " + err.Error())
viambot
added
safe to test
|
Note: merge base coverage results not available, comparing against closest 9ed8da6~1=(a345ee0) instead
|
To rescue an arm which has been initialized as out of bounds (OOB), we allow the following:
EndPositionandJointPositionsshould be able to report the robot's present state.MoveToPositionerrors as long as in OOB.MoveToJointPositionsworks only if we are bringing the OOB joint more in bounds.Once in bounds,
MoveToJointPositionscannot move a joint to an OOB state.The OOB error is surfaced through the
arm.CreateStatusmethod. This in turn prevents the arm card from showing up on the control card of the UI.RSDK-1836
The unit test for this lives inside
components/arm/arm_test.go.We construct an injected arm and define its
LocalArmas a fake ur5e.To initialize in an OOB state we define a mock
MoveToJointPositionsmethod.