def __init__(self, robot, state_estimator, pendulum, contact_feed,
forward_velocity, nb_mpc_steps, nb_lqr_steps,
max_swing_foot_accel):
super(WalkingPatternGenerator, self).__init__()
first_contact = contact_feed.pop()
second_contact = contact_feed.pop()
third_contact = contact_feed.pop()
support_contact = second_contact
swing_controller = SwingFootController(
robot.left_foot, max_swing_foot_accel)
swing_start = first_contact
swing_target = third_contact
com_target = PointMass(
[0, 0, 0], 0.5 * robot.mass, color='g', visible=True)
ds_com_target = PointMass(
[0, 0, 0], 0.5 * robot.mass, color='b', visible=False)
com_target.set_pos(swing_target.p + [0., 0., robot.leg_length])
com_target.set_vel(forward_velocity * swing_target.t)
time_to_heel_strike = 1. # not computed yet
com_mpc = PredictiveController(
nb_mpc_steps, state_estimator, com_target,
contact_sequence=[support_contact, swing_target],
omega2=pendulum.omega2, swing_duration=time_to_heel_strike)
self.__disable_lqr = False
self.__draw_support_tube = False
self.__ds_tube_handle = None
self.__max_swing_weight = 2e-2
self.__min_swing_weight = 1e-3
self.__simulate_instant_mpc = False
self.com_lqr = None
self.com_mpc = None
self.com_target = com_target
self.contact_feed = contact_feed
self.ds_com_target = ds_com_target
self.forward_velocity = forward_velocity
self.is_in_double_support = False
self.last_mpc_success = None
self.mpc_wait_time = 0.
self.nb_ds_steps = nb_lqr_steps
self.nb_lqr_steps = nb_lqr_steps
self.nb_mpc_steps = nb_mpc_steps
self.next_com_mpc = com_mpc
self.pendulum = pendulum
self.robot = robot
self.startup_flag = True # krooOon (ˆ(oo)ˆ)
self.state_estimator = state_estimator
self.strat_counts = {'cp': 0, 'ds': 0, 'lqr': 0, 'mpc': 0}
self.support_contact = support_contact
self.swing_controller = swing_controller
self.swing_start = swing_start
self.swing_target = swing_target
#
self.switch_controllers(None)
self.switch_ik_tasks('SS')
class COMTubePredictiveControl(pymanoid.Process):
"""
Feedback controller that continuously runs the preview controller and sends
outputs to a COMAccelBuffer.
Parameters
----------
com : PointMass
Current state (position and velocity) of the COM.
fsm : WalkingFSM
Instance of finite state machine.
preview_buffer : PreviewBuffer
MPC outputs are sent to this buffer.
nb_mpc_steps : int
Discretization step of the preview window.
tube_radius : scalar
Tube radius in [m] for the L1 norm.
"""
def __init__(self, com, fsm, preview_buffer, nb_mpc_steps, tube_radius):
super(COMTubePredictiveControl, self).__init__()
self.com = com
self.fsm = fsm
self.last_phase_id = -1
self.nb_mpc_steps = nb_mpc_steps
self.preview_buffer = preview_buffer
self.preview_control = None
self.target_com = PointMass(fsm.cur_stance.com.p, 30., color='g')
self.tube = None
self.tube_radius = tube_radius
def on_tick(self, sim):
"""
Entry point called at each simulation tick.
Parameters
----------
sim : Simulation
Instance of the current simulation.
"""
preview_targets = self.fsm.get_preview_targets()
switch_time, horizon, target_com, target_comd = preview_targets
self.target_com.set_pos(target_com)
self.target_com.set_vel(target_comd)
try:
self.compute_preview_tube()
except Exception as e:
print("Tube error: %s" % str(e))
return
try:
self.compute_preview_control(switch_time, horizon)
except Exception as e:
print("COMTubePredictiveControl error: %s" % str(e))
return
sim.log_comp_time(
'qp_build', self.preview_control.build_time)
sim.log_comp_time(
'qp_solve', self.preview_control.solve_time)
sim.log_comp_time(
'qp_solve_and_build', self.preview_control.solve_and_build_time)
def compute_preview_tube(self):
"""Compute preview tube and store it in ``self.tube``."""
cur_com, target_com = self.com.p, self.target_com.p
cur_stance = self.fsm.cur_stance
next_stance = self.fsm.next_stance
self.tube = COMTube(
cur_com, target_com, cur_stance, next_stance, self.tube_radius)
t0 = time.time()
self.tube.compute_primal_vrep()
t1 = time.time()
self.tube.compute_primal_hrep()
t2 = time.time()
self.tube.compute_dual_vrep()
t3 = time.time()
self.tube.compute_dual_hrep()
t4 = time.time()
sim.log_comp_time('tube_primal_vrep', t1 - t0)
sim.log_comp_time('tube_primal_hrep', t2 - t1)
sim.log_comp_time('tube_dual_vrep', t3 - t2)
sim.log_comp_time('tube_dual_hrep', t4 - t3)
def compute_preview_control(self, switch_time, horizon,
state_constraints=False):
"""Compute controller and store it in ``self.preview_control``."""
cur_com = self.com.p
cur_comd = self.com.pd
target_com = self.target_com.p
target_comd = self.target_com.pd
dT = horizon / self.nb_mpc_steps
eye3 = eye(3)
A = array(bmat([[eye3, dT * eye3], [zeros((3, 3)), eye3]]))
B = array(bmat([[.5 * dT ** 2 * eye3], [dT * eye3]]))
x_init = hstack([cur_com, cur_comd])
x_goal = hstack([target_com, target_comd])
switch_step = int(switch_time / dT)
C = [None] * self.nb_mpc_steps
D = [None] * self.nb_mpc_steps
e = [None] * self.nb_mpc_steps
D1, e1 = self.tube.dual_hrep[0]
if 0 <= switch_step < self.nb_mpc_steps - 1:
D2, e2 = self.tube.dual_hrep[1]
for k in range(self.nb_mpc_steps):
D[k] = D1 if k <= switch_step else D2
e[k] = e1 if k <= switch_step else e2
if state_constraints:
E, f = self.tube.full_hrep # E * com[k] <= f
raise NotImplementedError("add state constraints to [CDe]_list")
self.preview_control = LinearPredictiveControl(
A, B, C, D, e, x_init, x_goal, self.nb_mpc_steps, wxt=1000., wu=1.)
self.preview_control.switch_step = switch_step
self.preview_control.timestep = dT
try:
self.preview_control.solve()
U = self.preview_control.U.flatten()
dT = [self.preview_control.timestep] * self.nb_mpc_steps
self.preview_buffer.update_preview(U, dT)
# <dirty why="used in PreviewDrawer">
self.preview_buffer.nb_steps = self.nb_mpc_steps
self.preview_buffer.switch_step = self.preview_control.switch_step
# </dirty>
except ValueError:
print("MPC couldn't solve QP, constraints may be inconsistent")
com_target = PointMass([0, 0, 0], 20.)
preview_buffer = PreviewBuffer(
u_dim=3,
callback=lambda u, dT: com_target.integrate_constant_accel(u, dT))
fsm = WalkingFSM(staircase, robot, swing_height=0.15, cycle=True)
mpc = COMTubePredictiveControl(
com_target, fsm, preview_buffer,
nb_mpc_steps=20,
tube_radius=0.01)
robot.ik.DEFAULT_WEIGHTS['POSTURE'] = 1e-5
robot.set_pos([0, 0, 2]) # robot initially above contacts
fsm.cur_stance.bind(robot)
robot.ik.solve(max_it=24)
com_target.set_pos(robot.com)
robot.ik.tasks['COM'].update_target(com_target)
robot.ik.solve(max_it=42)
robot.ik.add(DOFTask(robot, robot.WAIST_P, 0.2, weight=1e-3))
robot.ik.add(DOFTask(robot, robot.WAIST_Y, 0., weight=1e-3))
robot.ik.add(DOFTask(robot, robot.WAIST_R, 0., weight=1e-3))
robot.ik.add(DOFTask(robot, robot.ROT_P, 0., weight=1e-3))
robot.ik.add(DOFTask(robot, robot.R_SHOULDER_R, -0.5, weight=1e-3))
robot.ik.add(DOFTask(robot, robot.L_SHOULDER_R, 0.5, weight=1e-3))
robot.ik.add(MinCAMTask(robot, weight=1e-4))
sim.schedule(fsm)
sim.schedule(mpc)
sim.schedule(preview_buffer)
sim.schedule(robot.ik, log_comp_times=True)
class COMTubePreviewControl(Process):
def __init__(self, com, fsm, preview_buffer, nb_mpc_steps, tube_radius):
"""
Create a new feedback controller that continuously runs the preview
controller and sends outputs to a COMAccelBuffer.
INPUT:
- ``com`` -- PointMass containing current COM state
- ``fsm`` -- instance of finite state machine
- ``preview_buffer`` -- PreviewBuffer to send MPC outputs to
- ``nb_mpc_steps`` -- discretization step of the preview window
- ``tube_radius`` -- tube radius (in L1 norm)
"""
super(COMTubePreviewControl, self).__init__()
self.com = com
self.fsm = fsm
self.last_phase_id = -1
self.nb_mpc_steps = nb_mpc_steps
self.preview_buffer = preview_buffer
self.preview_control = None
self.target_com = PointMass(fsm.cur_stance.com.p, 30., color='g')
self.tube = None
self.tube_radius = tube_radius
def on_tick(self, sim):
"""
Entry point called at each simulation tick.
"""
preview_targets = self.fsm.get_preview_targets()
switch_time, horizon, target_com, target_comd = preview_targets
self.target_com.set_pos(target_com)
self.target_com.set_velocity(target_comd)
try:
self.compute_preview_tube()
except Exception as e:
print "Tube error: %s" % str(e)
return
try:
self.compute_preview_control(switch_time, horizon)
except Exception as e:
print "PreviewControl error: %s" % str(e)
return
def compute_preview_tube(self):
"""Compute preview tube and store it in ``self.tube``."""
cur_com, target_com = self.com.p, self.target_com.p
cur_stance = self.fsm.cur_stance
next_stance = self.fsm.next_stance
self.tube = COMTube(
cur_com, target_com, cur_stance, next_stance, self.tube_radius)
t0 = time.time()
self.tube.compute_primal_vrep()
t1 = time.time()
self.tube.compute_primal_hrep()
t2 = time.time()
self.tube.compute_dual_vrep()
t3 = time.time()
self.tube.compute_dual_hrep()
t4 = time.time()
sim.log_comp_time('tube_primal_vrep', t1 - t0)
sim.log_comp_time('tube_primal_hrep', t2 - t1)
sim.log_comp_time('tube_dual_vrep', t3 - t2)
sim.log_comp_time('tube_dual_hrep', t4 - t3)
def compute_preview_control(self, switch_time, horizon,
state_constraints=False):
"""Compute controller and store it in ``self.preview_control``."""
cur_com = self.com.p
cur_comd = self.com.pd
target_com = self.target_com.p
target_comd = self.target_com.pd
dT = horizon / self.nb_mpc_steps
I = eye(3)
A = array(bmat([[I, dT * I], [zeros((3, 3)), I]]))
B = array(bmat([[.5 * dT ** 2 * I], [dT * I]]))
x_init = hstack([cur_com, cur_comd])
x_goal = hstack([target_com, target_comd])
switch_step = int(switch_time / dT)
G_list = []
h_list = []
C1, d1 = self.tube.dual_hrep[0]
E, f = None, None
if state_constraints:
E, f = self.tube.full_hrep
if 0 <= switch_step < self.nb_mpc_steps - 1:
C2, d2 = self.tube.dual_hrep[1]
for k in xrange(self.nb_mpc_steps):
if k <= switch_step:
G_list.append(C1)
h_list.append(d1)
else: # k > switch_step
G_list.append(C2)
h_list.append(d2)
G = block_diag(*G_list)
h = hstack(h_list)
self.preview_control = PreviewControl(
A, B, G, h, x_init, x_goal, self.nb_mpc_steps, E, f)
self.preview_control.switch_step = switch_step
self.preview_control.timestep = dT
self.preview_control.compute_dynamics()
try:
self.preview_control.compute_control()
self.preview_buffer.update_preview(self.preview_control)
except ValueError:
print "MPC couldn't solve QP, constraints may be inconsistent"
com_target = PointMass([0, 0, 0], 20.)
preview_buffer = PreviewBuffer(
callback=lambda u, dT: com_target.integrate_acceleration(u, dT))
swing_foot = SwingFoot(swing_height=0.15)
fsm = WalkingFSM(staircase, robot, swing_foot, cycle=True)
mpc = COMTubePreviewControl(
com_target, fsm, preview_buffer,
nb_mpc_steps=10,
tube_radius=0.01)
robot.init_ik(robot.whole_body)
robot.set_ff_pos([0, 0, 2]) # start IK with the robot above contacts
robot.generate_posture(fsm.cur_stance, max_it=50)
com_target.set_pos(robot.com)
robot.ik.tasks['com'].update_target(com_target)
robot.ik.add_task(DOFTask(robot, robot.WAIST_P, 0.2, weight=1e-3))
robot.ik.add_task(DOFTask(robot, robot.WAIST_Y, 0., weight=1e-3))
robot.ik.add_task(DOFTask(robot, robot.WAIST_R, 0., weight=1e-3))
robot.ik.add_task(DOFTask(robot, robot.ROT_P, 0., weight=1e-3))
robot.ik.add_task(DOFTask(robot, robot.R_SHOULDER_R, -0.5, weight=1e-3))
robot.ik.add_task(DOFTask(robot, robot.L_SHOULDER_R, 0.5, weight=1e-3))
robot.ik.add_task(MinCAMTask(robot, weight=1e-4))
robot.ik.tasks['posture'].weight = 1e-5
sim.schedule(fsm)
sim.schedule(mpc)
sim.schedule(preview_buffer)
sim.schedule(robot.ik_process)
class TubePreviewControl(Process):
def __init__(self, com, fsm, preview_buffer, nb_mpc_steps, tube_radius):
"""
Create a new feedback controller that continuously runs the preview
controller and sends outputs to a COMAccelBuffer.
INPUT:
- ``com`` -- PointMass containing current COM state
- ``fsm`` -- instance of finite state machine
- ``preview_buffer`` -- PreviewBuffer to send MPC outputs to
- ``nb_mpc_steps`` -- discretization step of the preview window
- ``tube_radius`` -- tube radius (in L1 norm)
"""
self.com = com
self.fsm = fsm
self.last_phase_id = -1
self.nb_mpc_steps = nb_mpc_steps
self.preview_buffer = preview_buffer
self.target_box = PointMass(fsm.cur_stance.com, 30., color='g')
self.thread = None
self.thread_lock = None
self.tube = None
self.tube_radius = tube_radius
self.verbose = False
def on_tick(self, sim):
cur_com = self.com.p
cur_comd = self.com.pd
cur_stance = self.fsm.cur_stance
next_stance = self.fsm.next_stance
preview_targets = self.fsm.get_preview_targets()
switch_time, horizon, target_com, target_comd = preview_targets
if self.verbose:
print "\nVelocities:"
print "- |cur_comd| =", norm(cur_comd)
print "- |target_comd| =", norm(target_comd)
print "\nTime:"
print "- horizon =", horizon
print "- switch_time =", switch_time
print "- timestep = ", horizon / self.nb_mpc_steps
print ""
self.target_box.set_pos(target_com)
try:
self.tube = COMTube(cur_com, target_com, cur_stance, next_stance,
self.tube_radius)
except TubeError as e:
warning("Tube error: %s" % str(e))
return
preview_control = COMPreviewControl(cur_com, cur_comd, target_com,
target_comd, self.tube, horizon,
switch_time, self.nb_mpc_steps)
preview_control.compute_dynamics()
try:
preview_control.compute_control()
self.preview_buffer.update_preview(preview_control)
except ValueError as e:
warning("MPC couldn't solve QP, constraints may be inconsistent")
return
sim.report_comp_times({
'tube_primal_vrep': self.tube.comp_times[0],
'tube_primal_hrep': self.tube.comp_times[1],
'tube_dual_vrep': self.tube.comp_times[2],
'tube_dual_hrep': self.tube.comp_times[3],
'qp_build': preview_control.comp_times[0],
'qp_solve': preview_control.comp_times[1]
})
