def compute_primal_vrep(self):
"""Compute vertices of the primal tube."""
delta = self.target_com - self.start_com
n = normalize(delta)
t = array([0., 0., 1.])
t -= dot(t, n) * n
t = normalize(t)
b = cross(n, t)
cross_section = [dx * t + dy * b for (dx, dy) in [
(+self.radius, +self.radius),
(+self.radius, -self.radius),
(-self.radius, +self.radius),
(-self.radius, -self.radius)]]
tube_start = self.start_com - self.margin * n
tube_end = self.target_com + self.margin * n
vertices = (
[tube_start + s for s in cross_section] +
[tube_end + s for s in cross_section])
self.full_vrep = vertices
if self.start_stance.label.startswith('SS'):
if all(abs(self.start_stance.com.p - self.target_com) < 1e-3):
self.primal_vrep = [vertices]
else: # beginning of SS phase
self.primal_vrep = [
[self.start_com], # single-support
vertices] # ensuing double-support
else: # start_stance is DS
self.primal_vrep = [
vertices, # double-support
[self.target_com]] # final single-support
def __init__(self, start_com, end_com, stance, radius, margin):
n = normalize(end_com - start_com)
t = array([0., 0., 1.])
t -= dot(t, n) * n
t = normalize(t)
b = cross(n, t)
cross_section = [
dx * t + dy * b
for (dx, dy) in [(+radius,
+radius), (+radius,
-radius), (-radius,
+radius), (-radius,
-radius)]
]
tube_start = start_com - margin * n
tube_end = end_com + margin * n
primal_vrep = [tube_start + s for s in cross_section] + \
[tube_end + s for s in cross_section]
primal_hrep = compute_polytope_hrep(primal_vrep)
dual_vrep = stance.compute_pendular_accel_cone(
com_vertices=primal_vrep)
dual_hrep = compute_polytope_hrep(dual_vrep)
self.dual_hrep = dual_hrep
self.dual_vrep = dual_vrep
self.primal_hrep = primal_hrep
self.primal_vrep = primal_vrep
def compute_primal_vrep(self):
"""Compute vertices of the primal tube."""
delta = self.target_com - self.start_com
n = normalize(delta)
t = array([0., 0., 1.])
t -= dot(t, n) * n
t = normalize(t)
b = cross(n, t)
cross_section = [dx * t + dy * b for (dx, dy) in [
(+self.radius, +self.radius),
(+self.radius, -self.radius),
(-self.radius, +self.radius),
(-self.radius, -self.radius)]]
tube_start = self.start_com - self.margin * n
tube_end = self.target_com + self.margin * n
vertices = (
[tube_start + s for s in cross_section] +
[tube_end + s for s in cross_section])
self.full_vrep = vertices
if self.start_stance.label.startswith('SS'):
if all(abs(self.start_stance.com.p - self.target_com) < 1e-3):
self.primal_vrep = [vertices]
else: # beginning of SS phase
self.primal_vrep = [
[self.start_com], # single-support
vertices] # ensuing double-support
else: # start_stance is DS
self.primal_vrep = [
vertices, # double-support
[self.target_com]] # final single-support
def compute_primal_vrep(self):
"""Compute vertices for both primal tubes."""
delta = self.target_com - self.start_com
n = normalize(delta)
t = array([0., 0., 1.])
t -= dot(t, n) * n
t = normalize(t)
b = cross(n, t)
cross_section = [dx * t + dy * b for (dx, dy) in [
(+self.radius, +self.radius),
(+self.radius, -self.radius),
(-self.radius, +self.radius),
(-self.radius, -self.radius)]]
tube_start = self.start_com - self.margin * n
tube_end = self.target_com + self.margin * n
if self.start_stance.is_single_support:
sep = self.start_stance.sep
else: # self.start_stance.is_double_support
sep = self.next_stance.sep
vertices0, vertices1 = [], []
for s in cross_section:
start_vertex = tube_start + s
end_vertex = tube_end + s
# NB: the order in the intersection matters, see function doc
mid_vertex = intersect_line_cylinder(end_vertex, start_vertex, sep)
if mid_vertex is None:
# assuming that start_vertex is in the SEP, no intersection
# means that both COM are inside the polygon
vertices = (
[tube_start + s for s in cross_section] +
[tube_end + s for s in cross_section])
if self.start_stance.is_single_support:
# we are at the end of an SS phase
self.primal_vrep = [vertices]
else: # self.start_stance.is_double_support
# we are in DS but polytope is included in the next SS-SEP
self.primal_vrep = [vertices, vertices]
return
if self.start_stance.is_single_support:
mid_vertex = start_vertex + 0.95 * (mid_vertex - start_vertex)
vertices0.append(start_vertex)
vertices0.append(mid_vertex)
vertices1.append(start_vertex)
vertices1.append(end_vertex)
else: # self.start_stance.is_double_support
mid_vertex = end_vertex + 0.95 * (mid_vertex - end_vertex)
vertices0.append(start_vertex)
vertices0.append(end_vertex)
vertices1.append(mid_vertex)
vertices1.append(end_vertex)
self.primal_vrep = [vertices0, vertices1]
def interpolate_path(self):
target_p = self.fsm.next_stance.com
target_pd = self.fsm.next_stance.comd
p0 = self.body.p
p1 = target_p
if norm(p1 - p0) < 1e-3:
return None, None
v1 = target_pd
if norm(self.body.pd) > 1e-4:
v0 = self.body.pd
path = interpolate_houba_topp(p0, p1, v0, v1, hack=True)
sd_beg = norm(v0) / norm(path.Evald(0.))
else: # choose initial direction
delta = normalize(p1 - p0)
ref = normalize(self.fsm.cur_stance.comd)
v0 = 0.7 * delta + 0.3 * ref
path = interpolate_houba_topp(p0, p1, v0, v1, hack=True)
sd_beg = 0.
return path, sd_beg
def reset_state(scenario):
contact_pos, contact_rpy, z_crit_des, zd = scenario
contact.set_pos(contact_pos)
contact.set_rpy(contact_rpy)
pendulum.com.set_pos([0, 0, pendulum.z_target])
robot.ik.solve(max_it=100, impr_stop=1e-4)
e_z = array([0., 0., 1.])
e_x = pendulum.target - pendulum.com.p
e_x = normalize(e_x - dot(e_x, e_z) * e_z)
z_diff = (pendulum.com.z - contact.z) - z_crit_des
x = -dot(e_x, (contact.p - pendulum.com.p))
xd = -x / (2 * z_diff) * (-zd + sqrt(zd**2 + 2 * 9.81 * z_diff))
vel = xd * e_x + zd * e_z
if '--gui' in sys.argv:
global vel_handle
vel_handle = draw_line(pendulum.com.p, pendulum.com.p + 0.5 * vel)
pendulum.com.set_vel(vel)
sim.step()
def update_state(self):
"""
Compute the stationary frame and current pendulum state in that frame.
"""
com = self.pendulum.com
contact = self.pendulum.contact
delta = com.p - contact.p
e_z = array([0., 0., 1.])
e_x = -normalize(delta - dot(delta, e_z) * e_z)
e_y = cross(e_z, e_x)
R = vstack([e_x, e_y, e_z]) # from world to local frame
p, pd = dot(R, delta), dot(R, com.pd) # in local frame
T = transform_from_R_p(R.T, contact.p) # local to world frame
self.T = T
self.e_x = e_x
self.e_y = e_y
self.e_z = e_z
self.state.set_pos(p)
self.state.set_vel(pd)
friction=0.7)
mass = 38. # [kg]
z_target = 0.8 # [m]
init_pos = array([0., 0., z_target])
vel = (0.7, 0.1, 0.2)
draw_parabola = True
if "--comanoid" in sys.argv:
try:
import comanoid
vel = comanoid.setup(sim, robot, contact)
draw_parabola = False
except ImportError:
warn("comanoid module not available, switching to default")
delta = init_pos - contact.p
e_z = array([0., 0., 1.])
e_x = -normalize(delta - dot(delta, e_z) * e_z)
e_y = cross(e_z, e_x)
init_vel = vel[0] * e_x + vel[1] * e_y + vel[2] * e_z
pendulum = InvertedPendulum(mass, init_pos, init_vel, contact, z_target)
pendulum.draw_parabola = draw_parabola
stance = Stance(com=pendulum.com, right_foot=contact)
stance.bind(robot)
robot.ik.solve()
g = -sim.gravity[2]
lambda_min = 0.1 * g
lambda_max = 2.0 * g
stabilizer = Stabilizer3D(
pendulum, lambda_min, lambda_max, nb_steps=10, cop_gain=2.)
drawer = SolutionDrawer(stabilizer)