So I (and my family, thanks!) paid thousands of dollars for a mechanical engineering degree, but yet in my day job I really only use my problem solving skills. To prove to myself that I did learn something in the controls, mechatronics, differential equations, and electronics classes that I took, I wanted to solve a non-trivial kinematics equation and implement it in real life. 

Stepper motors are all our favorite “hammer” when it comes to motion control, but I have always wanted to understand how more complex coordinated motion controllers work when talking to more advanced motor controllers. What this mainly means is writing a controller to output velocity or acceleration, instead of position. With this requirement, I selected the ODrive motor controller as it is a great open-source project that accepts CAN bus messages to control brushless dc motors via acceleration, velocity, or position mode. 

The next choice I had to make was, from what device should I send the ODrive commands. Initially I wanted to use a STM32F4 board, but because I had never implemented inverse kinematics before, I figured I should stick with something that had better visualization support. I had used ROS before, and saw that ROS2 was coming out, so I jumped on the ROS2 train. ROS2 has a slight learning curve, but with the power of RVIZ2 and URDF, it was easy to stay excited. In order to get ROS2 talking to ODrive, I could have used the python3 ODrive tool, but I didn’t like the idea of non-deterministic USB, so I kept with CAN bus in the hopes that I could go fully embedded in the future. To get my computer running Ubuntu talking to the ODrive via CAN, I created a SocketCAN driver in ROS2 (https://github.com/camrbuss/ros2_odrive_can). 

After getting the main hardware and software building blocks mostly sorted out, it was onto the actual inverse kinematics implementation. Of course I could have used the MoveIt ROS package, but the goal of this project was to use my maphs skillz, so I used sympy to solve for the Jacobian. In general I just used the law of cosines to solve the forward kinematics, calculus to find the derivative of position, and some linear algebra to find the Jacobian. The derivation can be seen in the README of the repository, or in the LaTex file. 

Finally I needed a way to send the machine paths, and I didn’t feel like writing a G-code interpreter, so I wrote a Inkscape plugin to output a CSV file of text/paths.

https://github.com/camrbuss/nodes_to_csv

With some debugging using RVIZ2, and rqt I was able to get the whole machine moving by specifying end effector positions. It was a bit surprising how “unique” (small) the work area was, but it didn’t bother me too much as I chose the configuration so that I couldn’t just grab the kinematic equations off the internet. 

And finally, a video of it plotting:

https://photos.app.goo.gl/zSd6bQUmWiWb2oVM9

Maybe next I will implement the motion control with a microcontroller using the Zephyr RTOS project...