This page details the complete control stack of the MARS robot. From the Closed-Loop Driving Controller that tracks trajectories, to the Manipulation State Machine that orchestrates complex arm movements, and finally the PWM Gripper Control for physical interaction.
The robot's base is a differential drive system. To achieve smooth, human-like motion, we do not simply send "go" commands. Instead, we implemented a Closed-Loop Controller running at 20Hz.
This controller continuously compares the robot's current heading
(from Odometry/TF) against the desired Bézier waypoint. It
calculates error and outputs precise velocity commands (Twist
messages) to the /cmd_vel topic. We also implemented a
Low-Pass Filter on the angular velocity to prevent
wheel slip and mechanical oscillation.
# From move_to_object.py: Control Loop
def control_loop(self):
"""Executes at 20Hz to drive the robot"""
# 1. Get current pose from TF
pose = self.get_current_pose()
# 2. Calculate Heading Error to target waypoint
angle_to_target = math.atan2(dy, dx)
angle_error = self.normalize_angle(angle_to_target - pose.yaw)
# 3. Apply Low-Pass Filter (Smooths the turn)
raw_angular_vel = self.kp_angular * angle_error
self.filtered_angular_vel = (self.alpha * raw_angular_vel) + ((1 - self.alpha) * self.filtered_angular_vel)
# 4. Publish Velocity
twist = Twist()
twist.linear.x = linear_vel
twist.angular.z = self.filtered_angular_vel
self.cmd_vel_pub.publish(twist)Unlike the base, the arm requires discrete logic states to operate safely. We cannot simply "move to the ball" because the arm might collide with the object or block the camera view. We implemented a Finite State Machine (FSM) to strictly enforce the sequence of operations.
The system transitions through specific states: first moving to an Overhead position to verify the object location, then descending vertically to Grasp, and finally retracting to a Home pose.
# From arm_movement.py: State Machine Definitions
class GraspState(Enum):
IDLE = 0
MOVE_TO_OVERHEAD = 2 # Lift arm to clear camera view
POSITION_ABOVE_OBJECT = 6 # Align XY coordinates
LOWER_TO_OBJECT = 8 # Vertical Z descent
CLOSE_GRIPPER = 10 # Actuate end-effector
RETURN_TO_REST = 11 # Safe transport pose
def update_arm_state(self):
""" Transitions states based on motion completion flags """
if self.state == GraspState.MOVE_TO_OVERHEAD and self.motion_complete:
self.state = GraspState.POSITION_ABOVE_OBJECT
self.motion_complete = FalseTo move the arm to a specific 3D coordinate (x, y, z), we must calculate the required angles for all 5 joints. We use an Inverse Kinematics (IK) Solver that takes the target coordinate in the base frame and outputs the necessary joint configurations.
These joint targets are published to the
/joint_states topic, which the lower-level hardware
drivers use to move the servos.
# From arm_movement.py: Kinematics
def move_arm_to_coords(self, x, y, z):
"""Calculates joint angles for a target 3D point"""
# 1. Transform Camera Detection -> Base Frame
target_base = self.transform_camera_to_base(x, y, z)
# 2. Solve IK
joint_angles = self.ik_solver.calculate_ik(target_base.x, target_base.y, target_base.z)
# 3. Publish to hardware
msg = JointState()
msg.position = joint_angles
self.joint_pub.publish(msg)The final stage of actuation is physical interaction. The gripper is driven by a servo motor controlled via Pulse Width Modulation (PWM) signals. We determined specific duty cycles corresponding to the "Open" (7.5) and "Closed" (12.5) positions to ensure a firm grasp on the paper ball.
# From arm_movement.py: Servo Control
def activate_gripper(state):
"""Direct PWM control of the gripper servo"""
if state == "OPEN":
# Neutral position (0 degrees)
pwm.set_duty_cycle(SERVO_PIN, 7.5)
elif state == "CLOSE":
# 180 degree position (Fully clamped)
pwm.set_duty_cycle(SERVO_PIN, 12.5)
# Block thread to ensure mechanical completion
time.sleep(1.0)