diff --git a/script/scenarios/demos/siggraph_asia/scale_hrp2_interp.py b/script/scenarios/demos/siggraph_asia/scale_hrp2_interp.py index 3f6f8d2bdd119b5f7e8368ac795a1162a66f4e78..0a7ebc80a83e5d95728c9f20046021ca12c12ddd 100644 --- a/script/scenarios/demos/siggraph_asia/scale_hrp2_interp.py +++ b/script/scenarios/demos/siggraph_asia/scale_hrp2_interp.py @@ -281,7 +281,7 @@ def test(stateid = 1, path = False, use_rand = False, just_one_curve = False, nu c_bounds_1 = get_com_constraint(fullBody, stateid, configs[stateid], limbsCOMConstraints, interm = False) c_bounds_mid = get_com_constraint(fullBody, stateid, configs[stateid], limbsCOMConstraints, interm = True) c_bounds_2 = get_com_constraint(fullBody, stateid, configs[stateid+1], limbsCOMConstraints, interm = False) - success, c_mid_1, c_mid_2 = solve_quasi_static(data, c_bounds = [c_bounds_1, c_bounds_2, c_bounds_mid], use_rand = use_rand, mu = 0.8, fullBody = fullBody) + success, c_mid_1, c_mid_2 = solve_quasi_static(data, c_bounds = [c_bounds_1, c_bounds_2, c_bounds_mid], use_rand = use_rand, mu = 0.8) #~ success, c_mid_1, c_mid_2 = solve_dyn(data, c_bounds = [c_bounds_1, c_bounds_2, c_bounds_mid], use_rand = use_rand) #~ success, c_mid_1, c_mid_2 = solve_dyn(data, c_bounds = [c_bounds_1, c_bounds_2]) @@ -292,7 +292,23 @@ def test(stateid = 1, path = False, use_rand = False, just_one_curve = False, nu if just_one_curve: bezier_0 = __Bezier([com_1,c_mid_1[0].tolist(),c_mid_2[0].tolist(),com_2]) - p0 = fullBody.generateCurveTrajParts(bezier_0,[0.,0.3,0.9,1.]) + partions = [0.,0.3,0.9,1.] + p0 = fullBody.generateCurveTrajParts(bezier_0,partions) + #testing intermediary configurations + print 'wtf', partions[1], " " + com_interm1 = bezier_0[1] + com_interm2 = bezier_0[2] + success_proj1 = project_com_colfree(fullBody, stateid , com_interm1) + success_proj2 = project_com_colfree(fullBody, stateid+1, com_interm2) + + if not success_proj1: + print "proj 1 failed" + return False, c_mid_1, c_mid_2, paths_ids + + if not success_proj2: + print "proj 2 failed" + return False, c_mid_1, c_mid_2, paths_ids + print "p0", p0 #~ pp.displayPath(p0+1) #~ pp.displayPath(p0+2)