stretch4_kinematics

A kinematics and task-space control library designed for the Hello Robot Stretch 4. This repository provides solvers for forward kinematics (FK) and numerical inverse kinematics (IK) using Pinocchio under the hood, and includes wrappers/controllers to easily track Cartesian poses and control the physical or simulated robot.

Development Notice

This repository is currently under development. The code in this repo may be unstable, since we are actively conducting development in this branch. Since we have performed limited testing, you may encounter unexpected behaviors. Please exercise caution when using the code in this repository.

---

Table of Contents

1. Overview & Intent
2. Key Features
3. Installation & Dependencies
4. Getting Started (Quickstart)
5. Architecture & Key APIs
6. Demos

---

1. Overview & Intent

The primary purpose of stretch4_kinematics is to bridge the gap between high-level task-space goals (e.g., placing the gripper at a specific world-frame 3D coordinate $[X, Y, Z]$ with orientation $[R, P, Y]$) and the low-level joint/velocity commands sent to the Stretch 4 hardware.

It provides:

• IK Solvers: To find valid joint configurations matching a 6-DOF target pose.
• Task-Space Controllers: To dynamically track interactive targets (such as task space "flying gripper" targets) using velocity-based differential IK control.
• Hardware Interface Wrapper: High-level wrappers to query physical state and command the physical robot safely.

---

2. Key Features & Control Architecture

Key Features

• Fast Pinocchio Backend: Leverages Pinocchio's efficient rigid body dynamics algorithm for kinematics computation and Jacobian evaluation.
• Flexible Mobile Base Movement Modes:
  • BASE_PLANAR: 3-DOF mobile base representation ($x, y, \theta$).
  • BASE_ROTATE: 1-DOF mobile base representation (rotation $\theta$ only, no translation). Used for standard localized IK.
  • BASE_FIXED: 0-DOF mobile base (fixed in place).
• Weighted Jacobian Differential IK: Resolves kinematic redundancy using weighted least-squares with automated joint-limit deceleration/repulsion.
• State Abstractions: Dataclasses that automatically translate between Stretch 4 joint configuration conventions (8-DOF vector/dictionary) and Pinocchio-native configurations (9-element vector including the $SE(2)$ base pose in $(x, y, \cos\theta, \sin\theta)$ parametrization).

Control & State Architecture

An application utilizing this library operates under one of two paradigms:

Paradigm A: Stateless Solver

The app directly instantiates a stateless Kinematic Model (e.g., StretchKinematics or ToolFrameKinematics) to compute FK/IK on the fly. The app queries the current joint state, solves the position q for a target pose or velocity q_dot for a target velocity, and commands the robot interface to move to the solved q or q_dot.

Here is the data routing and control flow for Option A:

graph TD
    App["Application (User Script)"]
    Kin["Stateless Kinematic Model<br/>(e.g., StretchKinematics)"]

    subgraph Hardware_Layer [Hardware Layer]
        IF["StretchInterface (Wrapper)"]
        RC["RobotClient (Hardware Client) or ROS2 Driver"]
    end

    %% Data Flow
    RC -->|robot state| IF
    IF -->|robot state| App
    App -->|robot state, target| Kin
    Kin -->|q or q_dot| App
    App -->|q or q_dot| IF
    IF -->|q or q_dot| RC

Paradigm B: Stateful Velocity Controller

The app instantiates a stateful Velocity Controller (e.g., FlyingGripperTrackingController) which internally handles stateful tracking (e.g., PID states, accumulated error). A controller may instantiate its own stateless Kinematic Model to evaluate differential kinematics. The controller accepts current joint states and a target pose, and computes output velocities (q_dot) which are sent to the robot interface.

Here is the data routing and control flow for Option B:

graph TD
    App["Application (User Script)"]
    Ctrl["Stateful Controller<br/>(e.g., FlyingGripperTrackingController)"]
    Kin["Stateless Kinematic Model<br/>(e.g., ToolFrameKinematics)"]

    subgraph Hardware_Layer [Hardware Layer]
        IF["StretchInterface (Wrapper)"]
        RC["RobotClient (Hardware Client) or ROS2 Driver"]
    end

    %% Data Flow
    RC -->|robot state| IF
    IF -->|robot state| App
    App -->|robot state, target| Ctrl
    Ctrl -->|v_desired| Kin
    Kin -->|q_dot| Ctrl
    Ctrl -->|q_dot| App
    App -->|q_dot| IF
    IF -->|q_dot| RC

---

3. Installation & Dependencies

Install this package in editable mode within your workspace:

uv pip install -e .

To run demos and to use the provided stretch_interface, use:

uv pip install -e ".[demos]"

---

4. Getting Started (Quickstart)

Here are two quick examples demonstrating the core functionality of the package.

Example 1: Solving 6-DOF Inverse Kinematics

This script loads the kinematic model, defines a target frame and target coordinate in the local robot frame, solves numerical IK, and outputs the resulting joint positions.

import numpy as np
from stretch4_kinematics.kinematic_models import StretchKinematics

# Initialize the solver (automatically loads Stretch 4 URDF)
solver = StretchKinematics()

# Target frame name inside the URDF
target_frame = "grasp_center_link"

# Target position (x, y, z) in meters and orientation (roll, pitch, yaw) in radians
target_xyz = np.array([0.45, -0.15, 0.9])
target_rpy = np.array([0.0, 0.0, 0.3])

# Solve numerical IK starting from the neutral robot state
solved_joints = solver.inverse_6dof_local(
    target_frame=target_frame,
    target_xyz=target_xyz,
    target_rpy=target_rpy,
    q_guess=None  # defaults to neutral pose
)

print(f"For target {target_xyz.tolist()} with roll-pitch-yaw {target_rpy} in the robot's base frame...")
print("Solved joint positions:")
solved_joints.pretty_print()

Example 2: Velocity Control

This Examples utilizes FlyingGripperVelocityController to generate joint velocities to achieve a task space (gripper frame) velocity.

import numpy as np
import time

from stretch4_body.robot.robot_client import RobotClient
from stretch4_kinematics.stretch_interface import StretchInterface
from stretch4_kinematics.controllers import FlyingGripperVelocityController

# 1. Initialize hardware client and kinematics interface
robot = RobotClient()
robot.startup()
interface = StretchInterface(robot=robot)
interface.reset_odometry_offset()

# 2. Instantiate tracking controller
controller = FlyingGripperVelocityController()

# Define target velocity (twist) in end-effector space
target_twist = np.array([0.1, 0.0, 0.0, 0.0, 0.0, 0.0])

dt = 0.05  # Control loop timestep (20 Hz)
move_time = 1.0  # seconds

try:
    print("Tracking target velocity... Press Ctrl+C to stop.")
    start_time = time.time()
    while time.time() - start_time < move_time:
        # Query current state from hardware
        current_pos = interface.get_joint_position()
        current_vel = interface.get_joint_velocity()

        # Update controller to get new joint velocity commands
        v_cmd = controller.update(
            dt=dt,
            current_pos=current_pos,
            current_vel=current_vel,
            target_vel=target_twist
        )

        vel_str = np.array2string(v_cmd.to_numpy(), formatter={'float_kind': lambda x: f"{x:1.2f}"})
        print(f"q_dot: {vel_str}", end="\r")

        # Send command to physical robot
        interface.cmd_velocities(v_cmd)
        
        time.sleep(dt)

except KeyboardInterrupt:
    print("\nStopping...")
finally:
    interface.cmd_zero_velocity()
    robot.stop()

Example 3: Dynamic Task-Space Tracking (Velocity Control)

This example utilizes FlyingGripperTrackingController to dynamically generate joint velocities that drive the end effector to a target pose in real-time.

import numpy as np
from stretch4_body.robot.robot_client import RobotClient
from stretch4_kinematics.stretch_interface import StretchInterface
from stretch4_kinematics.controllers import FlyingGripperTrackingController

# 1. Initialize hardware client and kinematics interface
robot = RobotClient()
robot.startup()
interface = StretchInterface(robot=robot)
interface.reset_odometry_offset()

# 2. Instantiate tracking controller
controller = FlyingGripperTrackingController()

# Define a static target pose (X=0.8m forward, Y=0.0m, Z=0.9m high)
target_position = np.array([0.8, 0.0, 0.9])
target_rpy = np.array([0.0, 0.0, 0.0])

dt = 0.05  # Control loop timestep (20 Hz)

try:
    print("Tracking target pose... Press Ctrl+C to stop.")
    while True:
        # Query current state from hardware
        current_pos = interface.get_joint_position()
        current_vel = interface.get_joint_velocity()

        # Update controller to get new joint velocity commands
        v_cmd, state = controller.update(
            dt=dt,
            current_pos=current_pos,
            current_vel=current_vel,
            target_xyz=target_position,
            target_rpy=target_rpy
        )

        state_str = str(state).split(".")[1]
        vel_str = np.array2string(v_cmd.to_numpy(), formatter={'float_kind': lambda x: f"{x:1.2f}"})
        print(f"{state_str} | q_dot: {vel_str}", end="\r")

        # Send command to physical robot
        interface.cmd_velocities(v_cmd)
        
        import time
        time.sleep(dt)

except KeyboardInterrupt:
    print("\nStopping...")
finally:
    interface.cmd_zero_velocity()
    robot.stop()

---

5. Architecture & Key APIs

State Representations (stretch4_kinematics.state)

The physical configuration of Stretch 4 is mapped into structured classes:

• StretchJointPositions: Represents joint positions: base_x, base_y, base_theta (base pose in $SE(2)$), lift, arm (telescoping extension), wrist_yaw, wrist_pitch, and wrist_roll. Includes helper methods (to_pinocchio_q(), from_pinocchio_q()) to map to and from Pinocchio's parameterization where base rotation is represented as [cos(theta), sin(theta)].
• StretchJointVelocities: Represents joint velocities.

Kinematic Models (stretch4_kinematics.kinematic_models)

• StretchKinematics: The base class wrapping Pinocchio. It implements forward() (forward kinematics) and inverse_6dof_local() (numerical inverse kinematics using Levenberg-Marquardt damped CLIK).
• ToolFrameKinematics: Subclass optimizing task-space movements relative to the tool frame (e.g. gripper forward/left/up). It resolves base-manipulator redundancy using a Weighted Jacobian Pseudoinverse, which prioritizes base rotation and arm/lift movements over sliding base translations. It also integrates joint-limit avoidance penalties.

Interfaces & Controllers (stretch4_kinematics.controllers & stretch_interface.py)

• StretchInterface: High-level API to interact with RobotClient. It converts raw dictionary updates to StretchJointPositions/StretchJointVelocities, handles relative odometry offsets, and safety-clips velocity commands.
• FlyingGripperTrackingController: A closed-loop PID controller tracking 6-DOF Cartesian poses. It computes task-space errors and pipes them into ToolFrameKinematics to yield safe joint velocity controls.

---

6. Demos

The demos/ directory contains complete scripts ready to run on the robot or in a simulated numerical environment:

• ik_cli_script.py Allows users to input a target X, Y, Z, R, P, Y pose in the command line, solves the inverse kinematics, and commands the robot to move there.

  python demos/ik_cli_script.py
  

• flying_gripper_teleop.py Performs gripper-centric teleoperation using a connected gamepad controller. Translates joystick inputs into Cartesian velocity commands in the tool frame.

  • Run numerically (simulation/console prints only):

    python demos/flying_gripper_teleop.py --numerical
    

  • Run on physical hardware:

    python demos/flying_gripper_teleop.py
    

• flying_gripper_tracking_script.py Tracks a pre-defined 3D waypoint using the tracking controller, showing plots of target convergence when run numerically.

  • Changing Target Location: The target pose is currently hard-coded near the bottom of the script (e.g., target_pose.translation = np.array([1.1, -0.1, 1.0])). To track a different point, edit these coordinates directly in demos/flying_gripper_tracking_script.py.
  • Run numerically (simulation/plot mode):

    python demos/flying_gripper_tracking_script.py --numerical
    

  • Run on physical hardware (requires homed robot): Use the -m or --move flag to command the physical robot to track the waypoint:

    python demos/flying_gripper_tracking_script.py --move