chip updates: keeping the legs within their range of motion [updates]
So we've been noticing time and again there are certain positions we command the robot's legs to go to and the system just breaks down, over currents, you hear a click and the robot just turns off. We want to figure out (a) why that is in the case where the robot is in the workspace and (b) why that is when we command the robot to go outside the workspace. I think the manual answers to the kinematics problems will provide some inside. We'll account for how we are actuating the robot as well.
As we can see. We have tried to command a few points outside the circles of all-being over here. So we've found a solve that's robust and we're placing it inside inverseKinematics();
What this will do is instead of commanding a foot position outside of the range of possible reach of the legs. We will clip rSquared by the minimum and the maximum possible radii. Once it's clipped in this way, we'll still compute phi the same way so we end up commanding a foot position that is in the same direction as the commanded one, but within the workspace of the robot because that's likely the intent of a command like that.
We've also clipped "z" by a magnitude of 0.1 so we never reach the realm of hitting the legs against the body. This should solve some of the overcorrecting problems we are experiencing. The problem before is we were clipping by axis in ROS and not accounting for the circle.
Here's a video of it working... notice the test case of 0.6m which is definitely outside the leg's max range.
So here, we commanded the robot's leg to move to a point outside its range of motion. Instead of over-currenting and shutting off because the motors could not move to a point called "None" or "Infinity" or "No Solution," or whatever inverse kinematics was throwing out from that. It moves to the closest point to the target position within the range of motion.
We'll likely do some more testing on this but for now this solution is better than watching the robot shut itself off anytime we mess up. This also accounts for both x and y at the same time instead of the previous idea which was to limit the x and y axis independently. z is independently limited but this should not be an issue because the z position references the commanded y position after accounting for this radius thing which means if we limit the commanded y and then calculate z, we will always find a solution for z and we can limit z on its axis instead of in 3-D.
And since the forward and inverse kinematics are calculated separately, when the platform requests for the foot position currently and we have commanded it to go to a position outside its range, the method reads the actual values of the motor positions and uses the values of the legs to re-calculate leg endpoint position, it doesn't assume the foot is at the commanded position. So this limiting shouldn't affect any feedback-control algorithms we will develop. Not that those algorithms should be commanding positions outside the range of motion.