Robot arm movement (trajectory) issues

[TheEpicPlasmeticGuy]

Hello there!

(skip until the problem if you just want to see the problem)
currently, I’ve been working on a robot arm side project (like ones used on car manufacturing lines) and have gotten all the mathematics and correlated servos to work! my code currently gets a destination point, and then figures out how to rotate all the servos within the arm to make the hand reach that position. i’m very happy with how the code works and has worked flawlessly at moving to any destination, and stopping when it understands a destination is physically unreachable (too far, too close to shoulder thing attached to the base, etc)

THE PROBLEM
to put my issue simply, the arm will always reach its destination (if physically possible) but the trajectory it takes to get there is the issue.

due to all the servos having the same pre set AngularSpeed, some servos will reach their final angle a lot faster than others (for example, when turning, the base will need to turn possibly up to 180 degrees, while the rest of the servos will only need to move somewhat to compensate for any slight distance the destination has moved away from the arm.)

this creates a jarring movement shown in the clip below (or, if you dont want to look at the clip, you can see screenshots of the clip in the reply section to understand the issue) when all the servos are working at “full speed”, where you can see clearly that the trail at the hand of the “Servo” simulated arm is very unnaturally past halfway through the clip as i move the destination point fast enough to require all servos to move, and meanwhile a quick replica i made of the arm using aligning constraints to achieve the same effect has an extremely straight trajectory (at least while it was working lol)

External Media

THE SOLUTION i need is a way to calculate certain values to put into each servo’s AngularSpeed, so that servos that are going to move a smaller distance can move slower to compensate for other servos that may be moving much more, and in turn give a straight line or smooth curved trajectory.

for those who may ask why i don’t use this other method, (using align constraints) i think its way more interesting to use existing hinges as actual servos to cause the movement, and also as shown near the end of the clip this illusion of working servos can be break very easily at least in the choppy way i implemented it lol

[TheEpicPlasmeticGuy]

didnt realise devforum was going to turn the Medal link into a sketchy “external media” button, oops

here are screenshots of what i intended to show in the clip for those who would rather not click on the link/want to see the evidence straight up lol

main thing to note is how smooth the desired final version mockup with the blue hand looks, in comparison to the clearly wonky trajectory of the green handed arm that is using my script to move to the destination point (the part)

[TheEpicPlasmeticGuy]

bumping let me know if you dont understand/require elaboration on anything to make a reply

[TheEpicPlasmeticGuy]

im wondering if i can fix the issue by getting the physical difference in rotation between the two attachments of a joint to eventually do some calculations to change the servo speeds depending on the difference of this “current physical angle” the joint may be at and the goal angle given to the servo. this drawing elaborates on all the angles i’ll be talking about: (the angles in this drawing are just an example)

getting the “orange” angle will mean that when i update the servos in my function, i can guarantee this is (near) the rotation of the joint right now, instead of assuming that all servos actually reached their last destination angle when the function was called, as that may not be correct in the likely case that the arm may still be moving and never actually reached it (mb for calling it a destination point in the blue text)

however trying to find this angle has really annoyed me and showed off my terrible CFrame manipulation skills apparently…

this is how i am currently attempting to get this orange angle:

1. I get the two attachments within both limbs that create the joint using a HingeConstraint
2. I get a CFrame for their relative difference in their WorldCFrame’s, and this gives me the difference in position (should always be 0,0,0) and their difference in rotation (i want this)
3. get the LookVector of this offset CFrame to get the difference between their rotation, and then use Unit on it
4. the HingeConstraint for this joint rotates its attachments around their Z axis , so i get the Z axis and then multiply it back to 90 cuz i Unit’ed the LookVector and this should give me the orange angle?

(keep in mind i am doing this relative to the attachments and not the world, so that the angle will still be correct for all three joints that may have already been rotated by the floor that the shoulder is attached to, or each other)

local AttachmentAWorldCFrame: CFrame = AttachmentA.WorldCFrame
local AttachmentBWorldCFrame: CFrame = AttachmentB.WorldCFrame
-- lets pretend that AttachmentB is in motion due to the servo,
-- but currently has been rotated to "90 degrees" so far.

local OffsetCFrame: CFrame = AttachmentAWorldCFrame:ToObjectSpace(AttachmentBWorldCFrame)
-- this is only a CFrame telling me the difference in their position + rotation,
-- i want a number for the orange angle

local PhysicalAngle: number = OffsetCFrame.LookVector.Unit.Z * 90
-- the attachments of a HingeConstraint rotate around their Z axis, so i get the Z value
-- PhysicalAngle SHOULD be the orange angle

above is the concept for my function, and below is what i am actually using right now:

the objects underlined in red and blue are supposed to be equivalent to my example of an AttachmentA and AttachmentB

i do have to add some offsets to some joint angles to correct them, but this should give me the physical rotation of each limb (orange angle)

the function works for angles that are a factor of 90:

(the black squares in the images cover values on my debugging UI that were irrelevant to the post. furthermore, these values shown are rounded to the closest integer before being put in the TextLabel)

however, this code instead returns (seemingly) random values when rotates at any other angle that isnt 90, 180 or -90. for example, i put all the servos to 120 degrees, and the arm was stationary in this image, and therefore all values should have been “120” as they are at their destination. however, what they actually return…?

help on what may be wrong in my process + code would be very helpful, please and thanks i may also close this post and create a new post in Scripting Support as this “reply” relates to the original post, but is long enough + a different problem in itself to warrant becoming its own post, right? lmk, and thanks again

[TheEpicPlasmeticGuy]

just realised a week later when watching a video that HingeConstraints have a “CurrentAngle” property which is literally exactly what i want, and i wasted 8 days doing a different project thinking i wouldnt be able to fix this issue when all i needed to do is look better in the property tab…