constants_and_tools.py 4.83 KB
 stevet committed Oct 14, 2019 1 ``````import numpy as np `````` stevet committed Oct 28, 2019 2 3 ``````from sl1m.tools.obj_to_constraints import load_obj, as_inequalities, rotate_inequalities, inequalities_to_Inequalities_object `````` stevet committed Oct 14, 2019 4 5 6 7 8 9 ``````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 `````` Pierre Fernbach committed Jan 21, 2020 10 ``````from .qp import solve_lp `````` stevet committed Oct 14, 2019 11 `````` `````` stevet committed Oct 28, 2019 12 ``````#~ import eigenpy `````` stevet committed Oct 14, 2019 13 14 15 16 17 18 ``````#from curves import bezier3 from random import random as rd from random import randint as rdi from numpy import squeeze, asarray `````` stevet committed Oct 16, 2019 19 ``````Id = array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]]) `````` stevet committed Oct 14, 2019 20 21 22 23 24 25 26 27 28 29 ``````g = array([0.,0.,-9.81]) g6 = array([0.,0.,-9.81,0.,0.,0.]) mu = 0.45 x = array([1.,0.,0.]) z = array([0.,0.,1.]) zero3 = zeros(3) `````` stevet committed Oct 28, 2019 30 31 ``````eps =0.000001 `````` stevet committed Oct 14, 2019 32 ``````#### surface to inequalities #### `````` Steve T committed Feb 27, 2020 33 ``````def convert_surface_to_inequality(s, eqAsIneq): `````` stevet committed Oct 14, 2019 34 35 36 37 38 39 40 `````` #TODO does normal orientation matter ? #it will for collisions n = cross(s[:,1] - s[:,0], s[:,2] - s[:,0]) if n[2] <= 0.: for i in range(3): n[i] = -n[i] n /= norm(n) `````` Steve T committed Feb 27, 2020 41 `````` return surfacePointsToIneq(s, n, eqAsIneq) `````` stevet committed Oct 14, 2019 42 `````` `````` Steve T committed Feb 27, 2020 43 44 ``````def replace_surfaces_with_ineq_in_phaseData(phase, eqAsIneq): phase["S"] = [convert_surface_to_inequality(S, eqAsIneq) for S in phase["S"]] `````` stevet committed Oct 14, 2019 45 `````` `````` Steve T committed Feb 27, 2020 46 47 ``````def replace_surfaces_with_ineq_in_problem(pb, eqAsIneq = False): [ replace_surfaces_with_ineq_in_phaseData(phase, eqAsIneq) for phase in pb ["phaseData"]] `````` stevet committed Oct 14, 2019 48 49 50 51 52 53 54 55 56 57 58 59 `````` def ineqQHull(hull): A = hull.equations[:,:-1] b = -hull.equations[:,-1] return A,b def vectorProjection (v, n): v = v / norm(v) n = n / norm(n) proj = v - np.dot(v,n)*n return proj/norm(proj) `````` Pierre Fernbach committed Nov 13, 2019 60 61 62 63 64 65 ``````def addHeightConstraint(K,k, val): K1 = vstack([K, -z]) k1 = concatenate([k, -ones(1) * val]).reshape((-1,)) return K1, k1 `````` stevet committed Oct 14, 2019 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 ``````def default_transform_from_pos_normal_(transform, pos, normal): #return vstack( [hstack([transform,pos.reshape((-1,1))]), [ 0. , 0. , 0. , 1. ] ] ) # FIXME : temp stuff, only work on flat floor # FIXME : there is something wrong the the code above #~ print "pos ", pos #~ print "normal ", normal #align the x-axis of the foot to the root heading direction f = x xp = np.dot(transform, x).tolist() t = vectorProjection(xp, normal) v = np.cross(f, t) c = np.dot(f, t) if abs(c) > 0.99 : return vstack( [hstack([transform,pos.reshape((-1,1))]), [ 0. , 0. , 0. , 1. ] ] ) else: u = v/norm(v) h = (1. - c)/(1. - c**2) vx, vy, vz = v rot1 =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]]) #align the z-axis of the foot to the surface normal f = z t = array(normal) t = t / norm(t) v = np.cross(f, t) c = np.dot(f, t) if abs(c) > 0.99 : rot2 = identity(3) else: u = v/norm(v) h = (1. - c)/(1. - c**2) vx, vy, vz = v rot2 =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]]) rot = np.dot(rot1,rot2) return vstack( [hstack([rot,pos.reshape((-1,1))]), [ 0. , 0. , 0. , 1. ] ] ) def default_transform_from_pos_normal(pos, normal): f = array([0.,0.,1.]) t = array(normal) t = t / norm(t) v = np.cross(f, t) c = np.dot(f, t) if abs(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. ] ] ) `````` Steve T committed Feb 27, 2020 128 ``````EPSILON_EQ = 0.005 `````` stevet committed Oct 14, 2019 129 ``````#last is equality `````` Steve T committed Feb 27, 2020 130 ``````def surfacePointsToIneq(S, normal, eqAsIneq): `````` stevet committed Oct 14, 2019 131 132 133 134 135 136 137 138 139 140 141 142 143 `````` n = array(normal) tr = default_transform_from_pos_normal(array([0.,0.,0.]),n) trinv = tr.copy(); trinv[:3,:3] = tr[:3,:3].T; trpts = [tr[:3,:3].dot(s)[:2] for s in S.T] hull = ConvexHull(array(trpts)) A,b = ineqQHull(hull) A = hstack([A,zeros((A.shape[0],1))]) ine = inequalities_to_Inequalities_object(A,b) ine = rotate_inequalities(ine, trinv) #adding plane constraint #plane equation given by first point and normal for instance d = array([n.dot(S[:,0])]) `````` Steve T committed Feb 27, 2020 144 145 146 147 148 149 150 `````` if eqAsIneq: A = vstack([ine.A, n , -n ]) b = concatenate([ine.b, d + EPSILON_EQ, -d + EPSILON_EQ]).reshape((-1,)) else: A = vstack([ine.A, n]) b = concatenate([ine.b, d]).reshape((-1,)) `````` stevet committed Oct 14, 2019 151 152 `````` return A, b `````` stevet committed Oct 28, 2019 153 ``````############ BENCHMARKING ############### `````` stevet committed Oct 14, 2019 154 155 156 157 `````` from time import clock def timMs(t1, t2): return (t2-t1) * 1000.``````