def makeSpline(self,waypointTrajectory): """Computes natural velocities for a standard configuration- space Trajectory to make it smoother.""" velocities = [] t = waypointTrajectory d = len(t.milestones[0]) if len(t.milestones)==1: velocities.append([0]*d) elif len(t.milestones)==2: v = vectorops.mul(vectorops.sub(t.milestones[1],t.milestones[0]),1.0/(t.times[1]-t.times[0])) velocities.append(v) velocities.append(v) else : for i in range(1,len(waypointTrajectory.milestones)-1): v = vectorops.mul(vectorops.sub(t.milestones[i+1],t.milestones[i]),1.0/(t.times[i+1]-t.times[i-1])) velocities.append(v) #start velocity as quadratic x2 = vectorops.madd(t.milestones[1],velocities[0],-1.0/3.0) x1 = vectorops.madd(x2,vectorops.sub(t.milestones[1],t.milestones[0]),-1.0/3.0) v0 = vectorops.mul(vectorops.sub(x1,t.milestones[0]),3.0) #terminal velocity as quadratic xn_2 = vectorops.madd(t.milestones[-2],velocities[-1],1.0/3.0) xn_1 = vectorops.madd(xn_2,vectorops.sub(t.milestones[-1],t.milestones[-2]),1.0/3.0) vn = vectorops.mul(vectorops.sub(t.milestones[-1],xn_1),3.0) velocities = [v0]+velocities+[vn] self.__init__(waypointTrajectory.times[:],waypointTrajectory.milestones,velocities)
def eval_path(self, param):
index = self.findSegment(param)
if index < 0: return self.path[0]
if index + 1 >= len(self.path_times): return self.path[-1]
u = (param - self.path_times[index]) / (self.path_times[index + 1] -
self.path_times[index])
return vectorops.madd(
self.path[index],
vectorops.sub(self.path[index + 1], self.path[index]), u)
def matrix(self):
o = orientation_matrix(*self.ori)
Ry = so3.rotation([0.,1.,0.],self.rot[0])
Rx = so3.rotation([1.,0.,0.],self.rot[1])
Rz = so3.rotation([0.,0.,1.],self.rot[2])
R = so3.mul(Ry,so3.mul(Rx,Rz))
R = so3.mul(o,R);
t = so3.apply(R,self.tgt)
return (R,vectorops.madd(t,[0.,0.,1.],-self.dist))
def eval_path(self, param):
index = int(math.floor(param))
u = param - index
if index < 0:
return self.path[0]
if index + 1 >= len(self.path):
return self.path[-1]
return vectorops.madd(
self.path[index],
vectorops.sub(self.path[index + 1], self.path[index]), u)
def matrix(self):
"""Returns the camera transform."""
o = orientation_matrix(*self.ori)
Ry = so3.rotation([0., 1., 0.], self.rot[0])
Rx = so3.rotation([1., 0., 0.], self.rot[1])
Rz = so3.rotation([0., 0., 1.], self.rot[2])
R = so3.mul(Ry, so3.mul(Rx, Rz))
R = so3.mul(o, R)
t = so3.apply(R, self.tgt)
return (R, vectorops.madd(t, [0., 0., 1.], -self.dist))
def ray_ray_intersection(s1, d1, s2, d2, eps=1e-6):
#solve the coordinates x = s1+t*d1 = s2-u*d2 for some t, u
#[d1,d2]*[t,u]^T = [s2-s1]
det = d1[0] * d2[1] - d1[1] * d2[0]
if abs(det) < eps: return None
#inv mat is 1/det [d2[1] -d2[0]]
# [-d1[1] d1[0]]
b = vectorops.sub(s2, s1)
t = (d2[1] * b[0] - d2[0] * b[1]) / det
u = (-d1[1] * b[0] + d1[0] * b[1]) / det
return vectorops.madd(s1, d1, t)
def drawRawLine():
glDisable(GL_LIGHTING)
glEnable(GL_POINT_SMOOTH)
glPointSize(self.attributes.get("size",5.0))
glColor4f(*self.attributes.get("color",[0,0,0,1]))
glBegin(GL_POINTS)
glVertex3f(*p)
glEnd()
glColor4f(*self.attributes.get("color",[0.5,0,0.5,1]))
glLineWidth(self.attributes.get("width",3.0))
glBegin(GL_LINES)
glVertex3f(*p)
glVertex3f(*vectorops.madd(p,d,self.attributes.get("length",0.1)))
glEnd()
glLineWidth(1.0)
def fromJson(jsonobj,type='auto'):
"""Converts from a JSON object (e.g., from json.loads()) to a Klamp't
object of the appropriate type. If 'type' is not provided, this attempts
to infer the object type automatically."""
if type == 'auto':
if isinstance(jsonobj,(list,tuple)):
return jsonobj
elif isinstance(jsonobj,(bool,int,float,str,unicode)):
return jsonobj
elif isinstance(jsonobj,dict):
if 'type' in jsonobj:
type = jsonobj["type"]
elif 'times' in jsonobj and 'milestones' in jsonobj:
type = 'Trajectory'
elif 'x' in jsonobj and 'n' in jsonobj and 'kFriction' in jsonobj:
type = 'ContactPoint'
else:
raise RuntimeError("Unknown JSON object of type "+jsonobj.__class__.__name)
if type in ['Config','Configs','Vector','Matrix','Matrix3','RotationMatrix','Value','IntArray','StringArray']:
return jsonobj
elif type == 'RigidTransform':
return jsonobj
elif type == 'ContactPoint':
return ContactPoint(jsonobj['x'],jsonobj['n'],jsonobj['kFriction'])
elif type == 'Trajectory':
return Trajectory(jsonobj['times'],jsonobj['milestones'])
elif type == 'IKObjective':
link = jsonobj['link']
destlink = jsonobj['destLink'] if 'destLink' in jsonobj else -1
posConstraint = 'free'
rotConstraint = 'free'
localPosition = endPosition = None
direction = None
endRotation = None
localAxis = None
if 'posConstraint' in jsonobj:
posConstraint = jsonobj['posConstraint']
if 'rotConstraint' in jsonobj:
rotConstraint = jsonobj['rotConstraint']
if posConstraint == 'planar' or posConstraint == 'linear':
direction = jsonobj['direction']
if posConstraint != 'free':
localPosition = jsonobj['localPosition']
endPosition = jsonobj['endPosition']
if rotConstraint != 'free':
endRotation = jsonobj['endRotation']
if rotConstraint == 'axis' or rotConstraint == 'twoaxis':
localAxis = jsonobj['localAxis']
if posConstraint == 'free' and rotConstraint == 'free':
#empty
return IKObjective()
elif posConstraint != 'fixed':
raise NotImplementedError("Can't do non-fixed position constraints yet in Python API")
if rotConstraint == 'twoaxis':
raise NotImplementedError("twoaxis constraints are not implemented in Klampt")
if rotConstraint == 'free':
obj = IKObjective()
if destlink >= 0:
obj.setRelativePoint(link,destlink,localPosition,endPosition)
else:
obj.setFixedPoint(link,localPosition,endPosition)
return obj
elif rotConstraint == 'axis':
obj = IKObjective()
h = 0.1
lpts = [localPosition,vectorops.madd(localPosition,localAxis,h)]
wpts = [endPosition,vectorops.madd(endPosition,endRotation,h)]
if destlink >= 0:
obj.setRelativePoints(link,destlink,lpts,wpts)
else:
obj.setFixedPoints(link,lpts,wpts)
return obj
elif rotConstraint == 'fixed':
obj = IKObjective()
R = so3.from_moment(endRotation)
t = vectorops.sub(endPosition,so3.apply(R,localPosition))
obj.setFixedTransform(link,R,t)
return obj
else:
raise RuntimeError("Invalid IK rotation constraint "+rotConstraint)
elif type in readers:
return read(type,jsonobj["data"])
else:
raise RuntimeError("Unknown or unsupported type "+type)
def hermite_to_bezier(x1,v1,x2,v2):
"""Returns the cubic bezier representation of a hermite curve"""
c1 = vectorops.madd(x1,v1,1.0/3.0)
c2 = vectorops.madd(x2,v2,-1.0/3.0)
return x1,c1,c2,x2
def hermite_to_bezier(x1, v1, x2, v2):
"""Returns the cubic bezier representation of a hermite curve"""
c1 = vectorops.madd(x1, v1, 1.0 / 3.0)
c2 = vectorops.madd(x2, v2, -1.0 / 3.0)
return x1, c1, c2, x2
