def compute_dual_vrep(self):
"""Compute vertices of the dual cones."""
gravity = pymanoid.get_gravity()
def compute_stance_v2d(stance_id, primal_vertices):
stance = self.start_stance if stance_id == 0 else self.next_stance
A_O = stance.cwc
B_list, c_list = [], []
for (i, v) in enumerate(primal_vertices):
B = A_O[:, :3] + cross(A_O[:, 3:], v)
c = dot(B, gravity)
B_list.append(B)
c_list.append(c)
B = vstack(B_list)
c = hstack(c_list)
try:
return compute_dual_vertices_2d(B, c)
except QhullError:
raise TubeError("Cannot reduce polar of stance %d" % stance_id)
if len(self.primal_vrep) == 1:
vertices_2d = compute_stance_v2d(0, self.primal_vrep[0])
self.dual_vrep = [get_dual_vertices_3d(vertices_2d)]
else: # len(self.primal_vrep) == 2
ss_id, ds_id = (1, 0) if len(self.primal_vrep[0]) > 1 else (0, 1)
ds_vertices_2d = compute_stance_v2d(ds_id, self.full_vrep)
ss_vertices_2d = compute_stance_v2d(ss_id, self.primal_vrep[ss_id])
ss_vertices_2d = intersect_polygons(ds_vertices_2d, ss_vertices_2d)
ds_vertices = get_dual_vertices_3d(ds_vertices_2d)
ss_vertices = get_dual_vertices_3d(ss_vertices_2d)
if ss_id == 0:
self.dual_vrep = [ss_vertices, ds_vertices]
else: # ss_id == 1
self.dual_vrep = [ds_vertices, ss_vertices]
def compute_dual_vrep(self):
"""
Compute vertices of the dual cone for each primal tube.
NB: the two tubes can have shared vertices at which dual-cone
computations can be factored. We don't implement this optimization here.
"""
gravity = pymanoid.get_gravity()
for (stance_id, vertices) in enumerate(self.primal_vrep):
if stance_id == 0:
A_O = self.start_stance.cwc
else: # stance_id == 1
A_O = self.next_stance.cwc
B_list, c_list = [], []
for (i, v) in enumerate(vertices):
B = A_O[:, :3] + cross(A_O[:, 3:], v)
c = dot(B, gravity)
B_list.append(B)
c_list.append(c)
B = vstack(B_list)
c = hstack(c_list)
try:
cone_vertices = compute_dual_vertices(B, c)
self.dual_vrep.append(cone_vertices)
except QhullError:
raise TubeError("Cannot reduce polar of stance %d" % stance_id)
def compute_acceleration_set(contact_set, p_com, display_scale=0.05):
gravity = pymanoid.get_gravity()
# A_O = contacts.compute_wrench_cone([0, 0, 0])
A_O = compute_cwc_pyparma(contact_set, [0, 0, 0])
B = A_O[:, :3] + cross(A_O[:, 3:], p_com)
c = dot(B, gravity)
g = -gravity[2]
assert g > 0
# assert all(c > 0), "c > 0 assertion failed"
# assert all(B[:, 2] < 0)
if any(abs(c) < 1e-10):
I = [i for i in xrange(len(c)) if abs(c[i]) > 1e-10]
B, c = B[I], c[I]
check = c / B[:, 2]
assert max(check) - min(check) < 1e-10, "max - min failed"
assert abs(check[0] - (-g)) < 1e-10, "check is not -g?"
sigma = c / g
B2 = hstack([
(B[:, column] / sigma).reshape((B.shape[0], 1))
for column in [0, 1]])
vertices2d = compute_polygon_hull(B2, ones(len(c)))
def vertices_at(z):
v = [array([a * (g + z), b * (g + z)]) for (a, b) in vertices2d]
return [array([x, y, z]) for (x, y) in v]
return [array([0, 0, -g])] + vertices_at(z=+g)
def compute_dual_vertices_2d(B, c):
g = -pymanoid.get_gravity()[2]
check = c / B[:, 2]
assert max(check) - min(check) < 1e-10, "max - min failed (%.1e)" % (
(max(check) - min(check)))
assert abs(check[0] - (-g)) < 1e-10, "check is not -g?"
B_2d = hstack([B[:, column].reshape((B.shape[0], 1)) for column in [0, 1]])
sigma = c / g # algebraic distances to SEP (see paper for details)
return compute_polygon_hull(B_2d, sigma)
def check_contact_forces(com, comdd, camd):
if camd is None: # not set yet
camd = numpy.zeros(3)
try:
gravity = pymanoid.get_gravity()
wrench = numpy.hstack([robot.mass * (comdd - gravity), camd])
contacts = motion_plan.cur_stance.contacts
return contacts.find_supporting_forces(wrench, com)
except OptimalNotFound:
print "No contact forces here (t=%.2f)." % step_t
def run_forces_thread():
handles = []
while True:
try:
m = robot_mass
g = pymanoid.get_gravity()
wrench = hstack(
[m * 9.81 / Delta_zrp * (com.p - vrp.p) - m * g,
zeros(3)])
support = contacts.find_supporting_forces(wrench, com.p)
handles = [pymanoid.draw_force(c, fc) for (c, fc) in support]
viewer.SetBkgndColor([1., 1., 1.])
except Exception:
viewer.SetBkgndColor([1., 0.3, 0.3])
time.sleep(dt)
return handles
def on_tick(self, sim):
"""Find supporting contact forces at each COM acceleration update."""
comdd = preview_buffer.comdd
gravity = pymanoid.get_gravity()
wrench = hstack([robot_mass * (comdd - gravity), zeros(3)])
support = fsm.cur_stance.find_supporting_forces(
wrench, preview_buffer.com.p, robot_mass, 10.)
if not support:
self.handles = []
viewer.SetBkgndColor([.8, .4, .4])
self.last_bkgnd_switch = time.time()
else:
self.handles = [draw_force(c, fc, self.force_scale)
for (c, fc) in support]
if self.last_bkgnd_switch is not None \
and time.time() - self.last_bkgnd_switch > 0.2:
# let's keep epilepsy at bay
viewer.SetBkgndColor([.6, .6, .8])
self.last_bkgnd_switch = None
def __compute_topp_polygon_bretl(cwc, p, ps, pss, sdd_max=None, sd_max=None):
"""Compute the polygon in the (sddot,sdot^2) plane using Hauser method."""
g = get_gravity()
b1 = dot(cwc, hstack([ps, cross(p, ps)]))
b2 = dot(cwc, hstack([pss, cross(p, pss)]))
b3 = dot(cwc, hstack([g, cross(p, g)]))
n = len(b1)
G = zeros((n+1, 2))
G[0, :] = [0, -1]
G[1:, 0] = b1
G[1:, 1] = b2
h = zeros(n+1)
h[1:] = b3
if sdd_max is not None:
G = vstack([array([[1, 0], [-1, 0]]), G])
h = hstack([array([sdd_max, sdd_max]), h])
if sd_max is not None:
sd_max_2 = sd_max ** 2
G = vstack([array([[0, 1], [0, -1]]), G])
h = hstack([array([sd_max_2, sd_max_2]), h])
lp = cvxopt_matrix(G), cvxopt_matrix(h)
res, z1 = OptimizeDirection(array([1., 0.]), lp)
if not res:
return None
res, z2 = OptimizeDirection(array([cos(2*pi/3), sin(2*pi/3)]), lp)
if not res:
return None
res, z3 = OptimizeDirection(array([cos(4*pi/3), sin(4*pi/3)]), lp)
if not res:
return None
v1 = Vertex(z1)
v2 = Vertex(z2)
v3 = Vertex(z3)
P = Polygon()
P.fromVertices(v1, v2, v3)
P.iter_expand(lp, 100)
return P
def compute_inequalities(cwc, p, ps, pss):
"""
Compute the constraints (B, c) such that B * [sdd sd^2] <= c.
INPUT:
- ``cwc`` -- (K x 6) matrix
- ``p`` -- vector of length 6
- ``ps`` -- vector of length 6
- ``pss`` -- vector of length 6
OUTPUT:
- ``B`` -- ((K+1) x 6) matrix
- ``c`` -- vector of length K+1
"""
g = get_gravity()
b1 = dot(cwc, hstack([ps, cross(p, ps)]))
b2 = dot(cwc, hstack([pss, cross(p, pss)]))
c = dot(cwc, hstack([g, cross(p, g)]))
B = transpose(vstack([b1, b2]))
return \
vstack([array([0., -1.]), B]), \
hstack([array(0.), c])
def get_dual_vertices_3d(vertices_2d, z=None):
g = -pymanoid.get_gravity()[2]
z = +g if z is None else z
v = [array([a * (g + z), b * (g + z)]) for (a, b) in vertices_2d]
vertices_at_z = [array([x, y, z]) for (x, y) in v]
return [array([0, 0, -g])] + vertices_at_z
