deʃhipuAfter a whole day of debugging I finally found the error in mu routines that made the circle too small, and generally bad positions. Turns out that to compute the angle of the arm, you need more than just the distance to the target point -- because the arm+rod combo also need to be pointing towards that point. So you need to know the distance and the height.
I couldn't spot the mistake, because in all the pictures I drew, I drew the arm in approximately the same position, in which, by coincidence, the height was just right. But in the general case it won't work. So here are the corrected inverse kinematics routines:
def arm_ik(self, distance, z):
alpha = math.asin(z / distance)
beta = math.acos(
(distance ** 2 + self.ARM2ROD2) / (2 * self.ARM_LENGTH * distance)
)
return math.pi - alpha - beta
def robot_ik(self, x, y, z):
return tuple(math.sqrt(
(self.BASE_X[arm] - x) ** 2 +
(self.BASE_Y[arm] - y) ** 2 +
z ** 2
) for arm in (0, 1, 2))
def move_to(self, x, y, z):
for arm, distance in enumerate(self.robot_ik(x, y, z)):
self.move(arm, self.arm_ik(distance, z))
Oh, I also separated the ARM2ROD2 = ARM_LENGTH ** 2 - ROD_LENGTH ** 2 constant, so we don't have to recompute it each time.
Discussions
Become a Hackaday.io Member
Create an account to leave a comment. Already have an account? Log In.
? there are always right triangles from a ground plane. The only time you'll ever have trouble is when you have three angles and nothing else, then you really cant figure out where anything is.
This is Pythagorean. See #MillBits Engine for the math.
https://youtu.be/uR5sS0br1dM
Are you sure? yes | no
It's true that you can always draw additional triangles wherever you want, but you won't know their dimensions, and thus they are useless for calculating anything. You only need two angles for a triangle on a plane -- the third is always whatever is needed to get the sum to 180°. But you also need at least one side for the scale (on a sphere three angles define a triangle and its scale too). The law of cosines lets you solve pretty much any triangle as long as it is solvable at all.
Are you sure? yes | no
Radomir, did you look at the video. My drawing routine calculates the position of every single pixel on the screen. I provide it with the width, height, depth and trough sharpness of one tooth, and how many, and it calculates everything using right triangles.
There are many ways of calculating that, some easy, some hard. You've quotients, products and reciprocals besides pythagoreans. Personally I prefer to do a simple conversion between polar and cartesian :
W=COS(theta)R , H=SIN(theta)R : theta=ATN(H/W), R=SQRT(SUM(W squared+H squared))
and be done with it, thats how I think, and how I designed Cardware in my head and then drew it on cardboard, I do not use proprietary anything except Inkscape, a 2d Vector Art package, and a lot of 'useless' geometry.
Yeah, I nearly had a cow too, when I learned about things they didnt teach me in Maths, some 30 years ago that was now tho.
Are you sure? yes | no
The thing is, your program is calculating forward kinematics, so to speak -- given the angles, you calculate the positions. Here I have an inverse problem -- I already know the position, and I need to figure out what angle of the servo corresponds to that. Since the robot is so simple and the number of servos is exactly the same as the number of degrees of freedom, I can get a closed-form solution, and that's what I aimed for -- because that's the fastest and most accurate way. With more complex configurations it is no longer possible, and you have to fall back to basically running an optimization algorithm on the forward kinematics, to "brute force" the inverse kinematics -- I think that your approach would work well in that case. But that's an iterative algorithm, so it takes time and the precision depends on how many iterations you do. It could still be faster on a small microcontroller than the advanced floating point calculations -- especially if you implemented it using only integers, a la Bresenham's algorithms, so it would a viable solution.
Are you sure? yes | no
Iterative, yes, and self deterministic. My solutions come out of a wider remit is all, I'm trying to build an 'intelligent' system, so thats reflected in how I handle the fundamentals. You think you have trouble, I'm now well into the build for our stereoscopic vision system. I've got two cameras I have to point at the same thing, based on where they are in the frame. Obviously when I point the cams right at it, theyre in the same place so I have to compute range from the servo, not parallax in the video. And yes, its made entirely of cardboard...
Are you sure? yes | no
I'd have approached that just with right triangles in the first place. Yes its harder but by the time you've computed the end coordinates you have the full set from the base for reference... SOH-CAH-TOA ;-)
Are you sure? yes | no
I'm not sure how you would calculate beta with right-angle triangles -- there are simply none in that part of the image.
Are you sure? yes | no