This is a inverse kinematics solver I developed for the robotic arm which I designed, partially because I was frustrated by the fact that we couldn’t manage to get MoveIt working, so I decided to make one myself just based on vector math. I only wrote the solver code in Python, the actual implementation on the rover in ROS was done by members of the software team.
This project was born out of the desire for higher-precision control of the arm, allowing for small precise adjustments in 3D space and orientation of the end effector, as well as motions relative to the ‘frame’ or orientation of the end effector. If functional, this esentially nullifies the requirement for any kind of alignment of the rover with what it is interacting with, as long as it is within its large sphere of reach. In the context of the University Rover Competition, this is most important for the equipment servicing mission, in which the rover has to interact and manipulate several types of interfaces, including buttons, joysticks, switches, handles, a keyboard, screws, and a drawer.
This solver takes the simple approach of calculating all the vectors which represent the centerline of each part of the arm, and determines the angle in between them with respect to the axis of the arm. One signifigant challenge in making this work was ensuring that the angle was always measured in the same direction, which was accomplished through the use of a reference axis normal to both vectors. In addition, the code solves for both the “forward” and “inverse states of the end effector (as it can be bent in either direction and simply roll to compensate) and finds the closest option.
To aid in the development of the solver and to ensure proper functionality, as well as provide a visual demonstration, I also created a forward solver, which takes the arm angles and recaclulates the 3D position, then graphs them in 3D in matplotlib. Keyboard controls were also created for testing purposes.
This code also provides movement and rotation vectors which allow the gripper to move and tilt with respect to its current orientation, meaning that it can align with what it is working on and simply move in its coordinate system.
https://github.com/VSoubbotin/6DOF-Simple-Inverse-Kinematics-Solver
Ultimately, other members of the software team created a more robust inverse kinematics system which was what finally was implemented, however, my project was proven to be functional.