Control

Control Architecture


Target pose / trajectory
        │
   [ IK solver ]
        │  joint angles θ_des
   [ Impedance controller ]
        │  joint torques τ
   [ Robstride 00 (MIT mode) ]
        │  CAN frames @ 1 kHz
   [ Physical joints ]
        │  θ, θ̇ (encoder feedback)
        └──────────────────────►

Joint Impedance Control

Each joint runs an independent PD + feedforward law in the Robstride MIT control frame:


τ_i = Kp_i · (θ_des,i − θ_i) + Kd_i · (θ̇_des,i − θ̇_i) + τ_ff,i

Typical gains for the manipulator:

Joint	Kp (N·m/rad)	Kd (N·m·s/rad)
1 (shoulder pan)	80	2.0
2 (shoulder lift)	100	2.5
3 (shoulder roll)	60	1.5
4 (elbow)	80	2.0
5 (wrist pitch)	30	0.8
6 (wrist roll)	20	0.5

Gravity Compensation

Without gravity compensation the arm droops when Kp is low. Gravity torques are computed from the robot’s dynamics model:


import numpy as np
 
# Simplified gravity torque for joint i (planar case)
# g_vec: gravity vector in base frame [0, 0, -9.81]
# m_i: link mass, r_i: distance from joint to link CoM
 
def gravity_torques(q, masses, com_distances):
    """
    Rough gravity compensation for 6-DOF arm.
    Returns array of 6 gravity torques.
    """
    g = 9.81
    tau_g = np.zeros(6)
    # Accumulate from distal to proximal
    for i in range(5, -1, -1):
        T = forward_kinematics(q[:i+1] + [0]*(6-i-1))
        # z-component of joint axis in world frame
        z_axis = T[:3, 2]
        # CoM position relative to joint i
        r = com_distances[i] * T[:3, 0]   # along x of DH frame
        tau_g[i] = masses[i] * g * np.cross(z_axis, r)[2]
    return tau_g

Pass tau_g[i] as torque_ff in the MIT control frame for each joint.

Trajectory Generation

Minimum-jerk interpolation

For point-to-point moves, minimum-jerk trajectories avoid abrupt acceleration:


def min_jerk(q_start, q_end, T, dt):
    """
    Generate minimum-jerk trajectory.
    T: total duration (s), dt: time step (s)
    Returns: array of shape (n_steps, n_joints)
    """
    t_arr = np.arange(0, T, dt)
    s = (t_arr / T)
    # Minimum-jerk polynomial
    s_mj = 10*s**3 - 15*s**4 + 6*s**5
    return q_start + np.outer(s_mj, (q_end - q_start))

Cartesian-space trajectory

For straight-line end-effector motion, interpolate in Cartesian space and run IK at each step:


def cartesian_line(T_start, T_end, steps):
    """Linear interpolation of position; SLERP of orientation."""
    poses = []
    for i, alpha in enumerate(np.linspace(0, 1, steps)):
        T = T_start.copy()
        T[:3, 3] = (1-alpha)*T_start[:3, 3] + alpha*T_end[:3, 3]
        # orientation: simple linear blend (replace with SLERP for large rotations)
        T[:3, :3] = (1-alpha)*T_start[:3, :3] + alpha*T_end[:3, :3]
        poses.append(T)
    return poses

Control Loop


import time
from robstride import Robstride00
 
motors = [Robstride00(motor_id=i) for i in range(1, 7)]
for m in motors:
    m.enter_mode()
 
trajectory = min_jerk(q_start, q_end, T=3.0, dt=0.001)
 
try:
    for q_des in trajectory:
        tau_g = gravity_torques(q_des, masses, com_distances)
        for i, motor in enumerate(motors):
            motor.control(
                pos_des=q_des[i],
                vel_des=0.0,
                kp=KP[i],
                kd=KD[i],
                torque_ff=tau_g[i]
            )
        time.sleep(0.001)
finally:
    for m in motors:
        m.shutdown()

Run the control loop in a threading.Thread with time.sleep(0.001) for ~1 kHz on CPython. For hard real-time on a Raspberry Pi, consider PREEMPT_RT kernel or offloading the loop to a microcontroller.