RSDK-11810: Remove unused deriv function. (viamrobotics#5251)

Author: dgottlieb

motionplan/armplanning/kinematic.go

@@ -8,7 +8,6 @@ import (
 	"github.com/golang/geo/r3"
 	pb "go.viam.com/api/component/arm/v1"
 	"go.viam.com/test"
-	"gonum.org/v1/gonum/num/quat"
 
 	frame "go.viam.com/rdk/referenceframe"
 	spatial "go.viam.com/rdk/spatialmath"
@@ -92,67 +91,6 @@ func TestForwardKinematics(t *testing.T) {
 	test.That(t, spatial.PoseAlmostEqualEps(expect, pos, 0.01), test.ShouldBeTrue)
 }
 
-const derivEqualityEpsilon = 1e-16
-
-func derivComponentAlmostEqual(left, right float64) bool {
-	return math.Abs(left-right) <= derivEqualityEpsilon
-}
-
-func areDerivsEqual(q1, q2 []quat.Number) bool {
-	if len(q1) != len(q2) {
-		return false
-	}
-	for i, dq1 := range q1 {
-		dq2 := q2[i]
-		if !derivComponentAlmostEqual(dq1.Real, dq2.Real) {
-			return false
-		}
-		if !derivComponentAlmostEqual(dq1.Imag, dq2.Imag) {
-			return false
-		}
-		if !derivComponentAlmostEqual(dq1.Jmag, dq2.Jmag) {
-			return false
-		}
-		if !derivComponentAlmostEqual(dq1.Kmag, dq2.Kmag) {
-			return false
-		}
-	}
-	return true
-}
-
-func TestDeriv(t *testing.T) {
-	// Test identity quaternion
-	q := quat.Number{1, 0, 0, 0}
-	qDeriv := []quat.Number{{0, 1, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 1}}
-
-	match := areDerivsEqual(qDeriv, deriv(q))
-	test.That(t, match, test.ShouldBeTrue)
-
-	// Test non-identity single-axis unit quaternion
-	q = quat.Exp(quat.Number{0, 2, 0, 0})
-
-	qDeriv = []quat.Number{
-		{-0.9092974268256816, -0.4161468365471424, 0, 0},
-		{0, 0, 0.4546487134128408, 0},
-		{0, 0, 0, 0.4546487134128408},
-	}
-
-	match = areDerivsEqual(qDeriv, deriv(q))
-	test.That(t, match, test.ShouldBeTrue)
-
-	// Test non-identity multi-axis unit quaternion
-	q = quat.Exp(quat.Number{0, 2, 1.5, 0.2})
-
-	qDeriv = []quat.Number{
-		{-0.472134934000233, -0.42654977821280804, -0.4969629339096933, -0.06626172452129245},
-		{-0.35410120050017474, -0.4969629339096933, -0.13665473343215354, -0.049696293390969336},
-		{-0.0472134934000233, -0.06626172452129245, -0.049696293390969336, 0.22944129454798728},
-	}
-
-	match = areDerivsEqual(qDeriv, deriv(q))
-	test.That(t, match, test.ShouldBeTrue)
-}
-
 // Test dynamic frame systems
 // Since kinematics imports reference frame, this needs to be here to avoid circular dependencies.
 func TestDynamicFrameSystemXArm(t *testing.T) {