def triangle(a,b,c,lighting=True): if lighting: n = vectorops.cross(vectorops.sub(b,a),vectorops.sub(c,a)) n = vectorops.mul(n,1.0/vectorops.norm(n)) glNormal3f(*n) glBegin(GL_TRIANGLES) glVertex3f(*a) glVertex3f(*b) glVertex3f(*c) glEnd();
def make_outline(self): left = [] right = [] for i in xrange(len(self.path)): s1l = None s1r = None s2l = None s2r = None if i > 0: d = vectorops.unit( vectorops.sub(self.path[i], self.path[i - 1])) ofs = (self.gutter * d[1], -self.gutter * d[0]) s1l = (d, ofs) s1r = (d, (-ofs[0], -ofs[1])) if i + 1 < len(self.path): d = vectorops.unit( vectorops.sub(self.path[i + 1], self.path[i])) ofs = (self.gutter * d[1], -self.gutter * d[0]) s2l = (d, ofs) s2r = (d, (-ofs[0], -ofs[1])) if i == 0: left.append(tuple(vectorops.add(self.path[i], s2l[1]))) right.append(tuple(vectorops.add(self.path[i], s2r[1]))) elif i + 1 == len(self.path): left.append(tuple(vectorops.add(self.path[i], s1l[1]))) right.append(tuple(vectorops.add(self.path[i], s1r[1]))) else: #figure out intersections if vectorops.dot(s1r[0], s2r[1]) < 0: #inner junction, find line intersection right.append( tuple( vectorops.add( self.path[i], ray_ray_intersection(s1r[1], s1r[0], s2r[1], s2r[0])))) else: #outer junction right.append(tuple(vectorops.add(self.path[i], s1r[1]))) right.append(tuple(vectorops.add(self.path[i], s2r[1]))) if vectorops.dot(s1l[0], s2l[1]) < 0: #inner junction, find line intersection left.append( tuple( vectorops.add( self.path[i], ray_ray_intersection(s1l[1], s1l[0], s2l[1], s2l[0])))) else: #outer junction left.append(tuple(vectorops.add(self.path[i], s1l[1]))) left.append(tuple(vectorops.add(self.path[i], s2l[1]))) self.outline = left + list(reversed(right))
def triangle(a, b, c, lighting=True): """Draws a 3D triangle with points a,b,c. If lighting is true, computes a GL normal vector dynamically""" if lighting: n = vectorops.cross(vectorops.sub(b, a), vectorops.sub(c, a)) n = vectorops.mul(n, 1.0 / vectorops.norm(n)) glNormal3f(*n) glBegin(GL_TRIANGLES) glVertex3f(*a) glVertex3f(*b) glVertex3f(*c) glEnd()
def triangle(a,b,c,lighting=True): """Draws a 3D triangle with points a,b,c. If lighting is true, computes a GL normal vector dynamically""" if lighting: n = vectorops.cross(vectorops.sub(b,a),vectorops.sub(c,a)) n = vectorops.mul(n,1.0/vectorops.norm(n)) glNormal3f(*n) glBegin(GL_TRIANGLES) glVertex3f(*a) glVertex3f(*b) glVertex3f(*c) glEnd();
def segment_closest_point(s, x): """Given a segment s=(a,b) and a point x, returns a tuple containing the closest point parameter in [0,1] and the closest point on s""" dir = vectorops.sub(s[1], s[0]) d = vectorops.sub(x, s[0]) dp = vectorops.dot(d, dir) dnorm2 = vectorops.dot(dir, dir) if dp < 0: return (0.0, s[0]) if dp > dnorm2: return (1.0, s[1]) proj = vectorops.add(s[0], vectorops.mul(dir, dp / dnorm2)) return (dp / dnorm2, proj)
def triangle(a, b, c, lighting=True, filled=True): """Draws a 3D triangle with points a,b,c. If lighting is true, computes a GL normal vector dynamically. If filled=true, it is drawn filled, otherwise, it is drawn as a wireframe.""" if lighting: n = vectorops.cross(vectorops.sub(b, a), vectorops.sub(c, a)) n = vectorops.mul(n, 1.0 / vectorops.norm(n)) glNormal3f(*n) if filled: glBegin(GL_TRIANGLES) else: glBegin(GL_LINE_LOOP) glVertex3f(*a) glVertex3f(*b) glVertex3f(*c) glEnd()
def triangle(a,b,c,lighting=True,filled=True): """Draws a 3D triangle with points a,b,c. If lighting is true, computes a GL normal vector dynamically. If filled=true, it is drawn filled, otherwise, it is drawn as a wireframe.""" if lighting: n = vectorops.cross(vectorops.sub(b,a),vectorops.sub(c,a)) n = vectorops.mul(n,1.0/vectorops.norm(n)) glNormal3f(*n) if filled: glBegin(GL_TRIANGLES) else: glBegin(GL_LINE_LOOP) glVertex3f(*a) glVertex3f(*b) glVertex3f(*c) glEnd();
def error(T1,T2): """Returns a 6D "difference vector" that describes how far T1 is from T2. More precisely, this is the Lie derivative (w,v).""" (R1,t1)=T1 (R2,t2)=T2 #concatenate lists return so3.error(R1,R2) + vectorops.sub(t1,t2)
def error(T1, T2): """Returns a 6D "difference vector" that describes how far T1 is from T2. More precisely, this is the Lie derivative (w,v).""" (R1, t1) = T1 (R2, t2) = T2 #concatenate lists return so3.error(R1, R2) + vectorops.sub(t1, t2)
def is_rotation(R,tol=1e-5): """Returns true if R is a rotation matrix, i.e. is orthogonal to the given tolerance and has + determinant""" RRt = mul(R,inv(R)) err = vectorops.sub(RRt,identity()) if any(abs(v) > tol for v in err): return False if det(R) < 0: return False return True
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 is_rotation(R, tol=1e-5): """Returns true if R is a rotation matrix, i.e. is orthogonal to the given tolerance and has + determinant""" RRt = mul(R, inv(R)) err = vectorops.sub(RRt, identity()) if any(abs(v) > tol for v in err): return False if det(R) < 0: return False return True
def readHold(text): """Loads a Hold from a string""" lines = parseLines(text) if lines[0] != 'begin hold': raise ValueError('Invalid hold begin text') if lines[-1] != 'end': raise ValueError('Invalid hold end text') h = hold.Hold() posLocal = None posWorld = None localPos0 = None rotAxis = None rotWorld = None iktype = 0 for l in lines[1:-1]: items = l.split() if items[1] != '=': raise ValueError("Invalid line format") if items[0] == 'link': h.link = int(items[2]) elif items[0] == 'contacts': ind = 2 while ind < len(items): h.contacts.append(readContactPoint(' '.join(items[ind:ind+7]))) ind += 7 elif items[0] == "position": posLocal = [float(v) for v in items[2:5]] posWorld = [float(v) for v in items[5:8]] elif items[0] == "axis": rotAxis = [float(v) for v in items[2:5]] rotWorld = [float(v) for v in items[5:8]] elif items[0] == "rotation": rotWorld = [float(v) for v in items[2:5]] elif items[0] == "ikparams": localPos0 = [float(v) for v in items[2:5]] rotWorld = [float(v) for v in items[5:8]] iktype = 1 else: raise ValueError("Invalid item "+items[0]) if iktype == 0: h.ikConstraint = IKObjective() if posLocal==None: raise ValueError("Hold must have some point constraint") if rotWorld==None: h.ikConstraint.setFixedPoint(h.link,posLocal,posWorld) elif rotAxis==None: R = momentToMatrix(rotWorld) t = vectorops.sub(posWorld,so3.apply(R,posLocal)) h.ikConstraint.setFixedTransform(h.link,R,t) else: raise NotImplementedError("Can't do axis rotation yet") h.ikConstraint.setAxisRotation(h.link,posLocal,posWorld,localAxis,worldAxis) else: raise NotImplementedError("other ik specifications not done yet") h.setupIKConstraint(localPos0,rotWorld) return h
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 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 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 click_world(self, x, y): """Helper: returns a list of world objects sorted in order of increasing distance.""" #get the viewport ray (s, d) = self.click_ray(x, y) #run the collision tests collided = [] for g in self.collider.geomList: (hit, pt) = g[1].rayCast(s, d) if hit: dist = vectorops.dot(vectorops.sub(pt, s), d) collided.append((dist, g[0])) if len(collided) == 0: return id = collided[0][1].getID() if id in self.value: self.value.remove(id) else: self.value.append(id) self.lastClicked = id self.refresh()
def click_robot(self,x,y): """Helper: returns a list of robot objects sorted in order of increasing distance.""" #get the viewport ray (s,d) = self.click_ray(x,y) #run the collision tests collided = [] for l in range(self.robot.numLinks()): (hit,pt) = self.robot.link(l).geometry().rayCast(s,d) if hit: dist = vectorops.dot(vectorops.sub(pt,s),d) collided.append((dist,l)) if len(collided) == 0: return id = collided[0][1] if id in self.value: self.value.remove(id) else: self.value.append(id) self.lastClicked = id self.refresh()
def click_world(self,x,y): """Helper: returns a list of world objects sorted in order of increasing distance.""" #get the viewport ray (s,d) = self.click_ray(x,y) #run the collision tests collided = [] for g in self.collider.geomList: (hit,pt) = g[1].rayCast(s,d) if hit: dist = vectorops.dot(vectorops.sub(pt,s),d) collided.append((dist,g[0])) if len(collided) == 0: return id = collided[0][1].getID() if id in self.value: self.value.remove(id) else: self.value.append(id) self.lastClicked = id self.refresh()
def click_robot(self, x, y): """Helper: returns a list of robot objects sorted in order of increasing distance.""" #get the viewport ray (s, d) = self.click_ray(x, y) #run the collision tests collided = [] for l in range(self.robot.numLinks()): (hit, pt) = self.robot.link(l).geometry().rayCast(s, d) if hit: dist = vectorops.dot(vectorops.sub(pt, s), d) collided.append((dist, l)) if len(collided) == 0: return id = collided[0][1] if id in self.value: self.value.remove(id) else: self.value.append(id) self.lastClicked = id self.refresh()
def draw(self,world=None): """Draws the specified item in the specified world. If name is given and text_hidden != False, then the name of the item is shown.""" if self.hidden: return item = self.item name = self.name #set appearance if not self.useDefaultAppearance and hasattr(item,'appearance'): if not hasattr(self,'oldAppearance'): self.oldAppearance = item.appearance().clone() if self.customAppearance != None: print "Changing appearance of",name item.appearance().set(self.customAppearance) elif "color" in self.attributes: print "Changing color of",name item.appearance().setColor(*self.attributes["color"]) if hasattr(item,'drawGL'): item.drawGL() elif len(self.subAppearances)!=0: for n,app in self.subAppearances.iteritems(): app.widget = self.widget app.draw(world) elif isinstance(item,coordinates.Point): def drawRaw(): glDisable(GL_DEPTH_TEST) 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(0,0,0) glEnd() glEnable(GL_DEPTH_TEST) #write name self.displayCache[0].draw(drawRaw,[so3.identity(),item.worldCoordinates()]) if name != None: self.drawText(name,vectorops.add(item.worldCoordinates(),[0,0,-0.05])) elif isinstance(item,coordinates.Direction): def drawRaw(): glDisable(GL_LIGHTING) glDisable(GL_DEPTH_TEST) L = self.attributes.get("length",0.15) source = [0,0,0] glColor4f(*self.attributes.get("color",[0,1,1,1])) glBegin(GL_LINES) glVertex3f(*source) glVertex3f(*vectorops.mul(item.localCoordinates(),L)) glEnd() glEnable(GL_DEPTH_TEST) #write name self.displayCache[0].draw(drawRaw,item.frame().worldCoordinates(),parameters = item.localCoordinates()) if name != None: self.drawText(name,vectorops.add(vectorops.add(item.frame().worldCoordinates()[1],item.worldCoordinates()),[0,0,-0.05])) elif isinstance(item,coordinates.Frame): t = item.worldCoordinates() if item.parent() != None: tp = item.parent().worldCoordinates() else: tp = se3.identity() tlocal = item.relativeCoordinates() def drawRaw(): glDisable(GL_DEPTH_TEST) glDisable(GL_LIGHTING) glLineWidth(2.0) gldraw.xform_widget(tlocal,self.attributes.get("length",0.1),self.attributes.get("width",0.01)) glLineWidth(1.0) #draw curve between frame and parent if item.parent() != None: d = vectorops.norm(tlocal[1]) vlen = d*0.5 v1 = so3.apply(tlocal[0],[-vlen]*3) v2 = [vlen]*3 #glEnable(GL_BLEND) #glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA) #glColor4f(1,1,0,0.5) glColor3f(1,1,0) gldraw.hermite_curve(tlocal[1],v1,[0,0,0],v2,0.03) #glDisable(GL_BLEND) glEnable(GL_DEPTH_TEST) #For some reason, cached drawing is causing OpenGL problems #when the frame is rapidly changing #self.displayCache[0].draw(drawRaw,transform=tp, parameters = tlocal) glPushMatrix() glMultMatrixf(sum(zip(*se3.homogeneous(tp)),())) drawRaw() glPopMatrix() #write name if name != None: self.drawText(name,se3.apply(t,[-0.05]*3)) elif isinstance(item,coordinates.Transform): #draw curve between frames t1 = item.source().worldCoordinates() if item.destination() != None: t2 = item.destination().worldCoordinates() else: t2 = se3.identity() d = vectorops.distance(t1[1],t2[1]) vlen = d*0.5 v1 = so3.apply(t1[0],[-vlen]*3) v2 = so3.apply(t2[0],[vlen]*3) def drawRaw(): glDisable(GL_DEPTH_TEST) glDisable(GL_LIGHTING) glColor3f(1,1,1) gldraw.hermite_curve(t1[1],v1,t2[1],v2,0.03) glEnable(GL_DEPTH_TEST) #write name at curve self.displayCache[0].draw(drawRaw,transform=None,parameters = (t1,t2)) if name != None: self.drawText(name,spline.hermite_eval(t1[1],v1,t2[1],v2,0.5)) else: types = resource.objectToTypes(item,world) if isinstance(types,(list,tuple)): #ambiguous, still need to figure out what to draw validtypes = [] for t in types: if t == 'Config': if world != None and len(t) == world.robot(0).numLinks(): validtypes.append(t) elif t=='Vector3': validtypes.append(t) elif t=='RigidTransform': validtypes.append(t) if len(validtypes) > 1: print "Unable to draw item of ambiguous types",validtypes return if len(validtypes) == 0: print "Unable to draw any of types",types return types = validtypes[0] if types == 'Config': if world: robot = world.robot(0) if not self.useDefaultAppearance: oldAppearance = [robot.link(i).appearance().clone() for i in xrange(robot.numLinks())] for i in xrange(robot.numLinks()): robot.link(i).appearance().set(self.customAppearance) oldconfig = robot.getConfig() robot.setConfig(item) robot.drawGL() robot.setConfig(oldconfig) if not self.useDefaultAppearance: for (i,app) in enumerate(oldAppearance): robot.link(i).appearance().set(app) else: print "Unable to draw Config's without a world" elif types == 'Vector3': def drawRaw(): 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(0,0,0) glEnd() self.displayCache[0].draw(drawRaw,[so3.identity(),item]) if name != None: self.drawText(name,vectorops.add(item,[0,0,-0.05])) elif types == 'RigidTransform': def drawRaw(): gldraw.xform_widget(se3.identity(),self.attributes.get("length",0.1),self.attributes.get("width",0.01)) self.displayCache[0].draw(drawRaw,transform=item) if name != None: self.drawText(name,se3.apply(item,[-0.05]*3)) elif types == 'IKGoal': if hasattr(item,'robot'): #need this to be built with a robot element. #Otherwise, can't determine the correct transforms robot = item.robot elif world: if world.numRobots() >= 1: robot = world.robot(0) else: robot = None else: robot = None if robot != None: link = robot.link(item.link()) dest = robot.link(item.destLink()) if item.destLink()>=0 else None while len(self.displayCache) < 3: self.displayCache.append(CachedGLObject()) self.displayCache[1].name = self.name+" target position" self.displayCache[2].name = self.name+" curve" if item.numPosDims() != 0: lp,wp = item.getPosition() #set up parameters of connector p1 = se3.apply(link.getTransform(),lp) if dest != None: p2 = se3.apply(dest.getTransform(),wp) else: p2 = wp d = vectorops.distance(p1,p2) v1 = [0.0]*3 v2 = [0.0]*3 if item.numRotDims()==3: #full constraint R = item.getRotation() def drawRaw(): gldraw.xform_widget(se3.identity(),self.attributes.get("length",0.1),self.attributes.get("width",0.01)) t1 = se3.mul(link.getTransform(),(so3.identity(),lp)) t2 = (R,wp) if dest==None else se3.mul(dest.getTransform(),(R,wp)) self.displayCache[0].draw(drawRaw,transform=t1) self.displayCache[1].draw(drawRaw,transform=t2) vlen = d*0.1 v1 = so3.apply(t1[0],[-vlen]*3) v2 = so3.apply(t2[0],[vlen]*3) elif item.numRotDims()==0: #point constraint def drawRaw(): 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(0,0,0) glEnd() self.displayCache[0].draw(drawRaw,transform=(so3.identity(),p1)) self.displayCache[1].draw(drawRaw,transform=(so3.identity(),p2)) #set up the connecting curve vlen = d*0.5 d = vectorops.sub(p2,p1) v1 = vectorops.mul(d,0.5) #curve in the destination v2 = vectorops.cross((0,0,0.5),d) else: #hinge constraint p = [0,0,0] d = [0,0,0] 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) ld,wd = item.getRotationAxis() p = lp d = ld self.displayCache[0].draw(drawRawLine,transform=link.getTransform(),parameters=(p,d)) p = wp d = wd self.displayCache[1].draw(drawRawLine,transform=dest.getTransform() if dest else se3.identity(),parameters=(p,d)) #set up the connecting curve d = vectorops.sub(p2,p1) v1 = vectorops.mul(d,0.5) #curve in the destination v2 = vectorops.cross((0,0,0.5),d) def drawConnection(): glDisable(GL_DEPTH_TEST) glDisable(GL_LIGHTING) glColor3f(1,0.5,0) gldraw.hermite_curve(p1,v1,p2,v2,0.03) glEnable(GL_DEPTH_TEST) self.displayCache[2].draw(drawConnection,transform=None,parameters = (p1,v1,p2,v2)) if name != None: self.drawText(name,vectorops.add(wp,[-0.05]*3)) else: wp = link.getTransform()[1] if item.numRotDims()==3: #full constraint R = item.getRotation() def drawRaw(): gldraw.xform_widget(se3.identity(),self.attributes.get("length",0.1),self.attributes.get("width",0.01)) self.displayCache[0].draw(drawRaw,transform=link.getTransform()) self.displayCache[1].draw(drawRaw,transform=se3.mul(link.getTransform(),(R,[0,0,0]))) elif item.numRotDims() > 0: #axis constraint d = [0,0,0] def drawRawLine(): glDisable(GL_LIGHTING) glColor4f(*self.attributes.get("color",[0.5,0,0.5,1])) glLineWidth(self.attributes.get("width",3.0)) glBegin(GL_LINES) glVertex3f(0,0,0) glVertex3f(*vectorops.mul(d,self.attributes.get("length",0.1))) glEnd() glLineWidth(1.0) ld,wd = item.getRotationAxis() d = ld self.displayCache[0].draw(drawRawLine,transform=link.getTransform(),parameters=d) d = wd self.displayCache[1].draw(drawRawLine,transform=(dest.getTransform()[0] if dest else so3.identity(),wp),parameters=d) else: #no drawing pass if name != None: self.drawText(name,se3.apply(wp,[-0.05]*3)) else: print "Unable to draw item of type",types #revert appearance if not self.useDefaultAppearance and hasattr(item,'appearance'): item.appearance().set(self.oldAppearance)
def path_tangent(self, eps=0.1): p1 = self.eval_path(self.slider_param + eps) p2 = self.eval_path(self.slider_param - eps) return vectorops.mul(vectorops.sub(p1, p2), 1.0 / vectorops.distance(p1, p2))
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 readIKObjective(text): """Reads an IKObjective from text""" items = text.split() if len(items) < 4: raise ValueError("Not enough items to unpack") link = int(items[0]) destlink = int(items[1]) ind = 2 posType = None posLocal = None posWorld = None posDirection = None rotType = None rotWorld = None rotAxis = None #read pos constraint posType = items[ind] ind += 1 if posType=='N': #no constraint pass elif posType=='P' or posType=='L': posLocal = items[ind:ind+3] ind += 3 posWorld = items[ind:ind+3] ind += 3 posDirection = items[ind:ind+3] ind += 3 elif posType=='F': posLocal = items[ind:ind+3] ind += 3 posWorld = items[ind:ind+3] ind += 3 else: raise ValueError("Invalid pos type "+posType+", must be N,P,L or F") rotType = items[ind] ind += 1 if rotType=='N': #no constraint pass elif rotType=='T' or rotType=='A': rotAxis = items[ind:ind+3] ind += 3 rotWorld = items[ind:ind+3] ind += 3 elif rotType=='F': rotWorld = items[ind:ind+3] ind += 3 else: raise ValueError("Invalid rot type "+rotType+", must be N,T,A or F") if posType != 'F': raise NotImplementedError("Python API can only do point and fixed IK goals") obj = IKObjective() if rotType == 'N': #point constraint obj.setRelativePoint(link,destlink,[float(v) for v in posLocal],[float(v) for v in posWorld]) elif rotType == 'F': #fixed constraint R = so3.from_moment([float(v) for v in rotWorld]) t = vectorops.sub([float(v) for v in posWorld],so3.apply(R,[float(v) for v in posLocal])) obj.setRelativeTransform(link,destlink,R,t) elif rotType == 'A': #TODO: rotational axis constraints actually can be set in the python API raise NotImplementedError("Axis rotations not yet implemented") else: raise NotImplementedError("Two-axis rotational constraints not supported") return obj
def difference(self,a,b): """Can override this to implement non-Cartesian spaces""" return vectorops.sub(a,b)
def difference(self,a,b,u): """Subclasses can override this to implement non-Cartesian spaces. Returns the derivative along the geodesic from b to a.""" return vectorops.sub(a,b)
def bezier_to_hermite(x1, x2, x3, x4): """Returns the cubic bezier representation of a hermite curve""" v1 = vectorops.mul(vectorops.sub(x2, x1), 3.0) v2 = vectorops.mul(vectorops.sub(x4, x3), 3.0) return x1, v1, x4, v2
def difference(self,a,b): """For Lie groups, returns a difference vector that, when integrated would get to a from b. In Cartesian spaces it is a-b.""" return vectorops.sub(a,b)
def path_velocity(self, eps=0.1): p1 = self.eval_path(self.current_time) p2 = self.eval_path(self.current_time + eps) return vectorops.mul(vectorops.sub(p2, p1), 1.0 / eps)
def difference(self, a, b): """For Lie groups, returns a difference vector that, when integrated would get to a from b. In Cartesian spaces it is a-b.""" return vectorops.sub(a, b)
def bezier_to_hermite(x1,x2,x3,x4): """Returns the cubic bezier representation of a hermite curve""" v1 = vectorops.mul(vectorops.sub(x2,x1),3.0) v2 = vectorops.mul(vectorops.sub(x4,x3),3.0) return x1,v1,x4,v2