Write ROS2 Controllers for Forward and Inverse Kinematics for Trajectory and Build Robotic Manipulators from Scratch