Skip to content

Commit 8365ed3

Browse files
philippwulffphilippwulff
committed
minor
1 parent 2f9f24c commit 8365ed3

File tree

2 files changed

+237
-0
lines changed

2 files changed

+237
-0
lines changed

‎examples/test_2021F.ipynb‎

Lines changed: 225 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,225 @@
1+
{
2+
"cells": [
3+
{
4+
"cell_type": "code",
5+
"execution_count": 33,
6+
"metadata": {
7+
"collapsed": true
8+
},
9+
"outputs": [
10+
{
11+
"name": "stdout",
12+
"output_type": "stream",
13+
"text": [
14+
"The autoreload extension is already loaded. To reload it, use:\n",
15+
" %reload_ext autoreload\n"
16+
]
17+
}
18+
],
19+
"source": [
20+
"import utils\n",
21+
"%load_ext autoreload\n",
22+
"%autoreload 2\n",
23+
"\n",
24+
"from utils import build_transf, full_homo_transf, prop_velo, prop_force_torque, comp_jacobian\n",
25+
"import utils\n",
26+
"from sympy import sqrt\n",
27+
"import sympy as sy\n",
28+
"from IPython.display import display, Math"
29+
]
30+
},
31+
{
32+
"cell_type": "markdown",
33+
"source": [
34+
"# Final 20/21 Problem 3\n",
35+
"\n",
36+
"Denavit-Hartenberg Parameters are from problem 3a)."
37+
],
38+
"metadata": {
39+
"collapsed": false,
40+
"pycharm": {
41+
"name": "#%% md\n"
42+
}
43+
}
44+
},
45+
{
46+
"cell_type": "code",
47+
"execution_count": 6,
48+
"outputs": [],
49+
"source": [
50+
"dh_params = [\n",
51+
" [90, 0, \"d_1\", 0],\n",
52+
" [90, 0, sy.Symbol(\"l_1\")+sy.Symbol(\"d_3\"), \"theta_2\"],\n",
53+
" [0, \"l_2\", 0, 0],\n",
54+
" [0, 0, 0, 0]\n",
55+
"]\n",
56+
"\n",
57+
"pc1_0 = sy.Matrix([0, -2/3 * sy.Symbol(\"d_1\")])\n",
58+
"pc2_0 = sy.Matrix([1/2 * sy.Symbol(\"l_2\") * sy.cos(sy.Symbol(\"theta_2\")),\n",
59+
" -sy.Symbol(\"d_1\") -1/2 * sy.Symbol(\"l_2\") * sy.sin(sy.Symbol(\"theta_2\")),\n",
60+
" -sy.Symbol(\"l_1\")])\n",
61+
"pc3_0 = sy.Matrix([sy.Symbol(\"l_2\") * sy.cos(sy.Symbol(\"theta_2\")),\n",
62+
" -sy.Symbol(\"d_1\") - sy.Symbol(\"l_2\") * sy.sin(sy.Symbol(\"theta_2\")),\n",
63+
" -sy.Symbol(\"l_1\") -1/2 * sy.Symbol(\"d_3\")])"
64+
],
65+
"metadata": {
66+
"collapsed": false,
67+
"pycharm": {
68+
"name": "#%%\n"
69+
}
70+
}
71+
},
72+
{
73+
"cell_type": "code",
74+
"execution_count": 34,
75+
"outputs": [
76+
{
77+
"data": {
78+
"text/plain": "<IPython.core.display.Math object>",
79+
"text/latex": "$\\displaystyle {}^0_1T = \\left[\\begin{matrix}1 & 0 & 0 & 0\\\\0 & 0 & -1 & - d_{1}\\\\0 & 1 & 0 & 0\\\\0 & 0 & 0 & 1\\end{matrix}\\right]$"
80+
},
81+
"metadata": {},
82+
"output_type": "display_data"
83+
},
84+
{
85+
"data": {
86+
"text/plain": "<IPython.core.display.Math object>",
87+
"text/latex": "$\\displaystyle {}^1_2T = \\left[\\begin{matrix}\\cos{\\left(\\theta_{2} \\right)} & - \\sin{\\left(\\theta_{2} \\right)} & 0 & 0\\\\0 & 0 & -1 & - d_{3} - l_{1}\\\\\\sin{\\left(\\theta_{2} \\right)} & \\cos{\\left(\\theta_{2} \\right)} & 0 & 0\\\\0 & 0 & 0 & 1\\end{matrix}\\right]$"
88+
},
89+
"metadata": {},
90+
"output_type": "display_data"
91+
},
92+
{
93+
"data": {
94+
"text/plain": "<IPython.core.display.Math object>",
95+
"text/latex": "$\\displaystyle {}^2_3T = \\left[\\begin{matrix}1 & 0 & 0 & l_{2}\\\\0 & 1 & 0 & 0\\\\0 & 0 & 1 & 0\\\\0 & 0 & 0 & 1\\end{matrix}\\right]$"
96+
},
97+
"metadata": {},
98+
"output_type": "display_data"
99+
},
100+
{
101+
"data": {
102+
"text/plain": "<IPython.core.display.Math object>",
103+
"text/latex": "$\\displaystyle {}^3_4T = \\left[\\begin{matrix}1 & 0 & 0 & 0\\\\0 & 1 & 0 & 0\\\\0 & 0 & 1 & 0\\\\0 & 0 & 0 & 1\\end{matrix}\\right]$"
104+
},
105+
"metadata": {},
106+
"output_type": "display_data"
107+
}
108+
],
109+
"source": [
110+
"transforms = utils.build_transf(dh_params)\n",
111+
"full_transform = full_homo_transf(transforms, verbose=False)"
112+
],
113+
"metadata": {
114+
"collapsed": false,
115+
"pycharm": {
116+
"name": "#%%\n"
117+
}
118+
}
119+
},
120+
{
121+
"cell_type": "code",
122+
"execution_count": 49,
123+
"outputs": [
124+
{
125+
"data": {
126+
"text/plain": "<IPython.core.display.Math object>",
127+
"text/latex": "$\\displaystyle \\left[\\begin{matrix}\\cos{\\left(\\theta_{2} \\right)} & - \\sin{\\left(\\theta_{2} \\right)} & 0 & 0\\\\- \\sin{\\left(\\theta_{2} \\right)} & - \\cos{\\left(\\theta_{2} \\right)} & 0 & - d_{1}\\\\0 & 0 & -1 & - d_{3} - l_{1}\\\\0 & 0 & 0 & 1\\end{matrix}\\right]$"
128+
},
129+
"metadata": {},
130+
"output_type": "display_data"
131+
},
132+
{
133+
"data": {
134+
"text/plain": "<IPython.core.display.Math object>",
135+
"text/latex": "$\\displaystyle \\left[\\begin{matrix}\\cos{\\left(\\theta_{2} \\right)} & - \\sin{\\left(\\theta_{2} \\right)} & 0 & l_{2} \\cos{\\left(\\theta_{2} \\right)}\\\\- \\sin{\\left(\\theta_{2} \\right)} & - \\cos{\\left(\\theta_{2} \\right)} & 0 & - d_{1} - l_{2} \\sin{\\left(\\theta_{2} \\right)}\\\\0 & 0 & -1 & - d_{3} - l_{1}\\\\0 & 0 & 0 & 1\\end{matrix}\\right]$"
136+
},
137+
"metadata": {},
138+
"output_type": "display_data"
139+
},
140+
{
141+
"data": {
142+
"text/plain": "<IPython.core.display.Math object>",
143+
"text/latex": "$\\displaystyle \\left[\\begin{matrix}\\cos{\\left(\\theta_{2} \\right)} & - \\sin{\\left(\\theta_{2} \\right)} & 0 & l_{2} \\cos{\\left(\\theta_{2} \\right)}\\\\- \\sin{\\left(\\theta_{2} \\right)} & - \\cos{\\left(\\theta_{2} \\right)} & 0 & - d_{1} - l_{2} \\sin{\\left(\\theta_{2} \\right)}\\\\0 & 0 & -1 & - d_{3} - l_{1}\\\\0 & 0 & 0 & 1\\end{matrix}\\right]$"
144+
},
145+
"metadata": {},
146+
"output_type": "display_data"
147+
}
148+
],
149+
"source": [
150+
"T02 = transforms[0] @ transforms[1]\n",
151+
"T03 = T02 @ transforms[2]\n",
152+
"T04 = T03 @ transforms[3]\n",
153+
"for t in (T02, T03, T04):\n",
154+
" display(Math(sy.latex(t)))"
155+
],
156+
"metadata": {
157+
"collapsed": false,
158+
"pycharm": {
159+
"name": "#%%\n"
160+
}
161+
}
162+
},
163+
{
164+
"cell_type": "code",
165+
"execution_count": 59,
166+
"outputs": [
167+
{
168+
"data": {
169+
"text/plain": "<IPython.core.display.Math object>",
170+
"text/latex": "$\\displaystyle \\left[\\begin{matrix}\\cos{\\left(\\theta_{2} \\right)} & - \\sin{\\left(\\theta_{2} \\right)} & 0 & - d_{1} \\sin{\\left(\\theta_{2} \\right)}\\\\- \\sin{\\left(\\theta_{2} \\right)} & - \\cos{\\left(\\theta_{2} \\right)} & 0 & - d_{1} \\cos{\\left(\\theta_{2} \\right)}\\\\0 & 0 & -1 & - d_{3} - l_{1}\\\\0 & 0 & 0 & 1\\end{matrix}\\right]$"
171+
},
172+
"metadata": {},
173+
"output_type": "display_data"
174+
},
175+
{
176+
"data": {
177+
"text/plain": "<IPython.core.display.Math object>",
178+
"text/latex": "$\\displaystyle \\left[\\begin{matrix}\\cos{\\left(\\theta_{2} \\right)} & - \\sin{\\left(\\theta_{2} \\right)} & 0 & - d_{1} \\sin{\\left(\\theta_{2} \\right)} - l_{2}\\\\- \\sin{\\left(\\theta_{2} \\right)} & - \\cos{\\left(\\theta_{2} \\right)} & 0 & - d_{1} \\cos{\\left(\\theta_{2} \\right)}\\\\0 & 0 & -1 & - d_{3} - l_{1}\\\\0 & 0 & 0 & 1\\end{matrix}\\right]$"
179+
},
180+
"metadata": {},
181+
"output_type": "display_data"
182+
},
183+
{
184+
"data": {
185+
"text/plain": "<IPython.core.display.Math object>",
186+
"text/latex": "$\\displaystyle \\left[\\begin{matrix}\\cos{\\left(\\theta_{2} \\right)} & - \\sin{\\left(\\theta_{2} \\right)} & 0 & - d_{1} \\sin{\\left(\\theta_{2} \\right)} - l_{2}\\\\- \\sin{\\left(\\theta_{2} \\right)} & - \\cos{\\left(\\theta_{2} \\right)} & 0 & - d_{1} \\cos{\\left(\\theta_{2} \\right)}\\\\0 & 0 & -1 & - d_{3} - l_{1}\\\\0 & 0 & 0 & 1\\end{matrix}\\right]$"
187+
},
188+
"metadata": {},
189+
"output_type": "display_data"
190+
}
191+
],
192+
"source": [
193+
"for t in (T02, T03, T04):\n",
194+
" display(Math(sy.latex(utils.homo_transpose(t))))"
195+
],
196+
"metadata": {
197+
"collapsed": false,
198+
"pycharm": {
199+
"name": "#%%\n"
200+
}
201+
}
202+
}
203+
],
204+
"metadata": {
205+
"kernelspec": {
206+
"display_name": "Python 3",
207+
"language": "python",
208+
"name": "python3"
209+
},
210+
"language_info": {
211+
"codemirror_mode": {
212+
"name": "ipython",
213+
"version": 2
214+
},
215+
"file_extension": ".py",
216+
"mimetype": "text/x-python",
217+
"name": "python",
218+
"nbconvert_exporter": "python",
219+
"pygments_lexer": "ipython2",
220+
"version": "2.7.6"
221+
}
222+
},
223+
"nbformat": 4,
224+
"nbformat_minor": 0
225+
}

‎utils.py‎

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,7 @@
22
from sympy import Matrix, Symbol, sin, cos, latex, simplify
33
from IPython.display import display, Math
44
from sympy.physics.mechanics import dynamicsymbols, init_vprinting
5+
from copy import copy
56

67

78
def rad(degrees):
@@ -46,6 +47,17 @@ def homo_transf(alpha, a, d, theta):
4647
return simplify(transf)
4748

4849

50+
def homo_transpose(transform, simple=True):
51+
52+
R = transform[:3, :3].T
53+
transform_T = copy(transform.as_mutable())
54+
transform_T[:3, :3] = R
55+
transform_T[:3, 3] = -R @ transform[:3, 3]
56+
if simple:
57+
transform_T = sy.simplify(sy.expand(transform_T))
58+
return transform_T
59+
60+
4961
def build_transf(dh_params, verbose=True):
5062
"""Computes the homogeneous transforms for all links.
5163

0 commit comments

Comments
 (0)