dft_python/GripperSensor.py at master · mathisrichter/dft_python · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
from naoqi import ALProxy
import numpy
import math
import DynamicField
import math_tools

class NaoGripperSensorRight(DynamicField.DynamicField):
    "Nao gripper sensor"

    def __init__(self, gripper_field_size, use_robot_sensors=True):
        "Constructor"
        DynamicField.DynamicField.__init__(self, dimension_bounds = [[gripper_field_size]])

        self._motion_proxy = ALProxy("ALMotion", "nao.ini.rub.de", 9559)
        self._name = "nao_gripper_sensor_right"
        self._use_robot_sensors = use_robot_sensors

    def _step_computation(self):
        # get the current position of the gripper
        gripper_pos = self._motion_proxy.getAngles("RHand", self._use_robot_sensors)[0]

        gripper_field_pos = gripper_pos * (self._output_dimension_sizes[0] - 1)

        # create a Gaussian activation pattern at the target location
        activation = math_tools.gauss_1d(self._output_dimension_sizes[0], 6.0, 1.0, gripper_field_pos) - 5.0
        self._activation = activation

        # compute the thresholded activation of the field
        self._output_buffer = self.compute_thresholded_activation(self._activation)

class NaoGripperSensorLeft(DynamicField.DynamicField):
    "Nao gripper sensor"

    def __init__(self, gripper_field_size, use_robot_sensors=True):
        "Constructor"
        DynamicField.DynamicField.__init__(self, dimension_bounds = [[gripper_field_size]])

        self._motion_proxy = ALProxy("ALMotion", "nao.ini.rub.de", 9559)
        self._name = "nao_gripper_sensor_left"
        self._use_robot_sensors = use_robot_sensors

    def _step_computation(self):
        # get the current position of the gripper
        gripper_pos = self._motion_proxy.getAngles("LHand", self._use_robot_sensors)[0]

        gripper_field_pos = gripper_pos * (self._output_dimension_sizes[0] - 1)

        # create a Gaussian activation pattern at the target location
        activation = math_tools.gauss_1d(self._output_dimension_sizes[0], 6.0, 1.0, gripper_field_pos) - 5.0
        self._activation = activation

        # compute the thresholded activation of the field
        self._output_buffer = self.compute_thresholded_activation(self._activation)