Here is the first experiment with a robot leg and solving inverse kinematic problem algorithm. Software was written in C++ running ROS Kinetic. A marker was attached to the end of the leg in order to check correctness of the algorithm. Also reference trajectory is a circle.

This is a visualized scheme of software.

Every node has a special function:

• circle_traj_generator - forms a reference circle trajectory for the end of the leg
• leg_inv_kinematic - solves inverse kinematic problem. Input: reference coordinates. Output: reference joint angles
• dynamixel_control - connects to servos, set their params and transforms angle data from double to format understandable for a dynamixel driver
• dynamixel_manager - dynamixel driver that gets data about angles and send it to a serial port

See my github page for the full source code https://github.com/yoggi56/hexapod