Skip to content

Commit 80d9b7a

Browse files
petercorkepetercorke
committed
changes to models and .bd files to track bdsim 1.1
1 parent 083cd0e commit 80d9b7a

34 files changed

+746
-381
lines changed

‎RVC3/models/IBVS-arm-main.py

100644100755
Lines changed: 14 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,11 @@
1+
#! /usr/bin/env python
2+
3+
"""
4+
Creates Fig 16.5
5+
Robotics, Vision & Control for Python, P. Corke, Springer 2023.
6+
Copyright (c) 2021- Peter Corke
7+
"""
8+
19
from pathlib import Path
210
from machinevisiontoolbox import *
311
from roboticstoolbox import *
@@ -7,19 +15,17 @@
715
from spatialmath import SE3
816
import matplotlib.pyplot as plt
917

10-
1118
sim = BDSim(animation=True)
1219

13-
1420
bd = sim.blockdiagram()
1521
print(sim.options)
1622
sim.set_options(animation=True)
1723

18-
clock = bd.clock(10, unit='Hz')
24+
clock = bd.clock(10, unit="Hz")
1925

2026
robot = models.DH.Puma560()
2127
print(robot)
22-
q0 = [0, pi/4, pi, 0, pi/4, -pi/4]
28+
q0 = [0, pi / 4, pi, 0, pi / 4, -pi / 4]
2329

2430
camera = CentralCamera.Default()
2531

@@ -29,23 +35,15 @@
2935
print(model)
3036
# print(sim.argv)
3137

38+
3239
def plot_init(camera):
33-
print('@@@@@plot_init')
34-
camera.plot_point(bd['p*'].value, 'b*')
40+
print("@@@@@plot_init")
41+
camera.plot_point(bd["p*"].value, "b*")
3542

3643

3744
bd = bdload(bd, model, globalvars=globals())
3845
bd.compile()
39-
bd.report()
40-
41-
print(bd['P'].value)
4246

43-
print('about to run')
44-
print(sim.options)
45-
sim.set_options(animation=False)
47+
sim.report(bd)
4648
out = sim.run(bd, 10)
47-
print('done')
4849
print(out)
49-
50-
# sim.done(bd)
51-
# plt.pause(5)

‎RVC3/models/IBVS-arm.bd

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -923,7 +923,7 @@
923923
"parameters": [
924924
[
925925
"nin",
926-
1
926+
null
927927
],
928928
[
929929
"vector",
@@ -979,7 +979,7 @@
979979
"parameters": [
980980
[
981981
"nin",
982-
1
982+
null
983983
],
984984
[
985985
"vector",

‎RVC3/models/IBVS-holonomic-main.py

100644100755
Lines changed: 21 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,11 @@
1+
#! /usr/bin/env python
2+
3+
"""
4+
Creates Fig 16.6
5+
Robotics, Vision & Control for Python, P. Corke, Springer 2023.
6+
Copyright (c) 2021- Peter Corke
7+
"""
8+
19
from pathlib import Path
210
from machinevisiontoolbox import *
311
from roboticstoolbox import *
@@ -8,41 +16,41 @@
816
from math import pi
917

1018

11-
sim = BDSim(animation=True) #debug='i')
19+
sim = BDSim(animation=True) # debug='i')
1220
bd = sim.blockdiagram()
1321
sim.set_options(animation=True)
14-
clock = bd.clock(0.1, unit='s')
22+
clock = bd.clock(0.1, unit="s")
1523

1624
# wide angle camera
1725
camera = CentralCamera.Default(f=0.002)
1826

1927
# camera optical axis is upward
20-
T_B_C = SE3(0.2, 0.1, 0.3)
28+
T_B_C = SE3(0.2, 0.1, 0.3)
2129

2230
# landmark points on the ceiling
23-
P = np.array([[0, 1, 3], [0, -1, 3]]).T;
31+
P = np.array([[0, 1, 3], [0, -1, 3]]).T
2432

2533
# desired landmark position on the image plane
26-
pd = camera.project_point(P, pose=SE3(-2, 0, 0)*T_B_C)
34+
pd = camera.project_point(P, pose=SE3(-2, 0, 0) * T_B_C)
2735

2836
q0 = [-6, 2, 0.6]
2937
lmbda = 1
3038

39+
3140
def plot_init(camera):
32-
camera.plot_point(pd, 'b*')
41+
camera.plot_point(pd, "b*")
42+
3343

3444
def world_init(ax):
35-
# plot X, Y coords of world points
36-
ax.plot(P[0, :], P[1, :], 'b*')
45+
# plot X, Y coords of world points
46+
ax.plot(P[0, :], P[1, :], "b*")
47+
3748

3849
# load the bdsim model
3950
model = Path(__file__).parent / "IBVS-holonomic.bd"
4051
bd = bdload(bd, model, globalvars=globals())
41-
4252
bd.compile()
43-
bd.report()
4453

54+
sim.report(bd)
55+
out = sim.run(bd, 10)
4556
print(out)
46-
47-
# sim.done(bd)
48-
# plt.pause(5)

‎RVC3/models/IBVS-holonomic.bd

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -702,7 +702,7 @@
702702
"parameters": [
703703
[
704704
"nin",
705-
1
705+
null
706706
],
707707
[
708708
"vector",
@@ -948,7 +948,7 @@
948948
"parameters": [
949949
[
950950
"pose",
951-
"=T_vc"
951+
"=T_B_C"
952952
],
953953
[
954954
"blockargs",

‎RVC3/models/IBVS-main.py

100644100755
File mode changed.

‎RVC3/models/IBVS-nonholonomic-main.py

100644100755
Lines changed: 42 additions & 32 deletions
Original file line numberDiff line numberDiff line change
@@ -1,75 +1,85 @@
1+
#! /usr/bin/env python
2+
3+
"""
4+
Creates Fig 16.8
5+
Robotics, Vision & Control for Python, P. Corke, Springer 2023.
6+
Copyright (c) 2021- Peter Corke
7+
"""
8+
19
from pathlib import Path
210
from machinevisiontoolbox import *
311
from roboticstoolbox import *
412

5-
from bdsim import Clock, bdload, BDSim
13+
from bdsim import bdload, BDSim
614
from spatialmath import SE3
715
import matplotlib.pyplot as plt
816
from math import pi, sqrt, atan2
917

1018

11-
sim = BDSim(animation=True) #debug='i')
19+
sim = BDSim(animation=True) # debug='i')
1220
bd = sim.blockdiagram()
1321

14-
clock = bd.clock(0.1, unit='s')
15-
1622
# wide angle camera
1723
camera = CentralCamera.Default(f=0.002)
18-
T_vc = SE3(0.2, 0.1, 0.3) * SE3.Ry(pi/2) * SE3.Rz(-pi/2) #*trotx(-pi/4);
24+
T_vc = SE3(0.2, 0.1, 0.3) * SE3.Ry(pi / 2) * SE3.Rz(-pi / 2) # *trotx(-pi/4);
1925
P = np.array([[4.0, 10, 3], [9, 9, 2], [6, 8, 3], [8, 10, 4], [10, 7, 2]]).T
20-
T_B_C = SE3(0.2, 0.1, 0.5) * SE3.Ry(pi/2) * SE3.Rz(-pi/2)
21-
x0 = [2, 2, 0];
26+
T_B_C = SE3(0.2, 0.1, 0.5) * SE3.Ry(pi / 2) * SE3.Rz(-pi / 2)
27+
x0 = [2, 2, 0]
2228
# xg = [5 5 pi/2];
2329

30+
31+
def plot_init(camera):
32+
pd = camera.project_point(P, pose=SE3(-2, 0, 0) * T_B_C)
33+
camera.plot_point(pd, "b*")
34+
35+
2436
# convert x,y,theta state to polar form
2537
def polar(x, dict):
26-
rho = sqrt(x[0]**2 + x[1]**2)
38+
rho = sqrt(x[0] ** 2 + x[1] ** 2)
2739

28-
if not 'direction' in dict:
40+
if not "direction" in dict:
2941
# direction not yet set, set it
3042
beta = -atan2(-x[1], -x[0])
3143
alpha = -x[2] - beta
32-
print('alpha', alpha)
44+
print("alpha", alpha)
3345
if -pi / 2 <= alpha <= pi / 2:
34-
dict['direction'] = 1
46+
dict["direction"] = 1
3547
else:
36-
dict['direction'] = -1
37-
print('set direction to ', dict['direction'])
48+
dict["direction"] = -1
49+
print("set direction to ", dict["direction"])
3850

39-
if dict['direction'] == -1:
40-
beta = -atan2(x[1], x[0]);
51+
if dict["direction"] == -1:
52+
beta = -atan2(x[1], x[0])
4153
else:
4254
beta = -atan2(-x[1], -x[0])
4355
alpha = -x[2] - beta
4456

4557
# clip alpha
46-
if alpha > pi/2:
47-
alpha = pi/2
48-
elif alpha < -pi/2:
49-
alpha = -pi/2
58+
if alpha > pi / 2:
59+
alpha = pi / 2
60+
elif alpha < -pi / 2:
61+
alpha = -pi / 2
62+
63+
return [
64+
dict["direction"],
65+
rho,
66+
alpha,
67+
beta,
68+
]
5069

51-
return [dict['direction'], rho, alpha, beta, ]
5270

5371
model = Path(__file__).parent / "IBVS-nonholonomic.bd"
5472
print(model)
5573

5674

5775
def world_init(ax):
58-
# plot X, Y coords of world points
59-
ax.plot(P[0, :], P[1, :], 'b*')
76+
# plot X, Y coords of world points
77+
ax.plot(P[0, :], P[1, :], "b*")
6078

6179

6280
bd = bdload(bd, model, globalvars=globals())
63-
bd.report()
6481
bd.compile()
65-
bd.report()
6682

67-
print('about to run')
68-
print(sim.options)
69-
sim.set_options(animation=False)
70-
out = sim.run(bd, 30)
71-
print('done')
83+
sim.report(bd)
84+
out = sim.run(bd, 10)
7285
print(out)
73-
74-
# sim.done(bd)
75-
# plt.pause(5)

‎RVC3/models/IBVS-nonholonomic.bd

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1219,7 +1219,7 @@
12191219
],
12201220
[
12211221
"init",
1222-
null
1222+
"=plot_init"
12231223
],
12241224
[
12251225
"blockargs",
@@ -1393,7 +1393,7 @@
13931393
"parameters": [
13941394
[
13951395
"nin",
1396-
1
1396+
null
13971397
],
13981398
[
13991399
"vector",

‎RVC3/models/IBVS-partitioned.bd

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1034,7 +1034,7 @@
10341034
],
10351035
[
10361036
"vector",
1037-
0
1037+
null
10381038
],
10391039
[
10401040
"styles",
@@ -1090,7 +1090,7 @@
10901090
"parameters": [
10911091
[
10921092
"nin",
1093-
1
1093+
null
10941094
],
10951095
[
10961096
"vector",

0 commit comments

Comments
 (0)
pFad - Phonifier reborn

Pfad - The Proxy pFad of © 2024 Garber Painting. All rights reserved.

Note: This service is not intended for secure transactions such as banking, social media, email, or purchasing. Use at your own risk. We assume no liability whatsoever for broken pages.


Alternative Proxies:

Alternative Proxy

pFad Proxy

pFad v3 Proxy

pFad v4 Proxy