joystick.py 文件源码

def getNumericalGradient(self, P, h = 1e-5):
        '''just to check the calculations...'''
        grad = []
        for i in range(6):
            dv = [0, 0, 0, 0, 0, 0]
            dv[i] = h
            gi = (1./h) * (self.getValuesFromPose(P * pose.exp(col(dv))) - self.getValuesFromPose(P))
            grad.append(gi)
        return np.bmat(grad)