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
