8000 fixed slerp · ZombyDogs/spatialmath-python@dffd28e · GitHub
[go: up one dir, main page]
Skip to content

Commit dffd28e

Browse files
petercorkepetercorke
committed
fixed slerp
1 parent 60a05d9 commit dffd28e

File tree

2 files changed

+78
-5
lines changed

2 files changed

+78
-5
lines changed

‎spatialmath/base/quaternions.py

Lines changed: 44 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,37 @@
66
@author: Peter Corke
77
"""
88

9+
# This file is part of the SpatialMath toolbox for Python
10+
# https://github.com/petercorke/spatialmath-python
11+
#
12+
# MIT License
13+
#
14+
# Copyright (c) 1993-2020 Peter Corke
15+
#
16+
# Permission is hereby granted, free of charge, to any person obtaining a copy
17+
# of this software and associated documentation files (the "Software"), to deal
18+
# in the Software without restriction, including without limitation the rights
19+
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
20+
# copies of the Software, and to permit persons to whom the Software is
21+
# furnished to do so, subject to the following conditions:
22+
#
23+
# The above copyright notice and this permission notice shall be included in all
24+
# copies or substantial portions of the Software.
25+
#
26+
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
27+
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
28+
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
29+
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
30+
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
31+
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
32+
# SOFTWARE.
33+
34+
# Contributors:
35+
#
36+
# 1. Luis Fernando Lara Tobar and Peter Corke, 2008
37+
# 2. Josh Carrigg Hodson, Aditya Dua, Chee Ho Chan, 2017 (robopy)
38+
# 3. Peter Corke, 2020
39+
940
import sys
1041
import math
1142
import numpy as np
@@ -422,6 +453,11 @@ def slerp(q0, q1, s, shortest=False):
422453
assert 0 <= s <= 1, 's must be in the interval [0,1]'
423454
q0 = argcheck.getvector(q0, 4)
424455
q1 = argcheck.getvector(q1, 4)
456+
457+
if s == 0:
458+
return q0
459+
elif s == 1:
460+
return q1
425461

426462
dot = np.dot(q0, q1)
427463

@@ -434,11 +470,14 @@ def slerp(q0, q1, s, shortest=False):
434470
dot = -dot
435471

436472
dot = np.clip(dot, -1, 1) # Clip within domain of acos()
437-
theta_0 = math.acos(dot) # theta_0 = angle between input vectors
438-
theta = theta_0 * s # theta = angle between v0 and result
439-
s0 = math.cos(theta) - dot * math.sin(theta) / math.sin(theta_0)
440-
s1 = math.sin(theta) / math.sin(theta_0)
441-
return (q0 * s0) + (q1 * s1)
473+
theta = math.acos(dot) # theta is the angle between rotation vectors
474+
if abs(theta) > 10*_eps:
475+
s0 = math.sin((1 - s) * theta)
476+
s1 = math.sin(s * theta)
477+
return ((q0 * s0) + (q1 * s1)) / math.sin(theta)
478+
else:
479+
# quaternions are identical
480+
return q0
442481

443482

444483
def rand():

‎spatialmath/base/test_quaternions.py

Lines changed: 34 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,34 @@
1+
# This file is part of the SpatialMath toolbox for Python
2+
# https://github.com/petercorke/spatialmath-python
3+
#
4+
# MIT License
5+
#
6+
# Copyright (c) 1993-2020 Peter Corke
7+
#
8+
# Permission is hereby granted, free of charge, to any person obtaining a copy
9+
# of this software and associated documentation files (the "Software"), to deal
10+
# in the Software without restriction, including without limitation the rights
11+
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
12+
# copies of the Software, and to permit persons to whom the Software is
13+
# furnished to do so, subject to the following conditions:
14+
#
15+
# The above copyright notice and this permission notice shall be included in all
16+
# copies or substantial portions of the Software.
17+
#
18+
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
19+
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
20+
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
21+
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
22+
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
23+
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
24+
# SOFTWARE.
25+
26+
# Contributors:
27+
#
28+
# 1. Luis Fernando Lara Tobar and Peter Corke, 2008
29+
# 2. Josh Carrigg Hodson, Aditya Dua, Chee Ho Chan, 2017 (robopy)
30+
# 3. Peter Corke, 2020
31+
132
import numpy.testing as nt
233
import unittest
334

@@ -84,6 +115,9 @@ def test_slerp(self):
84115
nt.assert_array_almost_equal(slerp(q1, q2, 0), q1)
85116
nt.assert_array_almost_equal(slerp(q1, q2, 1), q2)
86117
nt.assert_array_almost_equal(slerp(q1, q2, 0.5), np.r_[0, 1, 1, 0] / math.sqrt(2))
118+
119+
nt.assert_array_almost_equal(slerp( r2q(tr.rotx(-0.3)), r2q(tr.rotx(0.3)), 0.5), np.r_[1, 0, 0, 0])
120+
nt.assert_array_almost_equal(slerp( r2q(tr.roty(0.3)), r2q(tr.roty(0.5)), 0.5), r2q(tr.roty(0.4)))
87121

88122
def test_rotx(self):
89123
pass

0 commit comments

Comments
 (0)
0