Skip to content

Commit

Permalink
Tested getup on Bez2 hw (#843)
Browse files Browse the repository at this point in the history 
- fw + fw interface modified to support 2 additional arm motors
  • Loading branch information
@lkunelk
lkunelk authored Jul 16, 2024
1 parent b81d731 commit 99a2723
Show file tree
Hide file tree
Showing 9 changed files with 82 additions and 72 deletions.
4 changes: 2 additions & 2 deletions soccer_control/soccer_pycontrol/test/test_pybullet.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -58,8 +58,8 @@ def test_bez_motor_range_single(self):

for j in angles:
x = [0.0] * bez.motor_control.numb_of_motors
x[bez.data.motor_names.index("left_leg_motor_0")] = j
x[bez.data.motor_names.index("right_leg_motor_0")] = j
x[bez.data.motor_names.index("left_leg_motor_5")] = j
x[bez.data.motor_names.index("right_leg_motor_5")] = j

pb.setJointMotorControlArray(
bodyIndex=bez.model.body,
Expand Down
12 changes: 6 additions & 6 deletions soccer_control/soccer_trajectories/test/test_trajectory.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,16 +62,16 @@ def test_trajectory_sim(trajectory_name: str, robot_model: str, real_time: bool)
"""
# TODO update with pybullet updates
if trajectory_name == "getupfront":
pose = Transformation(position=[0, 0, 0.070], euler=[0, 1.57, 0])
pose = Transformation(position=[0, 0, 0.070], euler=[0, 1.57, 1.57])
elif trajectory_name == "getupback":
pose = Transformation(position=[0, 0, 0.070], euler=[0, -1.57, 0])
# pose = Transformation(position=[0, 0, 0.4], quaternion=[0.0, 0.0, 0.0, 1])
# else:
# pose = Transformation(position=[0, 0, 0.315], quaternion=[0.0, 0.0, 0.0, 1])

print(os.path.join(os.path.dirname(__file__), "../trajectories/bez2_sim/" + trajectory_name + ".csv"))
print(os.path.join(os.path.dirname(__file__), "../trajectories/bez2/" + trajectory_name + ".csv"))
tm = TrajectoryManagerSim(
os.path.join(os.path.dirname(__file__), "../trajectories/bez2_sim/" + trajectory_name + ".csv"), pose, robot_model, real_time
os.path.join(os.path.dirname(__file__), "../trajectories/bez2/" + trajectory_name + ".csv"), pose, robot_model, real_time
)
tm.send_trajectory()
tm.sim.wait(1000)
Expand Down Expand Up @@ -99,16 +99,16 @@ def run_real_trajectory(robot_model: str, trajectory_name: str, real_time: bool)
msg.mirror = False

joint_state = MagicMock()
joint_state.position = [0.0] * 18
joint_state.position = [0.0] * 20
rospy.wait_for_message = MagicMock(return_value=joint_state)

c.command_callback(command=msg)
c.send_trajectory(real_time=real_time)


@pytest.mark.parametrize("robot_model", ["bez1"])
@pytest.mark.parametrize("robot_model", ["bez2"])
@pytest.mark.parametrize("trajectory_name", ["getupfront"])
@pytest.mark.parametrize("real_time", [True])
@unittest.skip("Not integrated in CI")
# @unittest.skip("Not integrated in CI")
def test_traj_ros(robot_model: str, trajectory_name: str, real_time: bool):
run_real_trajectory(robot_model, trajectory_name, real_time)
40 changes: 21 additions & 19 deletions soccer_control/soccer_trajectories/trajectories/bez2/getupfront.csv
View file
Original file line number Diff line number Diff line change
@@ -1,19 +1,21 @@
time,0,0.75,1.5,2.25,3,3.75,4.5,5.25,6,6.75,7.5,8.25,9,9.75,10.5,11.5,13,14,15
right_leg_motor_0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
left_leg_motor_0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
right_leg_motor_1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
left_leg_motor_1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
right_leg_motor_2,0,0,0,0,0,1,1.5,1.5,2.3,2.8,2.8,2.8,2.8,2.8,2.8,2.8,2.0,1.3,0.6
left_leg_motor_2,0,0,0,0,0,1,1.5,1.5,2.3,2.8,2.8,2.8,2.8,2.8,1.5,2.8,2.0,1.3,0.6
right_leg_motor_3,0,0,0,0,0,-1,-1.3,-1.3,-2.3,-2.8,-2.8,-2.8,-2.8,-2.8,-2.8,-2.8,-2,-2,-0.8
left_leg_motor_3,0,0,0,0,0,-1,-1.3,-1.3,-2.3,-2.8,-2.8,-2.8,-2.8,-2.8,-2.2,-2.8,-2,-2,-0.8
right_leg_motor_4,0,0,0,0,1.1,1.2,1.2,1.2,1.2,1.2,1.2,1.2,1.0,0.8,0.3,0.3,0.3,0.8,0.3
left_leg_motor_4,0,0,0,0,1.1,1.2,1.2,1.2,1.2,1.2,1.2,1.2,1.0,0.8,1.2,0.3,0.3,0.8,0.3
right_leg_motor_5,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
left_leg_motor_5,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
right_arm_motor_0,0,-1.5,0,1.5,1.5,1.5,1.5,1,0.7,0.2,-0.2,0,0.3,0.8,0.7,0.5,0.3,0.3,0.3
left_arm_motor_0,0,-1.5,0,1.5,1.5,1.5,1.5,1,0.7,0.2,-0.2,0,0.3,0.8,0.7,0.5,0.3,0.3,0.3
right_arm_motor_1,0,2.5,3,1.5,2,2,2,2.3,2.5,2.8,2.6,2.6,1.3,0,0,0,0,0,0
left_arm_motor_1,0,2.5,3,1.5,2,2,2,2.3,2.5,2.8,2.6,2.6,1.3,0,0,0,0,0,0
head_motor_0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
head_motor_1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
time,0,0.75,1.5,2.25,3,3.75,4.5,5.25,6,6.75
right_leg_motor_0,0,0,0,0,0,0,0,0,0,0
left_leg_motor_0,0,0,0,0,0,0,0,0,0,0
right_leg_motor_1,0,0,0,0,0,0,0,0,0,0
left_leg_motor_1,0,0,0,0,0,0,0,0,0,0
right_leg_motor_2,0,0,0,1,1.5,2,2,2,1.7,0.564
left_leg_motor_2,0,0,0,1,1.5,2,2,2,1.7,0.564
right_leg_motor_3,0,0,0,-1,-1.3,-1.8,-2.2,-2.6,-2.6, -1.176
left_leg_motor_3,0,0,0,-1,-1.3,-1.8,-2.2,-2.6,-2.6, -1.176
right_leg_motor_4,0,0,1.1,1.5,1.5,1.5,1.5,1.2,1.2,0.613
left_leg_motor_4,0,0,1.1,1.5,1.5,1.5,1.5,1.2,1.2,0.613
right_leg_motor_5,0,0,0,0,0,0,0,0,0,0
left_leg_motor_5,0,0,0,0,0,0,0,0,0,0
right_arm_motor_0,0,0,1.5,1.5,2,1.3,1.8,1.0,0.9,0
left_arm_motor_0,0,0,1.5,1.5,2,1.3,1.8,1.0,0.9,0
right_arm_motor_2,0,3.14,3.14,2,2,1.3,0,0,0,0
left_arm_motor_2,0,3.14,3.14,2,2,1.3,0,0,0,0
head_motor_0,0,0,0,0,0,0,0,0,0,0
head_motor_1,0,0,0,0,0,0,0,0,0,0
right_arm_motor_1,0,0,0,0,0,0,0,0,0,0
left_arm_motor_1,0,0,0,0,0,0,0,0,0,0
2 changes: 1 addition & 1 deletion soccer_description/bez1_description/urdf/bez1.urdf
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- | This document was autogenerated by xacro from bez1.xacro | -->
<!-- | This document was autogenerated by xacro from urdf/bez1.xacro | -->
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
<!-- =================================================================================== -->
<robot name="bez1">
Expand Down
28 changes: 14 additions & 14 deletions soccer_firmware/Core/Src/main.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -100,9 +100,9 @@ void init_ports() {
.dirPinNum = USART1_DIR_Pin,
.dmaDoneReading = false,
.currMotor = 0,
.numMotors = 3,
.motorIds = {10, 11, 12}, //{16, 17},
.protocol = {2, 2, 2} //{1, 1}
.numMotors = 4,
.motorIds = {0, 1, 18, 16}, // left arm chain + bottom neck motor
.protocol = {2, 2, 2}
};

// port2 => UART2 ==> J7 on new PCB
Expand All @@ -114,9 +114,9 @@ void init_ports() {
.dirPinNum = USART2_DIR_Pin,
.dmaDoneReading = false,
.currMotor = 0,
.numMotors = 3,
.motorIds = {10, 11, 12},//{0, 1, 2, 3},
.protocol = {2, 2, 2}
.numMotors = 4,
.motorIds = {2, 3, 19, 17}, // right arm chain + top neck motor
.protocol = {2, 2, 2, 2}
};

// port3 => UART3 ==> J12 on new PCB
Expand All @@ -128,8 +128,8 @@ void init_ports() {
.dirPinNum = USART3_DIR_Pin,
.dmaDoneReading = false,
.numMotors = 3,
.motorIds = {10, 11, 12},//{13, 14, 15},
.protocol = {2, 2, 2}//{1, 1, 1}
.motorIds = {4, 5, 6}, // left hip
.protocol = {2, 2, 2}
};

// port4 => UART4 ==> J3 on new PCB
Expand All @@ -141,8 +141,8 @@ void init_ports() {
.dirPinNum = USART4_DIR_Pin,
.dmaDoneReading = false,
.numMotors = 3,
.motorIds = {10, 11, 12},//7, 8, 9},
.protocol = {2, 2, 2} //{1, 1, 1}
.motorIds = {7, 8, 9}, // left leg
.protocol = {2, 2, 2}
};

// port5 => UART5 ==> J6 on new PCB
Expand All @@ -154,8 +154,8 @@ void init_ports() {
.dirPinNum = USART5_DIR_Pin,
.dmaDoneReading = false,
.numMotors = 3,
.motorIds = {10, 11, 12}, //{4, 5, 6},
.protocol = {2,2,2}//{2, 2, 2}
.motorIds = {10, 11, 12}, // right hip
.protocol = {2,2,2}
};

// port6 => UART6 ==> J11 on new PCB
Expand All @@ -167,8 +167,8 @@ void init_ports() {
.dirPinNum = USART6_DIR_Pin,
.dmaDoneReading = false,
.numMotors = 3,
.motorIds = {10, 11, 12}, //{10, 11, 12},
.protocol = {2, 2, 2} //{2, 2, 2}
.motorIds = {13, 14, 15}, // right leg
.protocol = {2, 2, 2}
};

motorPorts[0] = &port1;
Expand Down
32 changes: 16 additions & 16 deletions soccer_firmware/Core/Src/update_loop.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@ BMI088 imu;

void update()
{
uint8_t txBuf[2 + 18 * 2 + 6 + 6] = {0}; // 18 motors * 2 bytes each + 6 bytes for IMU
uint8_t txBuf[2 + 20 * 2 + 6 + 6] = {0}; // 20 motors * 2 bytes each + 6 bytes for IMU
txBuf[0] = txBuf[1] = 0xfe;
uint32_t lastTime = HAL_GetTick();
while(1)
Expand Down Expand Up @@ -62,8 +62,8 @@ void set_robot_motor_limits() {

// use index as motorID!!
// 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19
uint32_t minLimits[20] = {1510, 1050, 2000, 2000, 1213, 1536, 655, 760, 1610, 1825, 1100, 1550, 1720, 2057, 920, 1870, 0, 0, 0, 1060};
uint32_t maxLimits[20] = {2400, 3100, 2945, 4096, 3155, 2569, 2373, 2070, 3160, 2260, 3060, 2550, 3560, 3300, 2580, 2245, 0, 0, 3000, 4096};
uint32_t minLimits[20] = {1510, 1050, 2000, 2000, 1213, 1536, 1650, 760, 1610, 1825, 1100, 1550, 1720, 2057, 920, 1870, 0, 0, 0, 1060};
uint32_t maxLimits[20] = {2400, 3100, 2945, 4096, 3155, 2569, 3230, 2070, 3160, 2260, 3060, 2550, 3560, 3300, 2580, 2245, 0, 0, 3000, 4096};

for (uint16_t i = 0; i < 6; i++) {// reset variables
motorPorts[i]->currMotor = 0;
Expand Down Expand Up @@ -156,19 +156,19 @@ void read_imu(uint8_t *rxBuf) {
// Read gyro after accel, otherwise we get HAL_error (not sure why :/)
BMI088_ReadGyroscope(&hi2c1, &gyroX, &gyroY, &gyroZ);

rxBuf[2 + 18 * 2 + 0] = (accX >> 8) & 0xFF; // MSB first means big endian
rxBuf[2 + 18 * 2 + 1] = accX & 0xFF;
rxBuf[2 + 18 * 2 + 2] = (accY >> 8) & 0xFF;
rxBuf[2 + 18 * 2 + 3] = accY & 0xFF;
rxBuf[2 + 18 * 2 + 4] = (accZ >> 8) & 0xFF;
rxBuf[2 + 18 * 2 + 5] = accZ & 0xFF;

rxBuf[2 + 18 * 2 + 6 + 0] = (gyroX >> 8) & 0xFF;
rxBuf[2 + 18 * 2 + 6 + 1] = gyroX & 0xFF;
rxBuf[2 + 18 * 2 + 6 + 2] = (gyroY >> 8) & 0xFF;
rxBuf[2 + 18 * 2 + 6 + 3] = gyroY & 0xFF;
rxBuf[2 + 18 * 2 + 6 + 4] = (gyroZ >> 8) & 0xFF;
rxBuf[2 + 18 * 2 + 6 + 5] = gyroZ & 0xFF;
rxBuf[2 + 20 * 2 + 0] = (accX >> 8) & 0xFF; // MSB first means big endian
rxBuf[2 + 20 * 2 + 1] = accX & 0xFF;
rxBuf[2 + 20 * 2 + 2] = (accY >> 8) & 0xFF;
rxBuf[2 + 20 * 2 + 3] = accY & 0xFF;
rxBuf[2 + 20 * 2 + 4] = (accZ >> 8) & 0xFF;
rxBuf[2 + 20 * 2 + 5] = accZ & 0xFF;

rxBuf[2 + 20 * 2 + 6 + 0] = (gyroX >> 8) & 0xFF;
rxBuf[2 + 20 * 2 + 6 + 1] = gyroX & 0xFF;
rxBuf[2 + 20 * 2 + 6 + 2] = (gyroY >> 8) & 0xFF;
rxBuf[2 + 20 * 2 + 6 + 3] = gyroY & 0xFF;
rxBuf[2 + 20 * 2 + 6 + 4] = (gyroZ >> 8) & 0xFF;
rxBuf[2 + 20 * 2 + 6 + 5] = gyroZ & 0xFF;

}

Expand Down
12 changes: 6 additions & 6 deletions soccer_hardware/soccer_firmware_interface/config/bez2.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,7 @@ left_leg_motor_2:
left_leg_motor_3:
id: 7
type: "mx64"
angle_zero: 180
angle_zero: 270
angle_max: 360
angle_min: 0

Expand Down Expand Up @@ -68,7 +68,7 @@ right_leg_motor_2:
right_leg_motor_3:
id: 13
type: "mx64"
angle_zero: 180
angle_zero: 90
angle_max: 360
angle_min: 0
flipped: "true"
Expand All @@ -89,26 +89,27 @@ right_leg_motor_5:
angle_min: 0

left_arm_motor_0:
id: 0
id: 18
type: "mx28"
angle_zero: 180
angle_max: 360
angle_min: 0
flipped: "true"

left_arm_motor_1:
id: 1
id: 0
type: "mx28"
angle_zero: 180
angle_max: 360
angle_min: 0

left_arm_motor_2:
id: 18
id: 1
type: "mx28"
angle_zero: 180
angle_max: 360
angle_min: 0
flipped: "true"

right_arm_motor_0: # Checked
id: 19
Expand All @@ -131,7 +132,6 @@ right_arm_motor_2:
angle_zero: 180
angle_max: 360
angle_min: 0
flipped: "true"

head_motor_0:
id: 16
Expand Down
11 changes: 6 additions & 5 deletions ...er_hardware/soccer_firmware_interface/src/soccer_firmware_interface/firmware_interface.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -79,12 +79,12 @@ def firmware_update_loop(self):
# data_h = self.serial.read()
# angle = data_l[0] | (data_h[0] << 8)
# print(data_h[0], data_l[0], angle)
data = self.serial.read(size=2 + 2 * 18 + 12)
data = self.serial.read(size=2 + 2 * 20 + 12)

# Publish the Joint State
j = JointState()
j.header.stamp = rospy.Time.now()
for i in range(18):
for i in range(20):
val = data[i * 2 + 2] | data[i * 2 + 3] << 8

motor_name = self.motor_id_to_name_dict[i]
Expand All @@ -111,7 +111,7 @@ def firmware_update_loop(self):
imu = Imu()
imu.header.stamp = rospy.Time.now()

imu_data = data[2 + 2 * 18 : 2 + 2 * 18 + 12]
imu_data = data[2 + 2 * 20 : 2 + 2 * 20 + 12]

# https://www.mouser.com/datasheet/2/783/BST_BMI088_DS001-1509549.pdf
ACC_RANGE = 32768.0 / 2.0 / 1.5 # page 27 datasheet bmi088
Expand Down Expand Up @@ -165,7 +165,7 @@ def joint_command_callback(self, joint_state: JointState):
self.reconnect_serial_port()

# [0xff, 0xff, angle1_lo, angle1_hi, angle2_lo ..., crc_lo, crc_hi]
bytes_to_write = [0x0] * (2 + 18 * 2)
bytes_to_write = [0x0] * (2 + 20 * 2)
bytes_to_write[0] = 0xFF
bytes_to_write[1] = 0xFF

Expand All @@ -175,6 +175,7 @@ def joint_command_callback(self, joint_state: JointState):
angle_zero = self.motor_mapping[name]["angle_zero"] / 180 * math.pi
angle_max = self.motor_mapping[name]["angle_max"] / 180 * math.pi
angle_min = self.motor_mapping[name]["angle_min"] / 180 * math.pi
ang_zero_deg = self.motor_mapping[name]["angle_zero"]

flipped = "flipped" in self.motor_mapping[name] and self.motor_mapping[name]["flipped"] == "true"
if flipped:
Expand All @@ -191,7 +192,7 @@ def joint_command_callback(self, joint_state: JointState):
angle_final_bytes_1 = angle_final_bytes & 0xFF
angle_final_bytes_2 = (angle_final_bytes >> 8) & 0xFF

assert id < 18
assert id < 20
bytes_to_write[2 + id * 2] = angle_final_bytes_1
bytes_to_write[2 + id * 2 + 1] = angle_final_bytes_2
pass
Expand Down
13 changes: 10 additions & 3 deletions ...rdware/soccer_firmware_interface/src/soccer_firmware_interface/test_firmware_interface.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,6 +57,8 @@ def test_firmware_interface():
for i in range(50000):
j = JointState()
j.name = [
"left_arm_motor_2",
"right_arm_motor_2",
"head_motor_0",
"head_motor_1",
"left_arm_motor_0",
Expand All @@ -77,9 +79,14 @@ def test_firmware_interface():
"right_leg_motor_5",
]
# j.position = [math.sin(i / 180 * math.pi) * 0.1, math.cos(i / 180 * math.pi) * 0.1]
ang = math.sin(i / 30 * math.pi) * 0.2
# ang = 0.0
j.position = [ang] * 18

ang = 0.0
ang = abs(math.sin(i / 180 * math.pi) * 0.3)
j.position = [ang] * 20

# j.position[0] = ang
# j.position[1] = ang

j.header.stamp = rospy.Time.now()

f.joint_command_callback(j)
Expand Down

0 comments on commit 99a2723

Please sign in to comment.