@shalinj The robot needs to be controlled via Xbox Series X controller without falling. However when running the Xbox teleoperation script (see below) then starting the simulation, the robot tilts forward and falls despite presence of self-balancing controller. Also Xbox controller input fails to articulate some joints.
P.S. The robot cannot be fixed to the ground as it needs to be able to navigate the environment.
xbox_teleop.py:
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import JointState
import pygame
import time
JOINT_MODES = {
'legs': [
"l_hip_yaw", "l_hip_pitch", "l_knee_pitch", "l_ankle_pitch", "l_ankle_roll",
"r_hip_yaw", "r_hip_pitch", "r_knee_pitch", "r_ankle_pitch", "r_ankle_roll"
],
'arms': [
"l_shoulder_pitch", "l_elbow_pitch", "l_wrist_pitch",
"r_shoulder_pitch", "r_elbow_pitch", "r_wrist_pitch"
],
'hands': [
"L_index_proximal_joint", "L_thumb_proximal_yaw_joint", "L_thumb_proximal_pitch_joint",
"R_index_proximal_joint", "R_thumb_proximal_yaw_joint", "R_thumb_proximal_pitch_joint"
]
}
# Adjusted INITIAL_POSE for a more neutral stable stance
INITIAL_POSE = {
"l_hip_pitch": 0.05, # Reduced backward tilt
"r_hip_pitch": 0.05,
"l_knee_pitch": 0.4, # Moderate knee bend
"r_knee_pitch": 0.4,
"l_ankle_pitch": -0.05, # Reduced forward ankle tilt
"r_ankle_pitch": -0.05,
"l_ankle_roll": 0.0,
"r_ankle_roll": 0.0,
"l_hip_yaw": 0.0,
"r_hip_yaw": 0.0,
"joint_waist_pitch": 0.0,
"joint_waist_yaw": 0.0,
"joint_waist_roll": 0.0,
"joint_head_pitch": 0.0,
"joint_head_yaw": 0.0,
"joint_head_roll": 0.0,
"l_shoulder_pitch": 0.2,
"r_shoulder_pitch": 0.2,
"l_elbow_pitch": 0.3,
"r_elbow_pitch": 0.3,
"l_wrist_pitch": 0.0,
"r_wrist_pitch": 0.0,
"l_wrist_roll": 0.0,
"r_wrist_roll": 0.0,
"l_wrist_yaw": 0.0,
"r_wrist_yaw": 0.0
}
class XboxTeleopInitPose(Node):
def __init__(self):
super().__init__('xbox_teleop_with_pose')
self.publisher = self.create_publisher(JointState, '/joint_command', 10)
try:
self.get_logger().set_level(10)
self.get_logger().info("Debug logging enabled.")
except AttributeError:
self.get_logger().info("Debug logging not supported; using default level.")
pygame.init()
pygame.joystick.init()
if pygame.joystick.get_count() > 0:
self.joystick = pygame.joystick.Joystick(0)
self.joystick.init()
self.get_logger().info(f"Controller detected: {self.joystick.get_name()}, Axes: {self.joystick.get_numaxes()}, Buttons: {self.joystick.get_numbuttons()}")
else:
self.get_logger().error("No controller detected.")
return
self.joint_positions = INITIAL_POSE.copy()
for mode_joints in JOINT_MODES.values():
for joint in mode_joints:
if joint not in self.joint_positions:
self.joint_positions[joint] = 0.0
self.mode = 'legs'
self.limb = 'left'
self.current_joint_index = 0
# Adjusted self-balancing controller parameters
self.target_pitch = 0.0
self.kp = 15.0 # Reduced proportional gain
self.kd = 2.0 # Reduced derivative gain
self.prev_error = 0.0
self.last_time = time.time()
time.sleep(2.0)
self.send_initial_pose()
self.timer = self.create_timer(0.01, self.update) # Increased to 100 Hz
def send_initial_pose(self):
joint_msg = JointState()
joint_msg.name = list(self.joint_positions.keys())
joint_msg.position = list(self.joint_positions.values())
for i in range(200): # Increased to 200 iterations (20 seconds)
self.publisher.publish(joint_msg)
time.sleep(0.1)
self.get_logger().info(f"Initial pose published, iteration {i+1}/200")
self.get_logger().info("Initial stable pose published and settled.")
def get_active_joints(self):
prefix = self.limb[0]
return [j for j in JOINT_MODES.get(self.mode, []) if j.startswith(prefix)]
def update(self):
if not hasattr(self, 'joystick') or not self.joystick:
return
pygame.event.pump()
axes = [self.joystick.get_axis(i) for i in range(self.joystick.get_numaxes())]
buttons = [self.joystick.get_button(i) for i in range(self.joystick.get_numbuttons())]
hats = self.joystick.get_hat(0)
self.get_logger().info(f"Axes: {axes}, Buttons: {buttons}, Hats: {hats}")
# Mode and limb switching
if hats[1] == 1: self.mode = 'arms'
elif hats[1] == -1: self.mode = 'legs'
elif hats[1] == 0 and hats[0] == 0: self.mode = 'hands'
if hats[0] == 1: self.limb = 'right'
elif hats[0] == -1: self.limb = 'left'
active_joints = self.get_active_joints()
if not active_joints:
self.get_logger().warning("No active joints for current mode/limb.")
return
if buttons[4]: self.current_joint_index = (self.current_joint_index - 1) % len(active_joints) # L1
if buttons[5]: self.current_joint_index = (self.current_joint_index + 1) % len(active_joints) # R1
selected_joint = active_joints[self.current_joint_index]
# Xbox control
delta_scale = 0.1
left_y = axes[1] if abs(axes[1]) > 0.1 else 0.0
right_x = axes[3] if abs(axes[3]) > 0.1 else 0.0
if left_y != 0.0 or right_x != 0.0:
self.joint_positions[selected_joint] += delta_scale * -left_y
if self.mode == 'legs' and 'yaw' in selected_joint:
self.joint_positions[selected_joint] += delta_scale * right_x
delta_fine = 0.02
l2 = axes[4] if abs(axes[4]) > 0.5 else 0.0
r2 = axes[5] if abs(axes[5]) > 0.5 else 0.0
if l2 < 0.0:
self.joint_positions[selected_joint] -= delta_fine
if r2 > 0.0:
self.joint_positions[selected_joint] += delta_fine
if self.mode == 'hands':
if buttons[0]: # A
for j in active_joints:
self.joint_positions[j] = min(self.joint_positions[j] + 0.1, 0.5)
if buttons[1]: # B
for j in active_joints:
self.joint_positions[j] = max(self.joint_positions[j] - 0.1, -0.5)
if buttons[2]: # X
for j in active_joints:
self.joint_positions[j] = 0.0
if buttons[3]: # Y
for j in active_joints:
self.joint_positions[j] = 0.3
# Enhanced self-balancing controller with debug logging
current_time = time.time()
dt = current_time - self.last_time
if dt > 0.0:
# Estimate tilt from average hip pitch
hip_pitch = (self.joint_positions["l_hip_pitch"] + self.joint_positions["r_hip_pitch"]) / 2.0
tilt_error = self.target_pitch - hip_pitch # Positive = tilting backward
d_error = (tilt_error - self.prev_error) / dt
# PD controller
correction = self.kp * tilt_error + self.kd * d_error
# Apply symmetric correction
for joint in ["l_ankle_pitch", "r_ankle_pitch", "l_hip_pitch", "r_hip_pitch"]:
if joint in self.joint_positions:
self.joint_positions[joint] += correction * 0.015 # Reduced scale
self.joint_positions[joint] = max(min(self.joint_positions[joint], 0.5), -0.5)
self.prev_error = tilt_error
self.last_time = current_time
self.get_logger().info(f"Balancing: Tilt Error={tilt_error:.3f}, Correction={correction:.3f}, KP={self.kp:.1f}, KD={self.kd:.1f}")
# Clamp joint positions
for joint in self.joint_positions:
self.joint_positions[joint] = max(min(self.joint_positions[joint], 0.5), -0.5)
# Publish joint commands
joint_msg = JointState()
joint_msg.name = list(self.joint_positions.keys())
joint_msg.position = list(self.joint_positions.values())
self.publisher.publish(joint_msg)
self.get_logger().info(f"Published {selected_joint}: {self.joint_positions[selected_joint]:.2f}")
self.get_logger().info(f"[{self.mode.upper()} - {self.limb.upper()}] Joint: {selected_joint}, Pos: {self.joint_positions[selected_joint]:.2f}")
def main(args=None):
rclpy.init(args=args)
node = XboxTeleopInitPose()
try:
rclpy.spin(node)
except KeyboardInterrupt:
node.get_logger().info("Shutting down")
pygame.quit()
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()