Exemplo n.º 1
0
	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)
Exemplo n.º 2
0
	def deriv(self,t,endBehavior='halt'):
		"""Evaluates the trajectory velocity using piecewise linear
		interpolation.  If endBehavior='loop' then the trajectory
		loops forver."""
		i,u = self.getSegment(t,endBehavior)
		if i<0: return [0.0]*len(self.milestones[0])
		elif i>=len(self.milestones): return [0.0]*len(self.milestones[-1])
		return vectorops.mul(self.difference(self.milestones[i+1],self.milestones[i],u),1.0/(self.times[i+1]-self.times[i]))
Exemplo n.º 3
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)
Exemplo n.º 4
0
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();
Exemplo n.º 5
0
 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)
Exemplo n.º 6
0
def sample():
    """Returns a uniformly distributed rotation matrix."""
    import random
    q = [random.gauss(0,1),random.gauss(0,1),random.gauss(0,1),random.gauss(0,1)]
    q = vectorops.unit(q)
    theta = math.acos(q[3])*2.0
    if abs(theta) < 1e-8:
        m = [0,0,0]
    else:
        m = vectorops.mul(vectorops.unit(q[0:3]),theta)
    return from_moment(m)
Exemplo n.º 7
0
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();
Exemplo n.º 8
0
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()
Exemplo n.º 9
0
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)
Exemplo n.º 10
0
def rotation(axis, angle):
    """Given a unit axis and an angle in radians, returns the rotation
    matrix."""
    cm = math.cos(angle)
    sm = math.sin(angle)

    #m = s[r]-c[r][r]+rrt = s[r]-c(rrt-I)+rrt = cI + rrt(1-c) + s[r]
    R = vectorops.mul(cross_product(axis), sm)
    for i in xrange(3):
        for j in xrange(3):
            R[i * 3 + j] += axis[i] * axis[j] * (1. - cm)
    R[0] += cm
    R[4] += cm
    R[8] += cm
    return R
Exemplo n.º 11
0
def rotation(axis,angle):
    """Given a unit axis and an angle in radians, returns the rotation
    matrix."""
    cm = math.cos(angle)
    sm = math.sin(angle)

    #m = s[r]-c[r][r]+rrt = s[r]-c(rrt-I)+rrt = cI + rrt(1-c) + s[r]
    R = vectorops.mul(cross_product(axis),sm)
    for i in xrange(3):
        for j in xrange(3):
            R[i*3+j] += axis[i]*axis[j]*(1.-cm)
    R[0] += cm
    R[4] += cm
    R[8] += cm
    return R
Exemplo n.º 12
0
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();
Exemplo n.º 13
0
def sample():
    """Returns a uniformly distributed rotation matrix."""
    import random
    q = [
        random.gauss(0, 1),
        random.gauss(0, 1),
        random.gauss(0, 1),
        random.gauss(0, 1)
    ]
    q = vectorops.unit(q)
    theta = math.acos(q[3]) * 2.0
    if abs(theta) < 1e-8:
        m = [0, 0, 0]
    else:
        m = vectorops.mul(vectorops.unit(q[0:3]), theta)
    return from_moment(m)
Exemplo n.º 14
0
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()
Exemplo n.º 15
0
def vector_rotation(v1,v2):
    """Finds the minimal-angle matrix that rotates v1 to v2.  v1 and v2
    are assumed to be nonzero"""
    a1 = vectorops.unit(v1)
    a2 = vectorops.unit(v2)
    cp = vectorops.cross(a1,a2)
    dp = vectorops.dot(a1,a2)
    if abs(vectorops.norm(cp)) < 1e-4:
        if dp < 0:
            R0 = canonical(a1)
            #return a rotation 180 degrees about the canonical y axis
            return rotation(R0[3:6],math.pi)
        else:
            return identity()
    else:
        angle = math.acos(max(min(dp,1.0),-1.0))
        axis = vectorops.mul(cp,1.0/vectorops.norm(cp))
        return rotation(axis,angle)
Exemplo n.º 16
0
def vector_rotation(v1, v2):
    """Finds the minimal-angle matrix that rotates v1 to v2.  v1 and v2
    are assumed to be nonzero"""
    a1 = vectorops.unit(v1)
    a2 = vectorops.unit(v2)
    cp = vectorops.cross(a1, a2)
    dp = vectorops.dot(a1, a2)
    if abs(vectorops.norm(cp)) < 1e-4:
        if dp < 0:
            R0 = canonical(a1)
            #return a rotation 180 degrees about the canonical y axis
            return rotation(R0[3:6], math.pi)
        else:
            return identity()
    else:
        angle = math.acos(max(min(dp, 1.0), -1.0))
        axis = vectorops.mul(cp, 1.0 / vectorops.norm(cp))
        return rotation(axis, angle)
Exemplo n.º 17
0
 def scale(self,amount):
     """Scales this direction by a scalar amount"""
     self._localCoordinates = vectorops.mul(self._localCoordinates,amount)
Exemplo n.º 18
0
 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))
Exemplo n.º 19
0
def from_moment(w):
    """Converts a moment representation w to a 3D rotation matrix."""
    length = vectorops.norm(w)
    if length < 1e-7: return identity()
    return rotation(vectorops.mul(w,1.0/length),length)
Exemplo n.º 20
0
 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)
Exemplo n.º 21
0
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
Exemplo n.º 22
0
def from_moment(w):
    """Converts a moment representation w to a 3D rotation matrix."""
    length = vectorops.norm(w)
    if length < 1e-7: return identity()
    return rotation(vectorops.mul(w, 1.0 / length), length)
Exemplo n.º 23
0
def hermite_subdivide(x1, v1, x2, v2, u=0.5):
    """Subdivides a hermite curve into two hermite curves."""
    xm = hermite_eval(x1, v1, x2, v2, u)
    vm = hermite_deriv(x1, v1, x2, v2, u)
    return [(x1, vectorops.mul(v1, u), xm, vectorops.mul(vm, u)),
            (xm, vectorops.mul(vm, 1.0 - u), x2, vectorops.mul(v2, 1.0 - u))]
Exemplo n.º 24
0
 def scale(self, amount):
     """Scales this direction by a scalar amount"""
     self._localCoordinates = vectorops.mul(self._localCoordinates, amount)
Exemplo n.º 25
0
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
Exemplo n.º 26
0
def hermite_subdivide(x1,v1,x2,v2,u=0.5):
    """Subdivides a hermite curve into two hermite curves."""
    xm = hermite_eval(x1,v1,x2,v2,u)
    vm = hermite_deriv(x1,v1,x2,v2,u)
    return [(x1,vectorops.mul(v1,u),xm,vectorops.mul(vm,u)),(xm,vectorops.mul(vm,1.0-u),x2,vectorops.mul(v2,1.0-u))]
Exemplo n.º 27
0
    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)
Exemplo n.º 28
0
	def difference(self,a,b,u):
		if u > 0.5:
			#do this from the b side
			return vectorops.mul(self.difference(b,a,1.0-u),-1.0)
		x = self.interpolate(a,b,u)
		return vectorops.mul(self.geodesic.difference(x,b),1.0/(1.0-u))