1. Hundreds of coffees, endless nights of debugging and coding, and countless feedback by our beta testers led to this new major release. SimTools 2.4 is probably the version with the most upgrades and improvements in a single release ever. Look at everything Dustin has included:
    SimTools 2.4 all features.
    Dismiss Notice
  2. For downloading SimTools plugins you need a Download Package. Get it with virtual coins that you receive for forum activity or Buy Download Package - We have a zero Spam tolerance so read our forum rules first.

    Buy Download Package Now!
  3. Do not try to cheat our system and do not post an unnecessary amount of useless posts only to earn credits here. We have a zero spam tolerance policy and this will cause a ban of your user account. Otherwise we wish you a pleasant stay here! Read the forum rules
  4. We have a few rules which you need to read and accept before posting anything here! Following these rules will keep the forum clean and your stay pleasant here. Do not following these rules will lead to permanent exclusion from this website: Read the forum rules.

Writing a motion cueing software from scratch.

Discussion in 'DIY Motion Simulator Projects' started by Dirty, Feb 28, 2019.

  1. Dirty

    Dirty Active Member Gold Contributor

    Joined:
    Oct 15, 2017
    Messages:
    269
    Occupation:
    All the way up front.
    Location:
    Germany
    Balance:
    2,906Coins
    Ratings:
    +245 / 1 / -0
    Hey @Trip Rodriguez,

    Sorry for the late reply.

    I'd point to the usual suspect, which in this case is: I think your physical rig does not perfectly correspond to the mathematical representation of it. Somewhere in the software you must have entered the mechanical parameters of the rig. Like the radius of the connection points of the upper or lower platforms, the servo arm lengths, their positions, angles, etc.
    The software uses this information to create a mathematical representation of your rig. Then for every frame it will calculate the servo arm angles as this virtual rig moves. When these angles are then being sent to the motors, the physical rig SHOULD be in the same position where the mathematical representation is. This does only work well if the physical rig was manufactured and measured precise enough.

    Maybe you can find measurements that are off. Check if all the parameters you entered into the 6DOF software are correct and the rods are all equally long. Are the servo motors positioned exactly where they belong? Is the spacing correct and are their axis' aligned at 120° angles?

    That's where I'd go looking....

    Dirty :)
  2. pmvcda

    pmvcda aka FlyPT

    Joined:
    Nov 3, 2010
    Messages:
    624
    Location:
    Portugal
    Balance:
    4,578Coins
    Ratings:
    +693 / 11 / -0
    My Motion Simulator:
    6DOF
    Can't make it now, but tomorrow I will post the code if you want it.
    Also explain how I made the calculation.
    • Winner Winner x 2
  3. pmvcda

    pmvcda aka FlyPT

    Joined:
    Nov 3, 2010
    Messages:
    624
    Location:
    Portugal
    Balance:
    4,578Coins
    Ratings:
    +693 / 11 / -0
    My Motion Simulator:
    6DOF
    So how do I make the calculation (this method works when the motor shafts are horizontal):

    This was the image I posted:

    IMG_20190404_174333.jpg

    Can't find it also... It's buried somewhere.

    So...

    In black:
    Point B (origin) is the intersection between the crank and the motor shaft. It's a solid connection. Like I said above, it's the bottom joint.
    O is the joint that connects the crank to the rod and she can rotate in any direction.
    T is the joint in the top moving platform.
    C is the crank length.
    R is the rod length.

    In gray:
    The 3 axis (x,y,z) with x and y defining the horizontal plane.
    The circle that represents the rotation of the crank.
    L is the distance between the bottom joint (origin) and the joint on the moving platform (T). That's where we would have a linear actuator.

    In red:
    A is the angle of the crank relative to the horizontal plane (what we want to calculate)
    B is the angle defined by the intersection of the horizontal plane with the plane where we have the crank rotating.

    So to define this we need...

    Fixed values:
    R = Rod length
    C = Crank length
    B = Angle of the motor axis + 90º around the z axis (90º, because crank is perpendicular to the motor axis)

    Constraints:
    Motor axis must belong to the horizontal plane

    Variables:
    L, the distance between the bottom and the top joint

    As we see in the image, we can calculate the point O with the system of equations I show.
    The the distance from O to T would be defined by the equation bellow.

    Replacing Ox, Oy and Oz and solving for A gives you more than one solution.
    Since I'm "lazy", I used this: https://www.wolframalpha.com/input/?i=solve+for+a+{r^2=(x-c*cos(a)cos(b))^2+(y-c+cos(a)sin(b))^2+(z-c+sin(a))^2}

    The solutions can be ignored and we just use the 3rd one.
    I don't remember now, but I took a look at all solutions and concluded that 3rd and 4th can be used without problems if crank and rod are more than zero. (not 100% sure. Would have to look again)

    So the code is (optimised, but maybe it could be more):
    Code:
       private double _storedAngle = 0; // We store calculated angle to use it if the new calculation has no solution (we stay where we where)
    
        private double calculateAngle(Point3D P)
        {
            double x = P.x - _fixedJoint.x;
            double y = P.y - _fixedJoint.y;
            double z = P.z - _fixedJoint.z;
            double a1 = 2.0 * _crank * x * _verticalAngleCOS;
            double a2 = 2.0 * _crank * y * _verticalAngleSIN;
            double a3 = _crank * _crank - _rod * _rod + x * x + y * y + z * z;
            double a4 = a1 + a2 + a3;
            double angle = 2.0 * (Math.Atan((2.0 * _crank * z - 0.5 * Math.Sqrt(16.0 * _crank * _crank * z * z - 4.0 * (-a1 - a2 + a3) * (a4))) / (a4)));
    
            // CALCULATION IS DONE
            // NOW LET'S SEE IF WE HAVE A SOLUTION
            // IF WE HAVE A SOLUTION, LET'S CALCULATE THE CRANK/ROD JOINT JUST TO SHOW IN THE INTERFACE
    
            // If we have a valid angle
            if (!double.IsNaN(angle)) // Testing if the result is a number
            {
                _outOfRange = false;
                _rodCrankJoint.x = _crank * Math.Cos(angle);
                _rodCrankJoint.y = _fixedJoint.y + _rodCrankJoint.x * _verticalAngleSIN;
                _rodCrankJoint.x *= _verticalAngleCOS;
                _rodCrankJoint.x += _fixedJoint.x;
                _rodCrankJoint.z = _fixedJoint.z + _crank * Math.Sin(angle);
                _storedAngle = angle;
            }
            // Not possible to calculate an angle, completely beyond reach, no result, so we stay at the last possible position and that was the _storedAngle
            else
            {
                _outOfRange = true;
                angle = _storedAngle;
            }
            return angle; // In radians
        }
    Where P is the top joint point for who we want to calculate the angle
    _fixedJoint is the coordinates of the point B. We are just dislocating the P to be relative to origin
    _verticalAngleCOS and _verticalAngleSIN are cosine and sine of the angle B refered above
    _crank is the length of the crank
    _rod the length of the rod
    • Winner Winner x 2
    • Informative Informative x 1
    Last edited: Aug 2, 2019
  4. Dirty

    Dirty Active Member Gold Contributor

    Joined:
    Oct 15, 2017
    Messages:
    269
    Occupation:
    All the way up front.
    Location:
    Germany
    Balance:
    2,906Coins
    Ratings:
    +245 / 1 / -0
    Holy smoke,...

    Yes, that was the picture I remembered. Thank you very much. Quite a lot to chew on. But I'm sure it will save me a lot of time.

    The first question (of potentially many to follow) would be: Is B the center of the circle/disk described by O? Or better: Does B lay in the plane described by the rotating position O?

    The reason I'm asking is, I think it would make sense if it was, but then the (red) angle B would have to be zero.

    I will need a little time to digest this, I guess. :)

    Thanks a lot! Btw, super cool of you to share code! I really owe you for that.

    Dirty :)
  5. pmvcda

    pmvcda aka FlyPT

    Joined:
    Nov 3, 2010
    Messages:
    624
    Location:
    Portugal
    Balance:
    4,578Coins
    Ratings:
    +693 / 11 / -0
    My Motion Simulator:
    6DOF
    It was a bad choice of letters... But that was the drawing.
    B is the origin and center of the circle.
    Angle B is the the...
    If we look at the rig from the top, the circle defined by the rotation of the rod/crank joint that has it's center in point B, makes the angle B around axis z

    Hope it's clear...
  6. Dirty

    Dirty Active Member Gold Contributor

    Joined:
    Oct 15, 2017
    Messages:
    269
    Occupation:
    All the way up front.
    Location:
    Germany
    Balance:
    2,906Coins
    Ratings:
    +245 / 1 / -0
    Hey @pmvcda,

    I think I understood the geometry.

    And I think I understood the math up until here:
    Bildschirmfoto 2019-08-11 um 16.55.40.png

    So you can calculate angle "a" (in radians?) with this formula:
    Bildschirmfoto 2019-08-11 um 16.55.40 2.png

    ...as long as you make sure that this term is not zero:
    Bildschirmfoto 2019-08-11 um 16.55.40 3.png

    ...and this term is not zero:
    Bildschirmfoto 2019-08-11 um 16.55.40 4.png
    ...and you plug in any integer for "n".
    Bildschirmfoto 2019-08-11 um 16.55.40 5.png


    Can you tell me if this "n" is important? As far as I can see, I can just ignore the "+ π n" Term at the end, right? I assume it is in there, because the tan function will repeat every π anyways. And the factor of 2 in front of the bracket means that the solutions will repeat for every revolution. Do you agree?

    What IF those two terms become zero? What does it mean for each of the two cases? Do you just stop and hold the last "good" position?

    Thanks for your help!

    Dirty :)
  7. Dirty

    Dirty Active Member Gold Contributor

    Joined:
    Oct 15, 2017
    Messages:
    269
    Occupation:
    All the way up front.
    Location:
    Germany
    Balance:
    2,906Coins
    Ratings:
    +245 / 1 / -0
    OK, I think now I understand you code as well.... :thumbs

    Is...
    Code:
    if (!double.IsNaN(angle))
    ...really all the checking you do? Is this sufficient? I mean it's short, and if it covers all cases then it sure is an elegant solution.

    The only thing I am still struggling with is the fact that there should always be two solutions. Two angles "a" that satisfy the equations. Did you ever run into problems with these equations when you use them in your graphical representation in your software?

    Dirty :)
  8. Dirty

    Dirty Active Member Gold Contributor

    Joined:
    Oct 15, 2017
    Messages:
    269
    Occupation:
    All the way up front.
    Location:
    Germany
    Balance:
    2,906Coins
    Ratings:
    +245 / 1 / -0
    One more thing about the terms "a1" to "a4" in your code:
    Code:
    double a1 = 2.0 * _crank * x * _verticalAngleCOS;
            double a2 = 2.0 * _crank * y * _verticalAngleSIN;
            double a3 = _crank * _crank - _rod * _rod + x * x + y * y + z * z;
            double a4 = a1 + a2 + a3;
    You're using variables called "_verticalAngleCOS" and "_verticalAngleSIN".

    In the equation (from WolframAlpha) it reads... Bildschirmfoto 2019-08-11 um 16.55.40.png cos(b) and sin(b) . With b being the horizontal angle.

    Am I correct to assume that it should definitely be angle "b" in these places? Any reason for calling those variables "_vertical angleCOS/SIN"?

    Dirty :)
  9. pmvcda

    pmvcda aka FlyPT

    Joined:
    Nov 3, 2010
    Messages:
    624
    Location:
    Portugal
    Balance:
    4,578Coins
    Ratings:
    +693 / 11 / -0
    My Motion Simulator:
    6DOF
    Sorry, I'm full of work and on the phone..
    Answering fast, but after tomorrow I will take a better look.
    n*PI is turns, so we can ignore n.
    In the code the idea is to calculate only 1x, something we use multiple times. That's the reason for the terms. Same for the vertical Angles, instead of calculating the cosine multiple times in the loop, I calculate it once, when the user changes the vertical angle. From then, it's fixed or pré calculated.
    The multiple solutions, I don't remember, I have to take a look, but I think it was zero, only if the rod or crank was zero, and that doesn't happen by design. (not sure here, but I think it was that)
  10. pmvcda

    pmvcda aka FlyPT

    Joined:
    Nov 3, 2010
    Messages:
    624
    Location:
    Portugal
    Balance:
    4,578Coins
    Ratings:
    +693 / 11 / -0
    My Motion Simulator:
    6DOF
    I'm mixing the angles when I talk about them. B is the angle in the horizontal plane, for the vertical axis. Hope I can explain it...
    In the equation c and r are cranck and rod
    • Agree Agree x 1
  11. Trip Rodriguez

    Trip Rodriguez VR Pilot

    Joined:
    May 8, 2016
    Messages:
    430
    Location:
    Lake Ariel, Pennsylvania
    Balance:
    2,026Coins
    Ratings:
    +194 / 2 / -0
    My Motion Simulator:
    6DOF
    I had another thought guys... something I've noticed that causes harsh cues is when multiple cues "stack up" on the same actuators and directions.... So like if you have a big heave up cue and a big pitch up cue the two front actuators get double the signal and maybe try to move up twice as fast? I say this because I can set to where I'm happy with heave, and set to where I'm happy with pitch, but then I turn them both on at the same time and the cue starts much too harshly.

    What do you think? If you agree that this is a thing, maybe worth thinking about the best way to deal with it.
  12. pmvcda

    pmvcda aka FlyPT

    Joined:
    Nov 3, 2010
    Messages:
    624
    Location:
    Portugal
    Balance:
    4,578Coins
    Ratings:
    +693 / 11 / -0
    My Motion Simulator:
    6DOF
    @Dirty

    So, I took a better look.

    The conditions to have zero don't happen, only if you have c or r (crank and rod) zero.
    That could cause the top joint to match the rod/crank joint and make a division by zero.
    By design, this kind of mechanism wont work having those values zero at the same time.

    The n*PI is ignored, that's what gives you multiple results. At each half turn (PI) you have a possible result.
    I ignore it, because we always want the lowest n. We want the closest possible angle near zero or the result could be strange.
    Imagine we are going up and you use the 2nd solution, that would be 1*PI.
    The rig would go up and then start going down to reach that 1*PI, while in the first solution it just goes up and stops. That's the effect of going through all the circle, you need to pass the highest value to reach the next 1*PI.
    Same happens going down. So we want the solution closest to zero. That's 0*PI.

    The way I test the result to know if it's valid is for me the fastest one. You just test to see if you have a number as a result.
    If there's no solution it returns nan (not a number).
    After getting a result I also make other verification's to know if the angle is in the range defined by the user. That is not on the code, but it's simple.
    If angle is not valid, I keep the rig in the last one.
    • Informative Informative x 1