[Python] format

data/talos/com_inequalities/__init__.py

@@ -1 +0,0 @@
-

script/constants_and_tools.py

@@ -1,153 +1,155 @@
 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);