Skip to content

Commit 595295c

Browse files
walchkowalchko
committed
up
1 parent c5fe22d commit 595295c

9 files changed

Lines changed: 284 additions & 132 deletions

File tree

‎dev/test.py‎

Lines changed: 10 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -14,13 +14,14 @@
1414
np.set_printoptions(suppress=True)
1515

1616
def kevin():
17+
mm = pi
1718
"""alpha a theta d"""
1819
dh = [
19-
{'alpha': 0, 'a': 0, 'theta': 0, 'd': 0, 'type': 1},
20-
{'alpha': pi/2, 'a': 52, 'theta': 0, 'd': 0, 'type': 1},
21-
{'alpha': 0, 'a': 89, 'theta': 0, 'd': 0, 'type': 1},
22-
{'alpha': 0, 'a': 90, 'theta': 0, 'd': 0, 'type': 1},
23-
{'alpha': 0, 'a': 95, 'theta': 0, 'd': 0, 'type': 1}
20+
{'alpha': 0, 'a': 0, 'theta': 0, 'd': 0, 'type': 1, "max_min": [mm,-mm]},
21+
{'alpha': pi/2, 'a': 52, 'theta': 0, 'd': 0, 'type': 1, "max_min": [mm,-mm]},
22+
{'alpha': 0, 'a': 89, 'theta': 0, 'd': 0, 'type': 1, "max_min": [mm,-mm]},
23+
{'alpha': 0, 'a': 90, 'theta': 0, 'd': 0, 'type': 1, "max_min": [mm,-mm]},
24+
{'alpha': 0, 'a': 95, 'theta': 0, 'd': 0, 'type': 1, "max_min": [mm,-mm]}
2425
]
2526

2627
return dh
@@ -38,10 +39,10 @@ def main():
3839
np.deg2rad([-45.00, 77.41, -98.15, -69.27, 0]) # 110, -110, -40
3940
]
4041

41-
# for jj in j:
42-
# # print(jj)
43-
# t = kc.transform(jj)
44-
# print(t)
42+
for jj in j:
43+
# print(jj)
44+
t = kc.transform(jj)
45+
print(t)
4546
# print("----------------------")
4647

4748
j = [

‎docs/basics.ipynb‎

Lines changed: 201 additions & 23 deletions
Original file line numberDiff line numberDiff line change
@@ -6,14 +6,10 @@
66
"metadata": {},
77
"outputs": [
88
{
9-
"ename": "ModuleNotFoundError",
10-
"evalue": "No module named 'mdh.models'",
11-
"output_type": "error",
12-
"traceback": [
13-
"\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
14-
"\u001b[0;31mModuleNotFoundError\u001b[0m Traceback (most recent call last)",
15-
"\u001b[0;32m<ipython-input-1-016134810506>\u001b[0m in \u001b[0;36m<module>\u001b[0;34m\u001b[0m\n\u001b[1;32m 5\u001b[0m \u001b[0;32mfrom\u001b[0m \u001b[0mmdh\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mlink\u001b[0m \u001b[0;32mimport\u001b[0m \u001b[0mmdh_params\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mJointType\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m 6\u001b[0m \u001b[0;32mfrom\u001b[0m \u001b[0mmdh\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mkinematic_chain\u001b[0m \u001b[0;32mimport\u001b[0m \u001b[0mKinematicChain\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m----> 7\u001b[0;31m \u001b[0;32mfrom\u001b[0m \u001b[0mmdh\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mmodels\u001b[0m \u001b[0;32mimport\u001b[0m \u001b[0mpuma500\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m 8\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m 9\u001b[0m \u001b[0;32mimport\u001b[0m \u001b[0mpandas\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
16-
"\u001b[0;31mModuleNotFoundError\u001b[0m: No module named 'mdh.models'"
9+
"name": "stdout",
10+
"output_type": "stream",
11+
"text": [
12+
"0.1.2\n"
1713
]
1814
}
1915
],
@@ -24,7 +20,7 @@
2420
"import mdh\n",
2521
"from mdh.link import mdh_params, JointType\n",
2622
"from mdh.kinematic_chain import KinematicChain\n",
27-
"from mdh.models import puma500\n",
23+
"from mdh.robots import puma560\n",
2824
"\n",
2925
"import pandas\n",
3026
"\n",
@@ -37,41 +33,223 @@
3733
"metadata": {},
3834
"outputs": [
3935
{
40-
"ename": "AttributeError",
41-
"evalue": "module 'mdh' has no attribute 'models'",
42-
"output_type": "error",
43-
"traceback": [
44-
"\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
45-
"\u001b[0;31mAttributeError\u001b[0m Traceback (most recent call last)",
46-
"\u001b[0;32m<ipython-input-2-ff10bdcf3762>\u001b[0m in \u001b[0;36m<module>\u001b[0;34m\u001b[0m\n\u001b[0;32m----> 1\u001b[0;31m \u001b[0mparams\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mmdh\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mmodels\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mpuma500\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m 2\u001b[0m \u001b[0mdf\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mpandas\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mDataFrame\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mparams\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
47-
"\u001b[0;31mAttributeError\u001b[0m: module 'mdh' has no attribute 'models'"
36+
"data": {
37+
"text/html": [
38+
"<div>\n",
39+
"<style scoped>\n",
40+
" .dataframe tbody tr th:only-of-type {\n",
41+
" vertical-align: middle;\n",
42+
" }\n",
43+
"\n",
44+
" .dataframe tbody tr th {\n",
45+
" vertical-align: top;\n",
46+
" }\n",
47+
"\n",
48+
" .dataframe thead th {\n",
49+
" text-align: right;\n",
50+
" }\n",
51+
"</style>\n",
52+
"<table border=\"1\" class=\"dataframe\">\n",
53+
" <thead>\n",
54+
" <tr style=\"text-align: right;\">\n",
55+
" <th></th>\n",
56+
" <th>alpha</th>\n",
57+
" <th>a</th>\n",
58+
" <th>theta</th>\n",
59+
" <th>d</th>\n",
60+
" <th>type</th>\n",
61+
" </tr>\n",
62+
" </thead>\n",
63+
" <tbody>\n",
64+
" <tr>\n",
65+
" <th>0</th>\n",
66+
" <td>0.000000</td>\n",
67+
" <td>0.0000</td>\n",
68+
" <td>0.0</td>\n",
69+
" <td>0.0000</td>\n",
70+
" <td>1</td>\n",
71+
" </tr>\n",
72+
" <tr>\n",
73+
" <th>1</th>\n",
74+
" <td>-1.570796</td>\n",
75+
" <td>0.0000</td>\n",
76+
" <td>0.0</td>\n",
77+
" <td>0.0000</td>\n",
78+
" <td>1</td>\n",
79+
" </tr>\n",
80+
" <tr>\n",
81+
" <th>2</th>\n",
82+
" <td>0.000000</td>\n",
83+
" <td>0.6127</td>\n",
84+
" <td>0.0</td>\n",
85+
" <td>0.0000</td>\n",
86+
" <td>1</td>\n",
87+
" </tr>\n",
88+
" <tr>\n",
89+
" <th>3</th>\n",
90+
" <td>0.000000</td>\n",
91+
" <td>0.5716</td>\n",
92+
" <td>0.0</td>\n",
93+
" <td>0.1639</td>\n",
94+
" <td>1</td>\n",
95+
" </tr>\n",
96+
" <tr>\n",
97+
" <th>4</th>\n",
98+
" <td>-1.570796</td>\n",
99+
" <td>0.0000</td>\n",
100+
" <td>0.0</td>\n",
101+
" <td>0.1157</td>\n",
102+
" <td>1</td>\n",
103+
" </tr>\n",
104+
" </tbody>\n",
105+
"</table>\n",
106+
"</div>"
107+
],
108+
"text/plain": [
109+
" alpha a theta d type\n",
110+
"0 0.000000 0.0000 0.0 0.0000 1\n",
111+
"1 -1.570796 0.0000 0.0 0.0000 1\n",
112+
"2 0.000000 0.6127 0.0 0.0000 1\n",
113+
"3 0.000000 0.5716 0.0 0.1639 1\n",
114+
"4 -1.570796 0.0000 0.0 0.1157 1"
115+
]
116+
},
117+
"execution_count": 2,
118+
"metadata": {},
119+
"output_type": "execute_result"
120+
}
121+
],
122+
"source": [
123+
"params = puma560()\n",
124+
"df = pandas.DataFrame(params)\n",
125+
"df.head()"
126+
]
127+
},
128+
{
129+
"cell_type": "code",
130+
"execution_count": 3,
131+
"metadata": {},
132+
"outputs": [
133+
{
134+
"name": "stdout",
135+
"output_type": "stream",
136+
"text": [
137+
"1: alpha: 0.0deg a: 0.0m theta: 0e+00deg d: 0.0\n",
138+
"1: alpha: -0.0deg a: 0.0m theta: 0e+00deg d: 0.0\n",
139+
"1: alpha: 0.0deg a: 0.6m theta: 0e+00deg d: 0.0\n",
140+
"1: alpha: 0.0deg a: 0.6m theta: 0e+00deg d: 0.2\n",
141+
"1: alpha: -0.0deg a: 0.0m theta: 0e+00deg d: 0.1\n",
142+
"1: alpha: 0.0deg a: 0.0m theta: 0.05deg d: 0.1\n"
48143
]
49144
}
50145
],
51146
"source": [
52-
"params = mdh.models.puma500()\n",
53-
"df = pandas.DataFrame(params)"
147+
"puma = KinematicChain.from_parameters(params)\n",
148+
"for l in puma:\n",
149+
" print(l)"
54150
]
55151
},
56152
{
57153
"cell_type": "code",
58154
"execution_count": 4,
59155
"metadata": {},
156+
"outputs": [
157+
{
158+
"name": "stdout",
159+
"output_type": "stream",
160+
"text": [
161+
"Can't reach: [0.015, 0.015, 0.15]m\n"
162+
]
163+
}
164+
],
165+
"source": [
166+
"try:\n",
167+
" rads = puma.inverse([.015,.015,.15])\n",
168+
" # degs = np.rad2deg(rads)\n",
169+
" print(rads)\n",
170+
"except mdh.UnReachable as e:\n",
171+
" print(e)"
172+
]
173+
},
174+
{
175+
"cell_type": "code",
176+
"execution_count": 5,
177+
"metadata": {},
178+
"outputs": [],
179+
"source": [
180+
"from scipy.spatial.transform import Rotation as R"
181+
]
182+
},
183+
{
184+
"cell_type": "code",
185+
"execution_count": 8,
186+
"metadata": {},
187+
"outputs": [],
188+
"source": [
189+
"from math import pi, atan2"
190+
]
191+
},
192+
{
193+
"cell_type": "code",
194+
"execution_count": 11,
195+
"metadata": {},
196+
"outputs": [
197+
{
198+
"name": "stdout",
199+
"output_type": "stream",
200+
"text": [
201+
"<scipy.spatial.transform.rotation.Rotation object at 0x7f6ed5e802d0>\n"
202+
]
203+
}
204+
],
205+
"source": [
206+
"r=R.from_euler('zyx', [-90,atan2(10,10)*180/pi,90], degrees=True)\n",
207+
"print(r)"
208+
]
209+
},
210+
{
211+
"cell_type": "code",
212+
"execution_count": 17,
213+
"metadata": {},
60214
"outputs": [
61215
{
62216
"data": {
63217
"text/plain": [
64-
"'0.1.2'"
218+
"array([-90., 45., 90.])"
65219
]
66220
},
67-
"execution_count": 4,
221+
"execution_count": 17,
68222
"metadata": {},
69223
"output_type": "execute_result"
70224
}
71225
],
72226
"source": [
73-
"from importlib_metadata import version\n",
74-
"version(\"mdh\")"
227+
"r.as_euler('zyx', degrees=True)"
228+
]
229+
},
230+
{
231+
"cell_type": "code",
232+
"execution_count": 21,
233+
"metadata": {},
234+
"outputs": [
235+
{
236+
"data": {
237+
"text/plain": [
238+
"array([-90.00999997, -44.99999992, 89.99999979])"
239+
]
240+
},
241+
"execution_count": 21,
242+
"metadata": {},
243+
"output_type": "execute_result"
244+
}
245+
],
246+
"source": [
247+
"rr = [[ -0.00012341, 0.70710677, -0.70710678],\n",
248+
" [ 0.00012341, -0.70710677, -0.70710678],\n",
249+
" [ -0.99999998, -0.00017453, 0.]]\n",
250+
"\n",
251+
"r = R.from_matrix(rr)\n",
252+
"r.as_euler('zyx', degrees=True)"
75253
]
76254
},
77255
{

‎mdh/__init__.py‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -9,3 +9,5 @@
99
__version__ = version("mdh")
1010
__author__ = "Kevin J. Walchko"
1111
__license__ = "MIT"
12+
13+
from .kinematic_chain import UnReachable

0 commit comments

Comments
 (0)