dora-rs
/
dora
Not watched
1
0
Fork 0
Code
Releases 124 Wiki evaluate Activity
Issues 0 Pull Requests 0 Datasets Model Cloudbrain HPC
You can not select more than 25 topics Topics must start with a chinese character,a letter or number, can include dashes ('-') and can be up to 35 characters long.
Tree: 416bd9ef81
Branches Tags
${ item.name }
Create branch ${ searchTerm }
from '416bd9ef81'
${ noResults }
dora/examples/lebai/nodes/interpolation.py

66 lines
2.3 kB
Raw Blame History 

  1. """Module for interpolating between robot configurations.

  2. 

  3. This module provides functionality for creating smooth transitions between

  4. robot configurations and movements.

  5. """

  6. 

  7. from enum import Enum

  8. 

  9. import pyarrow as pa

  10. from dora import Node

  11. 

  12. node = Node()

  13. 

  14. 

  15. class Action(Enum):

  16.     """Action abstraction for robot movement commands."""

  17. 

  18.     YAW_RIGHT = ("yaw right", "movej", [0, 0, 0, 0, -0.1, 0, 0.1])

  19.     YAW_LEFT = ("yaw left", "movej", [0, 0, 0, 0, 0.1, 0, 0.1])

  20.     PITCH_UP = ("pitch up", "movej", [0, 0, 0, -0.1, 0, 0, 0.1])

  21.     PITCH_DOWN = ("pitch down", "movej", [0, 0, 0.1, 0, 0, 0, 0.1])

  22.     ROLL_LEFT = ("roll left", "movej", [0, 0, 0, 0, 0, 0.1, 0.1])

  23.     ROLL_RIGHT = ("roll right", "movej", [0, 0, 0, 0, 0, -0.1, 0.1])

  24.     YAW_SHOULDER_RIGHT = (

  25.         "yaw shoulder right",

  26.         "movej",

  27.         [-0.1, 0, 0, 0, 0, 0, 0.1],

  28.     )

  29.     YAW_SHOULDER_LEFT = (

  30.         "yaw shoulder left",

  31.         "movej",

  32.         [0.1, 0, 0, 0, 0, 0, 0.1],

  33.     )

  34.     FORWARD = ("forward", "movec", [-0.02, 0, 0, 0, 0, 0, 0.1])

  35.     BACK = ("back", "movec", [0.02, 0, 0, 0, 0, 0, 0.1])

  36.     LEFT = ("left", "movec", [0, -0.02, 0, 0, 0, 0, 0.1])

  37.     RIGHT = ("right", "movec", [0, 0.02, 0, 0, 0, 0, 0.1])

  38.     UP = ("up", "movec", [0, 0, 0.02, 0, 0, 0, 0.1])

  39.     DOWN = ("down", "movec", [0, 0, -0.02, 0, 0, 0, 0.1])

  40.     CLOSE = ("close", "claw", [0])

  41.     OPEN = ("open", "claw", [100])

  42.     STOP = ("stop", "stop", [])

  43.     SAVE = ("save", "save", [])

  44.     GO_TO = ("go", "go_to", [])

  45.     END_TEACH = ("end of teach", "end_teach", [])

  46.     TEACH = ("teach", "teach", [])

  47. 

  48. 

  49. for event in node:

  50.     if event["type"] == "INPUT":

  51.         text = event["value"][0].as_py().lower()

  52.         text = text.replace(".", "")

  53.         text = text.replace(".", "")

  54. 

  55.         if "save" in text:

  56.             node.send_output("save", pa.array([text.replace("save ", "")]))

  57.         elif "go" in text:

  58.             node.send_output("go_to", pa.array([text.replace("go ", "")]))

  59.         elif "go to " in text:

  60.             node.send_output("go_to", pa.array([text.replace("go to ", "")]))

  61.         else:

  62.             for action in Action:

  63.                 if action.value[0] in text:

  64.                     node.send_output(action.value[1], pa.array(action.value[2]))

  65.                     break