Add Smooth Sinusoidal Interpolation for Natural Robot Motion

pollen_goto/README.md

@@ -14,11 +14,28 @@ Exposes 3 action servers:
   => This can be used to obtain continuous movements between gotos by dynamically adding a goto request whose start state matches the end state of the previous goto 
 
 Options:
-- Interpolation methods: linear or minimum jerk.
+- Interpolation methods: linear, minimum jerk, or sinusoidal
 - Interpolation frequency: the server will apply the frequency requested by the client.
 
 ## Examples
 Python examples on different ways to make client calls [here](./pollen_goto/goto_action_client.py)
+
+### Using Sinusoidal Interpolation
+```python
+from pollen_msgs.action import Goto
+from pollen_goto.interpolation import JointSpaceInterpolationMode
+
+# Create a goto request
+goto_request = Goto.Request()
+goto_request.interpolation_mode = JointSpaceInterpolationMode.SINUSOIDAL_FUNC
+goto_request.duration = 2.0  # 2 seconds duration
+goto_request.joints = ["r_shoulder_pitch", "r_shoulder_roll"]
+goto_request.positions = [0.5, 0.3]  # Target positions
+
+# Send the request to the action server
+# This will create a smooth, natural-looking motion
+```
+
 To run the server:
 ```
 ros2 run pollen_goto goto_server

pollen_goto/pollen_goto/interpolation.py

@@ -3,6 +3,7 @@
 Provides two main interpolation methods:
 - linear
 - minimum jerk
+- sinusoidal
 """
 
 from enum import Enum
@@ -84,6 +85,51 @@ def f(t: float) -> np.ndarray:
     return f
 
 
+def sinusoidal(
+    starting_position: np.ndarray,
+    goal_position: np.ndarray,
+    duration: float,
+    starting_velocity: Optional[np.ndarray] = None,
+    starting_acceleration: Optional[np.ndarray] = None,
+    final_velocity: Optional[np.ndarray] = None,
+    final_acceleration: Optional[np.ndarray] = None,
+) -> InterpolationFunc:
+    """Compute the sinusoidal interpolation function from starting position to goal position.
+
+    This method creates a smooth trajectory using a sinusoidal function that:
+    - Starts and ends with zero velocity
+    - Provides smooth acceleration and deceleration
+    - Is simple to compute and understand
+
+    Args:
+        starting_position: Initial position
+        goal_position: Final position
+        duration: Total duration of the trajectory
+        starting_velocity: Not used in this implementation
+        starting_acceleration: Not used in this implementation
+        final_velocity: Not used in this implementation
+        final_acceleration: Not used in this implementation
+
+    Returns:
+        A function that takes time as input and returns the interpolated position
+    """
+    def f(t: float) -> np.ndarray:
+        if t > duration:
+            return goal_position
+
+        # Normalize time to [0, 1]
+        t_norm = t / duration
+
+        # Sinusoidal interpolation: 0.5 - 0.5 * cos(π * t)
+        # This gives us a smooth S-curve from 0 to 1
+        progress = 0.5 - 0.5 * np.cos(np.pi * t_norm)
+
+        # Interpolate between start and goal positions
+        return starting_position + (goal_position - starting_position) * progress
+
+    return f
+
+
 def cartesian_linear(
     starting_pose: np.ndarray,
     goal_pose: PoseStamped,
@@ -220,6 +266,7 @@ class JointSpaceInterpolationMode(Enum):
 
     LINEAR_FUNC: Callable[[np.ndarray, np.ndarray, float], InterpolationFunc] = linear
     MINIMUM_JERK_FUNC: Callable[[np.ndarray, np.ndarray, float], InterpolationFunc] = minimum_jerk
+    SINUSOIDAL_FUNC: Callable[[np.ndarray, np.ndarray, float], InterpolationFunc] = sinusoidal
 
 
 class CartesianSpaceInterpolationMode(Enum):

pollen_goto/test/test_interpolation.py

@@ -0,0 +1,82 @@
+import numpy as np
+import pytest
+from pollen_goto.interpolation import sinusoidal, JointSpaceInterpolationMode
+
+def test_sinusoidal_basic():
+    """Test basic functionality of sinusoidal interpolation."""
+    # Test with single dimension
+    start_pos = np.array([0.0])
+    goal_pos = np.array([1.0])
+    duration = 1.0
+
+    # Create interpolation function
+    interp_func = sinusoidal(start_pos, goal_pos, duration)
+
+    # Test start position
+    assert np.allclose(interp_func(0.0), start_pos)
+
+    # Test end position
+    assert np.allclose(interp_func(1.0), goal_pos)
+
+    # Test middle position
+    middle = interp_func(0.5)
+    assert middle.shape == start_pos.shape
+    assert np.all(middle >= start_pos) and np.all(middle <= goal_pos)
+
+def test_sinusoidal_multi_dimension():
+    """Test sinusoidal interpolation with multiple dimensions."""
+    # Test with multiple dimensions
+    start_pos = np.array([0.0, 0.0, 0.0])
+    goal_pos = np.array([1.0, 2.0, 3.0])
+    duration = 1.0
+
+    # Create interpolation function
+    interp_func = sinusoidal(start_pos, goal_pos, duration)
+
+    # Test start position
+    assert np.allclose(interp_func(0.0), start_pos)
+
+    # Test end position
+    assert np.allclose(interp_func(1.0), goal_pos)
+
+    # Test middle position
+    middle = interp_func(0.5)
+    assert middle.shape == start_pos.shape
+    assert np.all(middle >= start_pos) and np.all(middle <= goal_pos)
+
+def test_sinusoidal_smoothness():
+    """Test that the interpolation is smooth."""
+    start_pos = np.array([0.0])
+    goal_pos = np.array([1.0])
+    duration = 1.0
+
+    # Create interpolation function
+    interp_func = sinusoidal(start_pos, goal_pos, duration)
+
+    # Test smoothness by checking multiple points
+    times = np.linspace(0, 1, 100)
+    positions = np.array([interp_func(t) for t in times])
+
+    # Check that there are no sudden jumps
+    differences = np.diff(positions, axis=0)
+    assert np.all(np.abs(differences) < 0.1)  # No sudden jumps
+
+    # Check that the path is monotonic
+    assert np.all(np.diff(positions) >= 0)  # Always increasing
+
+def test_sinusoidal_edge_cases():
+    """Test edge cases of sinusoidal interpolation."""
+    start_pos = np.array([0.0])
+    goal_pos = np.array([1.0])
+
+    # Test zero duration
+    with pytest.raises(ZeroDivisionError):
+        sinusoidal(start_pos, goal_pos, 0.0)
+
+    # Test negative duration
+    with pytest.raises(ValueError):
+        sinusoidal(start_pos, goal_pos, -1.0)
+
+    # Test different shaped arrays
+    with pytest.raises(ValueError):
+        sinusoidal(np.array([0.0, 0.0]), goal_pos, 1.0) 

pollen_goto/test/test_sinusoidal_robot.py

@@ -0,0 +1,48 @@
+import rclpy
+from rclpy.action import ActionClient
+from rclpy.node import Node
+from pollen_msgs.action import Goto
+from pollen_goto.interpolation import JointSpaceInterpolationMode
+
+class SinusoidalTest(Node):
+    def __init__(self):
+        super().__init__('sinusoidal_test')
+        self._action_client = ActionClient(self, Goto, 'r_arm_goto')
+        self.get_logger().info('Waiting for action server...')
+        self._action_client.wait_for_server()
+        self.get_logger().info('Action server found!')
+
+    def send_goal(self):
+        goal_msg = Goto.Goal()
+        goal_msg.request.interpolation_mode = JointSpaceInterpolationMode.SINUSOIDAL_FUNC
+        goal_msg.request.duration = 2.0  # 2 seconds
+        goal_msg.request.joints = ['r_shoulder_pitch', 'r_shoulder_roll']
+        goal_msg.request.positions = [0.5, 0.3]  # Target positions
+
+        self.get_logger().info('Sending goal request...')
+        self._send_goal_future = self._action_client.send_goal_async(goal_msg)
+        self._send_goal_future.add_done_callback(self.goal_response_callback)
+
+    def goal_response_callback(self, future):
+        goal_handle = future.result()
+        if not goal_handle.accepted:
+            self.get_logger().info('Goal rejected')
+            return
+
+        self.get_logger().info('Goal accepted')
+        self._get_result_future = goal_handle.get_result_async()
+        self._get_result_future.add_done_callback(self.get_result_callback)
+
+    def get_result_callback(self, future):
+        result = future.result().result
+        self.get_logger().info('Result: {0}'.format(result))
+        rclpy.shutdown()
+
+def main(args=None):
+    rclpy.init(args=args)
+    test_node = SinusoidalTest()
+    test_node.send_goal()
+    rclpy.spin(test_node)
+
+if __name__ == '__main__':
+    main() 

pollen_goto/test/test_sinusoidal_visualization.py

@@ -0,0 +1,57 @@
+import numpy as np
+import matplotlib.pyplot as plt
+from pollen_goto.interpolation import sinusoidal, linear, minimum_jerk
+
+def visualize_interpolation():
+    """Visualize and compare different interpolation methods."""
+    # Test parameters
+    start_pos = np.array([0.0])
+    goal_pos = np.array([1.0])
+    duration = 2.0  # 2 seconds
+
+    # Create interpolation functions
+    sin_func = sinusoidal(start_pos, goal_pos, duration)
+    lin_func = linear(start_pos, goal_pos, duration)
+    min_jerk_func = minimum_jerk(start_pos, goal_pos, duration)
+
+    # Generate time points
+    times = np.linspace(0, duration, 100)
+
+    # Calculate positions for each method
+    sin_positions = np.array([sin_func(t) for t in times])
+    lin_positions = np.array([lin_func(t) for t in times])
+    min_jerk_positions = np.array([min_jerk_func(t) for t in times])
+
+    # Calculate velocities (numerical differentiation)
+    sin_velocities = np.gradient(sin_positions, times)
+    lin_velocities = np.gradient(lin_positions, times)
+    min_jerk_velocities = np.gradient(min_jerk_positions, times)
+
+    # Create plots
+    fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(10, 8))
+
+    # Plot positions
+    ax1.plot(times, sin_positions, 'b-', label='Sinusoidal')
+    ax1.plot(times, lin_positions, 'r--', label='Linear')
+    ax1.plot(times, min_jerk_positions, 'g:', label='Minimum Jerk')
+    ax1.set_xlabel('Time (s)')
+    ax1.set_ylabel('Position')
+    ax1.set_title('Position vs Time')
+    ax1.grid(True)
+    ax1.legend()
+
+    # Plot velocities
+    ax2.plot(times, sin_velocities, 'b-', label='Sinusoidal')
+    ax2.plot(times, lin_velocities, 'r--', label='Linear')
+    ax2.plot(times, min_jerk_velocities, 'g:', label='Minimum Jerk')
+    ax2.set_xlabel('Time (s)')
+    ax2.set_ylabel('Velocity')
+    ax2.set_title('Velocity vs Time')
+    ax2.grid(True)
+    ax2.legend()
+
+    plt.tight_layout()
+    plt.show()
+
+if __name__ == "__main__":
+    visualize_interpolation()