Commit 57322723 authored by Steve T's avatar Steve T
Browse files

script for relative positions

parent 10824bd0
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);
#do the loading of the obj file
import numpy as np
from collections import namedtuple
ObjectData = namedtuple("ObjectData", "V T N F")
Inequalities = namedtuple("Inequality", "A b N V")
def toFloat(stringArray):
res= np.zeros(len(stringArray))
for i in range(0,len(stringArray)):
res[i] = float(stringArray[i])
return res
def load_obj(filename) :
V = [] #vertex
T = [] #texcoords
N = [] #normals
F = [] #face indexies
fh = open(filename)
for line in fh :
if line[0] == '#' : continue
line = line.strip().split(' ')
if line[0] == 'v' : #vertex
V.append(toFloat(line[1:]))
elif line[0] == 'vt' : #tex-coord
T.append(line[1:])
elif line[0] == 'vn' : #normal vector
N.append(toFloat(line[1:]))
elif line[0] == 'f' : #face
face = line[1:]
for i in range(0, len(face)) :
face[i] = face[i].split('/')
# OBJ indexies are 1 based not 0 based hence the -1
# convert indexies to integer
for j in range(0, len(face[i])):
if j!=1:
face[i][j] = int(face[i][j]) - 1
F.append(face)
return ObjectData(V, T, N, F)
def inequality(v, n):
#the plan has for equation ax + by + cz = d, with a b c coordinates of the normal
#inequality is then ax + by +cz -d <= 0
# last var is v because we need it
return [n[0], n[1], n[2], np.array(v).dot(np.array(n))]
def as_inequalities(obj):
#for each face, find first three points and deduce plane
#inequality is given by normal
A= np.empty([len(obj.F), 3])
b = np.empty(len(obj.F))
V = np.ones([len(obj.F), 4])
N = np.empty([len(obj.F), 3])
for f in range(0, len(obj.F)):
face = obj.F[f]
v = obj.V[face[0][0]]
# assume normals are in obj
n = obj.N[face[0][2]]
ineq = inequality(v,n)
A[f,:] = ineq[0:3]
b[f] = ineq[3]
V[f,0:3] = v
N[f,:] = n
return Inequalities(A,b, N, V)
def is_inside(inequalities, pt):
return ((inequalities.A.dot(pt) - inequalities.b) < 0).all()
#~ def rotate_inequalities_q():
# TODO this is naive, should be a way to simply update d
def rotate_inequalities(ineq, transform):
#for each face, find first three points and deduce plane
#inequality is given by normal
A = np.empty([len(ineq.A), 3])
b = np.empty(len(ineq.b))
V = np.ones([len(ineq.V), 4])
N = np.ones([len(ineq.N), 3])
for i in range(0, len(b)):
v = transform.dot(ineq.V[i,:])
n = transform[0:3,0:3].dot(ineq.N[i,0:3])
ine = inequality(v[0:3],n[0:3])
A[i,:] = ine[0:3]
b[i] = ine[3]
V[i,:] = v
N[i,:] = n
return Inequalities(A,b, N, V)
from pickle import dump
def ineq_to_file(ineq, filename):
f1=open(filename, 'w+')
res = { 'A' : ineq.A, 'b' : ineq.b, 'N' : ineq.N, 'V' : ineq.V}
dump(res, f1)
f1.close()
from pickle import load
def ineq_from_file(filename):
f1=open(filename, 'r')
res = load(f1)
return Inequalities(res['A'], res['b'],res['N'],res['V'])
def test_inequality():
n = np.array([0,-1,0])
v = np.array([0,1,1])
if inequality(v,n) != [0,-1,0,-1]:
print("error in test_inequality")
else:
print("test_inequality successful")
def __gen_data():
obj = load_obj('./hrp2/RL_com._reduced.obj')
ineq = as_inequalities(obj)
ok_points = [[0,0,0], [0.0813, 0.0974, 0.2326], [-0.3387, 0.1271, -0.5354]]
not_ok_points = [[-0.3399, 0.2478, -0.722],[-0.1385,-0.4401,-0.1071]]
return obj, ineq, ok_points, not_ok_points
def test_belonging():
data = __gen_data()
ineq = data[1]
ok_points = data[2]
not_ok_points = data[3]
for p in ok_points:
assert (is_inside(ineq, np.array(p))), "point " + str(p) + " should be inside object"
for p in not_ok_points:
assert (not is_inside(ineq, np.array(p))), "point " + str(p) + " should NOT be inside object"
print("test_belonging successful")
def test_rotate_inequalities():
tr = np.array([[ 1. , 0. , 0. , 0. ],
[ 0. , 0.98006658, -0.19866933, 2. ],
[ 0. , 0.19866933, 0.98006658, 0. ],
[ 0. , 0. , 0. , 1. ]])
data = __gen_data()
ineq = rotate_inequalities(data[1], tr)
ok_points = [tr.dot(np.array(el + [1]))[0:3] for el in data[2]]
not_ok_points = [tr.dot(np.array(el + [1]))[0:3] for el in data[3]]
for p in ok_points:
assert (is_inside(ineq, p)), "point " + str(p) + " should be inside object"
for p in not_ok_points:
assert (not is_inside(ineq, p)), "point " + str(p) + " should NOT be inside object"
print("test_rotate_inequalities successful")
def load_obj_and_save_ineq(in_name, out_name):
obj = load_obj(in_name)
ineq = as_inequalities(obj)
ineq_to_file (ineq, out_name)
load_obj_and_save_ineq('./lfleg_com_reduced.obj','./lfleg_com.ineq')
load_obj_and_save_ineq('./lhleg_com_reduced.obj','./lhleg_com.ineq')
load_obj_and_save_ineq('./rhleg_com_reduced.obj','./rhleg_com.ineq')
load_obj_and_save_ineq('./rfleg_com_reduced.obj','./rfleg_com.ineq')
import numpy as np
from hpp_centroidal_dynamics import *
from hpp_spline import *
from numpy import array, asmatrix, matrix, zeros, ones
from numpy import array, dot, vstack, hstack, asmatrix, identity, cross
from numpy.linalg import norm
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
eigenpy.switchToNumpyArray()
from constants_and_tools import *
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
def plot_hull_in_subplot(hull, pts, apts, ax, color = "r", just_pts = False):
# Plot defining corner points
ax.plot(apts.T[0], apts.T[1], apts.T[2], "ko")
if not just_pts:
for s in hull.simplices:
s = np.append(s, s[0]) # Here we cycle back to the first coordinate
ax.plot(apts[s, 0], apts[s, 1], apts[s, 2], color+"-")
def plot_hull(hull, pts, apts, color = "r", just_pts = False, plot = False, fig = None, ax = None):
if fig is None:
fig = plt.figure()
if ax is None:
print ("ax is none")
ax = fig.add_subplot(111, projection="3d")
plot_hull_in_subplot(hull, pts, array(pts), ax, color, just_pts)
if plot:
print (" PLOT" )
plt.show(block=False)
return ax
def plot_polytope_H_rep(A_in,b_in, color = "r", just_pts = False):
hull, pts, apts, cd = genPolytope(A_in,b_in)
plot_hull(hull, pts, apts, color, just_pts)
def plot_polytope_V_rep(pts, color = "r", just_pts = False):
pts = [array(el) for el in pts]
apts = array(pts)
hull = ConvexHull(apts, qhull_options='Q12')
plot_hull(hull, pts, apts, color, just_pts)
def plot_polytope_CDD_PolyHeron(H, color = "r", just_pts = False):
g = H.get_generators()
pts = [array(g[el][1:]) for el in range(g.row_size)]
plot_polytope_V_rep(pts, color, just_pts)
# Copyright (c) 2013 CNRS
# Author: Jorrit T'Hooft
#
# This file is part of hpp-corbaserver.
# hpp-corbaserver is free software: you can redistribute it
# and/or modify it under the terms of the GNU Lesser General Public
# License as published by the Free Software Foundation, either version
# 3 of the License, or (at your option) any later version.
#
# hpp-corbaserver is distributed in the hope that it will be
# useful, but WITHOUT ANY WARRANTY; without even the implied warranty
# of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
# General Lesser Public License for more details. You should have
# received a copy of the GNU Lesser General Public License along with
# hpp-corbaserver. If not, see
# <http://www.gnu.org/licenses/>.
import numpy as np
from numpy import linalg
class Quaternion (object):
"""
Quaternion class :
------------------
A quaternion has a scalar part and a vector part.
In this class the quaternion is represented as an array of 4 elements :
- the first element is the scalar part
- the next 3 elements represents the vector part
One can acces to the array directly with the attribute "array"
e.g. q1=Quaternion(1,0,0,0) --> q1.array
A quaternion can be instanciated with 1, 2 or 4 elements
(see : __init__() for more information).
It can also return a rotation vector, a rotation matrix, or a SO3
(see the methods : to...() for more information).
"""
def __init__(self,*args):
"""
Instanciation of the quaternion with 1, 2 or 4 arguments :
-----------------------------------------------------------
This creates a 4-sized array (self.array) representing the quaternion
with the first element representing the scalar part
and the 3 others the vector part.
With 4 arguments :
------------------
- the first one is used as the scalar part,
the other three as the vector part.
With 2 arguments :
------------------
- the 1-sized argument is used as the scalar part,
the 3-sized argument is used as the vector part.
With 1 argument :
-----------------
- if it is a quaternion it will create a copy of this quaternion.
- if it is a scalar, the scalar will be used as the scalar part
and the vector part will be set at (0,0,0).
- if it is an array, matrix, tuple or list of 4 elements,
the first element is used as the scalar part
and the rest as the vector part.
- if it is an array, matrix, tuple or list of 3 elements,
the 3 elements are interpreted as a rotation vector,
this creates a quaternion representing the same rotation.
- if it is a to 2 dimension array convertible array, matrix, tuple
or list with at least (3*3) elements,
the upper left (3*3) elements are interpreted as a rotation matrix,
this creates a quaternion representing the same rotation.
- if it is an instance of SO3, quaternion is built from rotation
matrix.
With 0 arguments :
------------------
If no argument is given, than the quaternion will be set by default
to with the scalar part set to 1 and the vector part to (0,0,0).
(this is the neutral element for multiplication in quaternion space)
To create a quaternion from Roll, Pitch, Yaw angles :
-----------------------------------------------------
first instanciate a quaternion and than use the method fromRPY()
to change the values of it to the dezired ones.
e.g. : quat().fromRPY(R,P,Y)
"""
error=False
if len(args)==0: # By default, if no argument is given
self.array=np.array([1.,0.,0.,0.])
elif len (args) == 4: # From 4 elements
if np.array(args).size==4:
self.array = np.double(np.array (args))
else:
error=True
elif len (args) == 1:
if type(args[0])==Quaternion: # From a Quaternion
self.array=args[0].array.copy()
elif np.array(args[0]).size==1: # From one sized element, this element will be the scalar part, the vector part will be set at (0,0,0)
self.array=np.double(np.hstack([np.array(args[0]),np.array([0,0,0])]))
elif np.array(args[0]).size==4 and max(np.array(args[0]).shape)==4: # From an array, matrix, tuple or list of 4 elements
self.array = np.double(np.array(args[0])).reshape(4,)
elif np.array(args[0]).size==3 and max(np.array(args[0]).shape)==3: # From an array, matrix, tuple or list of 3 elements interpreted as a rotation vector
rV=np.double(np.array(args[0])).reshape(3,)
alpha=np.double(linalg.norm(rV))
if alpha !=0:
e=rV/alpha
else:
e=rV
self.array=np.hstack([np.cos(alpha/2.),np.sin(alpha/2.)*e])
elif len(np.array(args[0]).shape)==2 and np.array(args[0]).shape[0]>=3 and np.array(args[0]).shape[1]>=3: # From a to 2 dimension array convertible array, matrix, tuple or list with at least (3*3) elements interpreted as a rotation matrix
rM=np.double(np.array(args[0])[:3,:3])
selec=np.zeros(4)
selec[0]=1+rM[0,0]+rM[1,1]+rM[2,2]
selec[1]=1+rM[0,0]-rM[1,1]-rM[2,2]
selec[2]=1-rM[0,0]+rM[1,1]-rM[2,2]
selec[3]=1-rM[0,0]-rM[1,1]+rM[2,2]
param=selec.argmax()
if selec[param]>0:
q=np.zeros(4)
if param==0:
q[0]=np.sqrt(selec[param])
q[1]=(rM[2,1]-rM[1,2])/q[0]
q[2]=(rM[0,2]-rM[2,0])/q[0]
q[3]=(rM[1,0]-rM[0,1])/q[0]
self.array=q*0.5
# print '--1--V3'
elif param==1:
q[1]=np.sqrt(selec[param])
q[0]=(rM[2,1]-rM[1,2])/q[1]
q[2]=(rM[1,0]+rM[0,1])/q[1]
q[3]=(rM[0,2]+rM[2,0])/q[1]
self.array=q*0.5
# print '--2--V3'
elif param==2:
q[2]=np.sqrt(selec[param])
q[0]=(rM[0,2]-rM[2,0])/q[2]
q[1]=(rM[1,0]+rM[0,1])/q[2]
q[3]=(rM[2,1]+rM[1,2])/q[2]
self.array=q*0.5
# print '--3--V3'
elif param==3:
q[3]=np.sqrt(selec[param])
q[0]=(rM[1,0]-rM[0,1])/q[3]
q[1]=(rM[0,2]+rM[2,0])/q[3]
q[2]=(rM[2,1]+rM[1,2])/q[3]