EmbodiedAI
/
unitree_mujoco
mirror of https://github.com/unitreerobotics/unitree_mujoco.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

add joystick support of low_state.wireless_remote()

This commit is contained in:
yangning wu 2024-06-17 10:38:08 +08:00
parent c8df8aaaf7
commit 030472693c
6 changed files with 127 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
example/python/stand_go2.py
View File

@ -29,9 +29,9 @@ input("Press enter to start")
if __name__ == '__main__':
if len(sys.argv) <2:
ChannelFactortyInitialize(1, "lo")
ChannelFactoryInitialize(1, "lo")
else:
ChannelFactortyInitialize(0, sys.argv[1])
ChannelFactoryInitialize(0, sys.argv[1])
# Create a publisher to publish the data defined in UserData class
pub = ChannelPublisher("rt/lowcmd", LowCmd_)

2
readme.md
View File

@ -60,7 +60,7 @@ sudo apt install libyaml-cpp-dev
```
### 2. Compile unitree_mujoco
```bash
cd simulate/
cd unitree_mujoco/simulate
mkdir build && cd build
cmake ..
make -j4

28
simulate/src/unitree_sdk2_bridge/unitree_sdk2_bridge.cc
View File

@ -66,6 +66,34 @@ void UnitreeSdk2Bridge::PublishLowState()
low_state.imu_state().accelerometer()[1] = mj_data_->sensordata[dim_motor_sensor_ + 8];
low_state.imu_state().accelerometer()[2] = mj_data_->sensordata[dim_motor_sensor_ + 9];
}
if (js_)
{
js_->getState();
wireless_remote_.btn.components.R1 = js_->button_[js_id_.button["RB"]];
wireless_remote_.btn.components.L1 = js_->button_[js_id_.button["LB"]];
wireless_remote_.btn.components.start = js_->button_[js_id_.button["START"]];
wireless_remote_.btn.components.select = js_->button_[js_id_.button["SELECT"]];
wireless_remote_.btn.components.R2 = (js_->axis_[js_id_.axis["RT"]] > 0);
wireless_remote_.btn.components.L2 = (js_->axis_[js_id_.axis["LT"]] > 0);
wireless_remote_.btn.components.F1 = 0;
wireless_remote_.btn.components.F2 = 0;
wireless_remote_.btn.components.A = js_->button_[js_id_.button["A"]];
wireless_remote_.btn.components.B = js_->button_[js_id_.button["B"]];
wireless_remote_.btn.components.X = js_->button_[js_id_.button["X"]];
wireless_remote_.btn.components.Y = js_->button_[js_id_.button["Y"]];
wireless_remote_.btn.components.up = (js_->axis_[js_id_.axis["DY"]] < 0);
wireless_remote_.btn.components.right = (js_->axis_[js_id_.axis["DX"]] > 0);
wireless_remote_.btn.components.down = (js_->axis_[js_id_.axis["DY"]] > 0);
wireless_remote_.btn.components.left = (js_->axis_[js_id_.axis["DX"]] < 0);
wireless_remote_.lx = double(js_->axis_[js_id_.axis["LX"]]) / max_value_;
wireless_remote_.ly = -double(js_->axis_[js_id_.axis["LY"]]) / max_value_;
wireless_remote_.rx = double(js_->axis_[js_id_.axis["RX"]]) / max_value_;
wireless_remote_.ry = -double(js_->axis_[js_id_.axis["RY"]]) / max_value_;
memcpy(&low_state.wireless_remote()[0], &wireless_remote_, 40);
}
low_state_puber_->Write(low_state);
}

17
simulate/src/unitree_sdk2_bridge/unitree_sdk2_bridge.h
View File

@ -48,6 +48,19 @@ typedef union
uint16_t value;
} xKeySwitchUnion;
typedef struct
{
uint8_t head[2];
xKeySwitchUnion btn;
float lx;
float rx;
float ry;
float L2;
float ly;
uint8_t idle[16];
} xRockerBtnDataStruct;
// Defaults to xbox gamepad
struct JoystickId
{
@ -106,7 +119,9 @@ public:
ThreadPtr HighStatePuberThreadPtr;
ThreadPtr WirelessControllerPuberThreadPtr;
xKeySwitchUnion dds_keys_;
xKeySwitchUnion dds_keys_ = {};
xRockerBtnDataStruct wireless_remote_ = {};
JoystickId js_id_;
Joystick *js_;
int max_value_ = (1 << 15); // 16 bits joystick

2
simulate_python/test/test_unitree_sdk2.py
View File

@ -21,7 +21,7 @@ def LowStateHandler(msg: LowState_):
if __name__ == "__main__":
ChannelFactortyInitialize(1, "lo")
ChannelFactoryInitialize(1, "lo")
hight_state_suber = ChannelSubscriber("rt/sportmodestate", SportModeState_)
low_state_suber = ChannelSubscriber("rt/lowstate", LowState_)

95
simulate_python/unitree_sdk2py_bridge.py
View File

@ -2,6 +2,7 @@ import mujoco
import numpy as np
import pygame
import sys
import struct
from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelPublisher
from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowCmd_
@ -34,6 +35,8 @@ class UnitreeSdk2Bridge:
self.have_frame_sensor = False
self.dt = self.mj_model.opt.timestep
self.joystick = None
# Check sensor
for i in range(self.dim_motor_sensor, self.mj_model.nsensor):
name = mujoco.mj_id2name(
@ -43,7 +46,6 @@ class UnitreeSdk2Bridge:
self.have_imu_ = True
if name == "frame_pos":
self.have_frame_sensor_ = True
# Unitree sdk2 message
self.low_state = unitree_go_msg_dds__LowState_()
@ -97,9 +99,6 @@ class UnitreeSdk2Bridge:
"left": 15,
}
self.joystick = None
def LowCmdHandler(self, msg: LowCmd_):
if self.mj_data != None:
for i in range(self.num_motor):
@ -160,6 +159,58 @@ class UnitreeSdk2Bridge:
self.dim_motor_sensor + 9
]
if self.joystick != None:
pygame.event.get()
# Buttons
self.low_state.wireless_remote[2] = int(
"".join(
[
f"{key}"
for key in [
0,
0,
int(self.joystick.get_axis(self.axis_id["LT"]) > 0),
int(self.joystick.get_axis(self.axis_id["RT"]) > 0),
int(self.joystick.get_button(self.button_id["SELECT"])),
int(self.joystick.get_button(self.button_id["START"])),
int(self.joystick.get_button(self.button_id["LB"])),
int(self.joystick.get_button(self.button_id["RB"])),
]
]
),
2,
)
self.low_state.wireless_remote[3] = int(
"".join(
[
f"{key}"
for key in [
int(self.joystick.get_hat(0)[0] < 0), # left
int(self.joystick.get_hat(0)[1] < 0), # down
int(self.joystick.get_hat(0)[0] > 0), # right
int(self.joystick.get_hat(0)[1] > 0), # up
int(self.joystick.get_button(self.button_id["Y"])), # Y
int(self.joystick.get_button(self.button_id["X"])), # X
int(self.joystick.get_button(self.button_id["B"])), # B
int(self.joystick.get_button(self.button_id["A"])), # A
]
]
),
2,
)
# Axes
sticks = [
self.joystick.get_axis(self.axis_id["LX"]),
self.joystick.get_axis(self.axis_id["RX"]),
-self.joystick.get_axis(self.axis_id["RY"]),
-self.joystick.get_axis(self.axis_id["LY"]),
]
packs = list(map(lambda x: struct.pack("f", x), sticks))
self.low_state.wireless_remote[4:8] = packs[0]
self.low_state.wireless_remote[8:12] = packs[1]
self.low_state.wireless_remote[12:16] = packs[2]
self.low_state.wireless_remote[20:24] = packs[3]
self.low_state_puber.Write(self.low_state)
def PublishHighState(self):
@ -190,23 +241,35 @@ class UnitreeSdk2Bridge:
def PublishWirelessController(self):
if self.joystick != None:
pygame.event.get()
key_state = [0]*16
key_state[self.key_map["R1"]] = self.joystick.get_button(self.button_id["RB"])
key_state[self.key_map["L1"]] = self.joystick.get_button(self.button_id["LB"])
key_state[self.key_map["start"]] = self.joystick.get_button(self.button_id["START"])
key_state[self.key_map["select"]] = self.joystick.get_button(self.button_id["SELECT"])
key_state[self.key_map["R2"]] = (self.joystick.get_axis(self.axis_id["RT"])>0)
key_state[self.key_map["L2"]] = (self.joystick.get_axis(self.axis_id["LT"])>0)
key_state = [0] * 16
key_state[self.key_map["R1"]] = self.joystick.get_button(
self.button_id["RB"]
)
key_state[self.key_map["L1"]] = self.joystick.get_button(
self.button_id["LB"]
)
key_state[self.key_map["start"]] = self.joystick.get_button(
self.button_id["START"]
)
key_state[self.key_map["select"]] = self.joystick.get_button(
self.button_id["SELECT"]
)
key_state[self.key_map["R2"]] = (
self.joystick.get_axis(self.axis_id["RT"]) > 0
)
key_state[self.key_map["L2"]] = (
self.joystick.get_axis(self.axis_id["LT"]) > 0
)
key_state[self.key_map["F1"]] = 0
key_state[self.key_map["F2"]] = 0
key_state[self.key_map["A"]] = self.joystick.get_button(self.button_id["A"])
key_state[self.key_map["B"]] = self.joystick.get_button(self.button_id["B"])
key_state[self.key_map["X"]] = self.joystick.get_button(self.button_id["X"])
key_state[self.key_map["Y"]] = self.joystick.get_button(self.button_id["Y"])
key_state[self.key_map["up"]] = (self.joystick.get_hat(0)[1]>0)
key_state[self.key_map["right"]] = (self.joystick.get_hat(0)[0]>0)
key_state[self.key_map["down"]] = (self.joystick.get_hat(0)[1]<0)
key_state[self.key_map["left"]] = (self.joystick.get_hat(0)[0]<0)
key_state[self.key_map["up"]] = self.joystick.get_hat(0)[1] > 0
key_state[self.key_map["right"]] = self.joystick.get_hat(0)[0] > 0
key_state[self.key_map["down"]] = self.joystick.get_hat(0)[1] < 0
key_state[self.key_map["left"]] = self.joystick.get_hat(0)[0] < 0
key_value = 0
for i in range(16):
@ -230,7 +293,7 @@ class UnitreeSdk2Bridge:
else:
print("No gamepad detected.")
sys.exit()
if js_type == "xbox":
self.axis_id = {
"LX": 0, # Left stick axis x