Search Unity

  1. Click here to receive a gift with your purchase of Unity Pro or Unity Enterprise.
    Dismiss Notice
  2. Good news ✨ We have more Unite Now videos available for you to watch on-demand! Come check them out and ask our experts any questions!
    Dismiss Notice

rigidbody lookat torque?

Discussion in 'Scripting' started by dogzerx2, Aug 7, 2012.

  1. dogzerx2

    dogzerx2

    Joined:
    Dec 27, 2009
    Posts:
    3,875
    I have this supposedly "aerodynamic" rigidbody object, and because it has wings that are supposed to "stabilize" its direction, I want it to look towards the velocity is going. And I want to do it using physics, because everything else is controlled with physics...

    Is this possible? I'm getting very weird results all over the place. I mean Quaternion.Slerp works great without physics, but addTorque uses only Vector3, and rotation starts to wiggle and go crazy when it tries to match a rotation.

    This is my current code btw, works with MAJOR limitations... if I only rotate in 1 axis... and I don't go over 90 degrees... but after that... calculations go whack and everything gets messy:

    Code (csharp):
    1. #pragma strict
    2.  
    3. var targetRotation : Vector3;
    4.  
    5. function Start () {
    6. }
    7.  
    8. function FixedUpdate () {
    9.     var torque : Vector3 = Vector3(Mathf.DeltaAngle(transform.rotation.eulerAngles.x + rigidbody.angularVelocity.x / Time.fixedDeltaTime, targetRotation.x),
    10.     Mathf.DeltaAngle(transform.rotation.eulerAngles.y + rigidbody.angularVelocity.y / Time.fixedDeltaTime, targetRotation.y),
    11.     Mathf.DeltaAngle(transform.rotation.eulerAngles.z + rigidbody.angularVelocity.z / Time.fixedDeltaTime, targetRotation.z));
    12.  
    13.     rigidbody.AddTorque(torque);
    14. }
     
    G-Power likes this.
  2. Vanamerax

    Vanamerax

    Joined:
    Jan 12, 2012
    Posts:
    894
    If you want an object to "lookat" it's velocity direction, I use this code for my arrows:

    Code (csharp):
    1.  myTransform.forward = Vector3.Slerp(transform.forward, rigidbody.velocity.normalized, gravityRotate * Time.deltaTime);
    Not sure if this is what you're looking for but hope it helps
     
  3. Brian-Stone

    Brian-Stone

    Joined:
    Jun 9, 2012
    Posts:
    222
    Basically, this is a control system problem. What you are currently have is, essentially, a poor mans proportional controller. However, if you don't take into account the error in the system and correct for it, then you will set up some repeating or maybe even unstable oscillations.

    I'd suggest you use a PID controller (or maybe just a PD controller) that takes the angular velocity as input and spits out correctional torque force as the output. It'll take a lot of experimentation to get the PID gains just right so the system behaves the way you want it to, but I can practically guarantee that it will work.

    If you would like an explanation of PID controllers, I'd be happy to oblige. If the community would like to see an explanation of PID control theory and how it applies to physics and games, I might even be persuaded to write up a blog article.
     
  4. ippdev

    ippdev

    Joined:
    Feb 7, 2010
    Posts:
    3,032
    Just add it to this thread please..


    TIA
     
  5. Brian-Stone

    Brian-Stone

    Joined:
    Jun 9, 2012
    Posts:
    222
    Okay, one crash course in PID control theory coming up. :)

    I’ll keep this discussion as simple as possible without any Calculus mixed in, since I’ve got a feeling this audience is considerably more interested in the practical application rather than the mathematical theory.

    I will post it later tonight*.

    edit:
    Tomorrow's tonight.
    Promise. ;-)
     
    Last edited: Aug 9, 2012
  6. ippdev

    ippdev

    Joined:
    Feb 7, 2010
    Posts:
    3,032

    Dang.. And here I am with my coffee and notebook! I will tune back in;)

    TIA
     
  7. Brian-Stone

    Brian-Stone

    Joined:
    Jun 9, 2012
    Posts:
    222
    Sorry for the delay. I wanted to make a little demo (see attachment) to make sure there weren't going to be any caveats. But, surely enough, it works pretty well. The demo shows what you can do with a basic PID controller without any real forethought spent on control theory. It demonstrates how you can solve a single-axis torque problem to steer a space ship to a desired heading quickly and accurately without oscillating. It can be easily modified to solve Dogzerx2's aerodynamics problem.

    Hopefully this ultra-short tutorial and demo will serve as an introduction to the classic PID controller algorithm. I'm not going to get into transfer functions, or how to work in design criteria, or feed-forward vs feed-back, et. al. since most of that is irrelevant in the context of ad-hoc video game control systems.

    ---------

    A PID controller, in a nut shell, iteratively drives a dynamic value (which could be force, velocity, position, angle, or anything) such that some error function that is related to the dynamic system is minimized.

    PID stands for Proportional Integral Derivative which refers to the three types of error measures used in the algorithm. The proportional error, P(t), is the immediate error in the system at time t, and is like asking, "Where is our target relative to where we are right now?". The integral error, I(t), is the sum of the proportional error over time, which is a measure of past error, and is a little bit like asking, "How far have we traveled from the target?". And the derivative error, D(t), is the difference in the proportional error between the previous time step and the current time step, and is ostensibly a prediction of future error, and is like asking, "How fast are we moving relative to the target?". The PID controller's output, u(t), is the weighted sum of these three errors.

    u(t) = P(t)*Kp + I(t)*Ki + D(t)*Kd

    Kp, Ki, and Kd are scalar constants, and they're called the "gains". These gains control how much each error measure contributes to the controller's output. There's no ideal way to figure out what these values need to be. Usually, we figure them out experimentally by choosing some gains that we think might work (but almost never do), observing how the system behaves, and then adjusting the gains as necessary until the desired behavior is achieved. For complex systems this can sometimes painfully frustrating, but for simple systems with only a few PID controllers it's pretty easy.

    The only error that you need to supply is the proportional error, P(t). The proportional error is usually defined as: "Where you are now" minus "where you want to be". Another way of saying it is that the immediate displacement in the system at time t.

    P(t) = (where you are now) - (where you want to be)

    The integral error, I(t), is the integration of the proportional error with respect to time. Those of you who have some calculus and/or physics knowledge might recall that multiplying a positional displacement to delta time is velocity at that time, and the integration of velocity with respect to time is total displacement. Therefore, I(t) is a measure of total displacement from the target value.

    I(t) = I(t) + P(t) * dt

    Finally, the derivative error, D(t), is the difference between the current error P(t) and the previous proportional error P(t-dt) divided by delta time. If you know calculus, then you'll quickly pick up that the derivative error is a discrete calculation of the first-derivative of the proportional error function. It is the slope of P(t) between time t-dt and time t. In other words, it is a measure of how fast the proportional error function is changing.

    D(t) = (P(t) - P(t-dt)) / dt

    And here's how you put it all together. This is the classic PID control loop. Each successive call of this function will drive the system (hopefully) towards zero error.

    C#
    Code (csharp):
    1.  
    2.     // The gains are chosen experimentally
    3.     public float Kp = 1;
    4.     public float Ki = 0;
    5.     public float Kd = 0.1f;
    6.  
    7.     private float prevError;
    8.     private float P, I, D;
    9.  
    10.     public float GetOutput(float currentError, float dt)
    11.     {
    12.         P = currentError;
    13.         I += P * dt;
    14.         D = (P - prevError) / dt;
    15.         prevError = currentError;
    16.        
    17.         return P*Kp + I*Ki + D*Kd;
    18.     }
    19.  
    So, how do we interpret the output of this function? The output is ambiguous. The output can mean anything you want it to mean. The meaning is defined by the gains, and the gains are chosen experimentally by observation. For the demo, I experimented with gains that produced an output that could be used as a torque force to steer a little space ship, but the same approach can be used to control anything. The only thing you have to do is experiment with the system and figure out what the gains need to be to give you the behavior that you're looking for.

    About the Demo
    The demo lets you play around with a little space ship which you can spin with the "A" and "D" keys on the keyboard. This ship has a RigidBody component attached and its rotation is being controlled by applying torque force calculated by PID controllers.

    There are two PID controllers in this simulation. One of them is correcting for the ship's angle, and the other one is correcting for the ship's angular velocity. The angle controller is attempting to force the ship to a target angle supplied by the user's keyboard inputs. The angular velocity control is continuously attempting to force the ship to stop rotating and bring its angular velocity to zero. The output from both controllers is added together and that sum is the torque force that drives the rotation of the ship.

    You can change the PID gains of both controllers while you rotate the ship. This is a great way to experiment with gains and figure out how the affect the system. You'll notice pretty quickly that modifying the integral gain on either controller will ruin the simulation and the ship will not track properly, so setting the integral gain to 0 eliminates it from the equation. You may also notice that if you set the derivative gain too high the ship will oscillate out of control and eventually "explode". This is because the derivative error is divided by the time step. So, large derivative error will result in huge values, which results in ultra-high frequency oscillations that will eventually make the system become unstable.

    I hope this little tutorial has been useful. If you have any questions, just ask! :)
     

    Attached Files:

    Last edited: Aug 10, 2012
    Lexone, iamarugin, Oyedoyin1 and 13 others like this.
  8. HarvesteR

    HarvesteR

    Joined:
    May 22, 2009
    Posts:
    499
    If you want to just apply torque to a rigidbody so that it 'weathervanes' into the velocity, you can do something like this:

    Code (csharp):
    1.  
    2. rigidbody.AddTorque(Vector3.Cross(transform.forward, rigidbody.velocity), ForceMode.FORCE);
    3.  
    (You may need to swap the cross vector parameters).

    The cross vector works nicely for torque, because it is already oriented to the axis you need to rotate around, and its magnitude grows in proportion to the alignment error. You might want to add a multiplier maybe, or clamp the magnitude of the cross vector, to let the ship slip a little.

    Mind that this will orient the ship as if it were an arrow. To separate yaw stability from pitch stability, you'll have to use a more complex scheme, but this should work if you just need it to point into a set direction. (Also, combining this with the PID controller above works great too. Use the line here to calculate the initial error.)

    Also, this here will leave your roll axis completely unchanged (like an arrow). To correct for roll, add torque using the same cross vector technique, only between transform.up and whatever direction you want to be your vertical (usually Vector3.up).

    Hope this helps.

    Cheers
     
    GuitarBro and NicRule like this.
  9. drudiverse

    drudiverse

    Joined:
    May 16, 2013
    Posts:
    218
    Thanks very much for this fascinating topic and Brian Stone for a great tutorial on PID. ...

    Harvester's line is Very cool. it just nudges the nose of the craft always towards the front facing direction.

    here is an enhancement that makes the corrective torque proportional to angle how far away from centre... you can multiply the an value after a knock so that you can see the object spin amusingly and over time the rightening force becomes strong so it doesnt spin too much, like there is a total out of control moment after collision and then rightening force gets ramped up:
    Code (csharp):
    1.         var an = 1800-Vector3.Angle(transform.forward, Vector3.forward)*10;
    2.         if(an>1700){an = 4000;}
    3.         var ty = Vector3.Cross(transform.forward, Vector3.forward);
    4.         // if(ty.z > 0){
    5.         rigidbody.AddTorque(ty*an, ForceMode.Force);
    Just messing with this, when the craft gets hit, i take it's angular velocity and lerp it with a forwards vector, so that the spin lines up with a spin lined with velocity direction and the craft has controlled itself.

    Code (csharp):
    1.            rigidbody.angularVelocity = Vector3.Lerp(origAngVel, Vector3.forward, stabilizetime_iterations);
    It doesnt work because, change angularVelocity, spins the object around that vector in space, starting from the position that it was at the time the angVel was changed, so if the craft faces up and you tell it to spin forwards, it will spin around the forward axis facing up and the nose will go clockwise around an around, not at all facing forward.

    change angularVelocity spins the object around that angle in the facing-position it was at time of change. weirdand fun!
     
    Last edited: May 14, 2014
    GuitarBro likes this.
  10. EasyKill

    EasyKill

    Joined:
    Nov 6, 2014
    Posts:
    1
    Hi, I've been all over the place searching for a solution to my problem and PID controllers sound very promising. I am trying to do something similar with torque to set the rotation of a rigid body. Given a target quaternion rotation I'd like to apply the torque needed to achieve the desired pitch, yaw, and roll. My rigid body character will be moving along the terrain and I'd like to rotate him using torque to match the slope of the terrain and the direction of his velocity. Gravity is also in play in my simulation. How would I go about re-purposing the above example for this scenario. Thanks in advance!
     
  11. Panzerhandschuh

    Panzerhandschuh

    Joined:
    Dec 4, 2012
    Posts:
    15
    I found a solution to the problem of applying torque to align to a target rotation yesterday. From my testing, my code is able to handle any from/to rotation without any problems. It works great for hovercraft vehicles. Here is the relevant code:

    Code (csharp):
    1.  
    2. // Compute target rotation (align rigidybody's up direction to the normal vector)
    3. Vector3 normal = Vector3.up;
    4. Vector3 proj = Vector3.ProjectOnPlane(transform.forward, normal);
    5. Quaternion targetRotation = Quaternion.LookRotation(proj, normal); // The target rotation can be replaced with whatever rotation you want to align to
    6.  
    7. Quaternion deltaRotation = Quaternion.Inverse(transform.rotation) * targetRotation;
    8. Vector3 deltaAngles = GetRelativeAngles(deltaRotation.eulerAngles);
    9. Vector3 worldDeltaAngles = transform.TransformDirection(deltaAngles);
    10.  
    11. // alignmentSpeed controls how fast you rotate the body towards the target rotation
    12. // alignmentDamping prevents overshooting the target rotation
    13. // Values used: alignmentSpeed = 0.025, alignmentDamping = 0.2
    14. rigidbody.AddTorque(alignmentSpeed * worldDeltaAngles - alignmentDamping * rigidbody.angularVelocity);
    15.  
    16. // Convert angles above 180 degrees into negative/relative angles
    17. Vector3 GetRelativeAngles(Vector3 angles)
    18. {
    19.   Vector3 relativeAngles = angles;
    20.   if (relativeAngles.x > 180f)
    21.     relativeAngles.x -= 360f;
    22.   if (relativeAngles.y > 180f)
    23.     relativeAngles.y -= 360f;
    24.   if (relativeAngles.z > 180f)
    25.     relativeAngles.z -= 360f;
    26.  
    27.   return relativeAngles;
    28. }
    29.  
    To explain the code, first you need to find a target rotation to align to. In my code, I align my rigidbody with a normal vector which is fixed at Vector3.up. But it can be replaced with hit.normal if you are firing a raycast at the surface below your rigidbody.

    Then you need to find the rotational difference between the current rotation (transform.rotation) and the target rotation. This is done by multiplying the inverse of the current rotation with the target rotation.

    You then convert the rotational difference in quaternion form to euler angles. This gives you the delta angles on each axis required to rotate from the current rotation to the target rotation. Because of this, the delta angles can be used to add torque to arrive to the target rotation. But since the angles are initially in local space, you need to transform them to world space with transform.TransformDirection (I'm not 100% sure why, but it's related to how the delta quaternion calculations work out).

    When I add torque, I multiply the deltaAngles by alignmentSpeed to slow down the rotational motion. I also subtract by the current angular velocity to mitigate oscillation. The results end up being similar to Quaternion.RotateTowards or Quaternion.Slerp.

    Edit: Updated the code to support Unity 5.3 changes with Quaternion.eulerAngles.
     
    Last edited: Dec 9, 2015
  12. MV10

    MV10

    Joined:
    Nov 6, 2015
    Posts:
    1,889
    Thread-necro time!

    Brian, I turned your cool little demo into a working PID-corrected 3D steering system. The problem is that I need to add positive or negative thrust along the .forward axis based on... well, that's the question. I'm thinking that thrust can/should be PID controlled but all of my attempts so far have felt rather forced (not to mention unsuccessful).

    Edit: Updated the code with something that seems sort of close to working. I'm thinking the real problem is the difference between the vehicle's forward direction versus the velocity direction. I could apply Vec3 forces but I'm trying to do this with thrust along the forward axis first. (Ironically, the endless hassles of PID tuning is why several thousand dollars of drones are collecting dust in my office.)

    I added a Vector3 override to the PID class, and here is the main part of the 3D version of your demo's steering code. I would expect to have a third PID in the mix which is ultimately applied to some thrust constant, positive or negative.

    Code (csharp):
    1.             float dt = Time.fixedDeltaTime;
    2.  
    3.             // Determine heading to point at target limited by maximum turn rate
    4.             Vector3 targetHeading = flightTarget.position - transform.position;
    5.             Vector3 adjustedTargetHeading = Vector3.RotateTowards(transform.forward, targetHeading, maxTurnRadians, maxTurnSpeed);
    6.  
    7.             // The angle controller drives the ship's angle towards the target angle.
    8.             Vector3 headingError = Vector3.Cross(transform.forward, adjustedTargetHeading);
    9.             Vector3 headingCorrection = VectorTuning.GetCorrection(headingError, dt);
    10.  
    11.             // The angular velocity controller drives the ship's angular velocity to 0.
    12.             Vector3 angularVeocityError = rigidbody.angularVelocity * -1f;
    13.             Vector3 angularVelocityCorrection = AngularVelocityTuning.GetCorrection(angularVeocityError, dt);
    14.  
    15.             // The total torque from both controllers is applied to the ship.
    16.             rigidbody.AddTorque(headingCorrection + angularVelocityCorrection);
    17.  
    18.             // Use the error between forward and velocity to apply thrust
    19.             float thrustVectorError = Vector3.Angle(rigidbody.velocity, transform.forward);
    20.             // for normalized vectors 1:ahead, -1:behind, 0:perpendicular
    21.             float relativeVectorError = Vector3.Dot(rigidbody.velocity.normalized, transform.forward.normalized);
    22.             float thrust = thrustMagnitude * ThrustTuning.GetCorrection(thrustVectorError, dt) * relativeVectorError;
    23.             Debug.Log("vec: " + thrustVectorError + " ... rel: " + relativeVectorError + " ... thr: " + thrust + " ... mag: " + rigidbody.velocity.magnitude);
    24.  
    25.             // Apply thrust
    26.             if ((thrust < 0 && rigidbody.velocity.magnitude > 0) || (thrust > 0 && rigidbody.velocity.magnitude < maxVelocity))
    27.             {
    28.                 rigidbody.AddForce(transform.forward * thrust);
    29.             }
     
    Last edited: Feb 25, 2016
  13. SpaceManDan

    SpaceManDan

    Joined:
    Aug 3, 2013
    Posts:
    15

    I can't thank you enough. This is like level 1000 stuff here and I would never have been able to understand this without your help.
     
  14. Panzerhandschuh

    Panzerhandschuh

    Joined:
    Dec 4, 2012
    Posts:
    15
    You're welcome. I'm glad to see some people getting use out of that code.
     
  15. SpaceManDan

    SpaceManDan

    Joined:
    Aug 3, 2013
    Posts:
    15
    I happen to know that that code is part of the core component in several open source VR projects using physics game object handling. You're little bit of code there has been used as the corner stone for handling physics objects by hand in realtime for VR. That code is definitely helping several projects I know of. (If you want to know some examples so you can check out how it's being used let me know and I'll link you the gitHub.)
     
  16. DylanF

    DylanF

    Joined:
    Jun 25, 2013
    Posts:
    13
    Good information here!

    Note that rigidbody.maxAngularVelocity is pretty low by default. You may need to increase that for faster rotations.
     
  17. YOSSI2010

    YOSSI2010

    Joined:
    Dec 26, 2016
    Posts:
    6
    I know it's an old post but
    If you don't care about roll I use this:

    rb.AddTorque (Vector3.Cross(transform.forward,rb.velocity)*Power*rb.velocity.magnitude, ForceMode.Acceleration);
    This is how to do it using physics calculations and torques.
    I used this to make a cylindrical object (football arrow bullet missle) face the direction of flight without having roll restrictions.
    you should add some rotational drag.
    Power controls how harsh it is

    you could also add
    rb.AddRelativeTorque (rb.velocity.magnitude*Vector3.forward, ForceMode.Acceleration);
    to make it rotate in a cool way along its forward axis.
     
unityunity