[script] add python scripts to generate constraints for the LP formulation

script/constants_and_tools.py

+import numpy as np
+#~ from hpp.corbaserver.rbprm.hrp2 import Robot as rob
+#~ from hpp.corbaserver.rbprm.tools.obj_to_constraints import load_obj, as_inequalities, rotate_inequalities
+#~ from hpp_centroidal_dynamics import *
+#~ from hpp_spline import *e
+from numpy import array, asmatrix, matrix, zeros, ones
+from numpy import array, dot, stack, vstack, hstack, asmatrix, identity, cross, concatenate
+from numpy.linalg import norm
+import numpy as np
+
+from scipy.spatial import ConvexHull
+#~ from hpp_bezier_com_traj import *
+#~ from qp import solve_lp
+
+#~ import eigenpy
+#~ import cdd
+#~ from curves import bezier3
+from random import random as rd
+from random import randint as rdi
+from numpy import squeeze, asarray
+
+
+Id = matrix([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]])
+z = array([0.,0.,1.])
+zero3 = zeros(3) 
+
+
+def generators(A,b, Aeq = None, beq = None):
+    m = np.hstack([b,-A])
+    matcdd = cdd.Matrix(m); matcdd.rep_type = cdd.RepType.INEQUALITY
+    
+    if Aeq is not None:
+        meq = np.hstack([beq,-Aeq])
+        matcdd.extend(meq.tolist(), True)
+    
+    H = cdd.Polyhedron(matcdd)
+    g = H.get_generators()
+    
+    return [array(g[el][1:]) for el in range(g.row_size)], H
+    
+def filter(pts):
+    hull = ConvexHull(pts, qhull_options='Q12')
+    return [pts[i] for i in hull.vertices.tolist()]
+    
+def ineq(pts, canonicalize = False):
+    apts = array(pts)
+    m = np.hstack([ones((apts.shape[0],1)),apts])
+    matcdd = cdd.Matrix(m); matcdd.rep_type = cdd.RepType.GENERATOR
+    H = cdd.Polyhedron(matcdd)
+    bmA = H.get_inequalities()
+    if canonicalize:
+        bmA.canonicalize()
+    Ares = zeros((bmA.row_size,bmA.col_size-1))
+    bres = zeros(bmA.row_size )
+    for i in range(bmA.row_size):
+        l = array(bmA[i])
+        Ares[i,:] = -l[1:]
+        bres[i]   =  l[0]
+    return Ares, bres
+    
+def ineqQHull(hull):
+    A = hull.equations[:,:-1]
+    b = -hull.equations[:,-1]
+    return A,b
+    
+    
+def canon(A,b):
+    m = np.hstack([b,-A])
+    matcdd = cdd.Matrix(m); matcdd.rep_type = 1
+    H = cdd.Polyhedron(matcdd)
+    bmA = H.get_inequalities()
+    #~ bmA.canonicalize()
+    Ares = zeros((bmA.row_size,bmA.col_size-1))
+    bres = zeros((bmA.row_size,1 ))
+    for i in range(bmA.row_size):
+        #~ print "line ", array(bmA[i])
+        #~ print "A ", A[i][:]
+        #~ print "b ", b[i]
+        l = array(bmA[i])
+        Ares[i,:] = -l[1:]
+        bres[i]   =  l[0]
+        #~ print "Ares ",Ares[i,:]
+        #~ print "bres ",bres[i]
+    return Ares, bres
+
+def genPolytope(A,b):
+    pts, H = generators(A,b)
+    apts = array(pts)
+    if len(apts) > 0:
+        hull = ConvexHull(apts)
+        return hull, pts, apts, H
+    return None, None, None, None
+    
+def convex_hull_ineq(pts):
+    return None
+    
+    
+    m = cData.contactPhase_.getMass()
+    #~ #get 6D polytope
+    (H, h) = ineqFromCdata(cData)
+    #project to the space where aceleration is 0
+    D = zeros((6,3))
+    D[3:,:] = m * gX
+    
+    d = zeros((6,))
+    d[:3] = -m * g
+    
+    A =     H.dot(D)
+    b = h.reshape((-1,)) - H.dot(d)    
+    #add kinematic polytope
+    (K,k) = (cData.Kin_[0], cData.Kin_[1].reshape(-1,))
+    
+    resA = vstack([A, K])
+    resb = concatenate([b, k]).reshape((-1,1))
+    
+    #DEBUG
+    allpts = generators(resA,resb)[0]
+    error = False
+    for pt in allpts:
+        print "pt ", pt
+        assert (resA.dot(pt.reshape((-1,1))) - resb).max() <0.001, "antecedent point not in End polytope"  + str((resA.dot(pt.reshape((-1,1))) - resb).max())
+        if (H.dot(w(m,pt).reshape((-1,1))) - h).max() > 0.001:
+            error = True
+            print "antecedent point not in End polytope"  + str((H.dot(w(m,pt).reshape((-1,1))) - h).max())
+    assert not error, str (len(allpts))
+    
+    return (resA, resb)
+    #~ return (A, b)
+    #~ return (vstack([A, K]), None)
+
+def default_transform_from_pos_normal(pos, normal):
+    #~ print "pos ", pos
+    #~ print "normal ", normal
+    f = array([0.,0.,1.])
+    t = array(normal)
+    v = np.cross(f, t)
+    c = np.dot(f, t)
+    if c > 0.99:
+        rot = identity(3)    
+    else:
+        u = v/norm(v)
+        h = (1. - c)/(1. - c**2)
+
+        vx, vy, vz = v
+        rot =array([[c + h*vx**2, h*vx*vy - vz, h*vx*vz + vy],
+              [h*vx*vy+vz, c+h*vy**2, h*vy*vz-vx],
+              [h*vx*vz - vy, h*vy*vz + vx, c+h*vz**2]])
+    return vstack( [hstack([rot,pos.reshape((-1,1))]), [ 0.        ,  0.        ,  0.        ,  1.        ] ] )
+
+import os
+
+def continuous(h, initpts):
+    dic = {}
+    pts = []
+    for i, pt in enumerate(h.vertices.tolist()):
+        pts += [initpts[pt]]
+        dic[pt] = i
+    faces = []
+    for f in h.simplices:
+        faces += [[dic[idx]+1 for idx in f ]]
+    return pts, faces
+
+def hull_to_obj(h,pts,name):
+    pts, faces = continuous(h, pts)
+    f = open(name, "w")
+    #first write points
+    for pt in pts:
+        #~ print "??"
+        f.write('v ' + str(pt[0]) + ' ' + str(pt[1]) + ' ' + str(pt[2]) + ' \n' );
+    f.write('g foo\n')
+    for pt in faces:
+        #~ print "???"
+        f.write('f ' + str(pt[0]) + ' ' + str(pt[1]) + ' ' + str(pt[2]) + ' \n' );
+    f.write('g \n')
+    f.close()
+        
+        
+        
+
+#~ function vertface2obj(v,f,name)
+#~ % VERTFACE2OBJ Save a set of vertice coordinates and faces as a Wavefront/Alias Obj file
+#~ % VERTFACE2OBJ(v,f,fname)
+#~ %     v is a Nx3 matrix of vertex coordinates.
+#~ %     f is a Mx3 matrix of vertex indices. 
+#~ %     fname is the filename to save the obj file.
+
+#~ fid = fopen(name,'w');
+
+#~ for i=1:size(v,1)
+#~ fprintf(fid,'v %f %f %f\n',v(i,1),v(i,2),v(i,3));
+#~ end
+
+#~ fprintf(fid,'g foo\n');
+
+#~ for i=1:size(f,1);
+#~ fprintf(fid,'f %d %d %d\n',f(i,1),f(i,2),f(i,3));
+#~ end
+#~ fprintf(fid,'g\n');
+
+#~ fclose(fid);

script/relativeFootPositionQuasiFlat.py

+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
+from hpp.gepetto import Viewer
+from hpp.gepetto import ViewerFactory
+from numpy import array
+from numpy.linalg import norm
+from hpp.corbaserver.rbprm.talos import Robot
+#from plot_plytopes import *
+from constants_and_tools import * 
+from pinocchio import Quaternion
+
+NUM_SAMPLES = 18000
+IT_DISPLAY_PROGRESS = NUM_SAMPLES/10
+MIN_DIST_BETWEEN_FEET_Y = 0.18
+MAX_DIST_BETWEEN_FEET_X = 0.35
+MIN_HEIGHT_COM = 0.3
+MARGIN_FEET_SIDE = 0.05 # margin used to constrain the com y position : if it's on the left of the left foot or on the right of the right foot for more than this margin, we reject this sample
+
+fullBody = Robot ()
+#~ fullBody.setJointBounds ("base_joint_xyz", [-2,2, -2, 2, -2, 2])
+fullBody.setJointBounds ("root_joint", [-20,20, -20, 20, -20, 20])
+fullBody.setConstrainedJointsBounds()
+
+#~ from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+from hpp.corbaserver import ProblemSolver
+nbSamples = 1
+
+ps = ProblemSolver( fullBody )
+vf = ViewerFactory (ps)
+v = vf.createViewer()
+rootName = 'root_joint'
+
+nbSamples = 10000
+heuristic = "static"
+print "gen limb db"
+fullBody.addLimb(fullBody.rLegId,fullBody.rleg,fullBody.rfoot,fullBody.rLegOffset,fullBody.rLegNormal, fullBody.rLegx, fullBody.rLegy, nbSamples, heuristic, 0.01,kinematicConstraintsPath=fullBody.rLegKinematicConstraints,kinematicConstraintsMin = 0.75)
+fullBody.runLimbSampleAnalysis(fullBody.rLegId, "ReferenceConfiguration", True)
+fullBody.addLimb(fullBody.lLegId,fullBody.lleg,fullBody.lfoot,fullBody.lLegOffset,fullBody.rLegNormal, fullBody.lLegx, fullBody.lLegy, nbSamples, heuristic, 0.01,kinematicConstraintsPath=fullBody.lLegKinematicConstraints,kinematicConstraintsMin = 0.75)
+fullBody.runLimbSampleAnalysis(fullBody.lLegId, "ReferenceConfiguration", True)
+print "db generated."
+rLegId = fullBody.rLegId
+lLegId = fullBody.lLegId
+rfoot = fullBody.rfoot
+lfoot = fullBody.lfoot
+rLegOffset = fullBody.rLegOffset
+lLegOffset = fullBody.lLegOffset
+
+
+#make sure this is 0
+q_0 = fullBody.getCurrentConfig ()
+zeroConf = [0,0,0, 0, 0, 0, 1.]
+q_0[0:7] = zeroConf
+fullBody.setCurrentConfig (q_0)
+
+effectors = [rfoot, lfoot]
+limbIds = [rLegId, lLegId]
+offsets = [array(rLegOffset), array(lLegOffset)]
+
+import numpy as np
+
+points = [[],[]]
+#~ compoints = [[[0.012471792486262121, 0.0015769611415203033, 0.8127583093263778]],[[0.012471792486262121, 0.0015769611415203033, 0.8127583093263778]]]
+compoints = [[],[]]
+
+success = 0
+fails = 0
+from hpp.corbaserver.rbprm import  rbprmstate, state_alg
+from scipy.spatial import ConvexHull
+
+def genFlat():
+        q = fullBody.shootRandomConfig()
+        q[0:7] = zeroConf
+        fullBody.setCurrentConfig(q)
+        #~ v(q)
+        posrf = fullBody.getJointPosition(rfoot)[:3]
+        poslf = fullBody.getJointPosition(lfoot)[:3]
+        s = rbprmstate.State(fullBody, q = q, limbsIncontact = limbIds) 
+        s, succ = state_alg.addNewContact(s, rLegId, posrf, [0.,0.,1.], num_max_sample = 0)
+        if succ:
+                s, succ = state_alg.addNewContact(s, lLegId, poslf, [0.,0.,1.], num_max_sample = 0)
+        if succ:
+                succ = fullBody.isConfigValid(q)[0] and norm (array(posrf[:2]) - array(poslf[:2]) ) >= 0.3
+        #print "sid = "+str(s.sId)
+        #~ if succ and norm (array(posrf[:2]) - array(poslf[:2]) ) <= 0.1:
+        if succ and norm (array(posrf) - array(poslf) ) <= 0.1:
+                v(s.q())
+        return s.q(), succ, s, [posrf, poslf]
+        
+def printFootPositionRelativeToOther(nbConfigs):
+	for i in range(0,nbConfigs):
+		if i > 0 and not i%IT_DISPLAY_PROGRESS:
+		  print str(int((i*100)/nbConfigs))+" % done"
+		q, succ, s,  pos = genFlat()
+                if succ:
+			global success
+			success +=1
+                        addCom = True
+			for j in range(0,len(effectors)):
+			#~ for j in range(1):
+                                fullBody.setCurrentConfig(q)
+                                otheridx = (j + 1)%2
+                                #~ print "otheridx", otheridx
+                                #~ print "q ", q[:3]
+                                oeffectorName = effectors[otheridx]
+                                #~ oqEffector = fullBody.getJointPosition(oeffectorName)
+                                pos_other = fullBody.getJointPosition(oeffectorName)
+                                #~ print "other pos 1", pos_other[:3]
+                                
+				effectorName = effectors[j]
+				limbId = limbIds[j]
+				qEffector = fullBody.getJointPosition(effectorName)
+                                #~ print "pos 1", qEffector[:3]
+                                
+                                
+                                qtr = q[:]
+                                qtr[:3] = [qtr[0] - pos_other[0], qtr[1] - pos_other[1], qtr[2] - pos_other[2] ]
+                                #~ for l in range(3):
+                                        #~ qtr[l] -= pos_other[l]
+                                        #~ qtr[i] -= qEffector[i]
+                                fullBody.setCurrentConfig(qtr)
+                                
+				effectorName = effectors[j]
+				limbId = limbIds[j]
+				qEffector = fullBody.getJointPosition(effectorName)
+                                #~ print "pos 2", qEffector[3:]
+                                #~ print "pos 2", qEffector[:3]
+                                
+                                #~ print "other effectorName 1", oeffectorName
+                                #~ print "effectorName 1", effectorName
+                                pos_other = fullBody.getJointPosition(oeffectorName)
+                                # check current joint pos is now zero
+                                #~ print "other pos 2", pos_other[:3]
+                                #~ v(qtr)
+				q0 = Quaternion(qEffector[6],qEffector[3],qEffector[4],qEffector[5])
+				rot = q0.matrix() #compute rotation matrix world -> local
+				p = qEffector[0:3] #(0,0,0) coordinate expressed in effector fram
+				rm=np.zeros((4,4))
+				for k in range(0,3):
+					for l in range(0,3):
+						rm[k,l] = rot[k,l]
+				for m in range(0,3):
+					rm[m,3] = qEffector[m]
+				rm[3,3] = 1
+				invrm = np.linalg.inv(rm)
+                                #~ print invrm
+				#~ p = invrm.dot([0,0,0,1])
+				p = invrm.dot([0,0,0.,1])
+                                #~ print "p ", p
+                                #~ print norm (array(posrf) - array(poslf) )
+                                if(j == 0 and  p[1] > MIN_DIST_BETWEEN_FEET_Y and abs(p[0]) < MAX_DIST_BETWEEN_FEET_X):
+                                        points[j].append(p[:3])
+                                elif(j == 1 and p[1] < -MIN_DIST_BETWEEN_FEET_Y and abs(p[0]) < MAX_DIST_BETWEEN_FEET_X):
+                                        points[j].append(p[:3])
+                                else:
+                                        addCom = False
+				#~ print (points[j])
+                        #now compute coms
+                        
+                        fullBody.setCurrentConfig(q)
+                        com = fullBody.getCenterOfMass()
+                        for x in range(0,3):
+                                q[x] = -com[x]               
+                        for j in range(0,len(effectors)):
+                                effectorName = effectors[j]
+				limbId = limbIds[j]
+				qEffector = fullBody.getJointPosition(effectorName)
+				q0 = Quaternion(qEffector[6],qEffector[3],qEffector[4],qEffector[5])
+				rot = q0.matrix() #compute rotation matrix world -> local
+				p = qEffector[0:3] #(0,0,0) coordinate expressed in effector fram
+				rm=np.zeros((4,4))
+				for k in range(0,3):
+					for l in range(0,3):
+						rm[k,l] = rot[k,l]
+				for m in range(0,3):
+					rm[m,3] = qEffector[m]
+				rm[3,3] = 1
+				invrm = np.linalg.inv(rm)
+				p = invrm.dot([0,0,0,1])
+                                # add offset 
+                                rp = array(p[:3] - offsets[j]).tolist()
+                                #~ print "p ", p
+                                #~ print "rp "
+                                
+                                if(rp[2] < MIN_HEIGHT_COM):
+                                  addCom = False
+                                if addCom :
+                                  if j == 1:
+                                    if rp[1] < MARGIN_FEET_SIDE:
+                                      compoints[j].append(rp)    
+                                  else:
+                                    if rp[1] > -MARGIN_FEET_SIDE:
+                                      compoints[j].append(rp)    
+                                #~ elif(p[1] < -0.05):
+                                        #~ points[j].append(p[:3])
+
+				#~ compoints[j].append(p[:3])
+                        
+		else:			
+			global fails
+			fails +=1
+			#~ print fullBody.isConfigValid(q)[1]
+	#~ for j in range(0,len(limbIds)):
+		#~ f1=open('./'+str(limbIds[j])+'_com.erom', 'w+')
+		#~ for p in points[j]:
+			#~ f1.write(str(p[0]) + "," + str(p[1]) + "," + str(p[2]) + "\n")
+		#~ f1.close()
+
+#~ printRootPosition(rLegId, rfoot, nbSamples)
+#~ printRootPosition(lLegId, lfoot, nbSamples)
+#~ printRootPosition(rarmId, rHand, nbSamples)
+#~ printRootPosition(larmId, lHand, nbSamples) 
+printFootPositionRelativeToOther(NUM_SAMPLES)
+print "successes ", success
+print "fails  ", fails
+
+
+hcomRF = ConvexHull(compoints[0])
+hcomLF = ConvexHull(compoints[1])
+hull_to_obj(hcomRF,compoints[0],"talos_COM_constraints_in_RF_effector_frame.obj")
+hull_to_obj(hcomLF,compoints[1],"talos_COM_constraints_in_LF_effector_frame.obj")
+
+hptsRF = ConvexHull(points[0])
+hptsLF = ConvexHull(points[1])
+hull_to_obj(hptsRF,points[0],"talos_LF_constraints_in_RF.obj")
+hull_to_obj(hptsLF,points[1],"talos_RF_constraints_in_LF.obj")
+
+        
+#~ for k in range(2):
+        #~ hcom = ConvexHull(compoints[k])
+        #~ plot_hull(hcom, compoints[k], array(compoints[k]))
+        
+        #~ hpts = ConvexHull(points[k])
+        #~ plot_hull(hpts, points[k], array(points[k]), color = "b", plot = k == 1 and True)