diff --git a/components/base/kinematicbase/execution.go b/components/base/kinematicbase/execution.go index 9064328cba1..4b5601ce95c 100644 --- a/components/base/kinematicbase/execution.go +++ b/components/base/kinematicbase/execution.go @@ -76,7 +76,7 @@ func (ptgk *ptgBaseKinematics) GoToInputs(ctx context.Context, inputSteps ...[]r defer func() { ptgk.inputLock.Lock() - ptgk.currentState.currentInputs = zeroInput + ptgk.currentState = baseState{currentInputs: zeroInput} ptgk.inputLock.Unlock() }() diff --git a/components/base/kinematicbase/fake_kinematics.go b/components/base/kinematicbase/fake_kinematics.go index b73dcb025ee..69ee71173cd 100644 --- a/components/base/kinematicbase/fake_kinematics.go +++ b/components/base/kinematicbase/fake_kinematics.go @@ -268,9 +268,11 @@ func (fk *fakePTGKinematics) GoToInputs(ctx context.Context, inputSteps ...[]ref fk.inputLock.Lock() fk.currentIndex = i - fk.currentInput = []referenceframe.Input{inputs[0], inputs[1], inputs[2], inputs[2]} + fk.currentInput, err = fk.frame.Interpolate(zeroInput, inputs, 0) fk.inputLock.Unlock() - + if err != nil { + return err + } finalPose, err := fk.frame.Transform(inputs) if err != nil { return err