How to use the klampt.math.se3.apply function in Klampt

def apply_tendon_forces(self,i,link1,link2,rest_length):
        tendon_c2 = 30000.0
        tendon_c1 = 10000.0
        b0 = self.sim.body(self.model.robot.link(self.model.proximal_links[0]-3))
        b1 = self.sim.body(self.model.robot.link(link1))
        b2 = self.sim.body(self.model.robot.link(link2))
        p0w = se3.apply(b1.getTransform(),self.tendon0_local)
        p1w = se3.apply(b1.getTransform(),self.tendon1_local)
        p2w = se3.apply(b2.getTransform(),self.tendon2_local)

        d = vectorops.distance(p1w,p2w)
        if d > rest_length:
            #apply tendon force
            direction = vectorops.unit(vectorops.sub(p2w,p1w))
            f = tendon_c2*(d - rest_length)**2+tendon_c1*(d - rest_length)
            #print d,rest_length
            #print "Force magnitude",self.model.robot.link(link1).getName(),":",f
            f = min(f,100)
            #tendon routing force
            straight = vectorops.unit(vectorops.sub(p2w,p0w))
            pulley_direction = vectorops.unit(vectorops.sub(p1w,p0w))
            pulley_axis = so3.apply(b1.getTransform()[0],(0,1,0))
            tangential_axis = vectorops.cross(pulley_axis,pulley_direction)
            cosangle = vectorops.dot(straight,tangential_axis)
            #print "Cosine angle",self.model.robot.link(link1).getName(),cosangle

def drawGL(self):
        #draw tendons
        glDisable(GL_LIGHTING)
        glDisable(GL_DEPTH_TEST)
        glLineWidth(4.0)
        glColor3f(0,1,1)
        glBegin(GL_LINES)
        for i in range(3):
            b1 = self.sim.body(self.model.robot.link(self.model.proximal_links[i]))
            b2 = self.sim.body(self.model.robot.link(self.model.distal_links[i]))
            glVertex3f(*se3.apply(b1.getTransform(),self.tendon0_local))
            glVertex3f(*se3.apply(b1.getTransform(),self.tendon1_local))
            glVertex3f(*se3.apply(b1.getTransform(),self.tendon1_local))
            glVertex3f(*se3.apply(b2.getTransform(),self.tendon2_local))
        glEnd()
        glLineWidth(1)
        glColor3f(1,0.5,0)
        glBegin(GL_LINES)
        fscale = 0.01
        for i in range(3):
            b1 = self.sim.body(self.model.robot.link(self.model.proximal_links[i]))
            b2 = self.sim.body(self.model.robot.link(self.model.distal_links[i]))
            if self.forces[i][0] != None:
                p = se3.apply(b1.getTransform(),self.tendon0_local)
                glVertex3f(*p)
                glVertex3f(*vectorops.madd(p,self.forces[i][0],fscale))
            if self.forces[i][1] != None:

def drawGL(self):
        #draw tendons
        glDisable(GL_LIGHTING)
        glDisable(GL_DEPTH_TEST)
        glLineWidth(4.0)
        glColor3f(0,1,1)
        glBegin(GL_LINES)
        for i in range(3):
            b1 = self.sim.body(self.model.robot.link(self.model.proximal_links[i]))
            b2 = self.sim.body(self.model.robot.link(self.model.distal_links[i]))
            glVertex3f(*se3.apply(b1.getTransform(),self.tendon0_local))
            glVertex3f(*se3.apply(b1.getTransform(),self.tendon1_local))
            glVertex3f(*se3.apply(b1.getTransform(),self.tendon1_local))
            glVertex3f(*se3.apply(b2.getTransform(),self.tendon2_local))
        glEnd()
        glLineWidth(1)
        glColor3f(1,0.5,0)
        glBegin(GL_LINES)
        fscale = 0.01
        for i in range(3):
            b1 = self.sim.body(self.model.robot.link(self.model.proximal_links[i]))
            b2 = self.sim.body(self.model.robot.link(self.model.distal_links[i]))
            if self.forces[i][0] != None:
                p = se3.apply(b1.getTransform(),self.tendon0_local)
                glVertex3f(*p)
                glVertex3f(*vectorops.madd(p,self.forces[i][0],fscale))
            if self.forces[i][1] != None:
                p = se3.apply(b1.getTransform(),self.tendon1_local)
                glVertex3f(*p)

def drawGL(self):
        #draw tendons
        glDisable(GL_LIGHTING)
        glDisable(GL_DEPTH_TEST)
        glLineWidth(4.0)
        glColor3f(0,1,1)
        glBegin(GL_LINES)
        for i in range(3):
            b1 = self.sim.body(self.model.robot.link(self.model.proximal_links[i]))
            b2 = self.sim.body(self.model.robot.link(self.model.distal_links[i]))
            glVertex3f(*se3.apply(b1.getTransform(),self.tendon0_local))
            glVertex3f(*se3.apply(b1.getTransform(),self.tendon1_local))
            glVertex3f(*se3.apply(b1.getTransform(),self.tendon1_local))
            glVertex3f(*se3.apply(b2.getTransform(),self.tendon2_local))
        glEnd()
        glLineWidth(1)
        glColor3f(1,0.5,0)
        glBegin(GL_LINES)
        fscale = 0.01
        for i in range(3):
            b1 = self.sim.body(self.model.robot.link(self.model.proximal_links[i]))
            b2 = self.sim.body(self.model.robot.link(self.model.distal_links[i]))
            if self.forces[i][0] != None:
                p = se3.apply(b1.getTransform(),self.tendon0_local)
                glVertex3f(*p)
                glVertex3f(*vectorops.madd(p,self.forces[i][0],fscale))
            if self.forces[i][1] != None:
                p = se3.apply(b1.getTransform(),self.tendon1_local)
                glVertex3f(*p)
                glVertex3f(*vectorops.madd(p,self.forces[i][1],fscale))

glColor3f(1,0.5,0)
        glBegin(GL_LINES)
        fscale = 0.01
        for i in range(3):
            b1 = self.sim.body(self.model.robot.link(self.model.proximal_links[i]))
            b2 = self.sim.body(self.model.robot.link(self.model.distal_links[i]))
            if self.forces[i][0] != None:
                p = se3.apply(b1.getTransform(),self.tendon0_local)
                glVertex3f(*p)
                glVertex3f(*vectorops.madd(p,self.forces[i][0],fscale))
            if self.forces[i][1] != None:
                p = se3.apply(b1.getTransform(),self.tendon1_local)
                glVertex3f(*p)
                glVertex3f(*vectorops.madd(p,self.forces[i][1],fscale))
            if self.forces[i][2] != None:
                p = se3.apply(b2.getTransform(),self.tendon2_local)
                glVertex3f(*p)
                glVertex3f(*vectorops.madd(p,self.forces[i][2],fscale))
        glEnd()
        glEnable(GL_DEPTH_TEST)
        glEnable(GL_LIGHTING)