RSDK-7670 Fix errorstate when plan is complete (viamrobotics#3985)

maximpertsov · May 22, 2024 · bf6023f · bf6023f
1 parent c2cc3ff
commit bf6023f
Showing 1 changed file with 5 additions and 0 deletions.
diff --git a/components/base/kinematicbase/ptgKinematics.go b/components/base/kinematicbase/ptgKinematics.go
@@ -212,6 +212,11 @@ func (ptgk *ptgBaseKinematics) ErrorState(ctx context.Context) (spatialmath.Pose
 	currentInputs := ptgk.currentState.currentInputs
 	ptgk.inputLock.RUnlock()
 
+	if currentExecutingSteps == nil {
+		// We are not currently executing a plan
+		return spatialmath.NewZeroPose(), nil
+	}
+
 	currPoseInArc, err := ptgk.frame.Transform(currentInputs)
 	if err != nil {
 		return nil, err