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 __init__(self,
mass,
omega2,
com,
comd=None,
zmp=None,
zmp_delay=None,
zmp_noise=None):
assert omega2 > 1e-10, "FIP constant must be positive"
super(FIP, self).__init__()
com_state = PointMass(com, mass, visible=False)
comd = comd if comd is not None else zeros(3)
zmp = zmp if zmp is not None else com + gravity / omega2
zmp_delay = 0. if zmp_delay is None else zmp_delay
zmp_noise = 0. if zmp_noise is None else zmp_noise
zmp_state = Point(zmp, visible=False)
zmp_state.hide()
self.__time_ds = 0
self.__time_nonqs = 0
self.com_state = com_state
self.mass = mass
self.next_zmp_target = None
self.omega = sqrt(omega2)
self.omega2 = omega2
self.zmp_delay = zmp_delay
self.zmp_noise = zmp_noise
self.zmp_state = zmp_state
def init_posture(contact_feed):
"""Put the robot in its initial posture."""
init_stance = pymanoid.Stance(com=PointMass(
pos=contact_feed.contacts[1].p + [0, 0, robot.leg_length],
mass=robot.mass),
left_foot=contact_feed.contacts[1],
right_foot=contact_feed.contacts[0])
robot.set_pos([0, 0, 2]) # start PG with the robot above contacts
init_stance.bind(robot)
robot.ik.solve(max_it=50)
def __init__(self, mass, pos, vel, contact, z_target):
super(InvertedPendulum, self).__init__()
com = PointMass(pos, mass, vel)
self.com = com
self.contact = contact
self.cop = array([0., 0., 0.])
self.draw_parabola = False
self.handles = {}
self.hidden = False
self.lambda_ = 9.81 * (com.z - contact.z)
self.pause_dynamics = False
self.z_target = z_target
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')
def setup_ik_from_tasks():
"""
Setup the inverse kinematics task by task.
Note
-----
This function is equivalent to :func:`setup_ik_from_stance` above.
Beginners should take a look at that one first.
Notes
-----
See this `tutorial on inverse kinematics
<https://scaron.info/teaching/inverse-kinematics.html>`_ for details.
"""
from pymanoid.tasks import COMTask, ContactTask, DOFTask, PostureTask
# Prepare targets
lf_target = Contact(robot.sole_shape, pos=[0, 0.3, 0])
rf_target = Contact(robot.sole_shape, pos=[0, -0.3, 0])
# Initial robot pose
robot.set_dof_values([0.8], dof_indices=[robot.TRANS_Z])
com = PointMass(pos=robot.com, mass=robot.mass)
# Prepare tasks
left_foot_task = ContactTask(robot,
robot.left_foot,
lf_target,
weight=1.,
gain=0.85)
right_foot_task = ContactTask(robot,
robot.right_foot,
rf_target,
weight=1.,
gain=0.85)
com_task = COMTask(robot, com, weight=1e-2, gain=0.85)
posture_task = PostureTask(robot, robot.q_halfsit, weight=1e-6, gain=0.85)
# Add tasks to the IK solver
robot.ik.add(left_foot_task)
robot.ik.add(right_foot_task)
robot.ik.add(com_task)
robot.ik.add(posture_task)
# Add shoulder DOF tasks for a nicer posture
robot.ik.add(
DOFTask(robot, robot.R_SHOULDER_R, -0.5, gain=0.5, weight=1e-5))
robot.ik.add(
DOFTask(robot, robot.L_SHOULDER_R, +0.5, gain=0.5, weight=1e-5))
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 __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
pymanoid.init(set_viewer=False)
robot = RobotModel(download_if_needed=True)
robot.set_transparency(0.3)
env = pymanoid.get_env()
env.SetViewer('qtcoin')
pymanoid.env.set_default_background_color()
viewer = pymanoid.get_viewer()
viewer.SetCamera(
[[5.39066316e-01, 3.61154816e-01, -7.60903874e-01, 6.57677031e+00],
[8.42254221e-01, -2.26944015e-01, 4.88982864e-01, -2.9774201e+00],
[3.91593606e-03, -9.04468691e-01, -4.26522042e-01, 2.25269456e+00],
[0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]])
contacts = generate_contacts()
com = PointMass([0, 0, 0], robot.mass, visible=False)
free_foot = FreeFoot(color='c', visible=False)
preview_buffer = PreviewBuffer(com, free_foot)
fsm = StateMachine(robot,
contacts,
com,
free_foot,
'DS-R',
callback=fsm_step_callback)
target_box = PointMass(fsm.cur_stance.com, 30., color='g')
mpc = TOPPPreviewControl(com, free_foot, fsm, preview_buffer)
initialize_robot()
camera_travel = CameraTravelling(viewer, dist=4)
com_traj_drawer = TrajectoryDrawer(com, 'b-')
force_checker = ForceChecker()
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"
robot_mass = robot.mass # saved here to avoid taking robot_lock
pymanoid.get_env().SetViewer('qtcoin')
viewer = pymanoid.get_viewer()
set_camera_0()
robot.set_transparency(0.3)
staircase = generate_staircase(
radius=1.4,
angular_step=0.5,
height=1.4,
roughness=0.5,
friction=0.7,
step_dim_x=0.2,
step_dim_y=0.1)
com = PointMass([0, 0, 0], robot_mass)
preview_buffer = PreviewBuffer(com)
fsm = StateMachine(
staircase,
com,
'DS-R',
ss_duration,
ds_duration,
init_com_offset=array([0.05, 0., 0.]),
cyclic=True,
callback=update_robot_ik)
mpc = TubePreviewControl(
com,
fsm,
preview_buffer,
nb_mpc_steps=nb_mpc_steps,
if __name__ == '__main__':
sim = pymanoid.Simulation(dt=0.03)
robot = JVRC1('JVRC-1.dae', download_if_needed=True)
sim.set_viewer()
sim.viewer.SetCamera([[-0.28985317, 0.40434422, -0.86746233, 2.73872042],
[0.95680251, 0.10095043, -0.2726499, 0.86080128],
[-0.02267371, -0.90901857, -0.41613837, 2.06654644],
[0., 0., 0., 1.]])
# IK targets
lf_target = Contact(robot.sole_shape, pos=[0, 0.3, 0], visible=True)
rf_target = Contact(robot.sole_shape, pos=[0, -0.3, 0], visible=True)
# Initial robot pose
robot.set_dof_values([0.8], dof_indices=[robot.TRANS_Z])
com = PointMass(pos=robot.com, mass=robot.mass)
# IK tasks
lf_task = ContactTask(robot,
robot.left_foot,
lf_target,
weight=1.,
gain=0.85)
rf_task = ContactTask(robot,
robot.right_foot,
rf_target,
weight=1.,
gain=0.85)
com_task = COMTask(robot, com, weight=1e-2, gain=0.85)
reg_task = PostureTask(robot, robot.q, weight=1e-6, gain=0.85)
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]
})
com_target.set_x(com_above.x)
com_target.set_y(com_above.y)
if __name__ == "__main__":
sim = pymanoid.Simulation(dt=0.03)
robot = pymanoid.robots.JVRC1('JVRC-1.dae', download_if_needed=True)
sim.set_viewer()
sim.viewer.SetCamera([[0.60587192, -0.36596244, 0.70639274, -2.4904027],
[-0.79126787, -0.36933163, 0.48732874, -1.6965636],
[0.08254916, -0.85420468, -0.51334199, 2.79584694],
[0., 0., 0., 1.]])
robot.set_transparency(0.25)
com_target = PointMass(pos=[0., 0., com_height],
mass=robot.mass,
color='b',
visible=False)
com_above = pymanoid.Cube(0.02, [0.05, 0.04, z_polygon], color='b')
stance = Stance(com=com_target,
left_foot=Contact(shape=robot.sole_shape,
pos=[0.20, 0.15, 0.1],
rpy=[0.4, 0, 0],
static_friction=0.5,
visible=True),
right_foot=Contact(shape=robot.sole_shape,
pos=[-0.2, -0.195, 0.],
rpy=[-0.4, 0, 0],
static_friction=0.5,
visible=True))
seed(42)
sim = pymanoid.Simulation(dt=0.03)
robot = JVRC1(download_if_needed=True)
sim.set_viewer()
robot.set_transparency(0.3)
staircase = generate_staircase(radius=1.4,
angular_step=0.5,
height=1.2,
roughness=0.5,
friction=0.7,
ds_duration=0.7,
ss_duration=1.0,
init_com_offset=array([0., 0., 0.]))
com_target = PointMass([0, 0, 0], 20.)
preview_buffer = PreviewBuffer(
u_dim=3, callback=lambda u, dT: com_target.integrate_euler(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.log_comp_times()
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)
sim = pymanoid.Simulation(dt=0.03)
robot = JVRC1(download_if_needed=True)
sim.set_viewer()
robot.set_transparency(0.3)
staircase = generate_staircase(
radius=1.4,
angular_step=0.5,
height=1.2,
roughness=0.5,
friction=0.7,
ds_duration=0.7,
ss_duration=1.0,
init_com_offset=array([0., 0., 0.]))
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)
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")
print "- half-length =", contact.shape[0]
print "- half-width =", contact.shape[1]
print "- friction =", contact.friction
print ""
if __name__ == "__main__":
sim = pymanoid.Simulation(dt=0.03)
robot = pymanoid.robots.JVRC1('JVRC-1.dae', download_if_needed=True)
sim.set_viewer()
sim.viewer.SetCamera([[0.60587192, -0.36596244, 0.70639274, -2.4904027],
[-0.79126787, -0.36933163, 0.48732874, -1.6965636],
[0.08254916, -0.85420468, -0.51334199, 2.79584694],
[0., 0., 0., 1.]])
robot.set_transparency(0.25)
stance = Stance(com=PointMass(pos=[0., 0., 0.9], mass=robot.mass),
left_foot=Contact(shape=robot.sole_shape,
pos=[0.20, 0.15, 0.1],
rpy=[0.4, 0, 0],
friction=0.5),
right_foot=Contact(shape=robot.sole_shape,
pos=[-0.2, -0.195, 0.],
rpy=[-0.4, 0, 0],
friction=0.5))
stance.bind(robot)
robot.ik.solve()
sim.schedule(robot.ik)
sim.start()
p = array([0., 0., 0.])
CWC_O = stance.compute_wrench_inequalities(p)
def generate_staircase(radius, angular_step, height, roughness, friction,
ds_duration, ss_duration, init_com_offset=None):
"""
Generate a new slanted staircase with tilted steps.
Parameters
----------
radius : scalar
Staircase radius in [m].
angular_step : scalar
Angular step between contacts in [rad].
height : scalar
Altitude variation in [m].
roughness : scalar
Amplitude of contact roll, pitch and yaw in [rad].
friction : scalar
Friction coefficient between a robot foot and a step.
ds_duration : scalar
Duration of double-support phases in [s].
ss_duration : scalar
Duration of single-support phases in [s].
init_com_offset : array, optional
Initial offset applied to first stance.
"""
stances = []
contact_shape = (0.12, 0.06)
first_left_foot = None
prev_right_foot = None
for theta in arange(0., 2 * pi, angular_step):
left_foot = Contact(
shape=contact_shape,
pos=[radius * cos(theta),
radius * sin(theta),
radius + .5 * height * sin(theta)],
rpy=(roughness * (random(3) - 0.5) + [0, 0, theta + .5 * pi]),
friction=friction)
if first_left_foot is None:
first_left_foot = left_foot
right_foot = Contact(
shape=contact_shape,
pos=[1.2 * radius * cos(theta + .5 * angular_step),
1.2 * radius * sin(theta + .5 * angular_step),
radius + .5 * height * sin(theta + .5 * angular_step)],
rpy=(roughness * (random(3) - 0.5) + [0, 0, theta + .5 * pi]),
friction=friction)
if prev_right_foot is not None:
com_target_pos = left_foot.p + [0., 0., JVRC1.leg_length]
com_target = PointMass(com_target_pos, robot.mass, visible=False)
dsl_stance = Stance(
com_target, left_foot=left_foot, right_foot=prev_right_foot)
dsl_stance.label = 'DS-L'
dsl_stance.duration = ds_duration
ssl_stance = Stance(com_target, left_foot=left_foot)
ssl_stance.label = 'SS-L'
ssl_stance.duration = ss_duration
dsl_stance.compute_static_equilibrium_polygon()
ssl_stance.compute_static_equilibrium_polygon()
stances.append(dsl_stance)
stances.append(ssl_stance)
com_target_pos = right_foot.p + [0., 0., JVRC1.leg_length]
if init_com_offset is not None:
com_target_pos += init_com_offset
init_com_offset = None
com_target = PointMass(com_target_pos, robot.mass, visible=False)
dsr_stance = Stance(
com_target, left_foot=left_foot, right_foot=right_foot)
dsr_stance.label = 'DS-R'
dsr_stance.duration = ds_duration
ssr_stance = Stance(com_target, right_foot=right_foot)
ssr_stance.label = 'SS-R'
ssr_stance.duration = ss_duration
dsr_stance.compute_static_equilibrium_polygon()
ssr_stance.compute_static_equilibrium_polygon()
stances.append(dsr_stance)
stances.append(ssr_stance)
prev_right_foot = right_foot
com_target_pos = first_left_foot.p + [0., 0., JVRC1.leg_length]
com_target = PointMass(com_target_pos, robot.mass, visible=False)
dsl_stance = Stance(
com_target, left_foot=first_left_foot, right_foot=prev_right_foot)
dsl_stance.label = 'DS-L'
dsl_stance.duration = ds_duration
ssl_stance = Stance(com_target, left_foot=first_left_foot)
ssl_stance.label = 'SS-L'
ssl_stance.duration = ss_duration
dsl_stance.compute_static_equilibrium_polygon()
ssl_stance.compute_static_equilibrium_polygon()
stances.append(dsl_stance)
stances.append(ssl_stance)
return stances
sim = pymanoid.Simulation(dt=0.03)
robot = JVRC1(download_if_needed=True)
sim.set_viewer()
robot.set_transparency(0.3)
staircase = generate_staircase(
radius=1.4,
angular_step=0.5,
height=1.2,
roughness=0.5,
friction=0.7,
ds_duration=0.7,
ss_duration=1.0,
init_com_offset=array([0., 0., 0.]))
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)
def __init__(self):
point_mass = PointMass(robot.com, robot.mass, visible=False)
contact_set = ContactSet([wpg.support_contact])
super(WrenchDrawer, self).__init__(point_mass, contact_set)
self.am = zeros(3)
1.12500819e+00, -1.91496369e-01, -2.06646315e-01,
1.39579597e-01, -1.33333598e-01, -8.72664626e-01,
0.00000000e+00, -9.81307787e-15, 0.00000000e+00,
-8.66484961e-02, -1.78097540e-01, -1.68940240e-03,
-5.31698601e-01, -1.00166891e-04, -6.74394930e-04,
-1.01552628e-04, -5.71121132e-15, -4.18037117e-15,
0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
0.00000000e+00, -7.06534763e-01, 1.67723830e-01,
2.40289101e-01, -1.11674923e+00, 6.23384177e-01,
-8.45611535e-01, 1.39994759e-02, 1.17756934e-16,
3.14018492e-16, -3.17943723e-15, -6.28036983e-16,
-3.17943723e-15, -6.28036983e-16, -6.88979202e-02,
-4.90099381e-02, 8.17415141e-01, -8.71841480e-02,
-1.36966665e-01, -4.26226421e-02])
com_target = PointMass(pos=[0., 0., com_height], mass=robot.mass, color='b')
com_target.hide()
com_above = pymanoid.Cube(0.02, [0.05, 0.04, z_polygon], color='b')
stance = Stance(
com=com_target,
left_foot=Contact(
shape=robot.sole_shape,
pos=[0.20, 0.15, 0.1],
rpy=[0.4, 0, 0],
friction=0.5),
right_foot=Contact(
shape=robot.sole_shape,
pos=[-0.2, -0.195, 0.],
rpy=[-0.4, 0, 0],
friction=0.5))
