Hello,
for simulating an IMU attached to a robot I need to calculate accelerations from GameObject Transformation. A rigidbody cannot be attached to an articulated body without affecting it. Therefore I need to come up with a “Passive”-RigidBody. While checking my script against the RigidBody in a simple example i.e. a cube falling under gravity things look good for typical default dt (0.01, 0.001). But when decreasing fixed dt (<=0.0001) to improve physics simulation quality things get awkward.
A body falling in constant gravity should build up velocity constantly and acceleration should be constant and equal gravity. This is the case for RigidBody.velocity. But velocity and acceleration calculated from differentiating transform.position shows step changes in velocity and jumps in acceleration. Indicating that transform.position is not updated for gravity and perhaps not synchronized with the physics system as I would expect.
Please use script attached on a GameObject with a rigidbody falling in gravity. VelocityError will be Zero for dt=0.01 but significant for dt<=0.0001. (Fixed Time Step). Sync Transformation in Physics script is enabled!
Any idea on how to measure velocity and acceleration with high quality in unity using a passive sensor?
Thanks a lot,
Tobias
sing System.Collections;
using System.Collections.Generic;
using UnityEngine;
public class IMUtest : MonoBehaviour
{
public Vector3 velocityError; //diff between velcity reported from rigidbody and calculated from transformation
public Vector3 velocity; //real velocity in world frame
public Vector3 acceleration; //real acceleration in world frame
public Vector3 angularVelocity { get; private set; } //real angularVelocity in world frame
public Vector3 angularAcceleration { get; private set; } //real angularVelocity in world frame
public Vector3 accel{ get; private set; } //measured accelerations in [g] in local frame
public Vector3 gyro{ get; private set; } //measured angular rates in [rad/s] in local frame
private Rigidbody rb;
private Vector3 posOld;
private Quaternion rotOld;
// Start is called before the first frame update
void Start()
{
rb = GetComponent<Rigidbody>();
posOld = transform.position;
rotOld = transform.rotation;
velocity = new Vector3(0.0f, 0.0f, 0.0f);
acceleration = new Vector3(0.0f, 0.0f, 0.0f);
angularVelocity = new Vector3(0.0f, 0.0f, 0.0f);
angularAcceleration = new Vector3(0.0f, 0.0f, 0.0f);
accel = new Vector3(0.0f, 0.0f, 0.0f);
gyro = new Vector3(0.0f, 0.0f, 0.0f);
}
void FixedUpdate()
{
FixedUpdateVelocityAndAcceleration();
FixedUpdateMeasurements();
//just debugging
velocityError = velocity- rb.velocity;
Debug.Log(velocityError);
}
private void FixedUpdateVelocityAndAcceleration()
{
//Calculate Linear Velocity and Acceleration in World Coordinates!
var vNew = (transform.position - posOld) / Time.fixedDeltaTime;
var aNew = (vNew - velocity) / Time.fixedDeltaTime;
posOld = transform.position;
velocity = vNew;
acceleration = aNew;
//Calculate Angular Velocity in World Coordinates!
var deltaRot = transform.rotation * Quaternion.Inverse(rotOld);
deltaRot.ToAngleAxis(out var angle, out var axis);
var angularVelocityNew = (angle * Mathf.Deg2Rad) * axis / Time.fixedDeltaTime;
var angularAccelerationNew = (angularVelocityNew - angularVelocity) / Time.fixedDeltaTime;
rotOld = transform.rotation;
angularVelocity = angularVelocityNew;
angularAcceleration = angularAccelerationNew;
}
private void FixedUpdateMeasurements()
{
accel = transform.InverseTransformDirection( acceleration - Physics.gravity) / Physics.gravity.magnitude;
gyro = transform.InverseTransformDirection( angularVelocity );
}
// Update is called once per frame
void Update()
{
Debug.DrawRay(transform.position, transform.TransformDirection(accel), Color.blue);
Debug.DrawRay(transform.position, transform.TransformDirection(gyro), Color.yellow);
}
}