Commit 9d1ff951 authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

[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!
Please register or to comment