I am working on firmware modification to allow running time parameterized trajectory points generated by fitting a cubic spline, which is generated by ROS Moveit software (which deals with robotic manipulators in ROS).
(link to branch on my fork)
The motivations are:
- a joint of the robotic arm have to be at a certain position synchronously with other joints for the pose to make sense. For example moving the arm while having the end effector parallel to the floor.
- sending fine-grain setpoints takes up communication bandwidth (I am using a CAN bus), and is difficult to synchronize accross multiple Axes.
Because the trapezoidal trajectory control is already implemented, I used the same pattern:
- save the starting time, plug in current time to a equation in each control loop to get pos_setpoint and vel_setpoint.
- The difference is that there is a buffer to hold multiple trajectory points which has its own cubic function for position and quadratic function for velocity. (new points are loaded as time progresses)
- synchronizing (start signal) will be done by a high priority CAN message meant for all ODrive nodes.
- the control ends when there is no next point in the buffer, switching to CTRL_MODE_POSITION_CONTROL
Here is a diagram of the implementation:
FIFO and ‘current region’ in the picture are queues implemented as linked list.
The equation used to make cubic polynomial (to compute pos_setpoint) from position, acceleration, and time of two points (from a spline) (velocity function used is just the derivative of this equation). (courtesy of Ruye Wang hmc.edu):
I used trajectory points generated from ROS moveit package which looks like following when the above equations are computed using python:
and here is a capture from ODrive liveplotter on hardware:
orange = vel_setpoint, green = pos_estimate
As you can see from the liveplotter output, it is not quite working right.
- The vel_setpoint seems to be lagging behind a bit and eventually causes an overshoot (seems small in the plot but its almost 1/8 turn of the motor).
- I have measured the time required to compute pos_setpoint and vel_setpoint with an oscilloscope to be 3 microseconds.
Is it taking up too much time in the control loop? Maybe the approach is just wrong? I appologize if the code is hard to read
(computing pos_setpoint, vel_setpoint)
I am kind of stuck and ask for some suggestion. Thanks.