Forward Kinematics

Textbook Chapter 4

Robot convenience wrappers

For simplified calls using a Robot model (e.g., forward_kinematics_space(robot, q)), see Robot Model.

ModernRoboticsBook.forward_kinematics_body — Method
forward_kinematics_body(home_ee_pose, body_screw_axes, joint_positions)

Computes forward kinematics in the body frame for an open chain robot.

Product of Exponentials (body frame)

Forward kinematics computes the end-effector pose from joint positions using the Product of Exponentials (PoE) formula. Each joint's contribution is a matrix exponential $e^{[B_i]\theta_i}$ that "moves" the end-effector from its home pose $M$:

\[T(\theta) = M \, e^{[B_1]\theta_1} \, e^{[B_2]\theta_2} \cdots e^{[B_n]\theta_n}\]

The body-frame formulation expresses the screw axes as seen from the end-effector's perspective. This is equivalent to forward_kinematics_space (which uses $T = e^{[S_1]\theta_1} \cdots e^{[S_n]\theta_n} \, M$) but can be more convenient when the task is defined relative to the tool.

Arguments

  • home_ee_pose: the $4 \times 4$ end-effector pose $M \in$ SE(3) when all joint positions are zero (home configuration).
  • body_screw_axes: the joint screw axes $B_i$ expressed in the end-effector (body) frame at the home configuration, as a $6 \times n$ matrix.
  • joint_positions: an $n$-vector of joint positions $\theta$.

Returns

The $4 \times 4$ end-effector transformation matrix $T \in$ SE(3).

Examples

julia> home_ee_pose = [ -1  0  0  0 ;
                         0  1  0  6 ;
                         0  0 -1  2 ;
                         0  0  0  1 ];

julia> body_screw_axes = [  0  0 -1  2  0  0   ;
                            0  0  0  0  1  0   ;
                            0  0  1  0  0  0.1 ]';

julia> joint_positions = [ π/2, 3, π ];

julia> forward_kinematics_body(home_ee_pose, body_screw_axes, joint_positions)
4×4 Matrix{Float64}:
 -1.14424e-17  1.0           0.0  -5.0
  1.0          1.14424e-17   0.0   4.0
  0.0          0.0          -1.0   1.68584
  0.0          0.0           0.0   1.0
source
ModernRoboticsBook.forward_kinematics_space — Method
forward_kinematics_space(home_ee_pose, screw_axes, joint_positions)

Computes forward kinematics in the space frame for an open chain robot.

Product of Exponentials (space frame)

Forward kinematics computes the end-effector pose from joint positions using the Product of Exponentials (PoE) formula. Each joint's contribution is a matrix exponential $e^{[S_i]\theta_i}$ applied to the home pose $M$:

\[T(\theta) = e^{[S_1]\theta_1} \, e^{[S_2]\theta_2} \cdots e^{[S_n]\theta_n} \, M\]

The space-frame formulation expresses screw axes relative to a fixed base frame. This is equivalent to forward_kinematics_body (which uses $T = M \, e^{[B_1]\theta_1} \cdots e^{[B_n]\theta_n}$) — both compute the same end-effector pose; the choice depends on which frame your screw axes are defined in.

Arguments

  • home_ee_pose: the $4 \times 4$ end-effector pose $M \in$ SE(3) when all joint positions are zero (home configuration).
  • screw_axes: the joint screw axes $S_i$ expressed in the space (base) frame at the home configuration, as a $6 \times n$ matrix.
  • joint_positions: an $n$-vector of joint positions $\theta$.

Returns

The $4 \times 4$ end-effector transformation matrix $T \in$ SE(3).

Examples

julia> home_ee_pose = [ -1  0  0  0 ;
                         0  1  0  6 ;
                         0  0 -1  2 ;
                         0  0  0  1 ];

julia> screw_axes = [  0  0  1  4  0  0   ;
                       0  0  0  0  1  0   ;
                       0  0 -1 -6  0 -0.1 ]';

julia> joint_positions = [ π/2, 3, π ];

julia> forward_kinematics_space(home_ee_pose, screw_axes, joint_positions)
4×4 Matrix{Float64}:
 -1.14424e-17  1.0           0.0  -5.0
  1.0          1.14424e-17   0.0   4.0
  0.0          0.0          -1.0   1.68584
  0.0          0.0           0.0   1.0
source