chip updates: thew new model and programming [updates]
OKAY SO! Today I have good news. After three days of struggling, limited documentation, and a whole lot of other nonsense. I finally go 3/4 legs to go to coordinates I specified them to go to. The fourth leg has an electrical issue with one of the encoder cables ripped out on the shoulder joint (that will be fixed when the cable to fix it comes in - in the meantime we're dealing with three legs).
So yesterday late last night, I decided we were doing way way way too much math to get t this thing to work. It wasn't practical because the amount of precision the code was trying to achieve we can't even achieve in real life due to the slop in the gear boxes. And we don't need to achieve that level of precision because there's no point. We just need to get the leg to go mostly to the right location and will still walk in theory. That's a theory we're going to test eventually.
So this post is going to explain a few things. First, it's going to explain the newest leg model. How simplified it is and why we chose that model. Second, it's going to explain the code we wrote today and what it achieves. Third, we will explain the plan for the future.
step one: the new leg model
The new leg model is an attempt to make the simplest case for controlling the leg system. What we've done is simplify the leg to a 2D case from a side view and front view to get x, y, and z. This simple model allows us to use trig and short formulas to get CLOSED-FORM SOLUTIONS for the desired leg angles.
FIRST let's talk leg geometry:
So here's the leg model. If we look at a leg. The origin is at the shoulder-joint with the Y-axis pointing towards the ground and the X-axis points towards the back of the robot. The Z-axis is specially defined. It does NOT follow the right hand convention (+Z points away from the robot on all four legs). Z=0 is defined as when the leg resides vertically making the Z-origin different than that of X/Y. The Z position of the leg is simply defined as Y (the height of the leg) times the sin of theta2 because we're assuming theta2 will be a small angle and this approximation will be valid. If theta2 is small moving the leg away from or towards the body will not execute large angles. The Z-position is mostly for rotational maneuvers and weight transfer so separating it from the X/Y coordinate frame will not matter so much. It will, however, need to be accounted for in path/step planning.
NOTE: for the rest (not including the diagram)
theta_1 --> position of shoulder
theta_2 --> position of hinge
theta_3 --> position of knee
Now for the math... we'll use the 2D simplification we obtained for inverse kinematics. In this case, L1 = L3 (according to the old model), L2 = L6, theta_1 is theta_1, theta_2 is theta_3 in the code.
Now all that's left is to define theta_2 = arcsin(zD/yD). The reason for this is the small angle approximation we made.
The only thing that's left is to convert the thetas into commands to the motors. We'll deal with that in the next section.
step two: putting some things into code (setup stuff)
So putting this into code is going to be harder than we think. The reason for this is that RoboRIO code works like Arduino on loop-based code. It executes a block of code each loop. The other issue is we take the above thetas, and if we simply convert them to PID rotation and command them, the whole robot over currents because these number of rotations are usually like 70. The error is huge so the attempted speed is huge which means we need to limit the error, create a saturation zone. We'll explain how we did all of this and even accounted for the fact that some of the motors have different + directions than we want. But first, we need to do some setup.
The first thing we did, was redefine some things in the constructor. The first thing we wanted to take care of is the "home" position for these legs is NOT (0,0,0), this leg can't actually go to that point if we notice the geometry. So we created a "home" variable that stores this so we don't forget to command it to the right home and the program crashes.
The other convention I want to make is all the "points" will be given as Arrays of doubles of length 3, this makes them un-mutable so they can't be changed accidentally like an ArrayList, any trajectory, however, will be a list.
The next thing you'll notice in the constructor, is the multipliers and the L1-L6. L1-L6 is defined the same way we defined it here:
We've defined this because the class needs to know what the leg looks like to make any inverse kinematic calculations. We've simplified the model past this, but it doesn't take much space to store the whole leg so we will do it anyways in case of future.
The final thing is the multipliers, this takes care of the + directions of the motors. If we want to reverse a direction, we set a multiplier to -1! And it just commands the leg to go the opposite way. The multipliers are applied to the THETAS because if the leg's definition of + rotation is different than ours, applying the multiplier to the direction it moves will not change it from going to the right angle, it'll only go the wrong direction to get to the same wrong angle. Right? What we really want in that case is to not ask the leg to go in reverse direction to +30 degrees, but to go to -30degrees because that's fitting the model to the reality.
The next step was to write some helper functions:
So there's a difference between THETA and motor command as we mentioned in a previous post. THETA is the angle in degrees, the motor command is actually in rotations of the motors. To convert theta to commands, we need to divide by 2*PI to get number of rotations, but then multiple by 100 b/c we have a 100:1 gearbox on it. And remember theta is defined as between parts of the leg like above.
So we wrote two functions, one that reads the current leg positions in terms of PID positions of the motors, and then converts them to thetas we can use for forwards kinematics (not written) and then we wrote a function that takes thetas (usually outputted by inverse kinematics) and converts them to motor commands. THIS IS WHERE WE APPLY THE MULTIPLIERS TO ACCOUNT FOR THE DIRECTION OF THE MOTORS +/- NOT IN THE CONTROL CODE. APPLYING IT TO THE CONTROL CODE WILL ONLY MAKE THE LEG TRAVEL TO THE WRONG DIRECTION, WE NEED TO CHANGE THE REFERENCE FRAME OF THE MOTOR ITSELF.
The final function is the inverse kinematic function which takes in an x, y, z desired values and determines what thetas the leg needs to be at to achieve that. NOTE: THESE ARE THE THETAS IT MUST BE AT NOT THE COMMANDS THAT NEED TO BE SENT TO THE MOTORS. BUT THAT'S JUST A SIMPLE CONVERSION.
Now we know, how many times to turn the motors and in what direction to get to a desired point in space. Now, how do we get it to go there? We tried just sending the command "turn 70 times" but because we're doing PID control, we can't just tell it that, this will over-current the motor. It'll try to move so fast it will blow the system. So we need to come up with another way.
step three: commanding the leg to go somewhere
So here's the code. And let me explain the logic. This code takes in a point and traverses the leg to that point. The point is in the form of the coordinates we defined above. The Y-axis points down because there's no reason to ever command the leg to go above the head of the robot, and we're trying to keep values as nice as possible. Downwards Y made this easy.
So the first thing we do is given this X, Y, Z we want to go to, we use the inverse kinematics to figure out what thetas the leg needs to go to, and then we use thetasToCMDS to find the actual commands to send to the legs.
Now what we will do is send the leg moving at some MAX speed which is an input parameter the user defines. That's the maximum increment in position a leg can do at a given time-step. If we command the leg to move 50 rotations in 0.001 seconds, it'll be very unhappy. That's what was happening before. We're going to have it move at this max speed towards its target until we get really close then it slows down to reach the target so we don't overshoot.
So the thing we do is define an epsilon. This is good practice when comparing values because we'll never get the leg to go to the EXACT position we want it to, and we don't need it to. Instead of letting it overshoot, come back, re-overshoot, etc. We can just stop it early. After we reach a position within this epsilon, we command the leg to move slower and slower until it reaches the target.
How we do is, is basically implement a modified P-controller. The simplest of feedback control systems. Measure the leg position now, measure where we want to go, move it at a speed proportional to the error. The sign just accounts for which direction to move it.
Then, when we get super super close, we set the command to the exact command we calculated. So instead of trying to shoot towards its target and over-currenting the leg traverses to the point and then slows down and stops.
This took us THREE DAYS to figure out how to do, but finally it works.
step four: the future
Okay so I can't believe I'm saying this but that was really the easy part. Getting the leg to go to one point in space is the pre-requisite for everything this robot needs to do. What comes next is figuring out where to put the leg to do certain things, and that's no easy task. We're going to start with making the leg follow trajectories. When it can do that, we'll try to make the robot sit down and stand up on its own, when we do that we'll try a simple walking trajectory. And all of this is before we add the Jetson board. We still have a long way to go.
But we will get there! Goal is to have this thing slowly walking with just the roborio in two weeks.