joystick.py 文件源码

def getValuesFromPose(self, P):
        '''return the virtual values of the pots corresponding to the pose P'''
        vals = []
        grads = []
        for i, r, l, placement, attach_p in zip(range(3), self.rs, self.ls, self.placements, self.attach_ps):
            #first pot axis
            a = placement.rot * col([1, 0, 0])
            #second pot axis
            b = placement.rot * col([0, 1, 0])
            #string axis
            c = placement.rot * col([0, 0, 1])

            #attach point on the joystick
            p_joystick = P * attach_p
            v = p_joystick - placement.trans
            va = v - dot(v, a)*a
            vb = v - dot(v, b)*b
            #angles of the pots
            alpha = math.atan2(dot(vb, a), dot(vb, c))
            beta = math.atan2(dot(va, b), dot(va, c))
            vals.append(alpha)
            vals.append(beta)

            #calculation of the derivatives
            dv = np.bmat([-P.rot.mat() * quat.skew(attach_p), P.rot.mat()])
            dva = (np.eye(3) - a*a.T) * dv
            dvb = (np.eye(3) - b*b.T) * dv
            dalpha = (1/dot(vb,vb)) * (dot(vb,c) * a.T - dot(vb,a) * c.T) * dvb
            dbeta = (1/dot(va,va)) * (dot(va,c) * b.T - dot(va,b) * c.T) * dva
            grads.append(dalpha)
            grads.append(dbeta)
        return (col(vals), np.bmat([[grads]]))