Forward Kinematics using Orocos KDL Check out the blog post Build and Run mkdir build cd build cmake .. make ./SimpleRobotFK Output: -0.7071 -0.7071 0 -1.414 -0.7071 0.7071 0 3.828 0 0 -1 0 0 0 0 1