Commit ecab1d26 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

handling fall in prediction

parent 9dc2c37f
...@@ -198,7 +198,7 @@ def predict_com_for_limb_candidate(c, limb, limbs, res, data, config_gepetto): ...@@ -198,7 +198,7 @@ def predict_com_for_limb_candidate(c, limb, limbs, res, data, config_gepetto):
for k, v in res["contact_points"].iteritems(): for k, v in res["contact_points"].iteritems():
if k != effector: if k != effector:
contact_points[k] = v contact_points[k] = v
success, dc, c_final, v0 = scv.comPosAfter07s(c, res["q"], contact_points) success, dc, c_final, v0 = scv.com_pos_after_t(c, res["q"], contact_points)
effector_data = {} effector_data = {}
state_id = fullBody.createState(config_gepetto, maintained_limbs) state_id = fullBody.createState(config_gepetto, maintained_limbs)
if(success and fullBody.projectStateToCOM(state_id,c_final.tolist())): #all good, all contacts kinematically maintained): if(success and fullBody.projectStateToCOM(state_id,c_final.tolist())): #all good, all contacts kinematically maintained):
......
...@@ -133,7 +133,7 @@ def gen_com_vel(q_hpp, contacts): ...@@ -133,7 +133,7 @@ def gen_com_vel(q_hpp, contacts):
return (success, v[:], invDynForm.J_com * v); return (success, v[:], invDynForm.J_com * v);
def comPosAfter07s(c, q_hpp, contacts, v = None): def com_pos_after_t(c, q_hpp, contacts, v = None, t = 0.7):
q0 = q_pin(q_hpp) q0 = q_pin(q_hpp)
init(q0); init(q0);
v0 = mat_zeros(nv); v0 = mat_zeros(nv);
...@@ -160,10 +160,11 @@ def comPosAfter07s(c, q_hpp, contacts, v = None): ...@@ -160,10 +160,11 @@ def comPosAfter07s(c, q_hpp, contacts, v = None):
P = np.matlib.copy(invDynForm.contact_points); P = np.matlib.copy(invDynForm.contact_points);
N = np.matlib.copy(invDynForm.contact_normals); N = np.matlib.copy(invDynForm.contact_normals);
stab_criterion = StabilityCriterion("default", invDynForm.x_com, dx_com_des, P.T, N.T, conf.mu[0], np.array([0,0,-9.81]), invDynForm.M[0,0]) stab_criterion = StabilityCriterion("default", invDynForm.x_com, dx_com_des, P.T, N.T, conf.mu[0], np.array([0,0,-9.81]), invDynForm.M[0,0])
res = stab_criterion.predict_future_state(0.7, c_init, invDynForm.J_com * v) res = stab_criterion.predict_future_state(t, c_init, invDynForm.J_com * v)
#TODO : res.t != 0.7
print "c ", res.c print "c ", res.c
print "dc ", res.dc print "dc ", res.dc
return success, res.dc, res.c, v0 return success and abs(res.t - t)< EPS , res.dc, res.c, v0
np.set_printoptions(precision=2, suppress=True); np.set_printoptions(precision=2, suppress=True);
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment