ergo.py 文件源码

def servo_axis_rotation(self, x):
        if abs(x) > self.params['sensitivity_joy']:
            x = x if abs(x) > self.params['sensitivity_joy'] else 0
            min_x = self.params['bounds'][0][0] + self.params['bounds'][3][0]
            max_x = self.params['bounds'][0][1] + self.params['bounds'][3][1]
            self.goal = min(max(min_x, self.goal + self.params['speed']*x*self.delta_t), max_x)

            if self.goal > self.params['bounds'][0][1]:
                new_x_m3 = self.goal - self.params['bounds'][0][1]
                new_x = self.params['bounds'][0][1]
            elif self.goal < self.params['bounds'][0][0]:
                new_x_m3 = self.goal - self.params['bounds'][0][0]
                new_x = self.params['bounds'][0][0]
            else:
                new_x = self.goal
                new_x_m3 = 0

            new_x_m3 = max(min(new_x_m3, self.params['bounds'][3][1]), self.params['bounds'][3][0])
            self.reach({'m1': new_x, 'm4': new_x_m3}, 0)  # Duration = 0 means joint teleportation