Skip to content

Commit 268f4d0

Browse files
committed
fix merge conf
2 parents 077f120 + b80b7c9 commit 268f4d0

File tree

19 files changed

+399
-193
lines changed

19 files changed

+399
-193
lines changed

.github/FUNDING.yml

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,4 @@
11
# These are supported funding model platforms
2+
github: AtsushiSakai
23
patreon: myenigma
34
custom: https://www.paypal.me/myenigmapay/

.github/workflows/greetings.yml

Lines changed: 13 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,13 @@
1+
name: Greetings
2+
3+
on: [pull_request, issues]
4+
5+
jobs:
6+
greeting:
7+
runs-on: ubuntu-latest
8+
steps:
9+
- uses: actions/first-interaction@v1
10+
with:
11+
repo-token: ${{ secrets.GITHUB_TOKEN }}
12+
issue-message: 'Message that will be displayed on users'' This is first issue for you on this project'
13+
pr-message: 'Message that will be displayed on users'' This is first pr for you on this project'

.github/workflows/pythonpackage.yml

Lines changed: 34 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,34 @@
1+
name: Python package
2+
3+
on: [push]
4+
5+
jobs:
6+
build:
7+
8+
runs-on: ubuntu-latest
9+
strategy:
10+
max-parallel: 4
11+
matrix:
12+
python-version: [3.6, 3.7]
13+
14+
steps:
15+
- uses: actions/checkout@v1
16+
- name: Set up Python ${{ matrix.python-version }}
17+
uses: actions/setup-python@v1
18+
with:
19+
python-version: ${{ matrix.python-version }}
20+
- name: Install dependencies
21+
run: |
22+
python -m pip install --upgrade pip
23+
pip install -r requirements.txt
24+
- name: Lint with flake8
25+
run: |
26+
pip install flake8
27+
# stop the build if there are Python syntax errors or undefined names
28+
flake8 . --count --select=E9,F63,F7,F82 --show-source --statistics
29+
# exit-zero treats all errors as warnings. The GitHub editor is 127 chars wide
30+
flake8 . --count --exit-zero --max-complexity=10 --max-line-length=127 --statistics
31+
- name: install coverage
32+
run: pip install coverage
33+
- name: Test
34+
run: bash runtests.sh
Lines changed: 196 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,196 @@
1+
"""
2+
Class of n-link arm in 3D
3+
Author: Takayuki Murooka (takayuki5168)
4+
"""
5+
import numpy as np
6+
import math
7+
from mpl_toolkits.mplot3d import Axes3D
8+
import matplotlib.pyplot as plt
9+
10+
class Link:
11+
def __init__(self, dh_params):
12+
self.dh_params_ = dh_params
13+
14+
def transformation_matrix(self):
15+
theta = self.dh_params_[0]
16+
alpha = self.dh_params_[1]
17+
a = self.dh_params_[2]
18+
d = self.dh_params_[3]
19+
20+
st = math.sin(theta)
21+
ct = math.cos(theta)
22+
sa = math.sin(alpha)
23+
ca = math.cos(alpha)
24+
trans = np.array([[ct, -st * ca, st * sa, a * ct],
25+
[st, ct * ca, -ct * sa, a * st],
26+
[0, sa, ca, d],
27+
[0, 0, 0, 1]])
28+
29+
return trans
30+
31+
def basic_jacobian(self, trans_prev, ee_pos):
32+
pos_prev = np.array([trans_prev[0, 3], trans_prev[1, 3], trans_prev[2, 3]])
33+
z_axis_prev = np.array([trans_prev[0, 2], trans_prev[1, 2], trans_prev[2, 2]])
34+
35+
basic_jacobian = np.hstack((np.cross(z_axis_prev, ee_pos - pos_prev), z_axis_prev))
36+
return basic_jacobian
37+
38+
39+
class NLinkArm:
40+
def __init__(self, dh_params_list):
41+
self.link_list = []
42+
for i in range(len(dh_params_list)):
43+
self.link_list.append(Link(dh_params_list[i]))
44+
45+
def transformation_matrix(self):
46+
trans = np.identity(4)
47+
for i in range(len(self.link_list)):
48+
trans = np.dot(trans, self.link_list[i].transformation_matrix())
49+
return trans
50+
51+
def forward_kinematics(self, plot=False):
52+
trans = self.transformation_matrix()
53+
54+
x = trans[0, 3]
55+
y = trans[1, 3]
56+
z = trans[2, 3]
57+
alpha, beta, gamma = self.euler_angle()
58+
59+
if plot:
60+
self.fig = plt.figure()
61+
self.ax = Axes3D(self.fig)
62+
63+
x_list = []
64+
y_list = []
65+
z_list = []
66+
67+
trans = np.identity(4)
68+
69+
x_list.append(trans[0, 3])
70+
y_list.append(trans[1, 3])
71+
z_list.append(trans[2, 3])
72+
for i in range(len(self.link_list)):
73+
trans = np.dot(trans, self.link_list[i].transformation_matrix())
74+
x_list.append(trans[0, 3])
75+
y_list.append(trans[1, 3])
76+
z_list.append(trans[2, 3])
77+
78+
self.ax.plot(x_list, y_list, z_list, "o-", color="#00aa00", ms=4, mew=0.5)
79+
self.ax.plot([0], [0], [0], "o")
80+
81+
self.ax.set_xlim(-1, 1)
82+
self.ax.set_ylim(-1, 1)
83+
self.ax.set_zlim(-1, 1)
84+
85+
plt.show()
86+
87+
return [x, y, z, alpha, beta, gamma]
88+
89+
def basic_jacobian(self):
90+
ee_pos = self.forward_kinematics()[0:3]
91+
basic_jacobian_mat = []
92+
93+
trans = np.identity(4)
94+
for i in range(len(self.link_list)):
95+
basic_jacobian_mat.append(self.link_list[i].basic_jacobian(trans, ee_pos))
96+
trans = np.dot(trans, self.link_list[i].transformation_matrix())
97+
98+
return np.array(basic_jacobian_mat).T
99+
100+
def inverse_kinematics(self, ref_ee_pose, plot=False):
101+
for cnt in range(500):
102+
ee_pose = self.forward_kinematics()
103+
diff_pose = np.array(ref_ee_pose) - ee_pose
104+
105+
basic_jacobian_mat = self.basic_jacobian()
106+
alpha, beta, gamma = self.euler_angle()
107+
108+
K_zyz = np.array([[0, -math.sin(alpha), math.cos(alpha) * math.sin(beta)],
109+
[0, math.cos(alpha), math.sin(alpha) * math.sin(beta)],
110+
[1, 0, math.cos(beta)]])
111+
K_alpha = np.identity(6)
112+
K_alpha[3:, 3:] = K_zyz
113+
114+
theta_dot = np.dot(np.dot(np.linalg.pinv(basic_jacobian_mat), K_alpha), np.array(diff_pose))
115+
self.update_joint_angles(theta_dot / 100.)
116+
117+
if plot:
118+
self.fig = plt.figure()
119+
self.ax = Axes3D(self.fig)
120+
121+
x_list = []
122+
y_list = []
123+
z_list = []
124+
125+
trans = np.identity(4)
126+
127+
x_list.append(trans[0, 3])
128+
y_list.append(trans[1, 3])
129+
z_list.append(trans[2, 3])
130+
for i in range(len(self.link_list)):
131+
trans = np.dot(trans, self.link_list[i].transformation_matrix())
132+
x_list.append(trans[0, 3])
133+
y_list.append(trans[1, 3])
134+
z_list.append(trans[2, 3])
135+
136+
self.ax.plot(x_list, y_list, z_list, "o-", color="#00aa00", ms=4, mew=0.5)
137+
self.ax.plot([0], [0], [0], "o")
138+
139+
self.ax.set_xlim(-1, 1)
140+
self.ax.set_ylim(-1, 1)
141+
self.ax.set_zlim(-1, 1)
142+
143+
self.ax.plot([ref_ee_pose[0]], [ref_ee_pose[1]], [ref_ee_pose[2]], "o")
144+
plt.show()
145+
146+
def euler_angle(self):
147+
trans = self.transformation_matrix()
148+
149+
alpha = math.atan2(trans[1][2], trans[0][2])
150+
if not (-math.pi / 2 <= alpha and alpha <= math.pi / 2):
151+
alpha = math.atan2(trans[1][2], trans[0][2]) + math.pi
152+
if not (-math.pi / 2 <= alpha and alpha <= math.pi / 2):
153+
alpha = math.atan2(trans[1][2], trans[0][2]) - math.pi
154+
beta = math.atan2(trans[0][2] * math.cos(alpha) + trans[1][2] * math.sin(alpha), trans[2][2])
155+
gamma = math.atan2(-trans[0][0] * math.sin(alpha) + trans[1][0] * math.cos(alpha), -trans[0][1] * math.sin(alpha) + trans[1][1] * math.cos(alpha))
156+
157+
return alpha, beta, gamma
158+
159+
def set_joint_angles(self, joint_angle_list):
160+
for i in range(len(self.link_list)):
161+
self.link_list[i].dh_params_[0] = joint_angle_list[i]
162+
163+
def update_joint_angles(self, diff_joint_angle_list):
164+
for i in range(len(self.link_list)):
165+
self.link_list[i].dh_params_[0] += diff_joint_angle_list[i]
166+
167+
def plot(self):
168+
self.fig = plt.figure()
169+
self.ax = Axes3D(self.fig)
170+
171+
x_list = []
172+
y_list = []
173+
z_list = []
174+
175+
trans = np.identity(4)
176+
177+
x_list.append(trans[0, 3])
178+
y_list.append(trans[1, 3])
179+
z_list.append(trans[2, 3])
180+
for i in range(len(self.link_list)):
181+
trans = np.dot(trans, self.link_list[i].transformation_matrix())
182+
x_list.append(trans[0, 3])
183+
y_list.append(trans[1, 3])
184+
z_list.append(trans[2, 3])
185+
186+
self.ax.plot(x_list, y_list, z_list, "o-", color="#00aa00", ms=4, mew=0.5)
187+
self.ax.plot([0], [0], [0], "o")
188+
189+
self.ax.set_xlabel("x")
190+
self.ax.set_ylabel("y")
191+
self.ax.set_zlabel("z")
192+
193+
self.ax.set_xlim(-1, 1)
194+
self.ax.set_ylim(-1, 1)
195+
self.ax.set_zlim(-1, 1)
196+
plt.show()
Lines changed: 30 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,30 @@
1+
"""
2+
Forward Kinematics for an n-link arm in 3D
3+
Author: Takayuki Murooka (takayuki5168)
4+
"""
5+
import math
6+
from NLinkArm import NLinkArm
7+
import random
8+
9+
def random_val(min_val, max_val):
10+
return min_val + random.random() * (max_val - min_val)
11+
12+
13+
if __name__ == "__main__":
14+
print("Start solving Forward Kinematics 10 times")
15+
16+
# init NLinkArm with Denavit-Hartenberg parameters of PR2
17+
n_link_arm = NLinkArm([[0., -math.pi/2, .1, 0.],
18+
[math.pi/2, math.pi/2, 0., 0.],
19+
[0., -math.pi/2, 0., .4],
20+
[0., math.pi/2, 0., 0.],
21+
[0., -math.pi/2, 0., .321],
22+
[0., math.pi/2, 0., 0.],
23+
[0., 0., 0., 0.]])
24+
25+
# execute FK 10 times
26+
for i in range(10):
27+
n_link_arm.set_joint_angles([random_val(-1, 1) for j in range(len(n_link_arm.link_list))])
28+
29+
ee_pose = n_link_arm.forward_kinematics(plot=True)
30+
Lines changed: 32 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,32 @@
1+
"""
2+
Inverse Kinematics for an n-link arm in 3D
3+
Author: Takayuki Murooka (takayuki5168)
4+
"""
5+
import math
6+
from NLinkArm import NLinkArm
7+
import random
8+
9+
10+
def random_val(min_val, max_val):
11+
return min_val + random.random() * (max_val - min_val)
12+
13+
if __name__ == "__main__":
14+
print("Start solving Inverse Kinematics 10 times")
15+
16+
# init NLinkArm with Denavit-Hartenberg parameters of PR2
17+
n_link_arm = NLinkArm([[0., -math.pi/2, .1, 0.],
18+
[math.pi/2, math.pi/2, 0., 0.],
19+
[0., -math.pi/2, 0., .4],
20+
[0., math.pi/2, 0., 0.],
21+
[0., -math.pi/2, 0., .321],
22+
[0., math.pi/2, 0., 0.],
23+
[0., 0., 0., 0.]])
24+
25+
# execute IK 10 times
26+
for i in range(10):
27+
n_link_arm.inverse_kinematics([random_val(-0.5, 0.5),
28+
random_val(-0.5, 0.5),
29+
random_val(-0.5, 0.5),
30+
random_val(-0.5, 0.5),
31+
random_val(-0.5, 0.5),
32+
random_val(-0.5, 0.5)], plot=True)

Localization/Kalmanfilter_basics.ipynb

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -228,7 +228,7 @@
228228
"\n",
229229
"#### Central Limit Theorem\n",
230230
"\n",
231-
"According to this theorem, the average of n samples of random and independant variables tends to follow a normal distribution as we increase the sample size.(Generally, for n>=30)"
231+
"According to this theorem, the average of n samples of random and independent variables tends to follow a normal distribution as we increase the sample size.(Generally, for n>=30)"
232232
]
233233
},
234234
{

PathPlanning/DynamicWindowApproach/dynamic_window_approach.py

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,7 @@
22
33
Mobile robot motion planning sample with Dynamic Window Approach
44
5-
author: Atsushi Sakai (@Atsushi_twi), Goktug Karakasli
5+
author: Atsushi Sakai (@Atsushi_twi), Göktuğ Karakaşlı
66
77
"""
88

0 commit comments

Comments
 (0)