Skip to content
GitLab
Commit 6442a0e4 authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

[Python] format

parent de5bd19b
Hide whitespace changes
Inline Side-by-side
script/constants_and_tools.py
View file @ 6442a0e4
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 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, hstack, identity, matrix, ones, vstack, zeros
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
# import eigenpy
import cdd
# from hpp_bezier_com_traj import *
# from qp import solve_lp
Id = matrix([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]])
z = array([0.,0.,1.])
zero3 = zeros(3)
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
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])
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):
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
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 )
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]
bmAl = array(bmA[i])
Ares[i, :] = -bmAl[1:]
bres[i] = bmAl[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
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 ))
# 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]
# print("line ", array(bmA[i]))
# print("A ", A[i][:])
# print("b ", b[i])
bmAl = array(bmA[i])
Ares[i, :] = -bmAl[1:]
bres[i] = bmAl[0]
# print("Ares ",Ares[i,:])
# print("bres ",bres[i])
return Ares, bres
def genPolytope(A,b):
pts, H = generators(A,b)
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
"""
# TODO: what is cData ?
m = cData.contactPhase_.getMass()
#~ #get 6D polytope
# 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 = 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)
A = H.dot(D)
b = h.reshape((-1, )) - H.dot(d)
#add kinematic polytope
(K,k) = (cData.Kin_[0], cData.Kin_[1].reshape(-1,))
(K, k) = (cData.Kin_[0], cData.Kin_[1].reshape(-1, ))
resA = vstack([A, K])
resb = concatenate([b, k]).reshape((-1,1))
resb = concatenate([b, k]).reshape((-1, 1))
#DEBUG
allpts = generators(resA,resb)[0]
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:
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))
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)
# 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.])
# 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)
rot = identity(3)
else:
u = v/norm(v)
h = (1. - c)/(1. - c**2)
# 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. ] ] )
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 = {}
......@@ -157,44 +159,43 @@ def continuous(h, initpts):
dic[pt] = i
faces = []
for f in h.simplices:
faces += [[dic[idx]+1 for idx in f ]]
faces += [[dic[idx] + 1 for idx in f]]
return pts, faces
def hull_to_obj(h,pts,name):
def hull_to_obj(h, pts, name):
pts, faces = continuous(h, pts)
f = open(name, "w")
#first write points
# first write points
for pt in pts:
#~ print "??"
f.write('v ' + str(pt[0]) + ' ' + str(pt[1]) + ' ' + str(pt[2]) + ' \n' );
# 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' );
# 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');
# 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
# 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');
# 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');
# 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);
# fclose(fid);
script/relativeFootPositionQuasiFlat.py
View file @ 6442a0e4
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.gepetto import Viewer
from __future__ import print_function
import numpy as np
# from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
from hpp.corbaserver import ProblemSolver
from hpp.corbaserver.rbprm import rbprmstate, state_alg
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
from scipy.spatial import ConvexHull
from talos_rbprm.talos import Robot
from .constants_and_tools import hull_to_obj
NUM_SAMPLES = 18000
IT_DISPLAY_PROGRESS = NUM_SAMPLES/10
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
# 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:
MARGIN_FEET_SIDE = 0.05
fullBody = Robot ()
#~ fullBody.setJointBounds ("base_joint_xyz", [-2,2, -2, 2, -2, 2])
fullBody.setJointBounds ("root_joint", [-20,20, -20, 20, -20, 20])
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)
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)
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.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."
print("db generated.")
rLegId = fullBody.rLegId
lLegId = fullBody.lLegId
rfoot = fullBody.rfoot
......@@ -45,188 +72,183 @@ 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.]
# 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)
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 = [[],[]]
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]
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 = ", 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[