Commit 9d1ff951 by Pierre Fernbach

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

parent 5273be39
 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);
 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)
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!