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_acceleration(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.init_ik(robot.whole_body)
robot.set_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))
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)
robot.ik.tasks['com'].update_target(com_target)
robot.ik.add_task(DOFTask(robot, robot.WAIST_P, 0.2, weight=1e-3))
