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(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)
com_target.set_pos(robot.com)
robot.ik.tasks['COM'].update_target(com_target)
