functional version of candidate sampling. must make call to...

functional version of candidate sampling. must make call to predict_future_state for each broken contact scenario

script/tools/tro_capture_point/gen_hrp2_statically_balanced_positions_2d.py

@@ -162,33 +162,66 @@ def pos_quat_to_pinocchio(q):
 	q_res[3:6] = quat_end
 	return q_res
 
-def gen_contact_candidates_one_limb(limb, config_gepetto, res, num_candidates = 10):
+def gen_contact_candidates_one_limb(limb, res, num_candidates = 10):
 	effectorName = limbsCOMConstraints[limb]['effector']
 	candidates = []
+	print "res", res
 	for i in range(num_candidates):
-		fullBody.setCurrentConfig(fullBody.getSample(limb,randint(0,n_samples - 1)))
-		#~ m = _getTransform(fullBody.getJointPosition(effectorName))
-		candidates.append(pos_quat_to_pinocchio(fullBody.getJointPosition(effectorName)))
-	res["candidates"][effectorName] = candidates
+		q_contact = fullBody.getSample(limb,randint(0,n_samples - 1))
+		fullBody.setCurrentConfig(q_contact)
+		m = _getTransform(fullBody.getJointPosition(effectorName))
+		candidates.append(m)		
+		#DEBUG
+		res["config_candidates"].append(q_contact)
+		#~ candidates.append(pos_quat_to_pinocchio(fullBody.getJointPosition(effectorName)))
+	res[effectorName] = candidates
 		
-		
-def find_limbs_broken(target_c, config, limbs):
+
+def removeLimb(limb, limbs):
+	return [l for l in limbs if l != limb]
 	
+#~ 
+#~ def find_limbs_broken(target_c, config, limbs):
+	#~ res = []
+	#~ for limb in limbs:
+		#~ nLimbs = removeLimb(limb, limbs)
+		#~ state_id = fullBody.createState(config, nLimbs)
+		#~ if (fullBody.projectStateToCOM(state_id,target_c)):
+			#~ res.append(limb)
+	#~ return res
 	
 def gen_contact_candidates(limbs, config_gepetto, res):
 	#first generate a com translation current configuration
-	success, dc, delta_c = scv.comTranslationAfter07s(res["q"], res["contact_points"])
-	#set it as new com objective for projection
 	fullBody.setCurrentConfig(config_gepetto)
-	c = array(fullBody.getCenterOfMass())
-	target_c = c + delta_c
-	state_id = fullBody.createState(config, limbs)
-	res["candidates"] = {}
-	brokenContacts = []
-	candidateLimbs = []
-	if(fullBody.projectStateToCOM(state_id,target_c)): #all good, all contacts kinematically maintained
-		for limb in limbs:
-			gen_contact_candidates_one_limb(limb, config_gepetto, res, 10)
+	success, dc, c_final, v0 = scv.comPosAfter07s(matrix(fullBody.getCenterOfMass()), res["q"], res["contact_points"])	
+	if(success):
+		data = {}
+		#set it as new com objective for projection
+		target_c = c_final.tolist()
+		state_id = fullBody.createState(config_gepetto, limbs)
+		#~ res["candidates"].append({})
+		data["v0"] = v0
+		data["dc"] = dc
+		data["c_final"] = c_final
+		data["candidateEffector"] = {}
+		#DEBUG
+		data["candidateEffector"]["config_candidates"] = []
+		if (fullBody.projectStateToCOM(state_id,target_c)): #all good, all contacts kinematically maintained
+			#~ res["candidates"][-1]["brokenContacts"] = find_limbs_broken(target_c, config_gepetto, limbs)
+			#~ for limb in limbs:
+			proj_config = fullBody.getConfigAtState(state_id)
+			#DEBUG
+			data["init_config"] = config_gepetto	
+			#DEBUG
+			data["projected_config"] = proj_config	
+			fullBody.setCurrentConfig(proj_config)
+			for limb in limbsCOMConstraints.keys():
+				gen_contact_candidates_one_limb(limb, data["candidateEffector"], 1)			
+			if (not res.has_key("candidates")):
+				res["candidates"] = []
+			res["candidates"].append(data)
+			
+		
 	
 from numpy import cos, sin, pi
 def __eulerToQuat(pitch, roll, yaw):
@@ -268,7 +301,10 @@ def gen(limbs, num_samples = 1000, coplanar = True):
 		q[3:6] = quat_end
 		qs.append(q)
 		qs_gepetto.append(q_gep)
-		states.append(fill_contact_points(limbs,q_gep,q))
+		res = fill_contact_points(limbs,q_gep,q)
+		for _ in range(10):
+			gen_contact_candidates(limbs, q_gep, res)
+		states.append(res)
 	global all_qs
 	all_qs += [qs_gepetto]
 	fname = ""

script/tools/tro_capture_point/sample_com_vel.py

@@ -10,6 +10,7 @@ if cwd+'/data/config' not in sys.path:
 
 import conf_hpp as conf
 from utils import compute_initial_joint_velocities_multi_contact
+from pinocchio_inv_dyn.multi_contact.stability_criterion import StabilityCriterion
 
 import pinocchio as se3
 from pinocchio.utils import zero as mat_zeros
@@ -86,28 +87,28 @@ def createSimulator(q0, v0):
     simulator.verb=0;
     return simulator;
 
-def draw_q(q0):	
-	p = np.matrix.copy(invDynForm.contact_points);
-	simulator.viewer.updateRobotConfig(q0);
-	simulator.updateComPositionInViewer(np.matrix([invDynForm.x_com[0,0], invDynForm.x_com[1,0], 0.]).T);
-	q0[2] -= 0.005
-	simulator.viewer.updateRobotConfig(q0);
-	p[2,:] -= 0.005;
-	for j in range(p.shape[1]):
-		simulator.viewer.addSphere("contact_point"+str(j), 0.005, p[:,j], (0.,0.,0.), (1, 1, 1, 1));
-		simulator.viewer.updateObjectConfigRpy("contact_point"+str(j), p[:,j]);
-	
-	f = open(conf.INITIAL_CONFIG_FILENAME, 'rb');
-	res = pickle.load(f);
-	f.close();
-	p_steve = np.matrix(res[conf.INITIAL_CONFIG_ID]['P']);
-	p_steve[:,2] -= 0.005;
-	for j in range(p_steve.shape[0]):
-		simulator.viewer.addSphere("contact_point_steve"+str(j), 0.005, p_steve[j,:].T, color=(1, 0, 0, 1));
-		simulator.viewer.updateObjectConfigRpy("contact_point_steve"+str(j), p_steve[j,:].T);  
+def draw_q(q0): 
+    p = np.matrix.copy(invDynForm.contact_points);
+    simulator.viewer.updateRobotConfig(q0);
+    simulator.updateComPositionInViewer(np.matrix([invDynForm.x_com[0,0], invDynForm.x_com[1,0], 0.]).T);
+    q0[2] -= 0.005
+    simulator.viewer.updateRobotConfig(q0);
+    p[2,:] -= 0.005;
+    for j in range(p.shape[1]):
+        simulator.viewer.addSphere("contact_point"+str(j), 0.005, p[:,j], (0.,0.,0.), (1, 1, 1, 1));
+        simulator.viewer.updateObjectConfigRpy("contact_point"+str(j), p[:,j]);
+    
+    f = open(conf.INITIAL_CONFIG_FILENAME, 'rb');
+    res = pickle.load(f);
+    f.close();
+    p_steve = np.matrix(res[conf.INITIAL_CONFIG_ID]['P']);
+    p_steve[:,2] -= 0.005;
+    for j in range(p_steve.shape[0]):
+        simulator.viewer.addSphere("contact_point_steve"+str(j), 0.005, p_steve[j,:].T, color=(1, 0, 0, 1));
+        simulator.viewer.updateObjectConfigRpy("contact_point_steve"+str(j), p_steve[j,:].T);  
 
 def q_pin(q_hpp):
-	return np.matrix(q_hpp).T
+    return np.matrix(q_hpp).T
 
 def gen_com_vel(q0, contacts):
     init(q0);
@@ -119,29 +120,54 @@ def gen_com_vel(q0, contacts):
     print 'Gonna compute initial joint velocities that satisfy contact constraints';
     print 'conf.MAX_INITIAL_COM_VEL', conf.MAX_INITIAL_COM_VEL
     (success, v)= compute_initial_joint_velocities_multi_contact(q0, invDynForm, conf.mu[0], 
-												conf.ZERO_INITIAL_ANGULAR_MOMENTUM, 
-												conf.ZERO_INITIAL_VERTICAL_COM_VEL,
-												False, #conf.ENSURE_STABILITY, 
+                                                conf.ZERO_INITIAL_ANGULAR_MOMENTUM, 
+                                                conf.ZERO_INITIAL_VERTICAL_COM_VEL,
+                                                False, #conf.ENSURE_STABILITY, 
                                                                   True, #conf.ENSURE_UNSTABILITY, 
                                                                   conf.MAX_INITIAL_COM_VEL, 100);
-    if success:						
+    if success:                     
         print "Initial velocities found"
-        return (success, invDynForm.J_com * v);
+        return (success, v, invDynForm.J_com * v);
     print "Could not find initial velocities"
     return (success, v[:]);
-						
-								
-def comTranslationAfter07s(q_hpp, contacts):
-	(success, dc0)= gen_com_vel(q_pin(q_hpp), contacts)
-	return success, dc0, dc0 * 0.7
-	
+                        
+                                
+def comPosAfter07s(c, q_hpp, contacts):
+    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
+    (success, v) = compute_initial_joint_velocities_multi_contact(q0, invDynForm, 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, 100);
+    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);
+    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)
+    print "c ", res.c
+    print "dc ", res.dc
+    return success, res.dc, res.c, v0
+    
 
 np.set_printoptions(precision=2, suppress=True);
 
 if(conf.freeFlyer):
-	robot = RobotWrapper(conf.urdfFileName, conf.model_path, root_joint=se3.JointModelFreeFlyer());
+    robot = RobotWrapper(conf.urdfFileName, conf.model_path, root_joint=se3.JointModelFreeFlyer());
 else:
-	robot = RobotWrapper(conf.urdfFileName, conf.model_path, None);
+    robot = RobotWrapper(conf.urdfFileName, conf.model_path, None);
 nq = robot.nq;
 nv = robot.nv;
 v0 = mat_zeros(nv);
@@ -151,14 +177,14 @@ na = None;    # number of joints
 simulator =  None;
 
 def init(q0):
-	''' CREATE CONTROLLER AND SIMULATOR '''
-	global invDynForm
-	global robot
-	global na
-	global simulator
-	print "reset invdyn"
-	invDynForm = createInvDynFormUtil(q0, v0);
-	robot = invDynForm.r;
-	na = invDynForm.na;    # number of joints
-	simulator = createSimulator(q0, v0);
-	#~ gen_com_vel(q0, config_test['contact_points'])
+    ''' CREATE CONTROLLER AND SIMULATOR '''
+    global invDynForm
+    global robot
+    global na
+    global simulator
+    print "reset invdyn"
+    invDynForm = createInvDynFormUtil(q0, v0);
+    robot = invDynForm.r;
+    na = invDynForm.na;    # number of joints
+    simulator = createSimulator(q0, v0);
+    #~ gen_com_vel(q0, config_test['contact_points'])

script/tools/tro_capture_point/utils.py

@@ -12,9 +12,141 @@ from pinocchio.utils import zero as mat_zeros
 from pinocchio.utils import rand as mat_rand
 from pinocchio_inv_dyn.acc_bounds_util import computeVelLimits
 from pinocchio_inv_dyn.sot_utils import solveLeastSquare
-from pinocchio_inv_dyn.multi_contact.utils import can_I_stop
 from pinocchio_inv_dyn.multi_contact.stability_criterion import StabilityCriterion
 
+EPS = 1e-5
+
+class Bunch:
+    def __init__(self, **kwds):
+        self.__dict__.update(kwds);
+
+    def __str__(self):
+        res = "";
+        for (key,value) in self.__dict__.iteritems():
+            if (isinstance(value, np.ndarray) and len(value.shape)==2 and value.shape[0]>value.shape[1]):
+                res += " - " + key + ": " + str(value.T) + "\n";
+            else:
+                res += " - " + key + ": " + str(value) + "\n";
+        return res[:-1];
+
+def select_contact_to_make(x_com, dx_com, robot, name_contact_to_break, contacts, contact_candidates, mu, gravity, mass, viewer=None, verb=0):
+    ncp = np.sum([np.matrix(c['P']).shape[0] for c in contacts.itervalues()]);
+    p = mat_zeros((3,ncp));  # contact points in world frame
+    N = mat_zeros((3,ncp));  # contact normals in world frame
+    N[2,:] = 1.0;
+    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
+    i = 0;
+    for (contact_name, PN) in contacts.iteritems():
+        if(contact_name==name_contact_to_break):
+            continue;
+        oMi = robot.framePosition(robot.model.getFrameId(contact_name));
+        Pi = np.matrix(PN['P']).T;
+        Ni = np.matrix(PN['N']).T;
+        for j in range(Pi.shape[1]):
+            p[:,i] = oMi.act(Pi[:,j]);
+            N[:,i] = oMi.rotation * Ni[:,j];
+            i += 1;
+    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(Pi.shape[1]):
+            p[:,i+j] = oMi.act(P_cand[:,j]);
+            N[:,i+j] = oMi.rotation * N_cand[:,j]; 
+
+        if(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):
+    ''' Select which contact to break
+    '''
+    ncp = np.sum([np.matrix(c['P']).shape[0] for c in contacts.itervalues()]);
+    # assume all contacts have the same number of contact points
+    ncp -= np.matrix(contacts.itervalues().next()['P']).shape[0];
+    p   = mat_zeros((3,ncp));
+    N   = mat_zeros((3,ncp));
+    N[2,:] = 1.0;
+    stabilityCriterion = StabilityCriterion("Stab_crit", x_com, dx_com, p.T, N.T, mu, gravity, mass);
+    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
+        i = 0;
+        for (contact_name, PN) in contacts.iteritems():    
+            if(contact_name==name_of_contact_to_break):
+                continue;
+            fid = robot.model.getFrameId(contact_name);
+            oMi = robot.framePosition(fid);
+            Pi = np.matrix(PN['P']).T;
+            Ni = np.matrix(PN['N']).T;
+            for j in range(Pi.shape[1]):
+                p[:,i] = oMi.act(Pi[:,j]);
+                N[:,i] = oMi.rotation * Ni[:,j];
+                i += 1;
+        # 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);
+#            print "Without contact %s robot will be able to maintain the contact for %.3f s"%(name_of_contact_to_break,t);
+#            print "  Predicted com state = (", c_t.T, dc_t.T, "), norm(dc)=%.3f"%norm(dc_t);
+        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]);
 
 ''' Solve a QP to compute initial joint velocities that satisfy contact constraints and optionally other
     specified constraints, such as having the capture point inside the convex hull of the contact points.