Skip to content

Commit 3e218bd

Browse files
micahpearlmanmicahpearlman
committed
IMU publishes linear acceleration separate from gravity acceleration.
1 parent 29bb4b1 commit 3e218bd

File tree

5 files changed

+37
-24
lines changed

5 files changed

+37
-24
lines changed

‎Runtime/Scripts/ROS/Unity/Publishers/ZOROSIMUPublisher.cs‎

Lines changed: 12 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -113,7 +113,7 @@ public override void OnROSBridgeDisconnected(ZOROSUnityManager rosUnityManager)
113113
ROSBridgeConnection.UnAdvertise(ROSTopic);
114114
}
115115

116-
private Task OnPublishImuDelegate(ZOIMU lidar, string name, Vector3 linearAccel, Vector3 angularVelocity, Quaternion orientation) {
116+
private Task OnPublishImuDelegate(ZOIMU lidar, string name, Vector3 linearAccel, Vector3 gravity, Vector3 angularVelocity, Quaternion orientation) {
117117
_imuMessage.header.Update();
118118
_imuMessage.header.frame_id = TransformPublisher.ChildFrameID;
119119

@@ -124,7 +124,7 @@ private Task OnPublishImuDelegate(ZOIMU lidar, string name, Vector3 linearAccel,
124124

125125

126126
if (_coordinateSystem == CoordinateSystemEnum.ROS_RightHanded_XForward_YLeft_ZUp) {
127-
_imuMessage.linear_acceleration.UnityVector3 = linearAccel;
127+
_imuMessage.linear_acceleration.UnityVector3 = (linearAccel + gravity);
128128
_imuMessage.angular_velocity.UnityVector3 = angularVelocity;
129129
_imuMessage.orientation.UnityQuaternion = orientation;
130130

@@ -135,23 +135,23 @@ private Task OnPublishImuDelegate(ZOIMU lidar, string name, Vector3 linearAccel,
135135
_imuMessage.orientation.z = orientation.y;
136136
_imuMessage.orientation.w = -orientation.w;
137137

138-
_imuMessage.linear_acceleration.x = -linearAccel.z;
139-
_imuMessage.linear_acceleration.y = -linearAccel.x;
140-
_imuMessage.linear_acceleration.z = linearAccel.y;
138+
_imuMessage.linear_acceleration.x = -linearAccel.z + -gravity.z;
139+
_imuMessage.linear_acceleration.y = -linearAccel.x + -gravity.x;
140+
_imuMessage.linear_acceleration.z = linearAccel.y + gravity.y;
141141

142142
_imuMessage.angular_velocity.x = -angularVelocity.z;
143143
_imuMessage.angular_velocity.y = -angularVelocity.x;
144144
_imuMessage.angular_velocity.z = angularVelocity.y;
145-
} else if (CoordinateSystem == CoordinateSystemEnum.RightHanded_XRight_YDown_ZForward) {
145+
} else if (CoordinateSystem == CoordinateSystemEnum.RightHanded_XRight_YDown_ZForward) { // aka RealSense
146146
Quaternion flippedYOrientation = Quaternion.Euler(Vector3.Scale(orientation.eulerAngles, Vector3.up * -1));
147147
_imuMessage.orientation.x = flippedYOrientation.x;
148148
_imuMessage.orientation.y = flippedYOrientation.y;
149149
_imuMessage.orientation.z = flippedYOrientation.z;
150150
_imuMessage.orientation.w = -flippedYOrientation.w; // negate left to right handed coordinate system
151151

152-
_imuMessage.linear_acceleration.x = linearAccel.x;
153-
_imuMessage.linear_acceleration.y = -linearAccel.y;
154-
_imuMessage.linear_acceleration.z = linearAccel.z;
152+
_imuMessage.linear_acceleration.x = -linearAccel.x + gravity.x;
153+
_imuMessage.linear_acceleration.y = linearAccel.y + gravity.y;
154+
_imuMessage.linear_acceleration.z = -linearAccel.z + gravity.z;
155155

156156
_imuMessage.angular_velocity.x = angularVelocity.x;
157157
_imuMessage.angular_velocity.y = angularVelocity.y;
@@ -163,9 +163,9 @@ private Task OnPublishImuDelegate(ZOIMU lidar, string name, Vector3 linearAccel,
163163
_imuMessage.orientation.z = orientation.z;
164164
_imuMessage.orientation.w = orientation.w;
165165

166-
_imuMessage.linear_acceleration.x = linearAccel.x;
167-
_imuMessage.linear_acceleration.y = linearAccel.y;
168-
_imuMessage.linear_acceleration.z = linearAccel.z;
166+
_imuMessage.linear_acceleration.x = linearAccel.x + gravity.x;
167+
_imuMessage.linear_acceleration.y = linearAccel.y + gravity.y;
168+
_imuMessage.linear_acceleration.z = linearAccel.z + gravity.z;
169169

170170
_imuMessage.angular_velocity.x = angularVelocity.x;
171171
_imuMessage.angular_velocity.y = angularVelocity.y;

‎Runtime/Scripts/Sensors/IMU/ZOIMU.cs‎

Lines changed: 16 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -45,6 +45,11 @@ public class ZOIMU : ZOGameObjectBase, ZOSerializationInterface {
4545
[ZO.Util.ZOReadOnly]
4646
[SerializeField]
4747
private Vector3 _acceleration = Vector3.zero;
48+
49+
[ZO.Util.ZOReadOnly]
50+
[SerializeField]
51+
private Vector3 _gravity = Vector3.zero;
52+
4853
[ZO.Util.ZOReadOnly]
4954
[SerializeField]
5055
private Vector3 _angularVelocity = Vector3.zero;
@@ -57,15 +62,16 @@ public class ZOIMU : ZOGameObjectBase, ZOSerializationInterface {
5762
/// Called every frame passing in:
5863
/// this,
5964
/// string imuID,
60-
/// Vector3 linear_accel,
65+
/// Vector3 linear_accel no gravity,
66+
/// Vector3 gravity_accel,
6167
/// Vector3 angular_velocity,
6268
/// Quaternion orientation
6369
///
6470
/// Note: is async so returns a task
6571
/// </summary>
6672
///
6773
/// <value></value>
68-
public Func<ZOIMU, string, Vector3, Vector3, Quaternion, Task> OnPublishDelegate { get; set; }
74+
public Func<ZOIMU, string, Vector3, Vector3, Vector3, Quaternion, Task> OnPublishDelegate { get; set; }
6975

7076

7177
private Rigidbody _rigidBody;
@@ -75,6 +81,10 @@ public Vector3 Acceleration {
7581
get { return _acceleration; }
7682
}
7783

84+
public Vector3 Gravity {
85+
get { return _gravity; }
86+
}
87+
7888

7989
public Vector3 AngularVelocity {
8090
get { return _angularVelocity; }
@@ -103,7 +113,8 @@ protected override async void ZOFixedUpdateHzSynchronized() {
103113

104114

105115
// calculate gravity
106-
_acceleration += transform.InverseTransformDirection(UnityEngine.Physics.gravity);
116+
_gravity = transform.InverseTransformDirection(UnityEngine.Physics.gravity);
117+
107118
_acceleration = _linearNoise.Apply(_acceleration);
108119
_acceleration = ZO.Math.ZOMathUtil.lowPassFilter(_acceleration, prevAcceleration, _preFilterAccelerationAlpha);
109120

@@ -112,13 +123,14 @@ protected override async void ZOFixedUpdateHzSynchronized() {
112123

113124
_orientation = transform.rotation;
114125

126+
Vector3 publishedGravity = _gravity;
115127
Vector3 publishedAcceleration = _acceleration;
116128
Vector3 publishedAngularVelocity = _angularVelocity;
117129
Quaternion publishedOrientation = transform.rotation;
118130

119131

120132
if (OnPublishDelegate != null) {
121-
await OnPublishDelegate(this, Name, publishedAcceleration, publishedAngularVelocity, publishedOrientation);
133+
await OnPublishDelegate(this, Name, publishedAcceleration, publishedGravity, publishedAngularVelocity, publishedOrientation);
122134
}
123135

124136
}

‎Runtime/Scripts/Util/GameObject/ZOMoveTest.cs‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -49,7 +49,7 @@ void Update() {
4949
// Update is called once per frame
5050
void FixedUpdate() {
5151
if (_isRunning) {
52-
if (_moveType == MoveTypeEnum.RotateRoll) {
52+
if (_moveType == MoveTypeEnum.RotateRoll) {
5353
Quaternion rotation = Quaternion.Euler(_startRotationEuler + new Vector3(0, 0, _currentValue));
5454
_rigidBody.MoveRotation(rotation);
5555
} else if (_moveType == MoveTypeEnum.RotatePitch) {
@@ -66,7 +66,7 @@ void FixedUpdate() {
6666
_rigidBody.MovePosition(_startPosition + velocity);
6767
} else if (_moveType == MoveTypeEnum.TranslateUp) {
6868
Vector3 velocity = new Vector3(0, _currentValue, 0);
69-
_rigidBody.MovePosition(_startPosition+ velocity);
69+
_rigidBody.MovePosition(_startPosition + velocity);
7070
}
7171

7272
_currentValue = _currentValue + (_speed * Time.fixedDeltaTime);

‎Samples~/ZeroSimSamples/Scenes/ROSIMUPublish_test.unity‎

Lines changed: 6 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -38,7 +38,7 @@ RenderSettings:
3838
m_ReflectionIntensity: 1
3939
m_CustomReflection: {fileID: 0}
4040
m_Sun: {fileID: 0}
41-
m_IndirectSpecularColor: {r: 0, g: 0, b: 0, a: 1}
41+
m_IndirectSpecularColor: {r: 0.44278473, g: 0.4919318, b: 0.5717111, a: 1}
4242
m_UseRadianceAmbientProbe: 0
4343
--- !u!157 &3
4444
LightmapSettings:
@@ -2139,6 +2139,7 @@ MonoBehaviour:
21392139
_biasStdDev: 0.0001
21402140
_preFilterAccelerationAlpha: 0.5
21412141
_acceleration: {x: 0, y: 0, z: 0}
2142+
_gravity: {x: 0, y: 0, z: 0}
21422143
_angularVelocity: {x: 0, y: 0, z: 0}
21432144
_orientation: {x: 0, y: 0, z: 0, w: 1}
21442145
_name:
@@ -2153,8 +2154,8 @@ Rigidbody:
21532154
m_Mass: 1
21542155
m_Drag: 0
21552156
m_AngularDrag: 0.05
2156-
m_UseGravity: 0
2157-
m_IsKinematic: 0
2157+
m_UseGravity: 1
2158+
m_IsKinematic: 1
21582159
m_Interpolate: 0
21592160
m_Constraints: 0
21602161
m_CollisionDetection: 0
@@ -2205,9 +2206,9 @@ MonoBehaviour:
22052206
m_Script: {fileID: 11500000, guid: 1ef77affe836d7c54adde928a35ad253, type: 3}
22062207
m_Name:
22072208
m_EditorClassIdentifier:
2208-
_minMax: {x: 0, y: 2}
2209+
_minMax: {x: 0, y: 1}
22092210
_speed: 2
2210-
_moveType: 3
2211+
_moveType: 4
22112212
--- !u!1 &1764802517
22122213
GameObject:
22132214
m_ObjectHideFlags: 0

‎package.json‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
{
22
"name": "com.fsstudio.zerosim",
3-
"version": "0.0.19",
3+
"version": "0.0.20",
44
"displayName": "ZeroSim",
55
"description": "ZeroSim ROS robotic simulator in Unity.",
66
"unity": "2020.1",

0 commit comments

Comments
 (0)