Examples

Basic example

A very simple example program using Copapy can look like this:

import copapy as cp

# Define variables
a = cp.value(0.25)
b = cp.value(0.87)

# Define computations
c = a + b * 2.0
d = c ** 2 + cp.sin(a)
e = cp.sqrt(b)

# Create a target (default is local), compile and run
tg = cp.Target()
tg.compile(c, d, e)
tg.run()

# Read the results
print("Result c:", tg.read_value(c))
print("Result d:", tg.read_value(d))
print("Result e:", tg.read_value(e))

IMU Sensor fusion

This example demonstrates how concisely the Madgwick AHRS algorithm for IMU sensor fusion can be expressed in Copapy. While the original C implementation contains more than 130 lines of code (excluding blank lines and comments), the algorithm can be expressed in about 10 lines with Copapy. One reason is Copapy’s automatic differentiation, which computes the Madgwick correction gradient with cp.grad():

import copapy as cp
from copapy import vector, quaternion

def madgwick_imu_update(q: quaternion, gyro: vector[float],
                        accel: vector[float]) -> quaternion:
    BETA: float = 0.1
    SAMPLE_INTERVAL: float = 0.01  # seconds
    objective = q.rotate_vector(vector([0.0, 0.0, 1.0])) - accel.normalize()
    cost = 0.5 * objective.dot(objective)
    gradient = cp.grad(cost, q).normalize()
    gyro_quat = quaternion(0.0, *gyro)
    q_dot_gyro = 0.5 * (q @ gyro_quat)
    q_dot = q_dot_gyro - BETA * gradient
    return (q + q_dot * SAMPLE_INTERVAL).normalize()

# Usage
q = quaternion(cp.value(0.7071), cp.value(0.7071), cp.value(0.0), cp.value(0.0))
gyro = vector([0.01, 0.02, 0.015])
accel = vector([0.0, 0.0, 1.0])

new_q = madgwick_imu_update(q, gyro, accel)

tg = cp.Target()
tg.compile(new_q)
tg.run()

print(f"Updated orientation: {tg.read_value(new_q)}")

See tests/test_quaternion.py for the documented version.

Inverse kinematics

Another example using autograd in Copapy, here implementing gradient descent to solve an inverse kinematics problem for a two-joint 2D arm:

import copapy as cp

# Arm lengths
l1, l2 = 1.8, 2.0

# Target position
target = cp.vector([0.7, 0.7])

# Learning rate for iterative adjustment
alpha = 0.1

def forward_kinematics(theta1, theta2):
    """Return positions of joint and end-effector."""
    joint = cp.vector([l1 * cp.cos(theta1), l1 * cp.sin(theta1)])
    end_effector = joint + cp.vector([l2 * cp.cos(theta1 + theta2),
                                     l2 * cp.sin(theta1 + theta2)])
    return joint, end_effector

# Start values
theta = cp.vector([cp.value(0.0), cp.value(0.0)])

# Iterative inverse kinematics
for _ in range(48):
    joint, effector = forward_kinematics(theta[0], theta[1])
    error = ((target - effector) ** 2).sum()

    theta -= alpha * cp.grad(error, theta)

tg = cp.Target()
tg.compile(error, theta, joint)
tg.run()

print(f"Joint angles: {tg.read_value(theta)}")
print(f"Joint position: {tg.read_value(joint)}")
print(f"End-effector position: {tg.read_value(effector)}")
print(f"quadratic error = {tg.read_value(error)}")

Joint angles: vector((-0.7229549288749695, 2.6246931552886963))
Joint position: vector((1.3498780727386475, -1.1907261610031128))
End-effector position: vector((0.6998459100723267, 0.7006908655166626))
quadratic error = 5.010516588299652e-07

Real-time tasks on a device

Periodic tasks run in real time on a device with the Zephyr runner. This example deploys a position controller to a Teensy 4.1. Every millisecond it reads an encoder and sets the duty cycle of a PWM output, while the host changes the setpoint:

import time
import copapy as cp

# Connect to a Teensy 4.1 running the Zephyr runner (port found by USB id)
tg = cp.SerialTarget(device=cp.devices.teensy41)

# Position controller: encoder in, PWM duty cycle out
setpoint = cp.value(0.0)  # written by the host while running
position = cp.value(0)    # read from the encoder at each release
tg.bind(position, tg.io.encoder('enc0'))

error = setpoint - position * 0.01
duty = cp.clamp(0.5 + error * 2.0, 0.0, 1.0)
tg.bind(duty, tg.io.pwm('pwm2'))

controller = cp.Task(duty, period=1e-3)  # released every millisecond
tg.deploy(controller)
tg.run()  # real-time execution on the device

tg.write_value(setpoint, 0.1)
time.sleep(1.0)
print(f"Duty cycle: {tg.read_value(duty)}")
print(f"Jobs run: {tg.stats(controller)['runs']}")
tg.stop()

Duty cycle: 0.699999988079071
Jobs run: 1002