def predict_future_com_state(x_com, dx_com, robot, mass, contacts, mu, gravity, time, verb=0):
''' Predict future com state
'''
(P,N) = compute_contact_points_from_contact_dictionary(robot, contacts);
stabilityCriterion = StabilityCriterion("Stab_crit", x_com, dx_com, P.T, N.T, mu, gravity, mass, verb=verb);
try:
res = stabilityCriterion.predict_future_state(time);
if(verb>0):
print "[predict_future_com_state] Contacts can be kept for %.3f s"%(res.t);
print " Predicted com state = (", res.c.T, res.dc.T, "), norm(dc)=%.3f"%norm(res.dc);
except Exception as e:
print "ERROR while computing stability criterion predict_future_state:", e;
return res;
def select_contact_to_break(x_com, dx_com, robot, mass, contacts, mu, gravity, step_time, verb=0):
''' Select which contact to break
'''
p = mat_zeros((3,1));
N = mat_zeros((3,1));
N[2,:] = 1.0;
stabilityCriterion = StabilityCriterion("Stab_crit", x_com, dx_com, p.T, N.T, mu, gravity, mass, verb=verb);
t_pred = mat_zeros(len(contacts));
t_min = mat_zeros(len(contacts));
v_pred = mat_zeros(len(contacts));
c_pred = mat_zeros((3,len(contacts)));
dc_pred = mat_zeros((3,len(contacts)));
dc_min = mat_zeros((3,len(contacts)));
for (contactId, name_of_contact_to_break) in enumerate(contacts):
# compute the contact points without name_of_contact_to_break
contacts_minus_one = dict(contacts);
del contacts_minus_one[name_of_contact_to_break];
(p,N) = compute_contact_points_from_contact_dictionary(robot, contacts_minus_one);
# predict future com state supposing to break name_of_contact_to_break
stabilityCriterion.set_contacts(p.T, N.T, mu);
try:
res = stabilityCriterion.predict_future_state(step_time);
t_pred[contactId] = res.t;
t_min[contactId] = res.t_min;
c_pred[:,contactId] = np.asmatrix(res.c).T;
dc_pred[:,contactId] = np.asmatrix(res.dc).T;
dc_min[:,contactId] = np.asmatrix(res.dc_min).T;
v_pred[contactId] = norm(res.dc);
if(verb>0):
print "[select_contact_to_break] Without contact %s contacts can be kept for %.3f s"%(name_of_contact_to_break,res.t);
print " Predicted com state = (", res.c.T, res.dc.T, "), norm(dc)=%.3f"%norm(res.dc);
except Exception as e:
print "ERROR while computing stability criterion:", e;
t_pred_sorted = sorted(t_pred.A.squeeze());
if(t_pred_sorted[-1] > t_pred_sorted[-2]+EPS):
id_contact_to_break = np.argmax(t_pred);
else:
id_contact_to_break = np.argmin(v_pred);
name_of_contact_to_break = contacts.keys()[id_contact_to_break];
return Bunch(name=name_of_contact_to_break, c=c_pred[:,id_contact_to_break],
dc=dc_pred[:,id_contact_to_break], t=t_pred[id_contact_to_break,0],
t_min=t_min[id_contact_to_break,0], dc_min=dc_min[:,id_contact_to_break]);
def com_pos_after_t(c, q_hpp, contacts, v = None, t = 0.7):
q0 = q_pin(q_hpp)
init(q0);
v0 = mat_zeros(nv);
#~ invDynForm.setNewSensorData(0, q0, v0);
#~ updateConstraints(0, q0, v0, invDynForm, contacts);
#~ draw_q(q0);
print 'Gonna compute initial joint velocities that satisfy contact constraints';
print 'conf.MAX_INITIAL_COM_VEL', conf.MAX_INITIAL_COM_VEL
if(v == None):
(success, v) = compute_initial_joint_velocities_multi_contact(q0, robot, contacts, [True]*6, conf.mu[0],
conf.ZERO_INITIAL_ANGULAR_MOMENTUM,
conf.ZERO_INITIAL_VERTICAL_COM_VEL,
False, #conf.ENSURE_STABILITY,
True, #conf.ENSURE_UNSTABILITY,
conf.MAX_INITIAL_COM_VEL,conf.MAX_MIN_JOINT_ACC , 100);
else:
success = True
if (not success):
print "Could not find initial velocities"
return (success, v[:], c, v0);
c_init = np.matrix(c)
dx_com_des = mat_zeros(3);
#~ P = np.matlib.copy(invDynForm.contact_points);
#~ N = np.matlib.copy(invDynForm.contact_normals);
robot.computeAllTerms(q0, mat_zeros(nv));
robot.framesKinematics(q0);
(P,N) = compute_contact_points_from_contact_dictionary(robot, contacts)
print "contacts ", len(contacts)
print "contacts ", contacts
print "normals ", N
M = robot.mass(q0, update_kinematics=True);
J_com = robot.Jcom(q0, update_kinematics=False);
print "Mas ", M
stab_criterion = StabilityCriterion("default", np.matrix(c), dx_com_des, P.T, N.T, conf.mu[0], np.array([0,0,-9.81]), M[0,0])
res = stab_criterion.predict_future_state(t, c_init, J_com * v)
#TODO : res.t != 0.7
print "c ", res.c
print "dc ", res.dc
return success and abs(res.t - t)< EPS , res.dc, res.c, v0