How to use the klampt.math.vectorops.madd function in Klampt

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
            base_direction = so3.apply(b0.getTransform()[0],[0,0,-1])
            b0.applyForceAtLocalPoint(vectorops.mul(base_direction,-f),vectorops.madd(p0w,base_direction,0.04))
            b1.applyForceAtLocalPoint(vectorops.mul(tangential_axis,cosangle*f),self.tendon1_local)
            b1.applyForceAtLocalPoint(vectorops.mul(tangential_axis,-cosangle*f),self.tendon0_local)
            b2.applyForceAtLocalPoint(vectorops.mul(direction,-f),self.tendon2_local)
            self.forces[i][1] = vectorops.mul(tangential_axis,cosangle*f)
            self.forces[i][2] = vectorops.mul(direction,f)
        else:
            self.forces[i] = [None,None,None]
        return

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)

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))
            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)