almond_axol.kinematics

from almond_axol.kinematics import KinematicsSolver, KinematicsConfig

import numpy as np
from almond_axol.kinematics import KinematicsSolver, KinematicsConfig

solver = KinematicsSolver(KinematicsConfig(pos_weight=100.0))
q = np.zeros(solver.num_joints, dtype=np.float32)  # [left s1…w3, right s1…w3]

# Inverse kinematics → new joint array
pos = np.array([0.3, 0.2, 0.4], dtype=np.float32)  # 30 cm forward, 20 cm left
rot = np.eye(3, dtype=np.float32)
q = solver.ik(q, left_pose=(pos, rot))
left_arm, right_arm = q[:7], q[7:]  # ARM_JOINTS order, ready for motion_control

# Forward kinematics → (left, right), each a (pos (3,), rot (3, 3)) numpy
# pair — the same format ik takes. Reading a pose, nudging it, and solving
# for the joints is a round trip:
(l_pos, l_rot), (r_pos, r_rot) = solver.fk(q)
q = solver.ik(q, left_pose=(l_pos + np.array([0.0, 0.0, 0.05]), l_rot))

​

Robot models

• freezes every joint except the arm joints. The base, lift, fingers and wheels stay at zero, so the lifts are fully raised.
• uses simpler collision shapes than each part’s hull. These are the custom <ik_collision> elements, which other importers ignore. They cover the shoulder bar, the head-camera mount, the lift column’s top plate and each section, the Jelly deck and the Owl Mount foot plate as strips, and each gripper’s jaws as one body over their whole motion.

• Ox Cart: the lift column. IK keeps the cart’s lift fully raised, so its caster frame is out of reach. If the lift is lowered, the frame comes up toward the hands, and IK doesn’t account for it.
• Owl Mount: the column and its foot plate, as six thin strips. The strips are left out of the hard upper-arm guard, since they sit under the arms’ path.
• Jelly: the deck, as three strips, also left out of the hard upper-arm guard. The lift carries the arms down while the deck stays on the ground. Teleop passes the height the lift reports to IK, which moves the deck and the lower column up toward the arms by that amount. Until the lift first reports a homed height, IK treats it as fully raised, and if the readings stop, IK keeps the last height.

• left_wrist_camera_optical and right_wrist_camera_optical (parallel gripper only)
• head_camera_left_optical and head_camera_right_optical

• Joints: Every moving part is a joint. The Jelly base moves through base_x, base_y and base_yaw. The x-drive’s omni rollers are not modelled, so plain wheels can’t strafe. Each finger pair is one joint (left_finger_1_joint, right_finger_1_joint), and the second finger mimics it. Importers that ignore <mimic> treat the mimicked joints as independent, so command them with the same value.
• Joint friction and drives: Each arm joint carries AxolConfig’s measured friction for its arm, as <dynamics friction> (Coulomb) and damping (viscous). The motors’ impedance gains are on the same joint as <drive stiffness damping> (kp in N·m/rad, kd in N·m·s/rad). URDF has no drive-gain element, so importers ignore this custom one: read it and set each joint’s position-drive stiffness and damping after import. The controller-only terms j_eff and kd_host are left out.
• Masses: The arm masses and centres of mass are AxolConfig’s tuned values, the same ones gravity compensation uses. Wrist 3’s mass is split across the gripper parts, keeping its total and centre of mass. The body, head, wheel and gripper-part masses are estimates, listed in each Body’s masses and each gripper’s wrist_parts in the generator. Replace them with measured values.
• MuJoCo: Compile with fusestatic="false". Fusing fixed-jointed links misplaces their centres of mass.
• Cameras: Attach Isaac Sim cameras to the *_optical links with ROS camera axes. Each wrist camera (ZED X One S fisheye) sits at the projection centre from Stereolabs’ camera model, rolled so the gripper is at the bottom of the image as in real frames. Render the wrist cameras with the factory fisheye (Kannala-Brandt) calibration, not a pinhole model. The head camera (ZED X Mini Wide) frames are at each lens’s projection centre from Stereolabs’ model, 7.2 mm behind its front cover and 50 mm apart.

​

KinematicsSolver interface

• the shoulder centre, where the three shoulder axes meet
• the elbow
• the wrist centre, where the forearm roll axis meets wrist_2, in the gripper mount frame
• the unit direction the elbow moves in as the arm swings with its mount held, from the null space of the arm’s Jacobian. The wrist isn’t spherical, so this swing isn’t a circle.

​

Straight-line paths

• The planner solves at least plan_rate samples per second of motion (default 10, capped at rate).
• It solves more often when the tip would otherwise move more than MAX_STEP_M (10 mm) or turn more than MAX_STEP_RAD (0.1 rad) between samples.
• A clamped cubic spline fills in the ticks between solved samples.

• settle_fraction: how close each sample is driven to its target, by solving again, before the planner moves on.
• pos_tolerance and ori_tolerance: the error at which a sample counts as unreachable. The planner then raises PathPlanningError before anything moves. It also checks the path between solved samples, where a chord strays furthest from the line.
• min_travel and min_rotation: an arm whose tip moves less than min_travel and turns less than min_rotation stays at q_from for the whole segment. At that scale the difference between two taught configurations is drift, and an arm chasing it would swing its elbow a long way for a tiny tip motion.

from almond_axol.kinematics import KinematicsSolver, plan_linear_segment

solver = KinematicsSolver()
for q in plan_linear_segment(solver, q_a, q_b, speed=0.25, ang_speed=1.2, rate=250.0):
    await robot.motion_control(left=..., right=...)

​

KinematicsConfig fields

​

Self-collision

• the capsule model is built, and a soft self-collision cost is part of the IK solve
• a hard guard between the upper arms and the base runs on every final step, so a pose target can’t push an arm into the torso
• the return-to-rest planner avoids collisions