8000 finally fixed error for derivative of inverse velocity mapping for ex… · astyl/spatialmath-python@df8fcad · GitHub
[go: up one dir, main page]
Skip to content

Commit df8fcad

Browse files
petercorkepetercorke
committed
finally fixed error for derivative of inverse velocity mapping for exponential coordinates case
1 parent e3dd635 commit df8fcad

File tree

3 files changed

+60
-139
lines changed

3 files changed

+60
-139
lines changed

‎spatialmath/base/transforms3d.py

Lines changed: 7 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -2404,31 +2404,26 @@ def rotvelxform_inv_dot(𝚪, 𝚪d, full=False, representation="rpy/xyz"):
24042404
)
24052405

24062406
elif representation == "exp":
2407-
# autogenerated by symbolic/angvelxform_dot.ipynb
2408-
v = 𝚪
2409-
vd = 𝚪d
2410-
sk = smb.skew(v)
2411-
skd = smb.skew(vd)
2412-
theta_dot = np.inner(𝚪, 𝚪d) / smb.norm(𝚪)
2407+
sk = smb.skew(𝚪)
24132408
theta = smb.norm(𝚪)
2409+
skd = smb.skew(𝚪d)
2410+
theta_dot = np.inner(𝚪, 𝚪d) / smb.norm(𝚪)
24142411
Theta = (1.0 - theta / 2.0 * np.sin(theta) / (1.0 - np.cos(theta))) / theta**2
24152412

2416-
# hand optimized version of code from notebook
2417-
# TODO:
2418-
# results are close but different to numerical cross check
2419-
# something wrong in the derivation
2413+
# hand optimized version of code from notebook symbolic/angvelxform_dot.ipynb
24202414
Theta_dot = (
24212415
-theta * C(theta) - S(theta) + theta * S(theta) ** 2 / (1 - C(theta))
24222416
) * theta_dot / 2 / (1 - C(theta)) / theta**2 - (
24232417
2 - theta * S(theta) / (1 - C(theta))
24242418
) * theta_dot / theta**3
24252419

2426-
Ainv_dot = -0.5 * skd + 2.0 * sk @ skd * Theta + sk @ sk * Theta_dot
2420+
Ainv_dot = -0.5 * skd + (sk @ skd + skd @ sk) * Theta + sk @ sk * Theta_dot
2421+
24272422
else:
24282423
raise ValueError("bad representation specified")
24292424

24302425
if full:
2431-
return sp.linalg.block_diag(np.eye(3, 3), Ainv_dot)
2426+
return sp.linalg.block_diag(np.zeros((3, 3)), Ainv_dot)
24322427
else:
24332428
return Ainv_dot
24342429

‎symbolic/angvelxform_dot.ipynb

Lines changed: 46 additions & 120 deletions
Original file line numberDiff line numberDiff line change
@@ -1,30 +1,32 @@
11
{
22
"cells": [
33
{
4+
"attachments": {},
45
"cell_type": "markdown",
56
"metadata": {},
67
"source": [
78
"# Determine derivative of Jacobian from angular velocity to exponential rates\n",
89
"\n",
9-
"Peter Corke 2021\n",
10+
"Peter Corke 2021, updated 1/23\n",
1011
"\n",
11-
"SymPy code to deterine the time derivative of the mapping from angular velocity to exponential coordinate rates."
12+
"SymPy code to determine the time derivative of the mapping from angular velocity to exponential coordinate rates."
1213
]
1314
},
1415
{
1516
"cell_type": "code",
16-
"execution_count": 2,
17+
"execution_count": null,
1718
"metadata": {},
1819
"outputs": [],
1920
"source": [
2021
"from sympy import *"
2122
]
2223
},
2324
{
25+
"attachments": {},
2426
"cell_type": "markdown",
2527
"metadata": {},
2628
"source": [
27-
"A rotation matrix can be expressed in terms of exponential coordinates (also called Euler vector)\n",
29+
"A rotation matrix can be expressed in terms of exponential coordinates (also called the Euler vector)\n",
2830
"\n",
2931
"$\n",
3032
"\\mathbf{R} = e^{[\\varphi]_\\times} \n",
@@ -37,18 +39,18 @@
3739
"\\dot{\\varphi} = \\mathbf{A} \\omega\n",
3840
"$\n",
3941
"\n",
40-
"where $\\mathbf{A}$ is given by (2.107) of [Robot Dynamics Lecture Notes, Robotic Systems Lab, ETH Zurich, 2018](https://ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/rsl-dam/documents/RobotDynamics2018/RD_HS2018script.pdf)\n",
42+
"where $\\mathbf{A}$ is given by (2.107) of [Robot Dynamics Lecture Notes, Robotic Systems Lab, ETH Zurich, 2017](https://ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/rsl-dam/documents/RobotDynamics2017/RD_HS2017script.pdf)\n",
4143
"\n",
4244
"\n",
4345
"$\n",
44-
"\\mathbf{A} = \\mathbf{1}_{3 \\times 3} - \\frac{1}{2} [v]_\\times + [v]^2_\\times \\frac{1}{\\theta^2} \\left( 1 - \\frac{\\theta}{2} \\frac{\\sin \\theta}{1 - \\cos \\theta} \\right)\n",
46+
"\\mathbf{A} = \\mathbf{1}_{3 \\times 3} - \\frac{1}{2} [\\varphi]_\\times + [\\varphi]^2_\\times \\frac{1}{\\theta^2} \\left( 1 - \\frac{\\theta}{2} \\frac{\\sin \\theta}{1 - \\cos \\theta} \\right),\n",
4547
"$\n",
46-
"where $\\theta = \\| \\varphi \\|$ and $v = \\hat{\\varphi}$\n",
48+
"where $\\theta = \\| \\varphi \\|$\n",
4749
"\n",
4850
"We simplify the equation as\n",
4951
"\n",
5052
"$\n",
51-
"\\mathbf{A} = \\mathbf{1}_{3 \\times 3} - \\frac{1}{2} [v]_\\times + [v]^2_\\times \\Theta\n",
53+
"\\mathbf{A} = \\mathbf{1}_{3 \\times 3} - \\frac{1}{2} [\\varphi]_\\times + [\\varphi]^2_\\times \\Theta\n",
5254
"$\n",
5355
"\n",
5456
"where\n",
@@ -59,19 +61,25 @@
5961
"We can find the derivative using the chain rule\n",
6062
"\n",
6163
"$\n",
62-
"\\dot{\\mathbf{A}} = - \\frac{1}{2} [\\dot{v}]_\\times + 2 [v]_\\times [\\dot{v}]_\\times \\Theta + [v]^2_\\times \\dot{\\Theta}\n",
64+
"\\dot{\\mathbf{A}} = - \\frac{1}{2} [\\dot{\\varphi}]_\\times + \\left( [\\varphi]_\\times [\\dot{\\varphi}]_\\times + [\\dot{\\varphi}]_\\times[\\varphi]_\\times \\right) \\Theta + [\\varphi]^2_\\times \\dot{\\Theta}\n",
65+
"$\n",
66+
"\n",
67+
"noting that the derivative of a matrix squared is\n",
68+
"\n",
69+
"$\n",
70+
"\\frac{d}{dt} (\\mathbf{M}^2) = (\\frac{d}{dt} \\mathbf{M}) \\mathbf{M} + \\mathbf{M} (\\frac{d}{dt} \\mathbf{M})\n",
6371
"$\n",
6472
"\n",
6573
"We start by defining some symbols"
6674
]
6775
},
6876
{
6977
"cell_type": "code",
70-
"execution_count": 21,
78+
"execution_count": null,
7179
"metadata": {},
7280
"outputs": [],
7381
"source": [
74-
"Theta, theta, theta_dot, t = symbols('Theta theta theta_dot t', real=True)"
82+
"theta, theta_dot, t = symbols('theta theta_dot t', real=True)"
7583
]
7684
},
7785
{
@@ -83,32 +91,19 @@
8391
},
8492
{
8593
"cell_type": "code",
86-
"execution_count": 22,
94+
"execution_count": null,
8795
"metadata": {},
8896
"outputs": [],
8997
"source": [
90-
"theta_t = Function(theta)(t)"
98+
"theta_t = Function(theta)(t)\n",
99+
"theta_t"
91100
]
92101
},
93102
{
94103
"cell_type": "code",
95-
"execution_count": 23,
104+
"execution_count": null,
96105
"metadata": {},
97-
"outputs": [
98-
{
99-
"data": {
100-
"text/latex": [
101-
"$\\displaystyle \\frac{1 - \\frac{\\theta{\\left(t \\right)} \\sin{\\left(\\theta{\\left(t \\right)} \\right)}}{2 \\left(1 - \\cos{\\left(\\theta{\\left(t \\right)} \\right)}\\right)}}{\\theta^{2}{\\left(t \\right)}}$"
102-
],
103-
"text/plain": [
104-
"(1 - theta(t)*sin(theta(t))/(2*(1 - cos(theta(t)))))/theta(t)**2"
105-
]
106-
},
107-
"execution_count": 23,
108-
"metadata": {},
109-
"output_type": "execute_result"
110-
}
111-
],
106+
"outputs": [],
112107
"source": [
113108
"Theta = 1 / theta_t ** 2 * (1 - theta_t / 2 * sin(theta_t) / (1 - cos(theta_t)))\n",
114109
"Theta"
@@ -123,23 +118,9 @@
123118
},
124119
{
125120
"cell_type": "code",
126-
"execution_count": 24,
121+
"execution_count": null,
127122
"metadata": {},
128-
"outputs": [
129-
{
130-
"data": {
131-
"text/latex": [
132-
"$\\displaystyle - \\frac{2 \\left(1 - \\frac{\\theta{\\left(t \\right)} \\sin{\\left(\\theta{\\left(t \\right)} \\right)}}{2 \\left(1 - \\cos{\\left(\\theta{\\left(t \\right)} \\right)}\\right)}\\right) \\frac{d}{d t} \\theta{\\left(t \\right)}}{\\theta^{3}{\\left(t \\right)}} + \\frac{- \\frac{\\theta{\\left(t \\right)} \\cos{\\left(\\theta{\\left(t \\right)} \\right)} \\frac{d}{d t} \\theta{\\left(t \\right)}}{2 \\left(1 - \\cos{\\left(\\theta{\\left(t \\right)} \\right)}\\right)} - \\frac{\\sin{\\left(\\theta{\\left(t \\right)} \\right)} \\frac{d}{d t} \\theta{\\left(t \\right)}}{2 \\left(1 - \\cos{\\left(\\theta{\\left(t \\right)} \\right)}\\right)} + \\frac{\\theta{\\left(t \\right)} \\sin^{2}{\\left(\\theta{\\left(t \\right)} \\right)} \\frac{d}{d t} \\theta{\\left(t \\right)}}{2 \\left(1 - \\cos{\\left(\\theta{\\left(t \\right)} \\right)}\\right)^{2}}}{\\theta^{2}{\\left(t \\right)}}$"
133-
],
134-
"text/plain": [
135-
"-2*(1 - theta(t)*sin(theta(t))/(2*(1 - cos(theta(t)))))*Derivative(theta(t), t)/theta(t)**3 + (-theta(t)*cos(theta(t))*Derivative(theta(t), t)/(2*(1 - cos(theta(t)))) - sin(theta(t))*Derivative(theta(t), t)/(2*(1 - cos(theta(t)))) + theta(t)*sin(theta(t))**2*Derivative(theta(t), t)/(2*(1 - cos(theta(t)))**2))/theta(t)**2"
136-
]
137-
},
138-
"execution_count": 24,
139-
"metadata": {},
140-
"output_type": "execute_result"
141-
}
142-
],
123+
"outputs": [],
143124
"source": [
144125
"T_dot = Theta.diff(t)\n",
145126
"T_dot"
@@ -156,29 +137,19 @@
156137
},
157138
{
158139
"cell_type": "code",
159-
"execution_count": 25,
140+
"execution_count": null,
160141
"metadata": {},
161142
"outputs": [],
162143
"source": [
163-
"T_dot = T_dot.subs([(theta_t.diff(t), theta_dot), (theta_t, theta)])"
144+
"T_dot = T_dot.subs([(theta_t.diff(t), theta_dot), (theta_t, theta)])\n",
145+
"T_dot"
164146
]
165147
},
166148
{
167149
"cell_type": "code",
168-
"execution_count": 26,
150+
"execution_count": null,
169151
"metadata": {},
170-
"outputs": [
171-
{
172-
"data": {
173-
"text/plain": [
174-
"'(-1/2*theta*theta_dot*math.cos(theta)/(1 - math.cos(theta)) + (1/2)*theta*theta_dot*math.sin(theta)**2/(1 - math.cos(theta))**2 - 1/2*theta_dot*math.sin(theta)/(1 - math.cos(theta)))/theta**2 - 2*theta_dot*(-1/2*theta*math.sin(theta)/(1 - math.cos(theta)) + 1)/theta**3'"
175-
]
176-
},
177-
"execution_count": 26,
178-
"metadata": {},
179-
"output_type": "execute_result"
180-
}
181-
],
152+
"outputs": [],
182153
"source": [
183154
"pycode(T_dot)"
184155
]
@@ -192,7 +163,7 @@
192163
},
193164
{
194165
"cell_type": "code",
195-
"execution_count": 38,
166+
"execution_count": null,
196167
"metadata": {},
197168
"outputs": [],
198169
"source": [
@@ -217,23 +188,9 @@
217188
},
218189
{
219190
"cell_type": "code",
220-
"execution_count": 40,
191+
"execution_count": null,
221192
"metadata": {},
222-
"outputs": [
223-
{
224-
"data": {
225-
"text/latex": [
226-
"$\\displaystyle \\sqrt{\\varphi_{0}^{2}{\\left(t \\right)} + \\varphi_{1}^{2}{\\left(t \\right)} + \\varphi_{2}^{2}{\\left(t \\right)}}$"
227-
],
228-
"text/plain": [
229-
"sqrt(varphi_0(t)**2 + varphi_1(t)**2 + varphi_2(t)**2)"
230-
]
231-
},
232-
"execution_count": 40,
233-
"metadata": {},
234-
"output_type": "execute_result"
235-
}
236-
],
193+
"outputs": [],
237194
"source": [
238195
"theta = Matrix(phi_t).norm()\n",
239196
"theta"
@@ -248,23 +205,9 @@
248205
},
249206
{
250207
"cell_type": "code",
251-
"execution_count": 41,
208+
"execution_count": null,
252209
"metadata": {},
253-
"outputs": [
254-
{
255-
"data": {
256-
"text/latex": [
257-
"$\\displaystyle \\frac{\\varphi_{0}{\\left(t \\right)} \\frac{d}{d t} \\varphi_{0}{\\left(t \\right)} + \\varphi_{1}{\\left(t \\right)} \\frac{d}{d t} \\varphi_{1}{\\left(t \\right)} + \\varphi_{2}{\\left(t \\right)} \\frac{d}{d t} \\varphi_{2}{\\left(t \\right)}}{\\sqrt{\\varphi_{0}^{2}{\\left(t \\right)} + \\varphi_{1}^{2}{\\left(t \\right)} + \\varphi_{2}^{2}{\\left(t \\right)}}}$"
258-
],
259-
"text/plain": [
260-
"(varphi_0(t)*Derivative(varphi_0(t), t) + varphi_1(t)*Derivative(varphi_1(t), t) + varphi_2(t)*Derivative(varphi_2(t), t))/sqrt(varphi_0(t)**2 + varphi_1(t)**2 + varphi_2(t)**2)"
261-
]
262-
},
263-
"execution_count": 41,
264-
"metadata": {},
265-
"output_type": "execute_result"
266-
}
267-
],
210+
"outputs": [],
268211
"source": [
269212
"theta_dot = theta.diff(t)\n",
270213
"theta_dot"
@@ -279,23 +222,9 @@
279222
},
280223
{
281224
"cell_type": "code",
282-
"execution_count": 42,
225+
"execution_count": null,
283226
"metadata": {},
284-
"outputs": [
285-
{
286-
"data": {
287-
"text/latex": [
288-
"$\\displaystyle \\frac{\\varphi_{0} \\varphi_{0 dot} + \\varphi_{1} \\varphi_{1 dot} + \\varphi_{2} \\varphi_{2 dot}}{\\sqrt{\\varphi_{0}^{2} + \\varphi_{1}^{2} + \\varphi_{2}^{2}}}$"
289-
],
290-
"text/plain": [
291-
"(varphi_0*varphi_0_dot + varphi_1*varphi_1_dot + varphi_2*varphi_2_dot)/sqrt(varphi_0**2 + varphi_1**2 + varphi_2**2)"
292-
]
293-
},
294-
"execution_count": 42,
295-
"metadata": {},
296-
"output_type": "execute_result"
297-
}
298-
],
227+
"outputs": [],
299228
"source": [
300229
"theta_dot = theta_dot.subs(a for a in zip(phi_d, phi_n))\n",
301230
"theta_dot = theta_dot.subs(a for a in zip(phi_t, phi))\n",
@@ -311,17 +240,9 @@
311240
},
312241
{
313242
"cell_type": "code",
314-
"execution_count": 7,
243+
"execution_count": null,
315244
"metadata": {},
316-
"outputs": [
317-
{
318-
"name": "stdout",
319-
"output_type": "stream",
320-
"text": [
321-
"exp(A(t))*Derivative(A(t), t)\n"
322-
]
323-
}
324-
],
245+
"outputs": [],
325246
"source": [
326247
"A, t = symbols('A t', real=True)\n",
327248
"A_t = Function(A)(t)\n",
@@ -332,7 +253,7 @@
332253
],
333254
"metadata": {
334255
"kernelspec": {
335-
"display_name": "Python 3",
256+
"display_name": "Python 3.8.5 ('dev')",
336257
"language": "python",
337258
"name": "python3"
338259
},
@@ -376,6 +297,11 @@
376297
"_Feature"
377298
],
378299
"window_display": false
300+
},
301+
"vscode": {
302+
"interpreter": {
303+
"hash": "b7d6b0d76025b9176285a6442c3dd6dd39bcfe7241029b7898b7106bd5e9b472"
304+
}
379305
}
380306
},
381307
"nbformat": 4,

0 commit comments

Comments
 (0)
0