mirror of
https://github.com/OpenSolo/OpenSolo.git
synced 2025-04-29 22:24:32 +02:00
314 lines
10 KiB
Python
314 lines
10 KiB
Python
# TestCatmullRom.py
|
|
# shotmanager
|
|
#
|
|
# Unit tests for the CatmullRom class in catmullRom.py
|
|
#
|
|
# Created by Will Silva on 11/22/2015.
|
|
# Copyright (c) 2015 3D Robotics.
|
|
# Licensed under the Apache License, Version 2.0 (the "License");
|
|
# you may not use this file except in compliance with the License.
|
|
# You may obtain a copy of the License at
|
|
#
|
|
# http://www.apache.org/licenses/LICENSE-2.0
|
|
#
|
|
# Unless required by applicable law or agreed to in writing, software
|
|
# distributed under the License is distributed on an "AS IS" BASIS,
|
|
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
# See the License for the specific language governing permissions and
|
|
# limitations under the License.
|
|
|
|
import unittest
|
|
from catmullRom import CatmullRom
|
|
from vector3 import Vector3
|
|
|
|
|
|
class TestConstructor(unittest.TestCase):
|
|
|
|
def testMinNumofPoints2(self):
|
|
'''Tests that catmull raises an exception if less than one waypoint is passed to it'''
|
|
Pts = [Vector3(0.0, 0.0, 0.0)] * 3
|
|
self.assertRaises(ValueError, CatmullRom, Pts)
|
|
|
|
|
|
class TestCatmullRom(unittest.TestCase):
|
|
|
|
def setUp(self):
|
|
self.spline = CatmullRom(
|
|
[Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(2, 0, 0), Vector3(3, 0, 0)])
|
|
|
|
def testUpdateSplineCoefficientsError(self):
|
|
'''Tests that an error is raised if we call updateSplineCoefficients with an out of range segment'''
|
|
self.assertRaises(ValueError, self.spline.updateSplineCoefficients, 3)
|
|
|
|
def testPosition(self):
|
|
'''For an evenly spaced straight-line spline, test that position is traversed'''
|
|
seg = 0
|
|
u = 0
|
|
q = self.spline.position(seg, u)
|
|
self.assertEqual(q, Vector3(1.0, 0.0, 0.0))
|
|
u = 0.5
|
|
q = self.spline.position(seg, u)
|
|
self.assertEqual(q, Vector3(1.5, 0.0, 0.0))
|
|
u = 1.0
|
|
q = self.spline.position(seg, u)
|
|
self.assertEqual(q, Vector3(2.0, 0.0, 0.0))
|
|
|
|
def testVelocity(self):
|
|
'''For an evenly spaced straight-line spline, test that velocity is constant'''
|
|
seg = 0
|
|
u = 0
|
|
dq = self.spline.velocity(seg, u)
|
|
self.assertEqual(dq, Vector3(1.0, 0.0, 0.0))
|
|
u = 0.5
|
|
dq = self.spline.velocity(seg, u)
|
|
self.assertEqual(dq, Vector3(1.0, 0.0, 0.0))
|
|
u = 1.0
|
|
dq = self.spline.velocity(seg, u)
|
|
self.assertEqual(dq, Vector3(1.0, 0.0, 0.0))
|
|
|
|
def testAcceleration(self):
|
|
'''For an evenly spaced straight-line spline, test that acceleration is zero'''
|
|
seg = 0
|
|
u = 0
|
|
ddq = self.spline.acceleration(seg, u)
|
|
self.assertEqual(ddq, Vector3(0.0, 0.0, 0.0))
|
|
u = 0.5
|
|
ddq = self.spline.acceleration(seg, u)
|
|
self.assertEqual(ddq, Vector3(0.0, 0.0, 0.0))
|
|
u = 1.0
|
|
ddq = self.spline.acceleration(seg, u)
|
|
self.assertEqual(ddq, Vector3(0.0, 0.0, 0.0))
|
|
|
|
def testCurvature(self):
|
|
'''For an evenly spaced straight-line spline, test that curvature is zero'''
|
|
seg = 0
|
|
u = 0
|
|
curv = self.spline.curvature(seg, u)
|
|
self.assertEqual(curv, 0.0)
|
|
u = 0.5
|
|
curv = self.spline.curvature(seg, u)
|
|
self.assertEqual(curv, 0.0)
|
|
u = 1.0
|
|
curv = self.spline.curvature(seg, u)
|
|
self.assertEqual(curv, 0.0)
|
|
|
|
|
|
class TestArcLength(unittest.TestCase):
|
|
|
|
def setUp(self):
|
|
self.spline = CatmullRom(
|
|
[Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(2, 0, 0), Vector3(3, 0, 0)])
|
|
|
|
def testStandardInput(self):
|
|
'''Calculate arc length of the segment and verify that it's close to 1 (within 7 decimal places)'''
|
|
seg = 0
|
|
u1 = 0
|
|
u2 = 1
|
|
length = self.spline.arcLength(seg, u1, u2)
|
|
self.assertAlmostEqual(1, length, 7)
|
|
|
|
def testU2LessU1(self):
|
|
'''Tests case where u2 < u1'''
|
|
seg = 0
|
|
u2 = 0.5
|
|
u1 = 0.75
|
|
length = self.spline.arcLength(seg, u1, u2)
|
|
self.assertEqual(0, length)
|
|
|
|
def testU1OutOfBounds(self):
|
|
'''Test when u1 is not between 0-1'''
|
|
seg = 0
|
|
u1 = -0.2
|
|
u2 = 0.5
|
|
length = self.spline.arcLength(seg, u1, u2)
|
|
self.assertAlmostEqual(0.5, length)
|
|
|
|
def testU2OutOfBounds(self):
|
|
'''Test when u2 is not between 0-1'''
|
|
seg = 0
|
|
u1 = 0.5
|
|
u2 = 1.5
|
|
length = self.spline.arcLength(seg, u1, u2)
|
|
self.assertAlmostEqual(0.5, length)
|
|
|
|
def testU1andU2OutOfBounds(self):
|
|
'''Test when u1 and u2 are not between 0-1'''
|
|
seg = 0
|
|
u1 = 1.1
|
|
u2 = 1.2
|
|
length = self.spline.arcLength(seg, u1, u2)
|
|
self.assertAlmostEqual(0, length)
|
|
|
|
u1 = -0.7
|
|
u2 = -0.5
|
|
length = self.spline.arcLength(seg, u1, u2)
|
|
self.assertAlmostEqual(0, length)
|
|
|
|
def testAgainstBruteForceIntegration(self):
|
|
'''Make sure our numerical integrator is doing a decent job at estimating arc length'''
|
|
|
|
self.spline = CatmullRom(
|
|
[Vector3(0, 0, 0), Vector3(1, 5, 0), Vector3(2, -3, 0), Vector3(3, 0, 0)])
|
|
seg = 0
|
|
u1 = 0
|
|
u2 = 1
|
|
gaussResult = self.spline.arcLength(seg, u1, u2)
|
|
|
|
# Let's use Brute Force
|
|
n = 1000
|
|
dt = 1.0 / n
|
|
L = 0
|
|
t = 0
|
|
for i in range(0, n):
|
|
L += self.spline.velocity(seg, t).length() * dt
|
|
t += dt
|
|
|
|
# within a millimeter
|
|
self.assertAlmostEqual(gaussResult, L, 3)
|
|
|
|
|
|
class TestParameterByDistance(unittest.TestCase):
|
|
|
|
def setUp(self):
|
|
self.spline = CatmullRom(
|
|
[Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(2, 0, 0), Vector3(3, 0, 0)])
|
|
|
|
def testStandardInput(self):
|
|
'''Calculate parameter u that is s meters ahead of u1 (within 7 decimal places'''
|
|
seg = 0
|
|
u1 = 0.25
|
|
s = 0.25 # meters
|
|
uLive = self.spline.findParameterByDistance(seg, u1, s)
|
|
# test with 300 newton iterations
|
|
uAccurate = self.spline.findParameterByDistance(seg, u1, s, 300)
|
|
self.assertAlmostEqual(uLive, uAccurate, 7)
|
|
|
|
def testSTooLong(self):
|
|
'''Test when s (in meters) extends beyond the segment'''
|
|
seg = 0
|
|
u1 = 0.5
|
|
s = 0.75
|
|
u = self.spline.findParameterByDistance(seg, u1, s)
|
|
self.assertEqual(1.0, u)
|
|
|
|
def testSNegative(self):
|
|
'''Test when s is negative (doesn't make sense for this algorithm)'''
|
|
seg = 0
|
|
u1 = 0.5
|
|
s = -0.75
|
|
u = self.spline.findParameterByDistance(seg, u1, s)
|
|
self.assertEqual(u1, u)
|
|
|
|
|
|
class TestArclengthToNonDimensional(unittest.TestCase):
|
|
|
|
def setUp(self):
|
|
# 1 meter long spline
|
|
self.spline = CatmullRom(
|
|
[Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(2, 0, 0), Vector3(3, 0, 0)])
|
|
|
|
def testPTooBig(self):
|
|
'''Test when p is not between 0-1'''
|
|
p = 1.5 # should default to 1.0
|
|
dp = .1 # should be .1 m/s
|
|
seg, u, v = self.spline.arclengthToNonDimensional(p, dp)
|
|
self.assertEqual(seg, 0)
|
|
self.assertEqual(u, 1.0)
|
|
self.assertAlmostEqual(v, 0.1)
|
|
|
|
def testPTooSmall(self):
|
|
'''Test when p is not between 0-1'''
|
|
p = -1.5 # should default to 0
|
|
dp = .1 # should be .1 m/s
|
|
seg, u, v = self.spline.arclengthToNonDimensional(p, dp)
|
|
self.assertEqual(seg, 0)
|
|
self.assertEqual(u, 0)
|
|
self.assertAlmostEqual(v, 0.1)
|
|
|
|
def testConversionFromArclength(self):
|
|
'''Test conversion in an ideal 1 meter spline'''
|
|
p = 0.5 # should be halfway through spline distance (0.5 meters)
|
|
dp = .1 # should be .1 m/s
|
|
seg, u, v = self.spline.arclengthToNonDimensional(p, dp)
|
|
self.assertEqual(seg, 0)
|
|
self.assertEqual(u, 0.5)
|
|
self.assertAlmostEqual(v, 0.1)
|
|
|
|
def testConversionFromArclength2Seg(self):
|
|
'''Test conversion on a 2 segment spline'''
|
|
self.spline = CatmullRom([Vector3(0, 0, 0), Vector3(
|
|
1, 0, 0), Vector3(2, 0, 0), Vector3(4, 0, 0), Vector3(5, 0, 0)])
|
|
# x x-x--x x
|
|
p = 0.5 # should be halfway through spline distance (1.5 meters)
|
|
dp = .1
|
|
seg, u, v = self.spline.arclengthToNonDimensional(p, dp)
|
|
self.assertEqual(seg, 1)
|
|
self.assertAlmostEqual(u, 0.27272727)
|
|
self.assertAlmostEqual(v, 0.3)
|
|
|
|
|
|
class TestNonDimensionalToArclength(unittest.TestCase):
|
|
|
|
def setUp(self):
|
|
# 1 meter long spline
|
|
self.spline = CatmullRom(
|
|
[Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(2, 0, 0), Vector3(3, 0, 0)])
|
|
|
|
def testSegTooBig(self):
|
|
'''Test when segment doesn't exist (too high)'''
|
|
seg = 1 # for a 4 point spline, we only have 1 segment with index 0 x x----x x
|
|
u = 0.5
|
|
v = 1
|
|
p, dp = self.spline.nonDimensionalToArclength(seg, u, v)
|
|
self.assertEqual(p, 0.5)
|
|
self.assertAlmostEqual(dp, 1.0)
|
|
|
|
def testSegTooSmall(self):
|
|
'''Test when segment doesn't exist (no negative segments allowed)'''
|
|
seg = - \
|
|
1 # for a 4 point spline, we only have 1 segment with index 0 x x----x x
|
|
u = 0.5
|
|
v = 1
|
|
p, dp = self.spline.nonDimensionalToArclength(seg, u, v)
|
|
self.assertEqual(p, 0.5)
|
|
self.assertAlmostEqual(dp, 1.0)
|
|
|
|
def testUTooBig(self):
|
|
'''Test when U is not between 0-1'''
|
|
seg = 0
|
|
u = 1.5
|
|
v = 1
|
|
p, dp = self.spline.nonDimensionalToArclength(seg, u, v)
|
|
self.assertEqual(p, 1.0)
|
|
self.assertAlmostEqual(dp, 1.0)
|
|
|
|
def testUTooSmall(self):
|
|
'''Test when U is not between 0-1'''
|
|
seg = 0
|
|
u = -1
|
|
v = 1
|
|
p, dp = self.spline.nonDimensionalToArclength(seg, u, v)
|
|
self.assertEqual(p, 0.0)
|
|
self.assertAlmostEqual(dp, 1.0)
|
|
|
|
def testConversionFromNonDimensional(self):
|
|
'''Test conversion on an ideal 1 meter spline'''
|
|
seg = 0
|
|
u = .75
|
|
v = 2
|
|
p, dp = self.spline.nonDimensionalToArclength(seg, u, v)
|
|
self.assertEqual(p, 0.75)
|
|
self.assertAlmostEqual(dp, 2.0)
|
|
|
|
def testConversionFromNonDimensional2Seg(self):
|
|
'''Test conversion on a 2 segment spline'''
|
|
self.spline = CatmullRom([Vector3(0, 0, 0), Vector3(
|
|
1, 0, 0), Vector3(2, 0, 0), Vector3(4, 0, 0), Vector3(5, 0, 0)])
|
|
# x x-x--x x
|
|
seg = 1
|
|
u = .5
|
|
v = 2
|
|
p, dp = self.spline.nonDimensionalToArclength(seg, u, v)
|
|
self.assertAlmostEqual(p, 0.66666666)
|
|
self.assertAlmostEqual(dp, 0.66666666)
|