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)
robot.ik.tasks['COM'].update_target(com_target)
robot.ik.solve(max_it=42)
