def init_scene_gui(self):
self.scene_controls = True
self.load_scene = False
self.object_color = contact_modes.get_color('clay')
self.object_color[3] = 0.5
self.obstacle_color = contact_modes.get_color('teal')
self.light_pos = [0, 2.0, 10.0]
def set_alpha(self, alpha):
alpha = max(min(alpha, 1.0), 0.0)
red = get_color('red')
red[3] = alpha
self.x_axis.set_color(red)
green = get_color('green')
green[3] = alpha
self.y_axis.set_color(green)
blue = get_color('blue')
blue[3] = alpha
self.z_axis.set_color(blue)
def init_scene_gui(self):
self.load_scene = True
self.solver_index = 1
self.solver_list = ['all-modes', 'cs-modes', 'csss-modes', 'exp']
self.case_index = 0
self.case_list = [
'box-case-1',
'box-case-2',
'box-case-3',
'box-case-4',
'box-case-5',
'peg-in-hole-4',
'peg-in-hole-8',
'box-box-1',
'box-box-2',
'box-box-3',
'box-box-4',
'hand-football',
'hand-football-fixed'
]
self.max_steps = 50
self.h = 0.001
self.peel_depth = 4
self.alpha = 0.7
self.object_color = get_color('clay')
self.object_color[3] = 0.5
self.obstacle_color = get_color('teal')
self.frame_scale = [0.02, 0.35, 0.50]
self.contact_color = get_color('yellow')
self.separating_color = get_color('purple')
self.contact_scale = 0.04
self.velocity_scale = [30, 0.02, 0.05, 0.035]
self.show_grid = False
self.show_contact_frames = False
self.show_contacts = False
self.show_velocities = False
self.big_lattice = True
self.lattice_height = 265
self.loop_time = 2.0
self.light_pos = [0, 2.0, 10.0]
self.cam_focus = [0.0, 0.0, 0.5]
self.plot_gui = False
def init_opengl(self, smooth=False):
# Get per face vertex positions, normals, and colors.
vertices = np.zeros((3, 3 * len(self.faces)), dtype='float32')
normals = np.zeros((3, 3 * len(self.faces)), dtype='float32')
colors = np.zeros((4, 3 * len(self.faces)), dtype='float32')
k = 0
for i in range(len(self.faces)):
f = self.faces[i]
assert (f.index == i)
h = f.halfedge
assert (k == 3 * f.index)
while True:
v = h.vertex
vertices[:, k, None] = v.position
if not smooth:
normals[:, k, None] = f.normal
else:
normals[:, k, None] = v.normal
colors[:, k] = get_color('clay')
# colors[:,k] = np.random.rand(3)
h = h.next
k += 1
if h is f.halfedge:
break
vertices = vertices.T.flatten()
normals = normals.T.flatten()
colors = colors.T.flatten()
self.num_elems_draw = len(vertices)
# Setup VAO.
self.vao = glGenVertexArrays(1)
self.vertex_vbo, self.normal_vbo, self.color_vbo = glGenBuffers(3)
glBindVertexArray(self.vao)
glBindBuffer(GL_ARRAY_BUFFER, self.vertex_vbo)
glBufferData(GL_ARRAY_BUFFER,
len(vertices) * 4, vertices, GL_STATIC_DRAW)
glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 3 * 4,
ctypes.c_void_p(0))
glEnableVertexAttribArray(0)
glBindBuffer(GL_ARRAY_BUFFER, self.normal_vbo)
glBufferData(GL_ARRAY_BUFFER,
len(normals) * 4, normals, GL_STATIC_DRAW)
glVertexAttribPointer(1, 3, GL_FLOAT, GL_FALSE, 3 * 4,
ctypes.c_void_p(0))
glEnableVertexAttribArray(1)
glBindBuffer(GL_ARRAY_BUFFER, self.color_vbo)
glBufferData(GL_ARRAY_BUFFER, len(colors) * 4, colors, GL_STATIC_DRAW)
glVertexAttribPointer(2, 4, GL_FLOAT, GL_FALSE, 4 * 4,
ctypes.c_void_p(0))
glEnableVertexAttribArray(2)
glBindVertexArray(0)
def init_hand_gui(self):
# Create hand + baton.
self.hand = AnthroHand()
self.baton = Body('baton')
self.baton.set_shape(Ellipse(10, 5, 5))
self.baton.set_transform_world(SE3.exp([0,2.5,5+0.6/2,0,0,0]))
self.collider = DynamicCollisionManager()
for link in self.hand.links:
self.collider.add_pair(self.baton, link)
manifolds = self.collider.collide()
for m in manifolds:
print(m)
self.system = System()
self.system.add_body(self.hand)
self.system.add_obstacle(self.baton)
self.system.set_collider(self.collider)
self.system.reindex_dof_masks()
# GUI parameters.
self.hand_color = get_color('clay')
self.hand_color[3] = 0.5
self.baton_color = get_color('teal')
self.baton_color[3] = 0.5
self.load_hand = True
def init_opengl(self):
# Get vertex positions, normals, and colors.
n_verts = self.num_vertices()
vertices = np.zeros((3, n_verts), dtype='float32')
normals = np.zeros((3, n_verts), dtype='float32')
colors = np.zeros((4, n_verts), dtype='float32')
for i in range(n_verts):
vertices[0:2, i] = self.vertices[:, i]
normals[2, i] = 1.0
colors[:, i] = cm.get_color('clay')
vertices = vertices.T.flatten()
normals = normals.T.flatten()
colors = colors.T.flatten()
# Setup VAO.
self.vao = glGenVertexArrays(1)
self.vertex_vbo, self.normal_vbo, self.color_vbo = glGenBuffers(3)
glBindVertexArray(self.vao)
glBindBuffer(GL_ARRAY_BUFFER, self.vertex_vbo)
glBufferData(GL_ARRAY_BUFFER,
len(vertices) * 4, vertices, GL_STATIC_DRAW)
glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 3 * 4,
ctypes.c_void_p(0))
glEnableVertexAttribArray(0)
glBindBuffer(GL_ARRAY_BUFFER, self.normal_vbo)
glBufferData(GL_ARRAY_BUFFER,
len(normals) * 4, normals, GL_STATIC_DRAW)
glVertexAttribPointer(1, 3, GL_FLOAT, GL_FALSE, 3 * 4,
ctypes.c_void_p(0))
glEnableVertexAttribArray(1)
glBindBuffer(GL_ARRAY_BUFFER, self.color_vbo)
glBufferData(GL_ARRAY_BUFFER, len(colors) * 4, colors, GL_STATIC_DRAW)
glVertexAttribPointer(2, 4, GL_FLOAT, GL_FALSE, 4 * 4,
ctypes.c_void_p(0))
glEnableVertexAttribArray(2)
glBindVertexArray(0)
def __init__(self, r1=1.0, r2=0.5, strip_color=get_color('clay')):
super(Torus, self).__init__()
self.strip_color = strip_color
# Load torus.
dae_path = os.path.join(get_data(), 'mesh', 'torus.dae')
import_mesh(dae_path, self)
# Compute toroidal coordinates.
n_verts = len(self.vertices)
P = np.zeros((3, n_verts))
for i in range(n_verts):
v = self.vertices[i]
x, y, z = v.position
theta = np.arctan2(y, x)[0]
R = SO3.exp(np.array([0, 0, -theta]))
P[:, i, None] = SO3.transform_point(R, v.position)
center = np.mean(P, axis=1, keepdims=True)
# radius = np.mean(norm(P - center, axis=0))
# self.subdivide_4_1()
# Rescale torus.
n_verts = len(self.vertices)
for i in range(n_verts):
v = self.vertices[i]
x, y, z = v.position
theta = np.arctan2(y, x)[0]
R = SO3.exp(np.array([0, 0, theta]))
c0 = SO3.transform_point(R, center)
o0 = v.position - c0
c1 = r1 * c0 / norm(c0)
o1 = r2 * o0 / norm(o0)
v.position = c1 + o1
# Rebuild torus.
self.init()
def draw_big_lattice(self, L, name='lattice', index=None):
imgui.begin_child(name, 0, self.lattice_height, border=True)
win_pos = imgui.get_window_position()
L = L.L
# Calculate rank and node separation sizes.
region_min = imgui.get_window_content_region_min()
region_max = imgui.get_window_content_region_max()
region_size = (region_max.x - region_min.x, region_max.y - region_min.y)
n_r = len(L)+1
n_n = np.max([len(l) for l in L])+1
rank_sep = region_size[1] / n_r
node_sep = region_size[0] / n_n
# radius = np.min([node_sep, rank_sep]) / 8
radius = 6.0
off_x = win_pos.x + region_min.x + node_sep
off_y = win_pos.y + region_min.y + rank_sep
draw_list = imgui.get_window_draw_list()
# Create ranks manually
color = imgui.get_color_u32_rgba(*get_color('safety yellow'))
offset = np.max([(len(l) - 1) * node_sep for l in L])/2
extents = []
for i in range(len(L)):
n_f = len(L[i])
l = max(radius, (n_f - 1) * node_sep)
x0 = round(off_x + offset + -l/2 + 0)
y0 = round(off_y + rank_sep * i)
x1 = round(off_x + offset + -l/2 + l)
y1 = round(off_y + rank_sep * i)
draw_list.add_line(x0, y0, x1, y1, color, radius)
extents.append([np.array([x0, y0]), np.array([x1, y1])])
# Add random lines to simulate a lattice structure.
thickness = 1
np.random.seed(0)
for i in range(1, len(L)):
for s0, s1 in [(1, 1), (0, 0)]:
e0 = extents[i-1]
e1 = extents[i]
# s0 = np.random.rand()
# s0 = s
p0 = s0*e0[0] + (1-s0)*e0[1]
# s1 = np.random.rand()
# s1 = 1-s
p1 = s1*e1[0] + (1-s1)*e1[1]
draw_list.add_line(p0[0], p0[1], p1[0], p1[1], color, thickness)
# Draw current index in red.
if index is not None:
e = extents[index[0]]
if len(L[index[0]]) > 1:
s = index[1]/(len(L[index[0]]) - 1)
else:
s = 0.5
p = (1-s)*e[0] + s*e[1]
red = imgui.get_color_u32_rgba(*get_color('red'))
draw_list.add_line(p[0]+radius/2, p[1], p[0]-radius/2, p[1], red, radius)
imgui.end_child()