def gen_sequence_data_from_state(fullBody,
stateid,
configs,
m=55.88363633,
mu=0.5):
print "state id", stateid
#first compute com #acceleration always assumed to be 0 and vel as wel
Ps, Ns = fullBody.computeContactPoints(stateid)
#~ states = [make_state(fullBody, Ps[i], Ns[i], configs[stateid+i], viewer) for i in range(0,3,2)]
states = []
states += [
make_state(fullBody, Ps[0], Ns[0], fullBody.getConfigAtState(stateid))
]
states += [
make_state(fullBody, Ps[-1], Ns[-1],
fullBody.getConfigAtState(stateid + 1))
]
K0 = -asarray(
compute_CWC(Ps[0], Ns[0], mass=m, mu=mu, simplify_cones=False))
K1 = -asarray(
compute_CWC(Ps[1], Ns[1], mass=m, mu=mu, simplify_cones=False))
#~ K2 = asarray(compute_CWC(Ps[2], Ns[2], mass=m, mu = mu, simplify_cones = False))
return {
"start_state": states[0],
"end_state": states[1],
"inter_contacts": make_state_contact_points(Ps[1], Ns[1]),
"cones": [K0, K1],
"stateid": stateid,
"mass": fullBody.getMass(),
"com1": __com(fullBody, stateid),
"com2": __com(fullBody, stateid + 1)
}
def projectToFeasibleCom(state, ddc =[0.,0.,0.], max_num_samples = 10, friction = 0.6):
#~ H, h = state.getContactCone(friction)
ps = state.getContactPosAndNormals()
p = ps[0][0]
N = ps[1][0]
print("p", p)
print("N", N)
#~ try:
H = compute_CWC(p, N, state.fullBody.client.basic.robot.getMass(), mu = friction, simplify_cones = False)
c_ref = state.getCenterOfMass()
#~ Kin = state.getComConstraint(limbsCOMConstraints, [])
#~ res = find_valid_c_cwc_qp(H, c_ref, Kin, ddc, state.fullBody.getMass())
success, p_solved , x = find_valid_c_cwc_qp(H, c_ref,None, ddc, state.fullBody.getMass())
#~ except:
#~ success = False
if success:
x = x.tolist()
#~ if x[2] < 0.9:
x[2] += 0.35
for i in range(10):
if state.fullBody.projectStateToCOM(state.sId ,x, max_num_samples):
print("success after " + str(i) + " trials")
return True
else:
x[2]-=0.05
else:
print("qp failed")
return False;
def plot_feasible(state):
com = array(state.getCenterOfMass())
ps = state.getContactPosAndNormals()
p = ps[0][0]
N = ps[1][0]
H = compute_CWC(p,
N,
state.fullBody.client.basic.robot.getMass(),
mu=1,
simplify_cones=False)
for i in range(5):
for j in range(5):
for k in range(10):
c = com + array([(i - 2.5) * 0.2, (j - 2.5) * 0.2,
(k - 5) * 0.2])
w = compute_w(c)
active_ineq = state.getComConstraint(limbsCOMConstraints, [])
if (active_ineq[0].dot(c) <=
active_ineq[1]).all() and (H.dot(w) <= 0).all():
#~ print 'active'
createPtBox(r.client.gui, 0, c, color=[0, 1, 0, 1])
else:
if (active_ineq[0].dot(c) >= active_ineq[1]).all():
#~ print "inactive"
createPtBox(r.client.gui, 0, c, color=[1, 0, 0, 1])
return -1
def plot_feasible_cone(state):
com = array(state.getCenterOfMass())
#~ H, h = state.getContactCone(0.6)
ps = state.getContactPosAndNormals()
p = ps[0][0]
N = ps[1][0]
H = compute_CWC(p,
N,
state.fullBody.client.basic.robot.getMass(),
mu=0.6,
simplify_cones=False)
#~ H = comp
#~ H = -array(H)
#~ h = array(h)
#~ print "h", h
for i in range(10):
for j in range(10):
for k in range(1):
c = com + array([(i - 5) * 0.1, (j - 5) * 0.1, k])
w = compute_w(c)
print "w, ", w
if (H.dot(w) <= 0).all():
#~ print 'active'
createPtBox(r.client.gui, 0, c, color=[0, 1, 0, 1])
else:
#~ if(H.dot( w )>= 0).all():
#~ print "inactive"
createPtBox(r.client.gui, 0, c, color=[1, 0, 0, 1])
return H
def computeSupportPolygon(state, filename="sp"):
com = array(state.getCenterOfMass())
ps = state.getContactPosAndNormals()
p = ps[0][0]
N = ps[1][0]
H = compute_CWC(p,
N,
state.fullBody.client.basic.robot.getMass(),
mu=1,
simplify_cones=False)
#~ return H
res = []
rg = 2000
eq = rg / 2
scale = 0.001
#~ rg = 20
#~ eq = rg /2
#~ scale = 0.1
for i in range(rg):
for j in range(rg):
for k in range(1):
c = com + array([(i - eq) * scale, (j - eq) * scale, k])
w = compute_w(c)
#~ print "c", c
if (H.dot(w) <= 0).all():
print 'active'
res += [c]
#~ createPtBox(r.client.gui, 0, c, color = [0,1,0,1])
#~ else:
#~ createPtBox(r.client.gui, 0, c, color = [1,0,0,1])
f = open(filename, "w")
dump(res, f)
f.close()
return res
def gen_non_trivial_data(idx=0, num_iters=100, plt=False):
all_data = load_data('stair_bauzil_contacts_data')
quasi_static_sol, c_ddc_0, c_ddc_mid, c_ddc_1, P0, N0, P1, N1 = generate_problem(
all_data[idx], test_quasi_static=False, m=m, mu=mu)
K0 = compute_CWC(P0, N0, mass=m, mu=mu, simplify_cones=False)
K1 = compute_CWC(P1, N1, mass=m, mu=mu, simplify_cones=False)
start = clock()
for i in range(num_iters):
quasi_static_sol, c_ddc_0, c_ddc_mid, c_ddc_1, P0, N0, P1, N1 = generate_problem(
all_data[idx], test_quasi_static=i == 0, m=m, mu=mu)
#~ if(quasi_static_sol):
#~ global results
#~ results['num_quasi_static'] += 1
#~ print "quasi static solution, "
#~ return c_ddc_mid
#~ else:
found, init_traj_ok = connect_two_points(c_ddc_0,
c_ddc_mid,
P0,
N0,
mu=mu,
m=m,
use_cone_for_eq=None,
plot=plt)
#~ if(not init_traj_ok):
#~ saveTrial(c_ddc_0 , c_ddc_mid, P0, N0, found, K0)
#~ found, init_traj_ok = connect_two_points(c_ddc_mid, c_ddc_1 , P1, N1, mu = mu, m = m,use_cone_for_eq = K1, plot = plt)
#~ if(not init_traj_ok):
#~ saveTrial(c_ddc_mid, c_ddc_1 , P1, N1, found, K1)
end = clock()
print "total time ", (end - start) * 1000
print "avg time ", float(end - start) / float(num_iters) * 1000.
print "num success, ", len(results['trials_success'])
print "num success, ", len(results['trials_success'])
print "num fails, ", len(results['trials_fail'])
print "num quasi static , ", results["num_quasi_static"]
results['instant_successes'] = num_iters - (
len(results['trials_success']) + len(results['trials_fail']))
print "num additional instant successes", results['instant_successes']
def gen_sequence_data_from_state_objects(s1,
s2,
sInt,
mu=0.5,
isContactCreated=False):
m = s1.fullBody.getMass()
print "state id", s1.sId
#first compute com #acceleration always assumed to be 0 and vel as wel
Ps1, Ns1 = s1.getContactPosAndNormals()
PsInt, NsInt = sInt.getContactPosAndNormals()
Ps2, Ns2 = s2.getContactPosAndNormals()
#~ states = [make_state(fullBody, Ps[i], Ns[i], configs[stateid+i], viewer) for i in range(0,3,2)]
states = []
states += [make_state(s1.fullBody, Ps1[0], Ns1[0], s1.q())]
states += [make_state(s1.fullBody, Ps2[0], Ns2[0], s2.q())]
print "KO"
K0 = asarray(
compute_CWC(Ps1[0], Ns1[0], mass=m, mu=mu, simplify_cones=False))
#~ K0 = asarray(s1.getContactCone(mu)[0])
print "K1"
#~ K1 = asarray(sInt.getContactCone(mu)[0])
K1 = asarray(
compute_CWC(PsInt[0], NsInt[0], mass=m, mu=mu, simplify_cones=False))
#~ K2 = asarray(compute_CWC(Ps[2], Ns[2], mass=m, mu = mu, simplify_cones = False))
print "ret"
return {
"start_state": states[0],
"end_state": states[1],
"inter_contacts": make_state_contact_points(PsInt[0], NsInt[0]),
"cones": [K0, K1],
"stateid": s1.sId,
"mass": m,
"com1": s1.getCenterOfMass(),
"com2": s2.getCenterOfMass(),
"isContactCreated": isContactCreated
}
g = np.array([0.,0.,-9.81]) def gen_contact(center = np.array([0,0,0]),R = identity_matrix()): c_4 = np.array(center.tolist()+[0]) p_rot = [R.dot(p_i + c_4)[0:3] for p_i in p ] n_rot = [R.dot(z)[0:3] for _ in range(4) ] return np.array(p_rot), np.array(n_rot) P0, N0 = gen_contact(center = np.array([0,0,0]),R = identity_matrix()) P1, N1 = gen_contact(center = np.array([1,0,0]),R = rotation_matrix(math.pi/8., y_axis)) P2, N2 = gen_contact(center = np.array([4,0,0]),R = identity_matrix()) def gen_phase(p_a, n_a, p_b, n_b): phase_p = np.zeros((p_a.shape[0] + p_b.shape[0],3)) phase_n = np.zeros((n_a.shape[0] + n_b.shape[0],3)) phase_p[0:4,:] = p_a[:]; phase_n[0:4,:] = n_a[:]; phase_p[4:8,:] = p_b[:]; phase_n[4:8,:] = n_b[:]; return phase_p, phase_n phase_p_1, phase_n_1 = gen_phase(P0, N0, P1, N1) phase_p_2, phase_n_2 = P1[:], N1[:] phase_p_3, phase_n_3 = gen_phase(P1, N1, P2, N2) #~ phase_p_3, phase_n_3 = P2, N2 H1 = compute_CWC(phase_p_1,phase_n_1,mass=mass,mu=mu) H2 = compute_CWC(phase_p_2,phase_n_2,mass=mass,mu=mu) H3 = compute_CWC(phase_p_3,phase_n_3,mass=mass,mu=mu)
def test_eq_cwc(P,N,mu,m, g_vec = array([0,0,-9.81])): H = compute_CWC(P,N,mass=m,mu=mu) def eval_wrench(c,ddc, dL=array([0.,0.,0.])): return is_stable(H, c, ddc=ddc, dL=dL, m=m, g_vec=g_vec) return eval_wrench
