def single_run(segment_id): segment = Trajectory.load('data/segment_%d.pos' % segment_id) contacting_links = contacting_links_for_segment(segment_id) print "\n\nWRENCH" from pymanoid_lite.retiming import compute_GI_face compute_GI_face(contacting_links) print "\n\nFORCES" from pymanoid_lite.retiming import compute_GI_face_forces compute_GI_face_forces(contacting_links) print "\n\n" hrp.play_trajectory(segment) retimed = retime_whole_body_trajectory(hrp, segment, contacting_links, TOPP_ndiscrsteps) hrp.play_trajectory(retimed) return segment, retimed
1.05909408e-03, 5.23164630e-01, 5.89733869e-02 - 0.2, 7.02909529e-02 + 0.791 - 0.03, 2.48327606e-01, -1.01304501e-01, -1.59442180e-01, ] ) ) robot = hrp.rave t0 = time.time() for i in range(100): CGI = compute_GI_face([hrp.right_foot, hrp.right_arm]) t1 = time.time() print "Compute GI face (100 iter):", (t1 - t0) * 1000 / 100, "ms / iter" xm = 0.7 ym = -0.3 zm = 0.5 d = 1.5 points = [] g1 = array([0.15, 0, -0.98]) g2 = array([-0.15, 0, -0.98]) g3 = array([0, 0.15, -0.98]) g4 = array([0, -0.15, -0.98]) ft1, ft2, ft3, ft4 = zeros(6), zeros(6), zeros(6), zeros(6) ft1[:3] = g1 ft2[:3] = g2