From 3e17527213aad40a94a1400ac95eab1ebb820e9d Mon Sep 17 00:00:00 2001 From: Rooholla-KhorramBakht Date: Fri, 9 Feb 2024 02:20:48 +0800 Subject: [PATCH] python interface added --- Go2Py/joy.py | 225 ++++++++++++++++++ Go2Py/robot/__init__.py | 0 Go2Py/robot/interface/__init__.py | 0 Go2Py/robot/interface/ros2.py | 193 +++++++++++++++ MANIFEST.in | 0 .../install/go2py_node/lib/go2py_node/bridge | Bin 4359944 -> 4359944 bytes .../parent_prefix_path/go2py_node | 2 +- .../share/go2py_node/local_setup.sh | 2 +- .../go2py_node/share/go2py_node/package.sh | 2 +- deploy/robot_ws/install/local_setup.sh | 2 +- deploy/robot_ws/install/setup.sh | 2 +- .../share/unitree_api/local_setup.sh | 2 +- .../unitree_api/share/unitree_api/package.sh | 2 +- .../share/unitree_go/local_setup.sh | 2 +- .../unitree_go/share/unitree_go/package.sh | 2 +- deploy/robot_ws/log/latest_build | 2 +- examples/highlevel_ros2_interface.ipynb | 103 ++++++++ setup.cfg | 23 ++ setup.py | 3 + 19 files changed, 557 insertions(+), 10 deletions(-) create mode 100644 Go2Py/joy.py create mode 100644 Go2Py/robot/__init__.py create mode 100644 Go2Py/robot/interface/__init__.py create mode 100644 Go2Py/robot/interface/ros2.py create mode 100644 MANIFEST.in create mode 100644 examples/highlevel_ros2_interface.ipynb create mode 100644 setup.cfg create mode 100644 setup.py diff --git a/Go2Py/joy.py b/Go2Py/joy.py new file mode 100644 index 0000000..c7f3fd9 --- /dev/null +++ b/Go2Py/joy.py @@ -0,0 +1,225 @@ +import threading +import time +from time import sleep +import numpy as np +import pygame +from dataclasses import dataclass +from pygame.locals import ( + JOYAXISMOTION, + JOYBUTTONDOWN, + JOYBUTTONUP, + K_ESCAPE, + KEYDOWN, + NOEVENT, + QUIT, + K_q, +) + +@dataclass +class xKeySwitch: + R1: int + L1: int + start: int + select: int + R2: int + L2: int + F1: int + F2: int + A: int + B: int + X: bool + Y: int + up: int + right: int + down: int + left: int + + +@dataclass +class xRockerBtn: + head: list + btn: xKeySwitch + lx: float + rx: float + ry: float + L2: float + ly: float + +class PyGameJoyManager: + def __init__(self, user_callback=None): + self.dt = 0.01 # Sampling frequence of the joystick + self.running = False + self.joy_thread_handle = threading.Thread(target=self.joy_thread) + # an optional callback that can be used to send the reading values to a user provided function + self.user_callback = user_callback + pygame.init() + self.offset = None + + def joy_thread(self): + while self.running: + for event in [ + pygame.event.wait(200), + ] + pygame.event.get(): + # QUIT none + # ACTIVEEVENT gain, state + # KEYDOWN unicode, key, mod + # KEYUP key, mod + # MOUSEMOTION pos, rel, buttons + # MOUSEBUTTONUP pos, button + # MOUSEBUTTONDOWN pos, button + # JOYAXISMOTION joy, axis, value + # JOYBALLMOTION joy, ball, rel + # JOYHATMOTION joy, hat, value + # JOYBUTTONUP joy, button + # JOYBUTTONDOWN joy, button + # VIDEORESIZE size, w, h + # VIDEOEXPOSE none + # USEREVENT code + if event.type == QUIT: + self.stop_daq() + elif event.type == KEYDOWN and event.key in [K_ESCAPE, K_q]: + self.stop_daq() + elif event.type == JOYAXISMOTION: + self.analog_cmd[event.axis] = event.value + elif event.type == JOYBUTTONUP: + self.digital_cmd[event.button] = 0 + elif event.type == JOYBUTTONDOWN: + self.digital_cmd[event.button] = 1 + if self.user_callback is not None and event.type != NOEVENT: + if self.offset is None: + self.user_callback(self.analog_cmd, self.digital_cmd) + else: + self.user_callback( + np.array(self.analog_cmd) - self.offset, self.digital_cmd + ) + + def start_daq(self, joy_idx): + # Get the joy object + assert ( + pygame.joystick.get_count() != 0 + ), "No joysticks detected, you can not start the class" + assert ( + pygame.joystick.get_count() >= joy_idx + ), "The requested joystick ID exceeds the number of available devices" + self.joy = pygame.joystick.Joystick(joy_idx) + + self.analog_cmd = [self.joy.get_axis(i) for i in range(self.joy.get_numaxes())] + self.digital_cmd = [ + self.joy.get_button(i) for i in range(self.joy.get_numbuttons()) + ] + self.running = True + self.joy_thread_handle.start() + + def stop_daq(self): + self.running = False + self.joy_thread_handle.join() + + def read_raw(self): + return self.analog_cmd, self.digital_cmd + + def offset_calibration(self): + analog, _ = self.read_raw() + offset = np.array(analog) + print("Put your stick at zero location and do not touch it") + sleep(1) + for i in range(2000): + sleep(0.001) + analog, _ = self.read_raw() + offset += np.array(analog) + self.offset = offset / 2000 + + def read_values(self): + analog, digital = self.read_raw() + if self.offset is None: + return analog, digital + else: + return analog - self.offset, digital + + +class Logitech3DPro: + """ + Handles operations specific to the Logitech 3D Pro joystick. + + This class serves as a wrapper around PyGameJoyManager, offering methods + for decoding and interpreting joystick values specific to Logitech 3D Pro. + + Attributes: + joymanager (PyGameJoyManager): Instance of PyGameJoyManager to handle + the joystick events. + joy_id (int): The identifier for the joystick. + analog_raw (list of float): The raw analog values from the joystick axes. + digital_raw (list of int): The raw digital values from the joystick buttons. + """ + + def __init__(self, joy_id=0): + """ + Initializes a Logitech3DPro object. + + Starts data acquisition and performs initial calibration. + + Parameters: + joy_id (int): Identifier for the joystick. Default is 0. + """ + self.joymanager = PyGameJoyManager() + self.joymanager.start_daq(joy_id) + print("Calibrating joystick, please do not touch the stick and wait...") + time.sleep(2) + self.joymanager.offset_calibration() + print("Calibration finished.") + self.joy_id = joy_id + + def decode(self): + """ + Decode Raw Values from Joystick. + + Decodes the raw analog and digital values from the joystick. The + decoded values are stored in the object's attributes. + """ + self.analog_raw, self.digital_raw = self.joymanager.read_values() + + def readAnalog(self): + """ + Reads Analog Values. + + Retrieves the analog values from the joystick and presents them in a + dictionary format. Performs decoding before fetching the values. + + Returns: + dict: A dictionary containing the analog axis values with keys as 'x', 'y', 'z', and 'aux'. + """ + self.decode() + data = { + "x": self.analog_raw[0], + "y": self.analog_raw[1], + "z": self.analog_raw[2], + "aux": self.analog_raw[3], + } + return data + + def readDigital(self): + """ + Reads Digital Values. + + Retrieves the digital button values from the joystick and presents + them in a dictionary format. Performs decoding before fetching the values. + + Returns: + dict: A dictionary containing the digital button states with keys as 'shoot', '2', '3', + up to '12'. + """ + self.decode() + data = { + "shoot": self.digital_raw[0], + "2": self.digital_raw[1], + "3": self.digital_raw[2], + "4": self.digital_raw[3], + "5": self.digital_raw[4], + "6": self.digital_raw[5], + "7": self.digital_raw[6], + "8": self.digital_raw[7], + "9": self.digital_raw[8], + "10": self.digital_raw[9], + "11": self.digital_raw[10], + "12": self.digital_raw[11], + } + return data \ No newline at end of file diff --git a/Go2Py/robot/__init__.py b/Go2Py/robot/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/Go2Py/robot/interface/__init__.py b/Go2Py/robot/interface/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/Go2Py/robot/interface/ros2.py b/Go2Py/robot/interface/ros2.py new file mode 100644 index 0000000..1130d6b --- /dev/null +++ b/Go2Py/robot/interface/ros2.py @@ -0,0 +1,193 @@ +import struct +import threading +import time +import numpy as np +import numpy.linalg as LA +from scipy.spatial.transform import Rotation as R +import rclpy +import tf2_ros +from rclpy.node import Node +from rclpy.qos import QoSProfile +from rclpy.executors import MultiThreadedExecutor +from geometry_msgs.msg import TransformStamped +from Go2Py.joy import xKeySwitch, xRockerBtn +from geometry_msgs.msg import TwistStamped +from unitree_go.msg import LowState + + +def ros2_init(args=None): + rclpy.init(args=args) + + +def ros2_close(): + rclpy.shutdown() + +class ROS2ExecutorManager: + """A class to manage the ROS2 executor. It allows to add nodes and start the executor in a separate thread.""" + def __init__(self): + self.executor = MultiThreadedExecutor() + self.nodes = [] + self.executor_thread = None + + def add_node(self, node: Node): + """Add a new node to the executor.""" + self.nodes.append(node) + self.executor.add_node(node) + + def _run_executor(self): + try: + self.executor.spin() + except KeyboardInterrupt: + pass + finally: + self.terminate() + + def start(self): + """Start spinning the nodes in a separate thread.""" + self.executor_thread = threading.Thread(target=self._run_executor) + self.executor_thread.start() + + def terminate(self): + """Terminate all nodes and shutdown rclpy.""" + for node in self.nodes: + node.destroy_node() + rclpy.shutdown() + if self.executor_thread: + self.executor_thread.join() + + +class GO2Robot(Node): + def __init__( + self, + test=False, + vx_max=0.5, + vy_max=0.4, + ωz_max=0.5, + node_name="go2py_highlevel_subscriber", + ): + self.node_name = node_name + self.highcmd_topic = "/go2/twist_cmd" + self.joint_state_topic = "/go2/joint_states" + self.lowstate_topic = "/lowstate" + super().__init__(self.node_name) + + self.subscription = self.create_subscription( + LowState, self.lowstate_topic, self.lowstate_callback, 1 + ) + self.cmd_publisher = self.create_publisher(TwistStamped, self.highcmd_topic, 1) + self.highcmd = TwistStamped() + # create pinocchio robot + # self.pin_robot = PinRobot() + + # for velocity clipping + self.vx_max = vx_max + self.vy_max = vy_max + self.P_v_max = np.diag([1 / self.vx_max**2, 1 / self.vy_max**2]) + self.ωz_max = ωz_max + self.ωz_min = -ωz_max + self.running = True + + def lowstate_callback(self, msg): + """ + Retrieve the state of the robot + """ + self.state = msg + + def getIMU(self): + accel = self.state.imu.accelerometer + gyro = self.state.imu.gyroscope + quat = self.state.imu.quaternion + rpy = self.state.imu.rpy + return accel, gyro, quat, rpy + + def getFootContacts(self): + """Returns the foot contact states""" + footContacts = self.state.foot_force_est + return np.array(footContacts) + + def getJointStates(self): + """Returns the joint angles (q) and velocities (dq) of the robot""" + motorStates = self.state.motor_state + _q, _dq = zip( + *[(motorState.q, motorState.dq) for motorState in motorStates[:12]] + ) + q, dq = np.array(_q), np.array(_dq) + + return q, dq + + def getRemoteState(self): + """Returns the state of the remote control""" + wirelessRemote = self.state.wireless_remote[:24] + + binary_data = bytes(wirelessRemote) + + format_str = "<2BH5f" + data = struct.unpack(format_str, binary_data) + + head = list(data[:2]) + lx = data[3] + rx = data[4] + ry = data[5] + L2 = data[6] + ly = data[7] + + _btn = bin(data[2])[2:].zfill(16) + btn = [int(char) for char in _btn] + btn.reverse() + + keySwitch = xKeySwitch(*btn) + rockerBtn = xRockerBtn(head, keySwitch, lx, rx, ry, L2, ly) + + return rockerBtn + + def getCommandFromRemote(self): + """Do not use directly for control!!!""" + rockerBtn = self.getRemoteState() + + lx = rockerBtn.lx + ly = rockerBtn.ly + rx = rockerBtn.rx + + v_x = ly * self.vx_max + v_y = lx * self.vy_max + ω = rx * self.ωz_max + + return v_x, v_y, ω + + def getBatteryState(self): + """Returns the battery percentage of the robot""" + batteryState = self.state.bms + return batteryState.SOC + + def setCommands(self, v_x, v_y, ω_z, bodyHeight=0.0, footRaiseHeight=0.0, mode=2): + assert mode in [0, 2] # Only mode 2: walking and mode 0: idea is allowed + self.cmd_watchdog_timer = time.time() + self.cmd.mode = mode + self.cmd.body_height = np.clip(bodyHeight, -0.15, 0.1) + self.cmd.foot_raise_height = np.clip(footRaiseHeight, -0.1, 0.1) + _v_x, _v_y, _ω_z = self.clip_velocity(v_x, v_y, ω_z) + self.cmd.velocity = [_v_x, _v_y] + self.cmd.yaw_speed = _ω_z + # Publish the command as a ROS2 message + self.cmd_publisher.publish(self.cmd) + + def close(self): + self.running = False + self.thread.join() + self.destroy_node() + + def check_calf_collision(self, q): + self.pin_robot.update(q) + in_collision = self.pin_robot.check_calf_collision(q) + return in_collision + + def clip_velocity(self, v_x, v_y, ω_z): + _v = np.array([[v_x], [v_y]]) + _scale = np.sqrt(_v.T @ self.P_v_max @ _v)[0, 0] + + if _scale > 1.0: + scale = 1.0 / _scale + else: + scale = 1.0 + + return scale * v_x, scale * v_y, np.clip(ω_z, self.ωz_min, self.ωz_max) \ No newline at end of file diff --git a/MANIFEST.in b/MANIFEST.in new file mode 100644 index 0000000..e69de29 diff --git a/deploy/robot_ws/install/go2py_node/lib/go2py_node/bridge b/deploy/robot_ws/install/go2py_node/lib/go2py_node/bridge index 71d6ee2b0fa1823ebe83bd40ed753a605dec3dda..cdc4f5a8d4f8360f50354c4c3898def97d7a830c 100755 GIT binary patch delta 437 zcmXxfy-Nad9LMqQ%*ynxr_(&Gv`5qGEPJ-p9w_=VDjKXID;ortq(Ny2gCNLNXbRkZ zW1~ZjP7Tr2|IiK5`wGAC8a{kJ%eQoUBbQJ+lC{@(%5CQN6V=^p|6MXtd3&kmuXD1< z$xW=tS z)S$E3j&+=5Gp9!S>`X*;^;rI&`K_5>-SaeOiDEjYq6^*VK`;75I;Pd`W%=vDSJyry zQ+(I8fJ-uqdP9R79`wTt9X|LmfI$o)fFOo3f>DfN91{p(5@AFzg=x&dKoql>Lk#m+ rz#^8gj1{aRjy0?!fh1Dc5PD;?>)HAxQrI#aW>{*m&J+iQttb00zB{B2 delta 433 zcmXxfOD_Xq6vpw+OHnmbmr$0M1 zVej8A;yFs69GeHJluO18?NI!-f=(-_ZTSB$5XUU$FpmW+VhPJwk;e0?@4@*cnoXD&2 diff --git a/deploy/robot_ws/install/local_setup.sh b/deploy/robot_ws/install/local_setup.sh index 9345c15..982598d 100644 --- a/deploy/robot_ws/install/local_setup.sh +++ b/deploy/robot_ws/install/local_setup.sh @@ -6,7 +6,7 @@ # since a plain shell script can't determine its own path when being sourced # either use the provided COLCON_CURRENT_PREFIX # or fall back to the build time prefix (if it exists) -_colcon_prefix_sh_COLCON_CURRENT_PREFIX="/home/unitree/locomotion/Go2Py/deploy/ros2_ws/install" +_colcon_prefix_sh_COLCON_CURRENT_PREFIX="/home/unitree/locomotion/Go2Py/deploy/robot_ws/install" if [ -z "$COLCON_CURRENT_PREFIX" ]; then if [ ! -d "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX" ]; then echo "The build time path \"$_colcon_prefix_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2 diff --git a/deploy/robot_ws/install/setup.sh b/deploy/robot_ws/install/setup.sh index 2a294d5..379a35a 100644 --- a/deploy/robot_ws/install/setup.sh +++ b/deploy/robot_ws/install/setup.sh @@ -7,7 +7,7 @@ # since a plain shell script can't determine its own path when being sourced # either use the provided COLCON_CURRENT_PREFIX # or fall back to the build time prefix (if it exists) -_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX=/home/unitree/locomotion/Go2Py/deploy/ros2_ws/install +_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX=/home/unitree/locomotion/Go2Py/deploy/robot_ws/install if [ ! -z "$COLCON_CURRENT_PREFIX" ]; then _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" elif [ ! -d "$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX" ]; then diff --git a/deploy/robot_ws/install/unitree_api/share/unitree_api/local_setup.sh b/deploy/robot_ws/install/unitree_api/share/unitree_api/local_setup.sh index 81dfc27..96c9022 100644 --- a/deploy/robot_ws/install/unitree_api/share/unitree_api/local_setup.sh +++ b/deploy/robot_ws/install/unitree_api/share/unitree_api/local_setup.sh @@ -2,7 +2,7 @@ # since this file is sourced use either the provided AMENT_CURRENT_PREFIX # or fall back to the destination set at configure time -: ${AMENT_CURRENT_PREFIX:="/home/unitree/locomotion/Go2Py/deploy/ros2_ws/install/unitree_api"} +: ${AMENT_CURRENT_PREFIX:="/home/unitree/locomotion/Go2Py/deploy/robot_ws/install/unitree_api"} if [ ! -d "$AMENT_CURRENT_PREFIX" ]; then if [ -z "$COLCON_CURRENT_PREFIX" ]; then echo "The compile time prefix path '$AMENT_CURRENT_PREFIX' doesn't " \ diff --git a/deploy/robot_ws/install/unitree_api/share/unitree_api/package.sh b/deploy/robot_ws/install/unitree_api/share/unitree_api/package.sh index a2f8237..317a8c3 100644 --- a/deploy/robot_ws/install/unitree_api/share/unitree_api/package.sh +++ b/deploy/robot_ws/install/unitree_api/share/unitree_api/package.sh @@ -52,7 +52,7 @@ _colcon_prepend_unique_value() { # since a plain shell script can't determine its own path when being sourced # either use the provided COLCON_CURRENT_PREFIX # or fall back to the build time prefix (if it exists) -_colcon_package_sh_COLCON_CURRENT_PREFIX="/home/unitree/locomotion/Go2Py/deploy/ros2_ws/install/unitree_api" +_colcon_package_sh_COLCON_CURRENT_PREFIX="/home/unitree/locomotion/Go2Py/deploy/robot_ws/install/unitree_api" if [ -z "$COLCON_CURRENT_PREFIX" ]; then if [ ! -d "$_colcon_package_sh_COLCON_CURRENT_PREFIX" ]; then echo "The build time path \"$_colcon_package_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2 diff --git a/deploy/robot_ws/install/unitree_go/share/unitree_go/local_setup.sh b/deploy/robot_ws/install/unitree_go/share/unitree_go/local_setup.sh index 855df3c..ba3326b 100644 --- a/deploy/robot_ws/install/unitree_go/share/unitree_go/local_setup.sh +++ b/deploy/robot_ws/install/unitree_go/share/unitree_go/local_setup.sh @@ -2,7 +2,7 @@ # since this file is sourced use either the provided AMENT_CURRENT_PREFIX # or fall back to the destination set at configure time -: ${AMENT_CURRENT_PREFIX:="/home/unitree/locomotion/Go2Py/deploy/ros2_ws/install/unitree_go"} +: ${AMENT_CURRENT_PREFIX:="/home/unitree/locomotion/Go2Py/deploy/robot_ws/install/unitree_go"} if [ ! -d "$AMENT_CURRENT_PREFIX" ]; then if [ -z "$COLCON_CURRENT_PREFIX" ]; then echo "The compile time prefix path '$AMENT_CURRENT_PREFIX' doesn't " \ diff --git a/deploy/robot_ws/install/unitree_go/share/unitree_go/package.sh b/deploy/robot_ws/install/unitree_go/share/unitree_go/package.sh index 0d5f19f..15b9264 100644 --- a/deploy/robot_ws/install/unitree_go/share/unitree_go/package.sh +++ b/deploy/robot_ws/install/unitree_go/share/unitree_go/package.sh @@ -52,7 +52,7 @@ _colcon_prepend_unique_value() { # since a plain shell script can't determine its own path when being sourced # either use the provided COLCON_CURRENT_PREFIX # or fall back to the build time prefix (if it exists) -_colcon_package_sh_COLCON_CURRENT_PREFIX="/home/unitree/locomotion/Go2Py/deploy/ros2_ws/install/unitree_go" +_colcon_package_sh_COLCON_CURRENT_PREFIX="/home/unitree/locomotion/Go2Py/deploy/robot_ws/install/unitree_go" if [ -z "$COLCON_CURRENT_PREFIX" ]; then if [ ! -d "$_colcon_package_sh_COLCON_CURRENT_PREFIX" ]; then echo "The build time path \"$_colcon_package_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2 diff --git a/deploy/robot_ws/log/latest_build b/deploy/robot_ws/log/latest_build index 06a8618..15ea434 120000 --- a/deploy/robot_ws/log/latest_build +++ b/deploy/robot_ws/log/latest_build @@ -1 +1 @@ -build_2024-02-09_01-44-22 \ No newline at end of file +build_2024-02-09_01-52-38 \ No newline at end of file diff --git a/examples/highlevel_ros2_interface.ipynb b/examples/highlevel_ros2_interface.ipynb new file mode 100644 index 0000000..8c2cad6 --- /dev/null +++ b/examples/highlevel_ros2_interface.ipynb @@ -0,0 +1,103 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# Joystick Control\n", + "As the first step after successfully communicating with the robot's onboard controller, we use a USB joystick (a Logitech Extreme 3D Pro) to command the robot with desired $x$, $y$, and $\\Psi$ velocities.\n", + "\n", + "First, use the Pygame-based joystick class provided with B1Py to connect to the joystick:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "from Go2Py.joy import Logitech3DPro\n", + "# joy = Logitech3DPro(joy_id=0)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "***Note:*** Initially when you instantiate the joystick, the class performs an offset calibration to maker sure the neutral configuration of the device reads zero. It is important to leave the device untouched during the couple of seconds after calling the cell above. The prompts tells you when you can continue. \n", + "\n", + "Then as explained in [hardware interface documentation](unitree_locomotion_controller_interface.ipynb), instantiate and communication link to high-level controller on the robot. Note that the robot should be standing and ready to walk before we can control it. Furthermore, the the highlevel node in b1py_node ROS2 package (in `deploy/ros2_ws`) should be launched on the robot. " + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "from Go2Py.robot.interface.ros2 import GO2Robot, ros2_init, ROS2ExecutorManager\n", + "import time\n", + "ros2_init()\n", + "robot = GO2Robot()\n", + "ros2_exec_manager = ROS2ExecutorManager()\n", + "ros2_exec_manager.add_node(robot)\n", + "ros2_exec_manager.start()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Finally, read the joystick values and send them to the robot in a loop:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "Duration = 20\n", + "N = Duration//0.01\n", + "for i in range(500):\n", + " cmd = joy.readAnalog()\n", + " vx = cmd['x']\n", + " vy = cmd['y']\n", + " w = cmd['z'] / 2\n", + " body_height = cmd['aux'] / 10\n", + " robot.setCommands(vx,vy,w, bodyHeight=body_height)\n", + " time.sleep(0.01)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "robot.getJointStates()" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.8.10" + } + }, + "nbformat": 4, + "nbformat_minor": 4 +} diff --git a/setup.cfg b/setup.cfg new file mode 100644 index 0000000..1abfcf7 --- /dev/null +++ b/setup.cfg @@ -0,0 +1,23 @@ +[metadata] +name = Go2Py +version = 1.0.0 +description = To Add +long_description = file: README.md +long_description_content_type = text/markdown +url = https://github.com/Rooholla-KhorramBakht/Go2Py +author = Rooholla Khorrambakht, Bolun Dai +author_email = rk4342@nyu.edu, bd1555@nyu.edu +license = MIT +license_file = LICENSE +classifiers = + License :: OSI Approved :: MIT License + +[options] +packages = find: +include_package_data = True +install_requires = + pygame + pyyaml + +# [options.package_data] +# * = unitree_legged_sdk/amd64/*.so, unitree_legged_sdk/arm64/*.so \ No newline at end of file diff --git a/setup.py b/setup.py new file mode 100644 index 0000000..fc1f76c --- /dev/null +++ b/setup.py @@ -0,0 +1,3 @@ +from setuptools import setup + +setup() \ No newline at end of file