python interface added
This commit is contained in:
Rooholla-KhorramBakht
parent
a895b6396a
commit
3e17527213
|
|
@ -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
|
||||
|
|
@ -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)
|
||||
Binary file not shown.
|
|
@ -1 +1 @@
|
|||
/home/unitree/locomotion/Go2Py/deploy/ros2_ws/install/go2py_node:/home/unitree/locomotion/Go2Py/deploy/ros2_ws/install/unitree_go:/home/unitree/locomotion/Go2Py/deploy/ros2_ws/install/unitree_api:/home/unitree/locomotion/Go2Py/deploy/ros2_ws/install/hesai_ros_driver:/home/unitree/locomotion/unitree_ros2/cyclonedds_ws/install/unitree_go:/home/unitree/locomotion/unitree_ros2/cyclonedds_ws/install/unitree_api:/home/unitree/locomotion/unitree_ros2/cyclonedds_ws/install/rmw_cyclonedds_cpp:/opt/ros/foxy
|
||||
/home/unitree/locomotion/Go2Py/deploy/robot_ws/install/unitree_go:/home/unitree/locomotion/Go2Py/deploy/robot_ws/install/unitree_api:/home/unitree/locomotion/Go2Py/deploy/ros2_ws/install/go2py_node:/home/unitree/locomotion/Go2Py/deploy/ros2_ws/install/unitree_go:/home/unitree/locomotion/Go2Py/deploy/ros2_ws/install/unitree_api:/home/unitree/locomotion/Go2Py/deploy/ros2_ws/install/hesai_ros_driver:/home/unitree/locomotion/unitree_ros2/cyclonedds_ws/install/unitree_go:/home/unitree/locomotion/unitree_ros2/cyclonedds_ws/install/unitree_api:/home/unitree/locomotion/unitree_ros2/cyclonedds_ws/install/rmw_cyclonedds_cpp:/opt/ros/foxy
|
||||
|
|
@ -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/go2py_node"}
|
||||
: ${AMENT_CURRENT_PREFIX:="/home/unitree/locomotion/Go2Py/deploy/robot_ws/install/go2py_node"}
|
||||
if [ ! -d "$AMENT_CURRENT_PREFIX" ]; then
|
||||
if [ -z "$COLCON_CURRENT_PREFIX" ]; then
|
||||
echo "The compile time prefix path '$AMENT_CURRENT_PREFIX' doesn't " \
|
||||
|
|
|
|||
|
|
@ -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/go2py_node"
|
||||
_colcon_package_sh_COLCON_CURRENT_PREFIX="/home/unitree/locomotion/Go2Py/deploy/robot_ws/install/go2py_node"
|
||||
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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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 " \
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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 " \
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -1 +1 @@
|
|||
build_2024-02-09_01-44-22
|
||||
build_2024-02-09_01-52-38
|
||||
|
|
@ -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
|
||||
}
|
||||
|
|
@ -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
|
||||
Loading…
Reference in New Issue