Mecanum Wheel Kinematics

 1     Front
 2       ↑
 3   1       2
 4    \  •  /     • = Robot center
 5    /     \
 6   4       3
 7
 8Wheel 1 (FL): Rollers at +45°
 9Wheel 2 (FR): Rollers at -45°
10Wheel 3 (RR): Rollers at +45°
11Wheel 4 (RL): Rollers at -45°

 1import numpy as np
 2
 3class MecanumRobot:
 4    def __init__(self, lx=0.2, ly=0.2, wheel_radius=0.05):
 5        """
 6        Args:
 7            lx: Half of wheelbase (m)
 8            ly: Half of track width (m)
 9            wheel_radius: Radius of wheels (m)
10        """
11        self.lx = lx
12        self.ly = ly
13        self.r = wheel_radius
14        
15        # State: [x, y, theta]
16        self.state = np.array([0.0, 0.0, 0.0])
17        
18        # Forward kinematics matrix
19        self.FK_matrix = (self.r / 4) * np.array([
20            [1,  1,  1,  1],
21            [-1, 1,  1, -1],
22            [-1/(lx+ly), 1/(lx+ly), -1/(lx+ly), 1/(lx+ly)]
23        ])
24    
25    def forward_kinematics(self, wheel_velocities, dt):
26        """
27        Compute robot velocity from wheel velocities
28        
29        Args:
30            wheel_velocities: [ω1, ω2, ω3, ω4] (rad/s)
31            dt: Time step (s)
32        
33        Returns:
34            Robot velocity [vx, vy, ω] in robot frame
35        """
36        wheel_velocities = np.array(wheel_velocities)
37        
38        # Robot velocity in robot frame
39        robot_vel = self.FK_matrix @ wheel_velocities
40        vx_robot, vy_robot, omega = robot_vel
41        
42        # Transform to world frame
43        x, y, theta = self.state
44        
45        vx_world = vx_robot * np.cos(theta) - vy_robot * np.sin(theta)
46        vy_world = vx_robot * np.sin(theta) + vy_robot * np.cos(theta)
47        
48        # Update pose
49        self.state[0] += vx_world * dt
50        self.state[1] += vy_world * dt
51        self.state[2] += omega * dt
52        
53        # Normalize angle
54        self.state[2] = np.arctan2(np.sin(self.state[2]), 
55                                   np.cos(self.state[2]))
56        
57        return self.state.copy()
58    
59    def get_robot_velocity(self, wheel_velocities):
60        """Get robot velocity without updating state"""
61        wheel_velocities = np.array(wheel_velocities)
62        return self.FK_matrix @ wheel_velocities
63
64# Example: Forward motion
65robot = MecanumRobot(lx=0.2, ly=0.2, wheel_radius=0.05)
66
67# All wheels same speed → forward motion
68for _ in range(100):
69    robot.forward_kinematics([10, 10, 10, 10], dt=0.01)
70
71print(f"Forward motion: {robot.state}")
72
73# Reset and move sideways
74robot.state = np.array([0.0, 0.0, 0.0])
75
76# Alternating wheel pattern → sideways motion
77for _ in range(100):
78    robot.forward_kinematics([-10, 10, 10, -10], dt=0.01)
79
80print(f"Sideways motion: {robot.state}")

 1def inverse_kinematics(self, vx, vy, omega):
 2    """
 3    Compute wheel velocities for desired robot motion
 4    
 5    Args:
 6        vx: Desired forward velocity (m/s) in robot frame
 7        vy: Desired sideways velocity (m/s) in robot frame
 8        omega: Desired angular velocity (rad/s)
 9    
10    Returns:
11        [ω1, ω2, ω3, ω4] wheel velocities (rad/s)
12    """
13    # Inverse kinematics matrix
14    IK_matrix = (1 / self.r) * np.array([
15        [1, -1, -(self.lx + self.ly)],
16        [1,  1,  (self.lx + self.ly)],
17        [1,  1, -(self.lx + self.ly)],
18        [1, -1,  (self.lx + self.ly)]
19    ])
20    
21    robot_vel = np.array([vx, vy, omega])
22    wheel_velocities = IK_matrix @ robot_vel
23    
24    return wheel_velocities
25
26# Add to MecanumRobot class
27MecanumRobot.inverse_kinematics = inverse_kinematics
28
29# Example usage
30robot = MecanumRobot()
31
32# Forward motion
33wheels = robot.inverse_kinematics(vx=0.5, vy=0, omega=0)
34print(f"Forward: {wheels}")
35
36# Sideways motion
37wheels = robot.inverse_kinematics(vx=0, vy=0.5, omega=0)
38print(f"Sideways: {wheels}")
39
40# Diagonal motion
41wheels = robot.inverse_kinematics(vx=0.5, vy=0.5, omega=0)
42print(f"Diagonal: {wheels}")
43
44# Rotation in place
45wheels = robot.inverse_kinematics(vx=0, vy=0, omega=1.0)
46print(f"Rotation: {wheels}")
47
48# Combined motion
49wheels = robot.inverse_kinematics(vx=0.3, vy=0.2, omega=0.5)
50print(f"Combined: {wheels}")

 1def apply_velocity_limits(self, wheel_velocities, omega_max=20.0):
 2    """
 3    Scale wheel velocities to respect limits while preserving motion direction
 4    
 5    Args:
 6        wheel_velocities: Desired wheel velocities [ω1, ω2, ω3, ω4]
 7        omega_max: Maximum wheel velocity (rad/s)
 8    
 9    Returns:
10        Limited wheel velocities
11    """
12    wheel_velocities = np.array(wheel_velocities)
13    
14    # Find maximum absolute velocity
15    max_vel = np.max(np.abs(wheel_velocities))
16    
17    if max_vel > omega_max:
18        # Scale all velocities proportionally
19        scale = omega_max / max_vel
20        wheel_velocities *= scale
21    
22    return wheel_velocities
23
24# Add to class
25MecanumRobot.apply_velocity_limits = apply_velocity_limits
26
27# Example
28robot = MecanumRobot()
29wheels = robot.inverse_kinematics(vx=2.0, vy=2.0, omega=5.0)
30print(f"Unlimited: {wheels}")
31
32wheels_limited = robot.apply_velocity_limits(wheels, omega_max=15.0)
33print(f"Limited: {wheels_limited}")

 1def velocity_control(self, vx_desired, vy_desired, omega_desired, 
 2                     world_frame=False):
 3    """
 4    Control robot to achieve desired velocity
 5    
 6    Args:
 7        vx_desired: Desired x velocity (m/s)
 8        vy_desired: Desired y velocity (m/s)
 9        omega_desired: Desired angular velocity (rad/s)
10        world_frame: If True, velocities are in world frame
11    
12    Returns:
13        Wheel velocities [ω1, ω2, ω3, ω4]
14    """
15    if world_frame:
16        # Transform to robot frame
17        theta = self.state[2]
18        vx_robot = vx_desired * np.cos(theta) + vy_desired * np.sin(theta)
19        vy_robot = -vx_desired * np.sin(theta) + vy_desired * np.cos(theta)
20    else:
21        vx_robot = vx_desired
22        vy_robot = vy_desired
23    
24    # Inverse kinematics
25    wheel_velocities = self.inverse_kinematics(vx_robot, vy_robot, 
26                                               omega_desired)
27    
28    # Apply limits
29    wheel_velocities = self.apply_velocity_limits(wheel_velocities)
30    
31    return wheel_velocities
32
33# Add to class
34MecanumRobot.velocity_control = velocity_control

 1def navigate_to_point(self, target_x, target_y, target_theta=None,
 2                     kp_linear=1.0, kp_angular=2.0):
 3    """
 4    Navigate to target position
 5    
 6    Args:
 7        target_x, target_y: Target position (world frame)
 8        target_theta: Target orientation (None = don't care)
 9        kp_linear: Linear velocity gain
10        kp_angular: Angular velocity gain
11    
12    Returns:
13        Wheel velocities and distance to target
14    """
15    x, y, theta = self.state
16    
17    # Position error (world frame)
18    dx_world = target_x - x
19    dy_world = target_y - y
20    distance = np.sqrt(dx_world**2 + dy_world**2)
21    
22    # Transform to robot frame
23    dx_robot = dx_world * np.cos(theta) + dy_world * np.sin(theta)
24    dy_robot = -dx_world * np.sin(theta) + dy_world * np.cos(theta)
25    
26    # Desired velocities (robot frame)
27    vx_desired = kp_linear * dx_robot
28    vy_desired = kp_linear * dy_robot
29    
30    # Orientation control
31    if target_theta is not None:
32        theta_error = np.arctan2(np.sin(target_theta - theta),
33                                np.cos(target_theta - theta))
34        omega_desired = kp_angular * theta_error
35    else:
36        omega_desired = 0
37    
38    # Velocity limits
39    v_max = 1.0  # m/s
40    omega_max_robot = 2.0  # rad/s
41    
42    vx_desired = np.clip(vx_desired, -v_max, v_max)
43    vy_desired = np.clip(vy_desired, -v_max, v_max)
44    omega_desired = np.clip(omega_desired, -omega_max_robot, omega_max_robot)
45    
46    # Inverse kinematics
47    wheel_velocities = self.inverse_kinematics(vx_desired, vy_desired, 
48                                               omega_desired)
49    wheel_velocities = self.apply_velocity_limits(wheel_velocities)
50    
51    return wheel_velocities, distance
52
53# Add to class
54MecanumRobot.navigate_to_point = navigate_to_point
55
56# Example: Navigate to point
57robot = MecanumRobot()
58target = (2.0, 1.5, np.pi/4)
59
60dt = 0.05
61trajectory = [robot.state.copy()]
62
63for _ in range(500):
64    wheels, dist = robot.navigate_to_point(*target)
65    
66    if dist < 0.05:
67        break
68    
69    robot.forward_kinematics(wheels, dt)
70    trajectory.append(robot.state.copy())
71
72print(f"Final position: {robot.state}")
73print(f"Distance to target: {dist:.4f} m")

 1class MecanumMotionPrimitives:
 2    @staticmethod
 3    def forward(distance, speed=0.5):
 4        """Move forward"""
 5        return {
 6            'vx': speed,
 7            'vy': 0,
 8            'omega': 0,
 9            'duration': distance / speed
10        }
11    
12    @staticmethod
13    def sideways(distance, speed=0.5):
14        """Move sideways (positive = right)"""
15        direction = 1 if distance > 0 else -1
16        return {
17            'vx': 0,
18            'vy': direction * speed,
19            'omega': 0,
20            'duration': abs(distance) / speed
21        }
22    
23    @staticmethod
24    def diagonal(distance_x, distance_y, speed=0.5):
25        """Move diagonally"""
26        distance = np.sqrt(distance_x**2 + distance_y**2)
27        duration = distance / speed
28        
29        vx = distance_x / duration
30        vy = distance_y / duration
31        
32        return {
33            'vx': vx,
34            'vy': vy,
35            'omega': 0,
36            'duration': duration
37        }
38    
39    @staticmethod
40    def rotate(angle, angular_speed=1.0):
41        """Rotate in place"""
42        duration = abs(angle) / angular_speed
43        omega = angular_speed if angle > 0 else -angular_speed
44        
45        return {
46            'vx': 0,
47            'vy': 0,
48            'omega': omega,
49            'duration': duration
50        }
51    
52    @staticmethod
53    def strafe_arc(radius, angle, speed=0.5):
54        """Move in arc while maintaining orientation"""
55        arc_length = radius * abs(angle)
56        duration = arc_length / speed
57        
58        # Velocity perpendicular to radius
59        vx = speed * np.cos(angle / 2)
60        vy = speed * np.sin(angle / 2) * (1 if angle > 0 else -1)
61        
62        return {
63            'vx': vx,
64            'vy': vy,
65            'omega': 0,
66            'duration': duration
67        }
68
69# Example: Execute sequence
70robot = MecanumRobot()
71
72sequence = [
73    MecanumMotionPrimitives.forward(1.0, 0.5),
74    MecanumMotionPrimitives.sideways(0.5, 0.3),
75    MecanumMotionPrimitives.diagonal(0.5, 0.5, 0.4),
76    MecanumMotionPrimitives.rotate(np.pi/2, 1.0)
77]
78
79dt = 0.01
80for primitive in sequence:
81    t = 0
82    while t < primitive['duration']:
83        wheels = robot.inverse_kinematics(
84            primitive['vx'], primitive['vy'], primitive['omega']
85        )
86        robot.forward_kinematics(wheels, dt)
87        t += dt
88
89print(f"Final pose: {robot.state}")

 1class MecanumRobotWithSlippage(MecanumRobot):
 2    def __init__(self, lx=0.2, ly=0.2, wheel_radius=0.05,
 3                 slip_factor_forward=0.95,
 4                 slip_factor_sideways=0.85):
 5        """
 6        Args:
 7            slip_factor_forward: Efficiency in forward direction (0-1)
 8            slip_factor_sideways: Efficiency in sideways direction (0-1)
 9        """
10        super().__init__(lx, ly, wheel_radius)
11        self.slip_forward = slip_factor_forward
12        self.slip_sideways = slip_factor_sideways
13    
14    def compensate_slippage(self, vx, vy, omega):
15        """Compensate for expected slippage"""
16        vx_compensated = vx / self.slip_forward
17        vy_compensated = vy / self.slip_sideways
18        
19        return vx_compensated, vy_compensated, omega
20    
21    def inverse_kinematics_compensated(self, vx, vy, omega):
22        """IK with slippage compensation"""
23        vx_comp, vy_comp, omega_comp = self.compensate_slippage(vx, vy, omega)
24        return self.inverse_kinematics(vx_comp, vy_comp, omega_comp)
25
26# Example
27robot = MecanumRobotWithSlippage(slip_factor_sideways=0.8)
28wheels = robot.inverse_kinematics_compensated(vx=0.5, vy=0.5, omega=0)
29print(f"Compensated wheel velocities: {wheels}")

 1def simulate_and_plot(robot, control_sequence, dt=0.01):
 2    """Simulate and visualize mecanum robot motion"""
 3    import matplotlib.pyplot as plt
 4    
 5    trajectory = [robot.state.copy()]
 6    
 7    for cmd in control_sequence:
 8        t = 0
 9        while t < cmd['duration']:
10            wheels = robot.inverse_kinematics(cmd['vx'], cmd['vy'], cmd['omega'])
11            robot.forward_kinematics(wheels, dt)
12            trajectory.append(robot.state.copy())
13            t += dt
14    
15    trajectory = np.array(trajectory)
16    
17    # Plot
18    fig, ax = plt.subplots(figsize=(10, 10))
19    
20    # Trajectory
21    ax.plot(trajectory[:, 0], trajectory[:, 1], 'b-', linewidth=2, label='Path')
22    ax.plot(trajectory[0, 0], trajectory[0, 1], 'go', markersize=12, label='Start')
23    ax.plot(trajectory[-1, 0], trajectory[-1, 1], 'ro', markersize=12, label='End')
24    
25    # Robot orientation at intervals
26    interval = len(trajectory) // 15
27    for i in range(0, len(trajectory), interval):
28        x, y, theta = trajectory[i]
29        
30        # Draw robot body
31        size = 0.15
32        corners = np.array([
33            [size, size],
34            [-size, size],
35            [-size, -size],
36            [size, -size],
37            [size, size]
38        ])
39        
40        # Rotate corners
41        rot = np.array([
42            [np.cos(theta), -np.sin(theta)],
43            [np.sin(theta), np.cos(theta)]
44        ])
45        corners_rot = (rot @ corners.T).T
46        corners_rot[:, 0] += x
47        corners_rot[:, 1] += y
48        
49        ax.plot(corners_rot[:, 0], corners_rot[:, 1], 'k-', linewidth=1)
50        
51        # Direction arrow
52        dx = 0.1 * np.cos(theta)
53        dy = 0.1 * np.sin(theta)
54        ax.arrow(x, y, dx, dy, head_width=0.05, head_length=0.05, 
55                fc='red', ec='red')
56    
57    ax.grid(True)
58    ax.axis('equal')
59    ax.set_xlabel('X (m)')
60    ax.set_ylabel('Y (m)')
61    ax.legend()
62    ax.set_title('Mecanum Robot Trajectory')
63    plt.show()
64
65# Example: Complex maneuver
66robot = MecanumRobot()
67
68maneuver = [
69    {'vx': 0.5, 'vy': 0, 'omega': 0, 'duration': 2.0},      # Forward
70    {'vx': 0, 'vy': 0.5, 'omega': 0, 'duration': 2.0},      # Right
71    {'vx': 0.5, 'vy': 0.5, 'omega': 0, 'duration': 2.0},    # Diagonal
72    {'vx': 0, 'vy': 0, 'omega': 1.0, 'duration': np.pi},    # Rotate 180°
73    {'vx': 0.5, 'vy': -0.5, 'omega': 0.5, 'duration': 3.0}  # Combined
74]
75
76simulate_and_plot(robot, maneuver)