chip updates: doing more with the IMU and we're using the terrain offset this time [updates]
So at the moment, the zeroing for chip is a little off so we're actually using a terrain offset of -0.25 to compensate. Let's see if that works. At max we'll bump that up to -0.5 because -1.0 made the platform fall backwards.
Yea, I think we actually are going to bump that up because -0.25 make the platform lean a little too far forward for my taste. -0.5 is too far back for my liking, so let's make it -0.35?
For reference, this is what we're looking for. This is what a GOOD stand looks like:
So -0.35 in this case is what we were looking for to get a good standup. -0.5 to +0.5 is probably the range of terrain offset we're looking at at any given time. All the offset does is fix any small error caused by slop when setting the zero. So the turn on procedure is:
(1) Zero the platform.
(2) Turn on and set the terrain offset to zero.
(3) Try to make platform stand.
(4) Tune the terrain offset until standing is to your liking. (- if platform is falling forward, + if it is falling backward)
Now then! Now that we've been tuning the stand up and it stands well, we can get back to yesterday's endeavor which was the IMU stuff.
Pseudo-code: if the pitch is larger than a certain threshold (small) value: then we'll move the shoulder joint a certain number of rotation in the correct direction to stabilize the platform (ALL SHOULDER JOINTS). Then we will edit trajectory so it knows that that the correct place to be. if the pitch is larger than a medium threshold value: then we will send the platform back to standing position and clear the trajectory. if the pitch is larger than a larger threshold value: we will make the platform SIT, we will clear the trajectory, and we will attempt to stand again. We will also BOUND the response of the shoulder in the parts so that the platform doesn't fall in the other direction.
That's the pseudo-code from yesterday. We've completed the last part of the three if statements, here it is: https://www.adim.io/post/chip-updates-finally-using-the-imu-and-i-give-up-mode-updates
We should also remind ourselves of the following
IMU ORI follows [roll, pitch, yaw] Pitching UP is (-) pitch on the IMU Rolling RIGHT is (+) roll Yawing RIGHT is (-) yaw
We're mostly concerned with PITCH at the moment. And we're separating the different responses using the absolute value of the PITCH of the platform and dealing with the responses according to the sign of the PITCH.
So now, let's try the first one. The reason we're going in this order is because once we determine the bound at which the first method can no longer work, we can find the bound for the second of the three statements.
If pitch is negative we need to move the feet back. If pitch is positive we need to move the feet forward. So if the pitch is negative we are DECREASING theta_1 and if pitch is positive we are INCREASING theta_1. So all we're going to do is add the pitch to the shoulder command and multiply by 5. Start with k=5!
elif (abs(PITCH) >= 0.02): kp=5 COMMAND = list(POS) COMMAND = COMMAND+kp*PITCH COMMAND = COMMAND+kp*PITCH COMMAND = COMMAND+kp*PITCH COMMAND = COMMAND+kp*PITCH publish_CMDS(COMMAND) trajRunner.saved = COMMAND trajRunner.waypoints = COMMAND
So here's what we came up with for that. If the pitch is greater than 0.02 we'll just add 5* the pitch to all the shoulder commands, publish that, and replace the trajectory point. But only if the length of trajectory is greater than 0...
We're going to test this with the front leg this time just to see if we can get the platform to stabilize.
Let's try it, the worst that can happen is it won't react fast enough to stabilize. Then we can try increasing the Kp and if that doesn't' work we know we'd need to do some predictive control as well.
In another classic CHIP moment, he just fell over. Maybe we need to drop that power supply to 24V or something. Maybe it was just a bad moment who knows. Maybe he was hot. Let's let the platform cool down for a while and come back.
I think we do need to turn this stuff off during stand sit, or at least increase the threshold for the IMU's lower bound. It was messing with the stand-up so we turned it off. I'm not sure if it's going to work this time round but we will try to put it in test mode and see if it does anything. We're going to leave Kp at 1 for now just for kicks.
Let's try kp = 5 and take a video... we might need to re-think this one.
Now here's an idea. What if we don't even touch trajectory...
elif (abs(PITCH) >= 0.025): kp=5 COMMAND = list(POS) COMMAND = COMMAND+kp*PITCH COMMAND = COMMAND+kp*PITCH COMMAND = COMMAND+kp*PITCH COMMAND = COMMAND+kp*PITCH publish_CMDS(COMMAND)
The trajectory manager will keep publishing till it gets to the point. But for now, what if we only publish commands to keep the platform stable, let's try it! I don't think it'll work, but let's try it.
Actually let's not because this really isn't going to go well. We need to re-zero the platform anyways.
Yes so, I think with the IMU, for now, we are done in that stabilization with the IMU isn't going to work. But step two might work which is to make the robot move back to the standing position if the IMU says it is within saving distance from standing, let's make that >=0.8. So let's do the second one is what I'm saying. Let's code it here, give the robot a bit of a break, and then turn it back on and try it.
#we're just going to copy the other one and say "hey if this happens go to standing position"
Let's turn the robot on and try it! Later we'll modify it so that this works at any stable position. First let's try this.
elif (abs(PITCH) >= 0.08): #first let's set the control mode to stand-sit global MODE MODE = CONTROL_MODE.STAND_SIT #then let's clear the trajectory and add some points COMMAND = STAND_COMMAND trajRunner.clear(COMMAND) elif (abs(PITCH) >= 0.025): pass else: global STABLE STABLE = CMDS
WE'RE NOT USING STABLE RIGHT NOW. JUST IGNORE THAT.
So this is with bringing it to a standing position. What I now want to do is just leave it at the last known stable position. So what we've done is already edited the code such that when we call publish_CMDS() it updates a global variable that tells us the last published command.
So all we need to do is now decide what the last stable published command is. We can do this by editing the command we get to see if the platform is experiencing any velocity at the moment.
So we edit the ORIENTATION data to contain the angular velocities of x y and z for the IMU. And then we also edit the code like this...
elif (abs(PITCH) >= 0.09): #first let's set the control mode to stand-sit global MODE MODE = CONTROL_MODE.STAND_SIT #then let's clear the trajectory and add some points COMMAND = STABLE trajRunner.clear(doIK(COMMAND)) else: if(vx==0 and vy==0 and vz==0): global STABLE STABLE = LAST_PUB
So if all the velocities are also zero, we can set the last stable value to the last published command only if the platform's angle is also stable. Let's try this.
We should now have all the safety measure in place that we need. We're going to run a few tests on this piece of code. Let's make the platform stand first.
So for some reason, what happened is the last stable point wasn't what we expected. I think that's because we set LAST_STABLE in the shifted coordinate frame, not the regular coordinate frame. So let's fix that real quick and then we're going to give the robot a break. Because the robot has fallen too many times today... and the last time was because I stepped on the tether oops. And he's getting little hot so lets' fix this really quick and then just turn off the robot.
I do think this stable thing is going to work when we try it next after fixing this issue.
So as you can see, we changed the last published thing to CMDS not CMDS_NEW, and we also posted the latest IMU code. We also "scp'ed" everything over to GitHub so that branch is updated as well. We'll continue working tomorrow I think this is good progress for a day.