def cairo_robot_skin_brai(cr, w=1.3, h=0.8, sensors=False):
with cairo_save(cr):
x0 = -.3
wheel_h = 0.15
wheel_w = 0.4
M = h / 2 + .8 * wheel_h
def body():
roundedrec(cr, x0, -h / 2, w, h, r=min(w, h) / 8.0)
# cr.rectangle(x0, -h / 2, w, h)
cr.move_to(0, M)
cr.line_to(0, -M)
cairo_set_color(cr, BLACK)
cr.set_line_width(0.05)
cr.stroke()
cairo_plot_with_style(cr, body, **CairoConstants.robot_body_style)
for y in [-M, +M]:
with cairo_transform(cr, t=[0, y]):
wheel(cr, w=wheel_w, h=wheel_h)
if sensors:
for y in [.3 * h, -.3 * h]:
with cairo_transform(cr, t=[0, y]):
cr.set_line_width(CairoConstants.robot_border_width)
cairo_set_color(cr, CairoConstants.robot_border_color)
cr.move_to(0, 0)
cr.line_to(1.1, 0)
cr.stroke()
r = .1
with cairo_transform(cr, t=[1.1 + r, 0]):
cr.arc(0, 0, r, +np.pi / 2, -np.pi / 2)
cr.stroke()
def cairo_skin_fly1(cr):
r = 1.0 # will be scaled by robot radius
with cairo_save(cr):
cr.scale(0.32, 0.32)
def wing():
rw = r
with cairo_save(cr):
cr.scale(1.5, 1)
cairo_plot_circle2(cr, rw / 2, 0, rw,
fill_color=[.5, .5, .5])
wa = 3 / 2
with cairo_transform(cr, t=[0, wa], r=np.pi / 2):
wing()
with cairo_transform(cr, t=[0, -wa], r=(-np.pi / 2)):
wing()
with cairo_save(cr):
cr.scale(1.5, 0.75)
cairo_plot_circle2(cr, 0, 0, r,
fill_color=WHITE, border_color=BLACK,
border_width=0.005)
cairo_plot_circle2(cr, r * 1.5, 0, r / 1.5,
fill_color=WHITE, border_color=BLACK,
border_width=0.005)
def cairo_robot_skin_omni(cr):
with cairo_save(cr):
for theta in [0, 2 * np.pi / 3, -2 * np.pi / 3]:
with cairo_transform(cr, r=theta - np.pi / 2):
with cairo_transform(cr, [0, +.8], np.pi / 2):
omni_wheel(cr)
cr.scale(0.7, 0.7) # make it stay in < 1
cairo_robot_skin_circular(cr)
def cairo_robot_skin_ddrive(cr):
with cairo_save(cr):
with cairo_transform(cr, [0, +.8], 0):
wheel(cr)
with cairo_transform(cr, [0, -.8], 0):
wheel(cr)
cr.scale(0.7, 0.7) # make it stay in < 1
cairo_robot_skin_circular(cr)
def cairo_robot_skin_tracked(cr, width=1.0, length=1.0):
with cairo_save(cr):
def track():
wheel(cr, w=length * .7, h=width / 3.0)
with cairo_transform(cr, t=[0, -width / 2.0]):
track()
with cairo_transform(cr, t=[0, +width / 2.0]):
track()
cairo_plot_rectangle(cr, -length / 2.0, -width / 2.0, length, width,
**CairoConstants.robot_body_style)
def cairo_plot_texture_circle(cr, radius, center, texture, resolution,
width_inside, width_outside):
diameter = np.pi * 2 * radius
npoints = int(np.ceil(diameter / resolution))
theta = np.linspace(0, np.pi * 2, npoints)
t = theta * radius
values = texture(t)
resolution_angle = np.pi * 2 / npoints
theta1 = theta - resolution_angle / 1.9 # 10% overlap
theta2 = theta + resolution_angle / 1.9
r0 = radius - width_inside
r1 = radius + width_outside
with cairo_transform(cr, t=center):
Ax = np.cos(theta1) * r0
Ay = np.sin(theta1) * r0
Bx = np.cos(theta1) * r1
By = np.sin(theta1) * r1
Cx = np.cos(theta2) * r1
Cy = np.sin(theta2) * r1
Dx = np.cos(theta2) * r0
Dy = np.sin(theta2) * r0
for i in range(len(t)):
cr.set_source_rgb(values[i], values[i], values[i])
cr.move_to(Ax[i], Ay[i])
cr.line_to(Bx[i], By[i])
cr.line_to(Cx[i], Cy[i])
cr.line_to(Dx[i], Dy[i])
cr.line_to(Ax[i], Ay[i])
cr.fill()
def cairo_plot_texture_circle(cr, radius, center, texture, resolution, width_inside, width_outside):
diameter = np.pi * 2 * radius
npoints = int(np.ceil(diameter / resolution))
theta = np.linspace(0, np.pi * 2, npoints)
t = theta * radius
values = texture(t)
resolution_angle = np.pi * 2 / npoints
theta1 = theta - resolution_angle / 1.9 # 10% overlap
theta2 = theta + resolution_angle / 1.9
r0 = radius - width_inside
r1 = radius + width_outside
with cairo_transform(cr, t=center):
Ax = np.cos(theta1) * r0
Ay = np.sin(theta1) * r0
Bx = np.cos(theta1) * r1
By = np.sin(theta1) * r1
Cx = np.cos(theta2) * r1
Cy = np.sin(theta2) * r1
Dx = np.cos(theta2) * r0
Dy = np.sin(theta2) * r0
for i in range(len(t)):
cr.set_source_rgb(values[i], values[i], values[i])
cr.move_to(Ax[i], Ay[i])
cr.line_to(Bx[i], By[i])
cr.line_to(Cx[i], Cy[i])
cr.line_to(Dx[i], Dy[i])
cr.line_to(Ax[i], Ay[i])
cr.fill()
def cairo_skin_fly1(cr):
r = 1.0 # will be scaled by robot radius
with cairo_save(cr):
cr.scale(0.32, 0.32)
def wing():
rw = r
with cairo_save(cr):
cr.scale(1.5, 1)
cairo_plot_circle2(cr, rw / 2, 0, rw, fill_color=[.5, .5, .5])
wa = 3 / 2
with cairo_transform(cr, t=[0, wa], r=np.pi / 2):
wing()
with cairo_transform(cr, t=[0, -wa], r=(-np.pi / 2)):
wing()
with cairo_save(cr):
cr.scale(1.5, 0.75)
cairo_plot_circle2(cr,
0,
0,
r,
fill_color=WHITE,
border_color=BLACK,
border_width=0.005)
cairo_plot_circle2(cr,
r * 1.5,
0,
r / 1.5,
fill_color=WHITE,
border_color=BLACK,
border_width=0.005)
def cairo_plot_textured_polyline(cr, points, texture, resolution, width_inside,
width_outside):
n = points.shape[1]
delta_t = 0.0
for i in range(n - 1):
p1 = points[:, i]
p2 = points[:, i + 1]
d = p2 - p1
segment_len = np.linalg.norm(d)
angle = np.arctan2(d[1], d[0])
with cairo_transform(cr, t=p1, r=angle):
cairo_plot_textured_segment(cr=cr,
length=segment_len,
texture=texture,
resolution=resolution,
offset=delta_t,
width_inside=width_inside,
width_outside=width_outside)
delta_t += segment_len
def cairo_plot_textured_polyline(cr, points, texture, resolution, width_inside, width_outside):
n = points.shape[1]
delta_t = 0.0
for i in range(n - 1):
p1 = points[:, i]
p2 = points[:, i + 1]
d = p2 - p1
segment_len = np.linalg.norm(d)
angle = np.arctan2(d[1], d[0])
with cairo_transform(cr, t=p1, r=angle):
cairo_plot_textured_segment(
cr=cr,
length=segment_len,
texture=texture,
resolution=resolution,
offset=delta_t,
width_inside=width_inside,
width_outside=width_outside,
)
delta_t += segment_len
def cairo_ref_frame(cr, l=1, x_color=[1, 0, 0], y_color=[0, 1, 0]):
with cairo_save(cr):
cairo_arrow(cr, length=l, border_width=0.05, border_color=x_color)
with cairo_transform(cr, r=np.pi / 2):
cairo_arrow(cr, length=l, border_width=0.05, border_color=y_color)
def cairo_rototranslate(cr, pose):
t, r = geometry.translation_angle_from_SE2(pose)
with cairo_transform(cr, t=t, r=r) as cr:
yield cr
def cairo_rototranslate(cr, pose):
t, r = geometry.translation_angle_from_SE2(pose)
with cairo_transform(cr, t=t, r=r)as cr:
yield cr
