def select_contact_to_make(x_com, dx_com, robot, name_contact_to_break, contacts, contact_candidates, mu, gravity, mass, viewer=None, verb=0):
(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);
c_pred = mat_zeros((3,len(contact_candidates)));
dc_pred = mat_zeros((3,len(contact_candidates)));
v_pred = mat_zeros(len(contact_candidates));
t_pred = mat_zeros(len(contact_candidates));
# compute contact points for all contacts except the one to move
contacts_minus_one = dict(contacts);
del contacts_minus_one[name_contact_to_break];
(P_minus_one, N_minus_one) = compute_contact_points_from_contact_dictionary(robot, contacts_minus_one);
i = P_minus_one.shape[1];
p[:,:i] = P_minus_one;
N[:,:i] = N_minus_one;
c_id=0;
P_cand = np.matrix(contacts[name_contact_to_break]['P']).T; # contact points of contact to break in local frame
N_cand = np.matrix(contacts[name_contact_to_break]['N']).T; # contact normals of contact to break in local frame
for (c_id,oMi) in enumerate(contact_candidates):
# compute new position of contact points
for j in range(P_cand.shape[1]):
p[:,i+j] = oMi.act(P_cand[:,j]);
N[:,i+j] = oMi.rotation * N_cand[:,j];
if(verb>0 and viewer is not None):
# update contact points in viewer
for j in range(p.shape[1]):
viewer.addSphere("contact_point"+str(j), 0.005, p[:,j], (0.,0.,0.), (1, 1, 1, 1));
viewer.updateObjectConfigRpy("contact_point"+str(j), p[:,j]);
# check whether the system can stop with this contacts
stabilityCriterion.set_contacts(p.T, N.T, mu);
try:
stab_res = stabilityCriterion.can_I_stop(x_com, dx_com);
c_pred[:,c_id] = np.asmatrix(stab_res.c).T;
dc_pred[:,c_id] = np.asmatrix(stab_res.dc).T;
v_pred[c_id] = norm(stab_res.dc);
t_pred[c_id] = stab_res.t;
if(verb>0):
print "[select_contact_to_make] Candidate %d, predicted com vel in %.3f s = %.3f"%(c_id, stab_res.t, norm(stab_res.dc));
# raw_input();
except Exception as e:
print "ERROR while computing stability criterion:", e;
best_candidate_ids = np.where(abs(v_pred-np.min(v_pred))<EPS)[0];
if(best_candidate_ids.shape[0]>1):
# if multiple contacts result in the same final com velocity (typically that would be 0),
# pick the one who does it faster
best_candidate_id = best_candidate_ids[np.argmin(t_pred[best_candidate_ids])];
else:
best_candidate_id = best_candidate_ids[0];
return Bunch(index=best_candidate_id, c=c_pred[:,best_candidate_id],
dc=dc_pred[:,best_candidate_id], t=t_pred[best_candidate_id,0]);
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]);