chip updates: FINALLY USING THE IMU and "I give up mode" [updates]
So here's what we're going to do, we're going to put the robot in test mode and we're going to have it stand up, and the joysticks are going to control the weight shift. We'll use buttons to select the LEG, we'll use another joystick to control the leg. We're doing this to see how far over the legs need to shift for there to be a weight transfer from the back to the front in this new standing configuration.
But before we do that, we're going to run the last test that we were going to run yesterday, which was to pick up a foot off the ground by just moving the knee joint. So let's do that.
if(MODE==CONTROL_MODE.TEST): #then publish stand/sit commands if(LB==1.0): COMMAND = [0.05, 0.5, 0.0, 0.05, 0.5, 0.0, 0.05, 0.5, 0.0, 0.05, 0.5, 0.0] CMD = doIK(COMMAND) CMD = CMD-10.0 publish_CMDS(CMD) if(RB==1.0): COMMAND = [0.05, 0.5, 0.0, 0.05, 0.5, 0.0, 0.05, 0.5, 0.0, 0.05, 0.5, 0.0] publish_CMDS(doIK(COMMAND))
Now for the video:
So as we can see that clearly did not work. The robot fell forward onto its foot that it lifted. Which means again, something is going on here that we aren't realizing. The CoG model we have isn't accurate enough to figure out where the feet should go, we need to do some predictive control for the weight shifting and the rest needs to be up to the IMU. We do have an IMU for a reason and we intend to use it so it might be time to figure out how. Now the most important thing we do know is, that if the platform is falling forward we need to shift all the feet forward and if its falling back we need to shift the feet back. This interferes with trajectory so we need to come up with a clever way for all of this to happen while not interrupting how trajectories operate.
Now we have an IMU callback method that takes in roll-pitch and yaw and saves it as an orientation value. But what we'll likely do is have this callback just be the stabilizer for the entire platform. When the RPY is very small, it doesn't do anything. But when it gets to be larger, it will replace whatever is in trajectory or it will publish a new CMD!
Let's do it that way!
First, let's see if ORI works and what directions are the correct ORIS.
IMU ORI follows [roll, pitch, yaw] Pitching UP is (-) pitch on the IMU Rolling RIGHT is (+) roll Yawing RIGHT is (-) yaw
So now that we have the correct ORIs. We know that if we see the PITCH (ORI) becoming negative we need to move the feet BACK, if we see it becoming positive we need to move them FRONT. And that's ALL the imu is going to do to attempt to stabilize the platform for now.
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.
So that's what we will do with the IMU since the CoG of the platform is so unpredictable. This should help us deal with things like the platform falling over and stop it (mostly) from doing so.
So now there are two questions. There's the question of how do we decide thresholds, and then there's the question of how do we decide the response itself. Let's first work on the thresholds. The first threshold would be the range above which we should start correcting the platform's falling. So let's make that 0.02 because why wouldn't we correct from early on. This threshold also serves as the ZERO threshold, below what value is ZERO. During standing the highest the platform achieves is 0.12 so we want to give it some threshold so it doesn't like stabilize when it's really fine.
The second threshold is the point past which we can't make the platform stabilize and not fall by moving the legs. To figure that out, let's inspect some things. Watch it stand and etc.
Now for the third threshold. This is the point where the platform is so far off balance, we'd need to sit and stand back up again to regain the feet positions. We're going to make this value 0.2 because, this is what 0.375 looks like and that's well-beyond the rescue point.
And this is what 0.25 looks like.
Both of these are well beyond the "save me" area. It turns out 0.16 is equally as bad so lets make the "save me" area like 0.12 past that we sit down and try again. So let's start by coding in that. If we pass the 0.12 threshold no batter the direct we sit down and try again.
#handles getting the current actual platform RPY def imu_callback(data): #set the current PITCH of the platform PITCH = data.data if (abs(PITCH) >= 0.12): #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 = [0.05, 0.2, 0.0, 0.05, 0.2, 0.0, 0.05, 0.2, 0.0, 0.05, 0.2, 0.0] trajRunner.clear(COMMAND) trajRunner.addWaypoint(doIK(COMMAND)) trajRunner.addWaypoint([-8.0, 0.0, 0.0, -8.0, 0.0, 0.0, -8.0, 0.0, 0.0, -8.0, 0.0, 0.0]) trajRunner.addWaypoint([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) trajRunner.addWaypoint(DEFAULTS.STARTING_CONFIG) #then it waits for human command to stand up again elif (abs(PITCH) >= 0.05): pass elif (abs(PITCH) >= 0.02): pass global ORI ORI = data.data
So all this code does so far is say okay if the platform leans more than 0.12 radians I'm going to sit down. Let's try it!
Yessssssssssss that workeeddddddddddddd which means we can stop worrying about things like the platform trying to kill itself after a point. It'll just sit. Which is good. And it'll only stand-up again once we tell it to. Now of-course for validity, I want to try it doing this in the reverse direction too. Then what we'll likely do it save, turn off, and give it a break because we're really working it right now by making it fall over. And I don't want to destroy anything. We're not going to video this because the response should be the SAME.
Yes so the platform did sit down and it actually turned off this time. I felt inside and he was getting a little hot which is the same thing he did yesterday when we wanted a break. So let's give the robot a break and come back. We might need to play with the zeroing. But that's OK.
I call this "I give up mode" !!!!! We might drop that upper threshold to 0.11 or 0.1-ish.