diff --git a/script/tools/tro_capture_point/gen_hrp2_statically_balanced_positions_2d.py b/script/tools/tro_capture_point/gen_hrp2_statically_balanced_positions_2d.py index 6d9d180b92a3e87735221f213c3692cef07d756b..dbd7c27352fb8c70ad33e36291fcd0719bf81cc6 100644 --- a/script/tools/tro_capture_point/gen_hrp2_statically_balanced_positions_2d.py +++ b/script/tools/tro_capture_point/gen_hrp2_statically_balanced_positions_2d.py @@ -318,18 +318,20 @@ def _boundSO3(q, num_limbs): return q -def _genbalance(limbs): +def _genbalance(limbs, unstable): for i in range(10000): q = fullBody.client.basic.robot.shootRandomConfig() q = _boundSO3(q, len(limbs)) - if fullBody.isConfigValid(q)[0] and fullBody.isConfigBalanced(q, limbs, 5) and __loosely_z_aligned(limbs[0], q) and __loosely_z_aligned(limbs[1], q): + if fullBody.isConfigValid(q)[0] and __loosely_z_aligned(limbs[0], q) and __loosely_z_aligned(limbs[1], q): + if (unstable and not fullBody.isConfigBalanced(q, limbs, 5)) or (not unstable and fullBody.isConfigBalanced(q, limbs, 5)): #~ if fullBody.isConfigValid(q)[0] and __loosely_z_aligned(limbs[0], q) and __loosely_z_aligned(limbs[1], q): - return q + return q print "can't generate equilibrium config" all_qs = [] all_states = [] -def gen(limbs, num_samples = 1000, coplanar = True, num_candidates_per_config = 0, num_contact_candidates = 10, q_entries = None, projectToObstacles = False): +all_data = [] +def gen(limbs, num_samples = 1000, coplanar = True, num_candidates_per_config = 0, num_contact_candidates = 10, q_entries = None, projectToObstacles = False, unstable = False): q_0 = fullBody.getCurrentConfig(); #~ fullBody.getSampleConfig() qs = []; qs_gepetto = []; states = [] @@ -343,7 +345,7 @@ def gen(limbs, num_samples = 1000, coplanar = True, num_candidates_per_config = if(coplanar): q = fullBody.generateGroundContact(limbs) else: - q = _genbalance(limbs) + q = _genbalance(limbs, unstable) else: q = q_entries[i] q_gep = q[:] @@ -366,7 +368,10 @@ def gen(limbs, num_samples = 1000, coplanar = True, num_candidates_per_config = fname += "configs" if(coplanar): fname += "_coplanar" + if(unstable): + fname += "_unstable" data["samples"] = states + all_data.append(data) from pickle import dump #~ f1=open("configs_feet_on_ground_static_eq", 'w+') f1=open(fname, 'w+') @@ -386,14 +391,15 @@ q_init = [ 0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0, # RLEG 31-36 ]; r (q_init) -limbs = [[lLegId,rLegId],[lLegId,rLegId, rarmId], [lLegId,rLegId, larmId], [lLegId,rLegId, rarmId, larmId] ] -#~ limbs = [[lLegId,rLegId, rarmId]] +#~ limbs = [[lLegId,rLegId],[lLegId,rLegId, rarmId], [lLegId,rLegId, larmId], [lLegId,rLegId, rarmId, larmId] ] +#~ limbs = [[lLegId,rLegId] ] +limbs = [[lLegId,rLegId, rarmId]] #~ limbs = [[larmId, rarmId]] -gen(limbs[0], 10) +#~ gen(limbs[0], 1000) for ls in limbs: - gen(ls, 10, False) -gen(limbs[0], 10) + gen(ls, 1000, False, unstable=True) +gen(limbs[0], 1000, unstable=True) i = 0 a = None