This repo contains the kinematics and motor control code developed for a humanoid robot leg. The work covers the full pipeline — from computing where the foot should be in space, to actually sending commands to physical Dynamixel servo motors.
A GUI simulator for the forward kinematics of a humanoid leg. You plug in joint angles with sliders, and it draws the leg in 3D in real time using Denavit-Hartenberg (D-H) parameters. The six joints modeled follow the NAO robot convention: hip yaw/pitch/roll, knee pitch, and ankle pitch/roll. Physical dimensions (thigh length, shin length, foot height) are also adjustable.
Built with customtkinter for the interface and matplotlib for the 3D plot.
Implements both forward and inverse kinematics for the right leg using the Kajita analytical method — the same approach described in the humanoid robotics literature. Given a target (x, y, z) position for the sole of the foot, it computes the six joint angles needed to reach it. There's also a closed-loop consistency test that runs FK → IK and checks whether you get the original angles back.
Running this file drops into teste_ik() by default, which solves IK for a hardcoded target and plots the leg in 3D.
A lighter, standalone version of the IK solver — same Kajita algorithm but with slightly different robot dimensions and no extra test infrastructure. Good for quick experiments or sanity-checking a specific target position without dealing with the full IK.py setup.
Command-line tool for talking directly to Dynamixel motors (Protocol 2.0) over a serial port. It enables torque on motors with IDs [0, 2, 4], then lets you type in target angles (0–360°) and sends them to the motors. You can also type ler to read back the current position of each motor, or sair to quit and release torque.
Install the dependencies with:
pip install numpy scipy matplotlib customtkinter dynamixel-sdk
customtkinteris only needed forforward_kinematics.py.
dynamixel-sdkis only needed fordynamixel_interface.py.
python forward_kinematics.pyOpens a window with sliders for each joint angle. Move them to see the leg update live in the 3D plot on the right.
python IK.pyBy default runs teste_ik() — solves IK for the target defined at the bottom of the file and shows a 3D plot of the computed leg pose.
To run the FK↔IK consistency test instead, swap the commented lines at the bottom:
if __name__ == "__main__":
teste_fk_ik() # uncomment this
# teste_ik() # comment this outTo test a different foot target, change these values in teste_ik():
alvo_x = 0.10 # meters
alvo_y = -0.05
alvo_z = 0.0python teste.pyRuns the IK solver for the target set at the bottom of the file and shows the result in a 3D plot. Edit alvo_x, alvo_y, alvo_z to try different positions.
python dynamixel_interface.pyConnects to the Dynamixel motors via /dev/ttyUSB0. Make sure the port name matches your setup — on Windows it would be something like COM3. Once running, type three space-separated angles to move the motors:
> 180 90 45
Type ler to read current positions, or sair to exit cleanly.
| Parameter | Value |
|---|---|
| Thigh length | 10–12 cm (varies by script) |
| Shin length | 8–10.29 cm |
| Hip lateral offset | 5 cm |
| Ankle to sole | 2.9–4.52 cm |
| Torso height | 14–20 cm |
These values differ slightly between IK.py and teste.py as the latter uses dimensions closer to the physical robot being tested.