From 4694b09bfbec4711f894e9950a90ed38bad0791e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Jasper=20G=C3=BCldenstein?=
 <5guelden@informatik.uni-hamburg.de>
Date: Fri, 15 Jan 2021 22:12:22 +0100
Subject: [PATCH] add dummy physics for imu sensor to make gyro work

---
 urdf2webots/writeProto.py | 9 ++++++++-
 1 file changed, 8 insertions(+), 1 deletion(-)

diff --git a/urdf2webots/writeProto.py b/urdf2webots/writeProto.py
index 49e8b763..da508731 100644
--- a/urdf2webots/writeProto.py
+++ b/urdf2webots/writeProto.py
@@ -2,7 +2,7 @@
 
 import math
 import numpy as np
-
+from urdf2webots.parserURDF import IMU
 from urdf2webots.math_utils import rotateVector, matrixFromRotation, multiplyMatrix, rotationFromMatrix
 
 toolSlot = None
@@ -112,6 +112,13 @@ def URDFLink(proto, link, level, parentList, childList, linkList, jointList, sen
         # 2: export Sensors
         for sensor in sensorList:
             if sensor.parentLink == link.name:
+                # add dummy physics for imu sensor if there is none since gyro requires it
+                if type(sensor) == IMU and link.inertia.mass is None:
+                    proto.write((level + 1) * indent + 'physics Physics {\n')
+                    proto.write((level + 1) * indent + '}\n')
+                    proto.write((level + 1) * indent + 'boundingObject Box {\n')
+                    proto.write((level + 1) * indent + 'size 0.01 0.01 0.01\n')
+                    proto.write((level + 1) * indent + '}\n')
                 if not haveChild:
                     haveChild = True
                     proto.write((level + 1) * indent + 'children [\n')