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)
com_traj_drawer = TrajectoryDrawer(com_target, 'b-')
lf_traj_drawer = TrajectoryDrawer(robot.left_foot, 'g-')
preview_drawer = PreviewDrawer()
rf_traj_drawer = TrajectoryDrawer(robot.right_foot, 'r-')
tube_drawer = TubeDrawer()
update_com_target = UpdateCOMTargetAccel(com_target, preview_buffer)
wrench_drawer = PointMassWrenchDrawer(com_target, lambda: fsm.cur_stance)
sim.schedule_extra(com_traj_drawer)
sim.schedule_extra(lf_traj_drawer)
sim.schedule_extra(preview_drawer)
sim.schedule_extra(rf_traj_drawer)
sim.schedule_extra(tube_drawer)
sim.schedule_extra(update_com_target)
sim.schedule_extra(wrench_drawer)
zmp_delay=ZMP_DELAY, zmp_noise=ZMP_NOISE)
state_estimator = StateEstimator(
pendulum, ESTIMATION_DELAY, COM_NOISE, COMD_NOISE)
wpg = WalkingPatternGenerator(
robot, state_estimator, pendulum, contact_feed, FORWARD_VELOCITY,
NB_MPC_STEPS, NB_LQR_STEPS, MAX_SWING_FOOT_ACCEL)
wpg.draw_support_tube(DRAW_DS_TUBE)
wpg.set_swing_ik_weights(MIN_SWING_IK_WEIGHT, MAX_SWING_IK_WEIGHT)
wpg.simulate_instant_mpc(SIMULATE_INSTANT_MPC)
update_robot_ik()
sim.schedule(state_estimator) # should be called before wpg and pendulum
sim.schedule(wpg) # should be called before pendulum
sim.schedule(pendulum) # end of topological sorting ;)
com_traj_drawer = TrajectoryDrawer(pendulum.com_state, 'b-')
lf_traj_drawer = TrajectoryDrawer(robot.left_foot, 'g-')
rf_traj_drawer = TrajectoryDrawer(robot.right_foot, 'r-')
preview_drawer = PreviewDrawer()
stats_collector = StatsCollector(count_nonqs=False)
wrench_drawer = WrenchDrawer()
sim.schedule_extra(com_traj_drawer)
sim.schedule_extra(lf_traj_drawer)
sim.schedule_extra(preview_drawer)
sim.schedule_extra(rf_traj_drawer)
sim.schedule_extra(robot.ik)
sim.schedule_extra(stats_collector)
sim.schedule_extra(wrench_drawer)
if '--record' in sys.argv:
com_target = PointMass([0, 0, robot.leg_length], robot.mass)
stance = Stance(com=com_target,
left_foot=footsteps[0].copy(hide=True),
right_foot=footsteps[1].copy(hide=True))
stance.bind(robot)
robot.ik.solve(max_it=42)
ssp_duration = round(0.7 / dt) * dt # close to 0.7 [s]
dsp_duration = round(0.1 / dt) * dt # close to 0.1 [s]
fsm = WalkingFSM(ssp_duration, dsp_duration)
sim.schedule(fsm)
sim.schedule(robot.ik, log_comp_times=True)
sim.schedule(robot.wrench_distributor, log_comp_times=True)
com_traj_drawer = TrajectoryDrawer(robot.stance.com, 'b-')
lf_traj_drawer = TrajectoryDrawer(robot.left_foot, 'g-')
rf_traj_drawer = TrajectoryDrawer(robot.right_foot, 'r-')
wrench_drawer = RobotWrenchDrawer(robot)
sim.schedule_extra(com_traj_drawer)
sim.schedule_extra(lf_traj_drawer)
sim.schedule_extra(rf_traj_drawer)
sim.schedule_extra(wrench_drawer)
sim.start()
def start_walking():
fsm.start_walking = True
start_walking() # comment this out to start walking manually