Skip to content

Commit 2d5e1be

Browse files
mstoelzlemstoelzle
committed
Add examples
1 parent 2d76495 commit 2d5e1be

File tree

3 files changed

+217
-1
lines changed

3 files changed

+217
-1
lines changed

‎README.md‎

Lines changed: 33 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -20,8 +20,40 @@ pip install turtle-loc-oracles
2020
```
2121

2222
## Usage
23+
We take a functional programming approach in this package. Here, is a simple example how the
24+
oracle motion functions can be constructed with the provided factory functions.
25+
26+
```python
27+
import numpy as np
28+
from turtle_loc_oracles.task_space import green_sea_turtle_task_space_trajectory_factory
29+
30+
# the spatial scaling factor
31+
sf = 1.0
32+
# the temporal scaling factor
33+
sw = 1.0
34+
# the positional offset
35+
x_off = np.array([1.0, 1.0, 1.0])
36+
37+
# call the oracle factory function
38+
x_fn, x_d_fn, x_dd_fn, th_fn, th_d_fn, th_dd_fn = green_sea_turtle_task_space_trajectory_factory(
39+
sf=sf, sw=sw, x_off=x_off
40+
)
41+
42+
# create a vector with time stamps
43+
ts = np.linspace(0, 10, 1000)
44+
45+
# evaluate the oracle at each time step
46+
# position, velocity, and acceleration
47+
x_ts = np.array([x_fn(t) for t in ts])
48+
x_d_ts = np.array([x_d_fn(t) for t in ts])
49+
x_dd_ts = np.array([x_dd_fn(t) for t in ts])
50+
# twist angle, twist angular velocity, and twist angular acceleration
51+
th_ts = np.array([th_fn(t) for t in ts])
52+
th_d_ts = np.array([th_d_fn(t) for t in ts])
53+
th_dd_ts = np.array([th_dd_fn(t) for t in ts])
54+
```
2355

24-
56+
More examples can be found in the `examples` directory.
2557

2658
## Provided Oracles
2759

‎examples/demo_cornelia_turtle_robot_joint_space_oracle.py‎

Lines changed: 67 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,67 @@
1+
import matplotlib.pyplot as plt
2+
import numpy as np
3+
4+
from turtle_loc_oracles.joint_space import cornelia_turtle_robot_joint_space_trajectory_factory
5+
6+
# the spatial scaling factor
7+
sf = 1.0
8+
# the temporal scaling factor
9+
sw = 1.0
10+
# the positional joint angle offset
11+
q_off = np.array([0.0, 0.0, 0.0])
12+
13+
if __name__ == "__main__":
14+
# call the oracle factory function
15+
q_fn, q_d_fn, q_dd_fn = cornelia_turtle_robot_joint_space_trajectory_factory(
16+
sf=sf, sw=sw, q_off=q_off
17+
)
18+
19+
# create a vector with time stamps
20+
ts = np.linspace(0, 10, 1000)
21+
22+
# evaluate the oracle at each time step
23+
# position, velocity, and acceleration in joint space
24+
q_ts = np.array([q_fn(t) for t in ts])
25+
q_d_ts = np.array([q_d_fn(t) for t in ts])
26+
q_dd_ts = np.array([q_dd_fn(t) for t in ts])
27+
28+
# plot the oracle
29+
dpi = 150
30+
fig = plt.figure(dpi=dpi)
31+
ax = fig.gca()
32+
ax.plot(ts, q_ts[:, 0], label=r"$q_1$")
33+
ax.plot(ts, q_ts[:, 1], label=r"$q_2$")
34+
ax.plot(ts, q_ts[:, 2], label=r"$q_3$")
35+
ax.set_xlabel("Time [s]")
36+
ax.set_ylabel("Joint angles [rad]")
37+
ax.legend()
38+
ax.grid(True)
39+
plt.box(True)
40+
plt.tight_layout()
41+
plt.show()
42+
43+
fig = plt.figure(dpi=dpi)
44+
ax = fig.gca()
45+
ax.plot(ts, q_d_ts[:, 0], label=r"$\dot{q}_1$")
46+
ax.plot(ts, q_d_ts[:, 1], label=r"$\dot{q}_2$")
47+
ax.plot(ts, q_d_ts[:, 2], label=r"$\dot{q}_3$")
48+
ax.set_xlabel("Time [s]")
49+
ax.set_ylabel("Joint velocities [rad/s]")
50+
ax.legend()
51+
ax.grid(True)
52+
plt.box(True)
53+
plt.tight_layout()
54+
plt.show()
55+
56+
# plot joint angles in 3D space with the velocity magnitude as color
57+
q_d_norm_ts = np.linalg.norm(q_d_ts, axis=1)
58+
fig = plt.figure()
59+
ax = fig.add_subplot(111, projection="3d")
60+
sc = ax.scatter(q_ts[:, 0], q_ts[:, 1], q_ts[:, 2], c=q_d_norm_ts, cmap="coolwarm")
61+
fig.colorbar(sc, shrink=0.7, label=r"$|\dot{q}|_2$")
62+
ax.set_xlabel(r"$q_1$ [m]")
63+
ax.set_ylabel(r"$q_2$ [m]")
64+
ax.set_zlabel(r"$q_3$ [m]")
65+
ax.grid(True)
66+
ax.set_aspect("equal")
67+
plt.show()

‎examples/demo_green_sea_turtle_swimming_oracle.py‎

Lines changed: 117 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,117 @@
1+
import matplotlib.pyplot as plt
2+
import numpy as np
3+
4+
from turtle_loc_oracles.task_space import green_sea_turtle_task_space_trajectory_factory
5+
6+
# the spatial scaling factor
7+
sf = 1.0
8+
# the temporal scaling factor
9+
sw = 1.0
10+
# the positional offset
11+
x_off = np.array([1.0, 1.0, 1.0])
12+
13+
if __name__ == "__main__":
14+
# call the oracle factory function
15+
x_fn, x_d_fn, x_dd_fn, th_fn, th_d_fn, th_dd_fn = green_sea_turtle_task_space_trajectory_factory(
16+
sf=sf, sw=sw, x_off=x_off
17+
)
18+
19+
# create a vector with time stamps
20+
ts = np.linspace(0, 10, 1000)
21+
22+
# evaluate the oracle at each time step
23+
# position, velocity, and acceleration
24+
x_ts = np.array([x_fn(t) for t in ts])
25+
x_d_ts = np.array([x_d_fn(t) for t in ts])
26+
x_dd_ts = np.array([x_dd_fn(t) for t in ts])
27+
# twist angle, twist angular velocity, and twist angular acceleration
28+
th_ts = np.array([th_fn(t) for t in ts])
29+
th_d_ts = np.array([th_d_fn(t) for t in ts])
30+
th_dd_ts = np.array([th_dd_fn(t) for t in ts])
31+
32+
# plot the oracle
33+
dpi = 150
34+
fig = plt.figure(dpi=dpi)
35+
ax = fig.gca()
36+
ax.plot(ts, x_ts[:, 0], label=r"$x$")
37+
ax.plot(ts, x_ts[:, 1], label=r"$y$")
38+
ax.plot(ts, x_ts[:, 2], label=r"$z$")
39+
ax.set_xlabel("Time [s]")
40+
ax.set_ylabel("Task space position [m]")
41+
ax.legend()
42+
ax.grid(True)
43+
plt.box(True)
44+
plt.tight_layout()
45+
plt.show()
46+
47+
fig = plt.figure(dpi=dpi)
48+
ax = fig.gca()
49+
ax.plot(ts, x_d_ts[:, 0], label=r"$\dot{x}$")
50+
ax.plot(ts, x_d_ts[:, 1], label=r"$\dot{y}$")
51+
ax.plot(ts, x_d_ts[:, 2], label=r"$\dot{z}$")
52+
ax.set_xlabel("Time [s]")
53+
ax.set_ylabel("Task space velocity [m/s]")
54+
ax.legend()
55+
ax.grid(True)
56+
plt.box(True)
57+
plt.tight_layout()
58+
plt.show()
59+
60+
fig = plt.figure(dpi=dpi)
61+
ax = fig.gca()
62+
ax.plot(ts, x_dd_ts[:, 0], label=r"$\ddot{x}$")
63+
ax.plot(ts, x_dd_ts[:, 1], label=r"$\ddot{y}$")
64+
ax.plot(ts, x_dd_ts[:, 2], label=r"$\ddot{z}$")
65+
ax.set_xlabel("Time [s]")
66+
ax.set_ylabel(r"Task space acceleration [m/s$^2$]")
67+
ax.legend()
68+
ax.grid(True)
69+
plt.box(True)
70+
plt.tight_layout()
71+
plt.show()
72+
73+
# plot the position in 3D space with the velocity magnitude as color
74+
x_d_norm_ts = np.linalg.norm(x_d_ts, axis=1)
75+
fig = plt.figure()
76+
ax = fig.add_subplot(111, projection="3d")
77+
sc = ax.scatter(x_ts[:, 0], x_ts[:, 1], x_ts[:, 2], c=x_d_norm_ts, cmap="coolwarm")
78+
fig.colorbar(sc, shrink=0.7, label=r"$|\dot{x}|_2$")
79+
ax.set_xlabel(r"$x$ [m]")
80+
ax.set_ylabel(r"$y$ [m]")
81+
ax.set_zlabel(r"$z$ [m]")
82+
ax.grid(True)
83+
ax.set_aspect("equal")
84+
plt.show()
85+
86+
fig = plt.figure(dpi=dpi)
87+
ax = fig.gca()
88+
ax.plot(ts, th_ts, label=r"$\theta$")
89+
ax.set_xlabel("Time [s]")
90+
ax.set_ylabel("Flipper twist angle [rad]")
91+
ax.legend()
92+
ax.grid(True)
93+
plt.box(True)
94+
plt.tight_layout()
95+
plt.show()
96+
97+
fig = plt.figure(dpi=dpi)
98+
ax = fig.gca()
99+
ax.plot(ts, th_d_ts, label=r"$\dot{\theta}$")
100+
ax.set_xlabel("Time [s]")
101+
ax.set_ylabel("Flipper twist angular velocity [rad/s]")
102+
ax.legend()
103+
ax.grid(True)
104+
plt.box(True)
105+
plt.tight_layout()
106+
plt.show()
107+
108+
fig = plt.figure(dpi=dpi)
109+
ax = fig.gca()
110+
ax.plot(ts, th_dd_ts, label=r"$\ddot{\theta}$")
111+
ax.set_xlabel("Time [s]")
112+
ax.set_ylabel(r"Flipper twist angular acceleration [rad/s$^2$]")
113+
ax.legend()
114+
ax.grid(True)
115+
plt.box(True)
116+
plt.tight_layout()
117+
plt.show()

0 commit comments

Comments
 (0)