Welcome to Keen Software House Forums! Log in or Sign up to interact with the KSH community.
  1. You are currently browsing our forum as a guest. Create your own forum account to access all forum functionality.

Kinematics - Kinematic measurement helper

Discussion in 'Programming Released Codes' started by plaYer2k, Feb 14, 2016.

Thread Status:
This last post in this thread was made more than 31 days old.
  1. plaYer2k Master Engineer


    this is a helper class that makes it easier to track and read simple kinematics (transition, velocity, acceleration and jerk for translation and rotation) of one entity relative to another.

    Possible applications:
    • meassure your ships translational velocity and acceleration
    • meassure the velocity of your targeted ship relative to your own in a free floating scenario (inertial dampeners disabled)
    • meassure the velocity of astronauts within your ship to predict their movement for certain tasks
    • meassure the velocity and acceleration of floating objects as projectile for gravity canons
    • meassure the rotational velocity of a rotor to use rotor gimbals as sensor
    • meassure the rotational velocity of a suspension wheel to throttle it down and prevent that naughtly wobble effect when it loses friction
    • drone stuff :p

    The code below contains the Kinematics class aswell as a simple example that gets the ships velocity, acceleration and jerk for translation and rotation relative to worldspace (reference == null) and displays it onto all text panels.

    The exposed properties are
    	public VRage.ModAPI.IMyEntity Anchor { get }
    	public VRage.ModAPI.IMyEntity Reference { get  set }
    	public MatrixD StateCurrent { get }
    	public MatrixD StateLast { get }
    	public MatrixD Transition { get }
    	public Vector3D TransitionLinearCurrent { get }
    	public Vector3D TransitionAngularCurrent { get }
    	public Vector3D TransitionLinearLast { get }
    	public Vector3D TransitionAngularLast { get }
    	public Vector3D VelocityLinearCurrent { get }
    	public Vector3D VelocityAngularCurrent { get }
    	public Vector3D VelocityLinearLast { get }
    	public Vector3D VelocityAngularLast { get }
    	public Vector3D AccelerationLinearCurrent { get }
    	public Vector3D AccelerationAngularCurrent { get }
    	public Vector3D AccelerationLinearLast { get }
    	public Vector3D AccelerationAngularLast { get }
    	public Vector3D JerkLinearCurrent { get }
    	public Vector3D JerkAngularCurrent { get }
    	public Vector3D JerkLinearLast { get }
    	public Vector3D JerkAngularLast { get }
    If the Reference is null, the worldspace is used as reference.
    Unlike the anchor, the reference can also be reassigned on the fly. The properties will however display errorous values for the next update.

    The Kinematics instance has to be updated for its properties to update aswell.
    Inaccurate values for the passed time (the update parameter) will lead to inaccurate properties so take care.
    Due to the currently flawed ElapsedTime discrepancy as simspeed drops, it is best to update each game tick with constant 1/60s steps.
    However, updating once per second or once based on certaion events is acceptable too but then the simspeed has to be approximated or ignored.

    The rotations are also each absolute for their projection onto a plane spanned by two euclidean base vectors (+X, +Y, +Z). Thus the rotation vectors do not represent rotation vectors like used by the games gyros. They however allow to be used individually that way.

    If any of the properties or general informations are not precise enough or functions could be added/changed, dont hesitate to tell/ask me.
    I hope this makes it easier for some to build more complex things and not redo the tedious task of doing such things youself.
    Last edited: Feb 14, 2016
    • Like Like x 2
  2. Lynnux Junior Engineer

    Of course, I tried this ;):
    What I observed:
    Values are inverted (speed of the vehicle is negative) with remote control as anchor and null as reference.
    Wheels begin with negative angular speed down to approx. -100 rad/s. Thereafter the rotational speed increases to 37.7 rad/s. So the detected rotation is inverting. I had the same effect on my Salamander vehicle after the wheels have been improved (used the suspension wheel for WASD control of rotor mounted wheels).
    Anchor is the wheel, reference the according suspension.
    The angular speed is increasing further up to approx. 90 rad/s when steering.

    On a vehicle with wheels in the air, pressing "w" all the time:
    I observed that only the Y-value of the JerkAngularCurrent was changing when the wheel accelerated whereas during wobbling especially the X- and Z-values were high.
    So I checked |JerkAngularCurrent.GetDim(0)| > 30.0 or |JerkAngularCurrent.GetDim(2)| > 30.0 on one wheel and switched off the propulsion in this case and switched it back on otherwise.
    At the beginning all wheels wobbled the same but after some seconds the controlled wheel seemed to "snap in" and thereafter it was not wobbling anymore.
    After applying the brakes and repeating the process the same behaviour occured.
    I.e. wobbling at the beginning can't be prevented.

    Is there a possibilty to get the entity of a wheel without using a sensor ?

    My test setup:

    The Tigershark vehicle is well suited for those tests with the pistons and two spare programmable blocks.
    When I load the containers with 180 tons, drive 300+ km/h on the lake and then steer to one side, the wing in front of the inner front wheel will be destroyed due to wobbling. This still happens with the prevention algorithm described above.
    Last edited: Feb 16, 2016
  3. plaYer2k Master Engineer

    Do you mean that the Z value of the Velocity/Acceleratin for linear movements is negative? If so, that is correct. +Z is actually backwards while -Z is forward. +X is right and -X is left. +Y is up and -Y is down.
    Thus if you move forward, you get a negative Z value.

    Here are the defined vectors with their respective values
     Vector3D.Up = new Vector3D(0.0, 1.0, 0.0);
    Vector3D.Down = new Vector3D(0.0, -1.0, 0.0);
    Vector3D.Right = new Vector3D(1.0, 0.0, 0.0);
    Vector3D.Left = new Vector3D(-1.0, 0.0, 0.0);
    Vector3D.Forward = new Vector3D(0.0, 0.0, -1.0);
    Vector3D.Backward = new Vector3D(0.0, 0.0, 1.0);
    The problem with suspension wheels also is that they can simply spin too fast. Suspension wheels spin faster and faster so that the roational velocity first increases, then decreases, goes negative and so forth.
    So over time it does more than a whole revolution per game update. At high enough revolutions per game update it starts with the weird and violent wobbling.
    100 rad/s is about 16 revolutions/s = 16 revolutions/s * 1/60 s/update ~= 1/4 revolution/update
    However as i use sine for the caluclations, the sine of a 1/4th revolution in any direction is actually the max.

    So the method sure falls flat for insane velocities due to the computational errors but i think that is okay. I mean,

    The rotational velocity/acceleration/jerk is negative when it is in negative mathematical rotation direction. In positive mathematical rotation the value is positive aswell.

    There is a small overview

    None i know about. I wish there would be a proper method for it. Same applies to rotors and pistons that lack this feature aswell.

    As for the suspension throttling to negate the wobbling, i would suggest to monitor both velocity and acceleration and manage power aswell as the "propulsion toggle" based on these two metrics.
  4. Lynnux Junior Engineer

    Oh, yes, sometimes going to bed is a good thing ;-) I didn't realise that the inverting rotational speed is the effect of undersampling until then.
    And to detect the wobbling the way I tried would be a generic way. A much easier method is maybe just to limit the max. speed of the wheel then.
    I have to try that later.

    The difference between detecting subgrids on rotor, pistons and wheels is that wheels currently cannot mount further blocks. So there is no terminal block which can be used to find that subgrid.
    This may not be the case with rotors and pistons either, but there is the possibility to at least mount a camera, soundblock or sensor to a subgrid just to find it via the GridTerminalSystem.
  5. plaYer2k Master Engineer

    You can get terminal blocks on the "wheel grid". You just need to remove the wheel from the wheel suspension and let a rotor head connect to it. That way you got a working rotor head with mount points on the rotating wheel grid and you can place anything you want there.

    This is especially funny when you simply place a few blocks and then passenger seats to it. Spinning at half a revolution per game update for max efficiency is best :D
  6. Lynnux Junior Engineer

    I'm still using this class. Updated it according to the changes of ingame programming and changed the constructor to

    public Kinematics(IMyEntity anchor, IMyEntity reference, Vector3D linSpeed, Vector3D rotSpeed)

    Passing an entity to anchor will use this class the "old" way using just the blue parameters and ignoring the green ones. The "old way" means also that the linear speed (and acceleration and jerk) will be calculated based on the volumetric center of the "anchor" object.
    Passing null to anchor will use the linSpeed and rotSpeed vectors which are meant to be taken directly from a ship controller block. The ship controller calculates the linear speed vector based on the center of mass which will give better results in most cases.
    Beware that the Update method has the linSpeed and rotSpeed parameters added, too. If you are using the anchor they will be ignored. So you can pass a zero vector in this case.

    public class Kinematics
      public IMyEntity Anchor { get { return _anchor; } }
      public IMyEntity Reference { get { return _reference; } set { _reference = value; } }
      public MatrixD StateCurrent { get { return _stateCurrent; } }
      public MatrixD StateLast { get { return _stateLast; } }
      public MatrixD Transition { get { return _transition; } }
      public Vector3D TransitionLinearCurrent { get { return _transitionLinearCurrent; } }
      public Vector3D TransitionAngularCurrent { get { return _transitionAngularCurrent; } }
      public Vector3D TransitionLinearLast { get { return _transitionLinearLast; } }
      public Vector3D TransitionAngularLast { get { return _transitionAngularLast; } }
      public Vector3D VelocityLinearCurrent { get { return _velocityLinearCurrent; } }
      public Vector3D VelocityAngularCurrent { get { return _velocityAngularCurrent; } }
      public Vector3D VelocityLinearLast { get { return _velocityLinearLast; } }
      public Vector3D VelocityAngularLast { get { return _velocityAngularLast; } }
      public Vector3D AccelerationLinearCurrent { get { return _accelerationLinearCurrent; } }
      public Vector3D AccelerationAngularCurrent { get { return _accelerationAngularCurrent; } }
      public Vector3D AccelerationLinearLast { get { return _accelerationLinearLast; } }
      public Vector3D AccelerationAngularLast { get { return _accelerationAngularLast; } }
      public Vector3D JerkLinearCurrent { get { return _jerkLinearCurrent; } }
      public Vector3D JerkAngularCurrent { get { return _jerkAngularCurrent; } }
      public Vector3D JerkLinearLast { get { return _jerkLinearLast; } }
      public Vector3D JerkAngularLast { get { return _jerkAngularLast; } }
      public float LinearSpeedCurrent { get { return (float)_velocityLinearCurrent.Length(); } }
      // Anchor and reference entities
      IMyEntity _anchor, _reference;
      // The stats current and last matrix. That is the orientation of the anchor relative to the reference
      MatrixD _stateCurrent, _stateLast;
      // The transition from the last state to the current state
      MatrixD _transition;
      // linear/angular transition values
      Vector3D _transitionLinearCurrent, _transitionAngularCurrent, _transitionLinearLast, _transitionAngularLast;
      // linear/angular velocity values
      Vector3D _velocityLinearCurrent, _velocityAngularCurrent, _velocityLinearLast, _velocityAngularLast;
      // linear/angular acceleration values
      Vector3D _accelerationLinearCurrent, _accelerationAngularCurrent, _accelerationLinearLast, _accelerationAngularLast;
      // linear/angular jerk values
      Vector3D _jerkLinearCurrent, _jerkAngularCurrent, _jerkLinearLast, _jerkAngularLast;
      public Kinematics(IMyEntity anchor, IMyEntity reference, Vector3D linSpeed, Vector3D rotSpeed)
    	  this._anchor = anchor;
    	  this._reference = reference;
    	  if(anchor != null) {
    		if(reference == null)
    		  _transition = this._anchor.WorldMatrix;
    		  _transition = this._anchor.WorldMatrix * MatrixD.Invert(this._reference.WorldMatrix);
    		this._stateCurrent = this._stateLast = _transition;
    	  this._transitionLinearLast = this._transitionAngularLast = this._transitionLinearCurrent = this._transitionAngularCurrent = Vector3D.Zero;
    	  this._velocityLinearCurrent = this._velocityLinearLast = linSpeed;
    	  this._velocityAngularCurrent = this._velocityAngularLast = rotSpeed;
    	  this._accelerationLinearCurrent = this._accelerationAngularCurrent = this._accelerationLinearLast = this._accelerationAngularLast = Vector3D.Zero;
    	  this._jerkLinearCurrent = this._jerkAngularCurrent = this._jerkLinearLast = this._jerkAngularLast = Vector3D.Zero;
      public void Update(Vector3D linSpeed, Vector3D rotSpeed, double deltaT)
    	if(this._anchor != null) {
    	  this._transition = this._reference == null ? this._anchor.WorldMatrix : this._anchor.WorldMatrix * MatrixD.Invert(this._reference.WorldMatrix);
    	  this._stateLast = this._stateCurrent;
    	  this._stateCurrent = _transition;
    	  _transition = this._stateCurrent * MatrixD.Invert(this._stateLast);
    	  this._transitionLinearLast = this._transitionLinearCurrent;
    	  this._transitionLinearCurrent = _transition.Translation;
    	  this._transitionAngularLast = this._transitionAngularCurrent;
    	  this._transitionAngularCurrent = new Vector3D(
    		Math.Asin(Vector3D.Dot(_transition.Up, Vector3D.Backward)),
    		Math.Asin(Vector3D.Dot(_transition.Backward, Vector3D.Right)),
    		Math.Asin(Vector3D.Dot(_transition.Right, Vector3D.Up))
    	this._velocityLinearLast = this._velocityLinearCurrent;
    	this._velocityAngularLast = this._velocityAngularCurrent;
    	this._accelerationLinearLast = this._accelerationLinearCurrent;
    	this._accelerationAngularLast = this._accelerationAngularCurrent;
    	this._jerkLinearLast = this._jerkLinearCurrent;
    	this._jerkAngularLast = this._jerkAngularCurrent;
    	if(this._anchor != null) {
    	  this._velocityLinearCurrent = this._transitionLinearCurrent * deltaT;
    	  this._velocityAngularCurrent = this._transitionAngularCurrent * deltaT;
    	} else {
    	  this._velocityLinearCurrent = linSpeed;
    	  this._velocityAngularCurrent = rotSpeed;
    	this._accelerationLinearCurrent = (this._velocityLinearCurrent - this._velocityLinearLast) * deltaT;
    	this._accelerationAngularCurrent = (this._velocityAngularCurrent - this._velocityAngularLast) * deltaT;
    	this._jerkLinearCurrent = (this._accelerationLinearCurrent - this._accelerationLinearLast) * deltaT;
    	this._jerkAngularCurrent = (this._accelerationAngularCurrent - this._accelerationAngularLast) * deltaT;
    Last edited: Dec 18, 2017
Thread Status:
This last post in this thread was made more than 31 days old.