Examples

This section demonstrates how to use the DexRobot Kinematics library with complete example scripts.

Hardware control using finger inverse kinematics

In this example, we solve inverse kinematics for a finger, and send the resulting joint angles to DexHand using the pyzlg_dexhand library.

examples/finger_ik_hardware.py

"""
Solve IK for a single finger and directly control the hardware.

The code works for the right hand by default, but you can change the hand as needed.
"""

import numpy as np
import time
from dexrobot_kinematics.hand import RightHandKinematics
from dexrobot_kinematics.utils.types import Position
from dexrobot_kinematics.utils.hardware import JointMapping
from pyzlg_dexhand.dexhand_interface import RightDexHand

# Initialize hand
kin = RightHandKinematics()
hand = RightDexHand()
joint_mapping = JointMapping(prefix="r")

# Define a circular trajectory
center = Position(x=0.01, y=0.04, z=0.22)
radius = 0.05
steps = 20

# Simulate motion
for i in range(2 * steps):
    # Calculate position along circle
    if i < steps:
        angle = np.pi * i / steps
    else:
        angle = np.pi * (2 * steps - i) / steps
    target = Position(
        x=center.x + radius * np.sin(angle),
        y=center.y,
        z=center.z + radius * np.cos(angle),
    )

    # Solve IK
    joint_angles, success = kin.inverse_kinematics_finger(
        finger="index", target_pos=target
    )

    print(f"Step {i}: Pos ({target.x:.4f}, {target.y:.4f}, {target.z:.4f}), Angles {list(joint_angles.values())}, Success: {success}")
    # In a real system, you would send these angles to the robot
    commands = joint_mapping.map_command(joint_angles)
    hand.move_joints(**commands)
    hand.clear_errors(clear_all=True)
    time.sleep(0.1)

Interfacing inverse kinematics with ROS

In this example, we solve inverse kinematics for a finger, and publish the resulting joint angles to a ROS topic. The script can be used in combination with ROS nodes in pyzlg_dexhand or dexrobot_mujoco to control either hardware or simulated hand.

examples/finger_ik_ros.py

"""
Solve IK for a single finger and send the result to ROS/ROS2.

Can be used together with the ROS nodes in dexrobot_mujoco or pyzlg_dexhand to control either the hardware or a simulation.
"""

import numpy as np
import time
from ros_compat import ROSNode
from sensor_msgs.msg import JointState
from dexrobot_kinematics.hand import RightHandKinematics
from dexrobot_kinematics.utils.types import Position
from dexrobot_kinematics.utils.hardware import JointMapping


class DexHandIKNode(ROSNode):
    def __init__(self):
        super().__init__("dexhand_ik_node")
        self.kin = RightHandKinematics()
        self.joint_pub = self.create_publisher(JointState, "joint_commands", 10)
        self.timer = self.create_timer(0.1, self.timer_callback)
        self.counter = 0

    def timer_callback(self):
        # Define a circular trajectory
        center = Position(x=0.01, y=0.04, z=0.22)
        radius = 0.05
        steps = 20

        # Calculate position along circle
        i = self.counter
        self.counter += 1
        if i < steps:
            angle = np.pi * i / steps
        else:
            angle = np.pi * (2 * steps - i) / steps
        target = Position(
            x=center.x + radius * np.sin(angle),
            y=center.y,
            z=center.z + radius * np.cos(angle),
        )

        # Solve IK
        joint_angles, success = self.kin.inverse_kinematics_finger(
            finger="index", target_pos=target
        )

        print(f"Step {i}: Pos ({target.x:.4f}, {target.y:.4f}, {target.z:.4f}), Angles {list(joint_angles.values())}, Success: {success}")

        # Publish joint angles
        msg = JointState()
        msg.header.stamp = self.get_ros_time().to_msg()
        msg.name = list(joint_angles.keys())
        msg.position = list(joint_angles.values())
        self.joint_pub.publish(msg)

if __name__ == "__main__":
    node = DexHandIKNode()
    node.spin()

Forward kinematics based on hardware feedback

In this example, we read joint angles from DexHand using the pyzlg_dexhand library, and compute the forward kinematics to get the fingerpad and fingertip poses.

examples/fk_hardware.py

"""
Run FK to obtain fingertip and fingerpad positions, from hardware joint angle readings.
"""

import numpy as np
from dexrobot_kinematics.hand import RightHandKinematics
from dexrobot_kinematics.utils.types import Position, Pose
from dexrobot_kinematics.utils.hardware import JointMapping
from pyzlg_dexhand.dexhand_interface import RightDexHand

# Initialize hand
kin = RightHandKinematics()
hand = RightDexHand()
joint_mapping = JointMapping(prefix="r")

# Drive the hand joints to a known position and read feedback
commands = {
    "th_rot": 30,
    "th_mcp": 30,
    "th_dip": 30,
    "ff_spr": 20,
    "ff_mcp": 30,
    "ff_dip": 30,
    "mf_mcp": 30,
    "mf_dip": 30,
    "rf_mcp": 30,
    "rf_dip": 30,
    "lf_mcp": 30,
    "lf_dip": 30,
}
for i in range(10):
    hand.move_joints(**commands)
    hand.clear_errors(clear_all=True)
feedback = hand.get_feedback()
hardware_feedback_angles = {name: joint.angle for (name, joint) in feedback.joints.items()}
joint_angles = joint_mapping.map_feedback(hardware_feedback_angles)

# Define base pose of the hand
R = pin.utils.rpyToMatrix(0, np.pi / 2, 0)
t = np.array([1.0, 0.0, 0.0])
base_pose = Pose(position=Position.from_array(t), orientation=R)

# Run forward kinematics
fingertip_poses_hand = kin.forward_kinematics(joint_angles, frame="hand", end_effector="fingertip")
fingerpad_poses_hand = kin.forward_kinematics(joint_angles, frame="hand", end_effector="fingerpad")
fingertip_poses_world = kin.forward_kinematics(joint_angles, base_pose=base_pose, frame="world", end_effector="fingertip")
fingerpad_poses_world = kin.forward_kinematics(joint_angles, base_pose=base_pose, frame="world", end_effector="fingerpad")

# Print results tables
print("Fingertip poses (hand frame):")
print("Finger | Position (m) | Quaternion")
for finger, pose in fingertip_poses_hand.items():
    print(f"{finger:6} | {pose.position.to_array()} | {pose.orientation.to_quaternion()}")
print("\nFingerpad poses (hand frame):")
print("Finger | Position (m) | Quaternion")
for finger, pose in fingerpad_poses_hand.items():
    print(f"{finger:6} | {pose.position.to_array()} | {pose.orientation.to_quaternion()}")
print("\nFingertip poses (world frame):")
print("Finger | Position (m) | Quaternion")
for finger, pose in fingertip_poses_world.items():
    print(f"{finger:6} | {pose.position.to_array()} | {pose.orientation.to_quaternion()}")
print("\nFingerpad poses (world frame):")
print("Finger | Position (m) | Quaternion")
for finger, pose in fingerpad_poses_world.items():
    print(f"{finger:6} | {pose.position.to_array()} | {pose.orientation.to_quaternion()}")