🦾 Robotic Arm Forward and Inverse Kinematics

1. Course Overview

This course introduces the forward kinematics (FK) and inverse kinematics (IK) computation principles for the DOFBOT Pro 6-DOF robotic arm, C++ KDL solver implementation, and ROS 2 service invocation methods:

• Forward Kinematics (FK): Input servo joint angles to calculate the pose of the end-effector gripper reference frame relative to the robotic arm base frame in 3D space

• Inverse Kinematics (IK): Input the target position and orientation of the end-effector, optionally with current joint angles, to solve for the target angles of servos 1-5

2. Preparation and Running

This case suggests opening 3 terminals for step-by-step startup and debugging:

• Terminal 1: Start the kinematics solver server node (kinemarics_arm)

• Terminal 2: Start the robot URDF model, RViz visualization, and driver

• Terminal 3: Initiate ROS 2 service requests, publish control commands, and compare TF coordinate transforms

2.1 Start the Kinematics Solver Service

Run in Terminal 1:

ros2 run dofbot_pro_info kinemarics_dofbot

This node listens for /get_kinemarics service requests in the background after startup, with the terminal remaining on standby.

2.2 Start the Theoretical Model and RViz Visualization Environment

Run in Terminal 2 (enable GUI joint slider mode):

xxxxxxxxxx
ros2 launch dofbot_pro_description display_A1pro.launch.py gui:=true 

In gui:=true mode, /joint_states is generated by the graphical sliders, and RViz renders the manually set theoretical model pose, facilitating comparison between KDL algorithm computed values and model TF.

2.3 Check Service and Node Status

Run in Terminal 3:

xxxxxxxxxx
ros2 service list | grep get_kinemarics
ros2 node info /kinemarics_arm

3. Service Interface and Data Structure

The kinematics node provides the /get_kinemarics service with type arm_interface/srv/ArmKinemarics:

xxxxxxxxxx
ros2 interface show arm_interface/srv/ArmKinemarics

3.1 Field Definitions and Mode Description

xxxxxxxxxx
# ====== Request ======
float64 tar_x       # Target X coordinate (meters, used in IK mode)
float64 tar_y       # Target Y coordinate (meters, used in IK mode)
float64 tar_z       # Target Z coordinate (meters, used in IK mode)
float64 roll        # Target Roll angle (radians, used in IK mode)
float64 pitch       # Target Pitch angle (radians, used in IK mode)
float64 yaw         # Target Yaw angle (radians, used in IK mode)
float64 cur_joint1  # Current servo 1 angle (degrees, used in FK; used as IK seed value)
float64 cur_joint2  # Current servo 2 angle (degrees, used in FK; used as IK seed value)
float64 cur_joint3  # Current servo 3 angle (degrees, used in FK; used as IK seed value)
float64 cur_joint4  # Current servo 4 angle (degrees, used in FK; used as IK seed value)
float64 cur_joint5  # Current servo 5 angle (degrees, used in FK; used as IK seed value)
float64 cur_joint6  # Current gripper servo angle (degrees, not used in computation)
string  kin_name    # "fk" for forward mode, "ik" for inverse mode
---
# ====== Response ======
float64 joint1      # Servo 1 target angle computed by IK (degrees)
float64 joint2      # Servo 2 target angle computed by IK (degrees)
float64 joint3      # Servo 3 target angle computed by IK (degrees)
float64 joint4      # Servo 4 target angle computed by IK (degrees)
float64 joint5      # Servo 5 target angle computed by IK (degrees)
float64 joint6      # Gripper servo reserved (default 0)
float64 x           # End-effector X coordinate computed by FK (meters)
float64 y           # End-effector Y coordinate computed by FK (meters)
float64 z           # End-effector Z coordinate computed by FK (meters)
float64 roll        # End-effector Roll angle computed by FK (radians)
float64 pitch       # End-effector Pitch angle computed by FK (radians)
float64 yaw         # End-effector Yaw angle computed by FK (radians)

• kin_name: 'fk': Reads cur_joint1 ~ cur_joint5, computes and returns x, y, z, roll, pitch, yaw;

• kin_name: 'ik': Reads target pose (tar_x, tar_y, tar_z, roll, pitch, yaw) and current joint angles as seed values, computes and returns joint1 ~ joint5.

4. Theoretical Model Verification and TF Comparison

4.1 Forward Kinematics (FK) Call Verification

Keep all joints centered in the GUI (servo angles corresponding to 90 degrees for each joint), and initiate the forward kinematics service request in Terminal 3:

xxxxxxxxxx
ros2 service call /get_kinemarics arm_interface/srv/ArmKinemarics "{tar_x: 0.0, tar_y: 0.0, tar_z: 0.0, roll: 0.0, pitch: 0.0, yaw: 0.0, cur_joint1: 90.0, cur_joint2: 90.0, cur_joint3: 90.0, cur_joint4: 90.0, cur_joint5: 90.0, cur_joint6: 90.0, kin_name: 'fk'}"

The service returns the current end-effector pose approximately as:

xxxxxxxxxx
x     = -0.0026619 m
y     =  0.0002654 m
z     =  0.5263000 m
roll  =  2.5984269 rad
pitch = -1.5707962 rad
yaw   = -2.5984559 rad

4.2 Inverse Kinematics (IK) Closed-Loop Verification

Fill the spatial pose obtained from forward kinematics into the inverse kinematics request to verify if it accurately back-calculates the initial servo angles [90, 90, 90, 90, 90]:

xxxxxxxxxx
ros2 service call /get_kinemarics arm_interface/srv/ArmKinemarics "{tar_x: -0.00266, tar_y: 0.000265, tar_z: 0.5263, roll: 2.598, pitch: -1.5707, yaw: -2.598, cur_joint1: 0.0, cur_joint2: 0.0, cur_joint3: 0.0, cur_joint4: 0.0, cur_joint5: 0.0, cur_joint6: 0.0, kin_name: 'ik'}"

4.3 TF Transform Ground Truth Comparison

FK computes the relative transform between arm_base_Link -> Gripping coordinate frames. Use the TF tool to print and compare values:

xxxxxxxxxx
ros2 run tf2_ros tf2_echo arm_base_Link Gripping

The Translation values in TF are completely consistent with FK's (x, y, z).

5. Physical Robotic Arm Motion and Physical Closed-Loop Verification

After completing theoretical verification, stop the GUI simulation in Terminal 2 with Ctrl+C and switch to physical robot real feedback mode:

xxxxxxxxxx
ros2 launch dofbot_pro_description display_A1pro.launch.py gui:=false 

At this point, the driver node reads physical servo angles in real-time and publishes /arm_status/arm_joint, and RViz and the TF tree faithfully reflect the real robotic arm pose.

5.1 Drive the Robotic Arm to a Specified Working Pose

Publish multi-joint control commands in Terminal 3:

xxxxxxxxxx
ros2 topic pub /arm6_joints dofbot_pro_interface/msg/ArmJoints "{joint1: 90, joint2: 120, joint3: 0, joint4: 0, joint5: 90, joint6: 90, time: 1500}" --once

5.2 Get End-Effector Pose Based on Current Actual Joint Angles (FK)

xxxxxxxxxx
ros2 service call /get_kinemarics arm_interface/srv/ArmKinemarics "{tar_x: 0.0, tar_y: 0.0, tar_z: 0.0, roll: 0.0, pitch: 0.0, yaw: 0.0, cur_joint1: 90.0, cur_joint2: 120.0, cur_joint3: 0.0, cur_joint4: 0.0, cur_joint5: 90.0, cur_joint6: 90.0, kin_name: 'fk'}"

Obtain the end-effector pose:

5.3 Back-Calculate Servo Angles Based on Obtained Pose (IK)

xxxxxxxxxx
ros2 service call /get_kinemarics arm_interface/srv/ArmKinemarics "{tar_x: 0.11178, tar_y: 0.000262, tar_z: 0.16021, roll: -3.61e-06, pitch: 1.04719, yaw: -2.74e-05, cur_joint1: 90.0, cur_joint2: 120.0, cur_joint3: 0.0, cur_joint4: 0.0, cur_joint5: 90.0, cur_joint6: 90.0, kin_name: 'ik'}"

Inverse solved joint angles:

5.4 View Physical Robot TF Transform

xxxxxxxxxx
ros2 run tf2_ros tf2_echo arm_base_Link Gripping

Verify the real transform correspondence: