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 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
def compute_initial_joint_velocities_multi_contact(q, robot, contacts, constraint_mask, mu,
ZERO_INITIAL_ANGULAR_MOMENTUM,
ZERO_INITIAL_VERTICAL_COM_VEL,
ENSURE_STABILITY, ENSURE_UNSTABILITY,
MAX_INITIAL_COM_VEL, MAX_MIN_JOINT_ACC, MAX_ITER):
nv = robot.nv;
na = robot.nv-6;
robot.computeAllTerms(q, mat_zeros(nv));
robot.framesKinematics(q);
x_com = robot.com(q, update_kinematics=False);
J_com = robot.Jcom(q, update_kinematics=False);
M = robot.mass(q, update_kinematics=False);
constr_dim = constraint_mask.sum();
k = int(np.sum([constr_dim for con in contacts]));
Jc = mat_zeros((k,nv));
i = 0;
for name in contacts:
fid = robot.model.getFrameId(name);
Jc[i:i+constr_dim,:] = robot.frameJacobian(q, fid, False);
i += constr_dim;
(P,N) = compute_contact_points_from_contact_dictionary(robot, contacts);
n_in = k+3 if ZERO_INITIAL_ANGULAR_MOMENTUM else k;
n_in += 1 if ZERO_INITIAL_VERTICAL_COM_VEL else 0;
A = J_com;
A_in = mat_zeros((n_in,nv));
lb_in = mat_zeros(n_in);
ub_in = mat_zeros(n_in);
A_in[:k,:] = Jc;
if(ZERO_INITIAL_ANGULAR_MOMENTUM):
A_in[k:k+3,:] = M[3:6,:];
if(ZERO_INITIAL_VERTICAL_COM_VEL):
A_in[-1,:] = J_com[2,:];
# compute joint velocity bounds for viability
v_lb = mat_zeros(nv)-1e10;
v_ub = mat_zeros(nv)+1e10;
qMin = robot.model.lowerPositionLimit;
qMax = robot.model.upperPositionLimit;
dqMax = robot.model.velocityLimit;
for j in range(na):
(v_lb[6+j], v_ub[6+j]) = computeVelLimits(q[7+j], qMin[7+j], qMax[7+j], dqMax[6+j], MAX_MIN_JOINT_ACC);
# verify that given CoM position allows for static equilibrium
dx_com_des = mat_zeros(3);
v0 = mat_zeros(3);
stab_criterion = StabilityCriterion("default", x_com, dx_com_des, P.T, N.T, mu, np.array([0,0,-9.81]), M[0,0]);
try:
# check initial state is in static equilibrium
res = stab_criterion.can_I_stop();
if(not res.is_stable):
print "[compute_initial_joint_velocities_multi_contact] ERROR: initial configuration is not in static equilibrium", res.c, res.dc;
return (False, v0);
except Exception as e:
print "[compute_initial_joint_velocities_multi_contact] Error while testing static equilibrium. %s"%str(e);
# sample desired CoM velocities
initial_state_generated = False;
counter = 0;
while(not initial_state_generated):
counter += 1;
if(counter > MAX_ITER):
return (False, v0);
dx_com_des = MAX_INITIAL_COM_VEL * mat_rand(3) / sqrt(3.0);
if(ENSURE_STABILITY or ENSURE_UNSTABILITY):
try:
res = stab_criterion.can_I_stop(x_com, dx_com_des);
if((ENSURE_STABILITY and not res.is_stable) or (ENSURE_UNSTABILITY and res.is_stable)):
continue;
except Exception as e:
print "[compute_initial_joint_velocities_multi_contact] Error while checking stability of desired com velocity. %s"%str(e);
continue;
# solve QP to find joint velocities
(imode,v0) = solveLeastSquare(A.A, dx_com_des.A, v_lb.A, v_ub.A, A_in.A, lb_in.A, ub_in.A, regularization=1e-4);
if(imode==0):
dx_com_0 = np.dot(J_com, v0);
try:
res = stab_criterion.can_I_stop(x_com, dx_com_0);
if((ENSURE_UNSTABILITY and not res.is_stable) or (ENSURE_STABILITY and res.is_stable) or
(not ENSURE_STABILITY and not ENSURE_UNSTABILITY)):
initial_state_generated = True;
except Exception as e:
print "[compute_initial_joint_velocities_multi_contact] Error while checking stability of com velocity. %s"%str(e);
return (True, v0);
def test_discretize_vs_discretize_momentum(N_CONTACTS=2,
solver='qpoases',
verb=0):
DO_PLOTS = False
PLOT_3D = False
mu = 0.5
# friction coefficient
lx = 0.1
# half foot size in x direction
ly = 0.07
# half foot size in y direction
#First, generate a contact configuration
CONTACT_POINT_UPPER_BOUNDS = [0.5, 0.5, 0.5]
CONTACT_POINT_LOWER_BOUNDS = [-0.5, -0.5, 0.0]
gamma = atan(mu)
# half friction cone angle
RPY_LOWER_BOUNDS = [-2 * gamma, -2 * gamma, -pi]
RPY_UPPER_BOUNDS = [+2 * gamma, +2 * gamma, +pi]
MIN_CONTACT_DISTANCE = 0.3
global mass
global g_vector
X_MARG = 0.07
Y_MARG = 0.07
succeeded = False
while (not succeeded):
(p, N) = generate_contacts(N_CONTACTS, lx, ly, mu,
CONTACT_POINT_LOWER_BOUNDS,
CONTACT_POINT_UPPER_BOUNDS,
RPY_LOWER_BOUNDS, RPY_UPPER_BOUNDS,
MIN_CONTACT_DISTANCE, False)
X_LB = np.min(p[:, 0] - X_MARG)
X_UB = np.max(p[:, 0] + X_MARG)
Y_LB = np.min(p[:, 1] - Y_MARG)
Y_UB = np.max(p[:, 1] + Y_MARG)
Z_LB = np.max(p[:, 2] + 0.3)
Z_UB = np.max(p[:, 2] + 1.5)
#~ (H,h) = compute_GIWC(p, N, mu, False);
H = -compute_CWC(p, N, mass, mu)
h = zeros(H.shape[0])
(succeeded, c0) = find_static_equilibrium_com(mass, [X_LB, Y_LB, Z_LB],
[X_UB, Y_UB, Z_UB], H, h)
dc0 = np.random.uniform(-1, 1, size=3)
Z_MIN = np.max(p[:, 2]) - 0.1
Ineq_kin = zeros([3, 3])
Ineq_kin[2, 2] = -1
ineq_kin = zeros(3)
ineq_kin[2] = -Z_MIN
#~
bezierSolver = BezierZeroStepCapturability(
"ss",
c0,
dc0,
p,
N,
mu,
g_vector,
mass,
verb=verb,
solver=solver,
kinematic_constraints=[Ineq_kin, ineq_kin])
#~ bezierSolver = BezierZeroStepCapturability("ss", c0, dc0, p, N, mu, g_vector, mass, verb=verb, solver=solver, kinematic_constraints = None);
stabilitySolver = StabilityCriterion("ss",
c0,
dc0,
p,
N,
mu,
g_vector,
mass,
verb=verb,
solver=solver)
window_times = [1] + [0.1 * i for i in range(1, 10)
] + [0.1 * i
for i in range(11, 21)] #try nominal time first
#~ window_times = [0.2*i for i in range(1,5)] + [0.2*i for i in range(6,11)] #try nominal time first
#~ window_times = [1]+ [0.4*i for i in range(1,4)] #try nominal time first
#~ window_times = [1]+ [0.4*i for i in range(3,6)] #try nominal time first
window_times = [0.7]
found = False
time_step_check = 0.01
for i, el in enumerate(window_times):
if (found):
break
res = bezierSolver.can_I_stop(T=el, time_step=time_step_check)
if (res.is_stable):
found = True
#~ print "continuous found at ", el
__check_trajectory(bezierSolver._p0,
bezierSolver._p1,
res.c,
res.c,
el,
bezierSolver._H,
bezierSolver._mass,
bezierSolver._g,
time_step=time_step_check,
dL=bezier(
matrix([p_i.tolist()
for p_i in res.wpsdL]).transpose()))
if i != 0:
print "discretize Failed to stop at 1, but managed to stop at ", el
found = False
time_step_check = 0.01
for i, el in enumerate(window_times):
if (found):
break
res2 = bezierSolver.can_I_stop(T=el,
time_step=time_step_check,
l0=zeros(3))
if (res2.is_stable):
found = True
#~ print "ang_momentum found at ", el
__check_trajectory(
bezierSolver._p0,
bezierSolver._p1,
res2.c,
res2.c,
el,
bezierSolver._H,
#~ bezierSolver._mass, bezierSolver._g, time_step = time_step_check, dL = res2.dL_of_t)
bezierSolver._mass,
bezierSolver._g,
time_step=time_step_check,
dL=bezier(
matrix([p_i.tolist() for p_i in res2.wpsdL]).transpose()))
if i != 0:
print "ang_momentum Failed to stop at 1, but managed to stop at ", el
#~ res2 = None
#~ try:
#~ res2 = stabilitySolver.can_I_stop();
#~ except Exception as e:
#~ pass
if (res2.is_stable != res.is_stable):
if (res.is_stable):
print "discretize won"
else:
print "ang_momentum won"
return res2.is_stable, res.is_stable, res2, res, c0, dc0, H, h, p, N