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