e_z = array([0., 0., 1.]) e_x = pendulum.target - pendulum.com.p e_x = normalize(e_x - dot(e_x, e_z) * e_z) z_diff = (pendulum.com.z - contact.z) - z_crit_des x = -dot(e_x, (contact.p - pendulum.com.p)) xd = -x / (2 * z_diff) * (-zd + sqrt(zd**2 + 2 * 9.81 * z_diff)) vel = xd * e_x + zd * e_z if '--gui' in sys.argv: global vel_handle vel_handle = draw_line(pendulum.com.p, pendulum.com.p + 0.5 * vel) pendulum.com.set_vel(vel) sim.step() if __name__ == "__main__": sim = pymanoid.Simulation(dt=3e-2) try: # use HRP4 if available robot = pymanoid.robots.HRP4() except: # otherwise use default model robot = pymanoid.robots.JVRC1() robot.set_transparency(0.5) robot.suntan() robot.add_shoulder_abduction_task(weight=1e-4) robot.add_shoulder_flexion_task(weight=1e-4) if '--gui' in sys.argv: sim.set_viewer() sim.viewer.SetBkgndColor([1, 1, 1]) sim.viewer.SetCamera( [[-0.43496840, 0.32393275, -0.84016074, 2.07345104], [+0.88804317, 0.30865173, -0.34075422, 0.77617097], [+0.14893562, -0.89431632, -0.42192002, 1.63681912],
print "\nNo scenario specified. Using elliptic staircase." staircase = "elliptic-staircase" contact_feed = ContactFeed( path='scenarios/%s/contacts.json' % staircase, cyclic=True) for (i, contact) in enumerate(contact_feed.contacts): contact.link = robot.right_foot if i % 2 == 0 else robot.left_foot return contact_feed if __name__ == "__main__": if "-h" in sys.argv or "--help" in sys.argv: print_usage() sys.exit() random.seed(34) sim = pymanoid.Simulation(dt=0.03) try: robot = pymanoid.robots.HRP4() except: # model not found robot = pymanoid.robots.JVRC1() robot.set_transparency(0.3) sim.set_viewer() set_camera_1() contact_feed = load_scenario() init_posture(contact_feed) pendulum = FIP( robot.mass, omega2=9.81 / robot.leg_length, com=robot.com, zmp_delay=ZMP_DELAY, zmp_noise=ZMP_NOISE) state_estimator = StateEstimator( pendulum, ESTIMATION_DELAY, COM_NOISE, COMD_NOISE) wpg = WalkingPatternGenerator(
""" Run CoM predictive control for single-support state. """ if self.preview_time >= self.mpc_timestep: if self.state == "DoubleSupport": self.update_mpc(self.rem_time, self.ssp_duration) else: # self.state == "SingleSupport": self.update_mpc(0., self.rem_time) com_jerk = array([self.x_mpc.U[0][0], self.y_mpc.U[0][0], 0.]) stance.com.integrate_constant_jerk(com_jerk, dt) self.preview_time += dt if __name__ == "__main__": dt = 0.03 # [s] sim = pymanoid.Simulation(dt=dt) robot = JVRC1(download_if_needed=True) sim.set_viewer() sim.set_camera_transform( [[-0.86825231, 0.13816899, -0.47649476, 2.95342016], [0.49606811, 0.22750768, -0.8379479, 3.26736617], [-0.0073722, -0.96392406, -0.2660753, 1.83063173], [0., 0., 0., 1.]]) robot.set_transparency(0.3) footsteps = generate_footsteps(distance=2.1, step_length=0.3, foot_spread=0.1, friction=0.7) com_target = PointMass([0, 0, robot.leg_length], robot.mass) stance = Stance(com=com_target, left_foot=footsteps[0].copy(hide=True),