Commit 6aae34f4 authored by Steve T's avatar Steve T
Browse files

new api is hpp_spline

parent c2ed8387
from spline import *
from hpp_spline import *
from varBezier import varBezier
from convex_hull import *
......
from spline import *
from hpp_spline import *
from numpy import matrix, array, zeros, ones, diag, cross
from numpy.linalg import norm
......@@ -130,7 +130,7 @@ def computeTrajectory(bezVar, splits, save, filename = uuid.uuid4().hex.upper()[
#solve and gen problem
def gen(save = False):
testConstant = genBezierInput(4)
testConstant = genBezierInput(20)
splits = genSplit(4)
return computeTrajectory(testConstant,splits, save), testConstant
......
from spline import *
from hpp_spline import *
from numpy import matrix, array, zeros, ones, diag, cross
from numpy.linalg import norm
......
from spline import *
from hpp_spline import *
from varBezier import varBezier
from numpy import matrix, array, zeros, ones, diag, cross
from numpy.linalg import norm
......
from hpp_spline import *
from numpy import matrix, array, zeros, ones, diag, cross
from numpy.linalg import norm
__EPS = 1e-6
import eigenpy
eigenpy.switchToNumpyArray()
_zeroMat = array([[0.,0.,0.],[0.,0.,0.],[0.,0.,0.]]).transpose()
_I3 = array([[1.,0.,0.],[0.,1.,0.],[0.,0.,1.]]).transpose()
_zeroVec = array([[0.,0.,0.]]).transpose()
def createControlPoint(val):
if type(val) == str:
return (_I3, _zeroVec)
else:
return (_zeroMat, val.reshape([3,1]))
def createWaypointList(waypoints):
mat = zeros([3, len(waypoints)*3])
vec = zeros([3, len(waypoints)])
for i, val in enumerate(waypoints):
mvar, vvar = createControlPoint(val)
mat [:,i*3:i*3+3] = mvar
vec [:,i:i+1] = vvar
return mat, vec
class varBezier:
#waypoints is a list that contains either 3d arrays (constants), or a string "variable"
def __init__(self, waypoints=None, time=1.):
if(waypoints != None):
mat, vec = createWaypointList(waypoints)
self.bezier = bezierVar(mat, vec, time)
def fromBezier(self, bez):
var = varBezier ()
var.bezier = bez
return var
#splits into n+1 continuous curves, with n the number of time values
def split(self, times):
timearray = array(times).reshape([-1,1])
subBeziers = self.bezier.split(timearray)
dim = subBeziers.size
return [self.fromBezier(subBeziers.at(i)) for i in range(dim)]
# for each control point of the curve, gives the linear depency
# of a given variable
def waypoints(self, varId=-1):
if varId < 0:
return self.bezier.waypoints().A, self.bezier.waypoints().b
assert self.bezier.nbWaypoints > varId
mat = self.bezier.waypoints().A[:, varId*3:varId*3+3]
vec = self.bezier.waypoints().b[:, varId]
return mat, vec
def matrixFromWaypoints (self, varId):
assert(varId >= 0)
mat, vec = self.waypoints(varId)
resvec = zeros(3)
for i in range(0, mat.shape[0]/3, 1):
resvec += vec[i*3:i*3+3]
return mat.transpose(), resvec
def toBezier3(self, x):
wps = []
for i in range(self.bezier.nbWaypoints):
mat, vec = self.matrixFromWaypoints(i)
wps += [mat.dot(x) + vec]
return bezier(array(wps).transpose(),self.bezier.max())
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