Skip to content

Commit f15f3f9

Browse files
author
Berkay Alp Cakal
committed
bug fix on UrdfIntertialEditor, see siemens#322 
1 parent 77f3aeb commit f15f3f9

File tree

4 files changed

+156
-41
lines changed

4 files changed

+156
-41
lines changed

‎Unity3D/Assets/RosSharp/Scripts/Urdf/Editor/CustomInspectors/UrdfInertialEditor.cs

Lines changed: 78 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,78 @@
1+
/*
2+
© Siemens AG, 2017
3+
Author: Dr. Martin Bischoff ([email protected])
4+
5+
Licensed under the Apache License, Version 2.0 (the "License");
6+
you may not use this file except in compliance with the License.
7+
You may obtain a copy of the License at
8+
9+
<http://www.apache.org/licenses/LICENSE-2.0>.
10+
11+
Unless required by applicable law or agreed to in writing, software
12+
distributed under the License is distributed on an "AS IS" BASIS,
13+
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14+
See the License for the specific language governing permissions and
15+
limitations under the License.
16+
*/
17+
18+
using UnityEditor;
19+
using UnityEngine;
20+
21+
namespace RosSharp.Urdf.Editor
22+
{
23+
[CustomEditor(typeof(UrdfInertial))]
24+
public class UrdfInertialEditor : UnityEditor.Editor
25+
{
26+
27+
Rigidbody rigidbody;
28+
SerializedProperty pDisplayInertiaGizmo;
29+
SerializedProperty pRigidbodyDataSource;
30+
31+
SerializedProperty pMass;
32+
SerializedProperty pCenterOfMass;
33+
SerializedProperty pInertiaTensor;
34+
SerializedProperty pInertiaTensorRotation;
35+
36+
protected virtual void OnEnable()
37+
{
38+
rigidbody = (Rigidbody)serializedObject.FindProperty("_rigidbody").objectReferenceValue;
39+
pDisplayInertiaGizmo = serializedObject.FindProperty("DisplayInertiaGizmo");
40+
pRigidbodyDataSource = serializedObject.FindProperty("rigidbodyDataSource");
41+
pMass = serializedObject.FindProperty("Mass");
42+
pCenterOfMass = serializedObject.FindProperty("CenterOfMass");
43+
pInertiaTensor = serializedObject.FindProperty("InertiaTensor");
44+
pInertiaTensorRotation = serializedObject.FindProperty("InertiaTensorRotation");
45+
}
46+
47+
public override void OnInspectorGUI()
48+
{
49+
serializedObject.Update();
50+
51+
GUILayout.Space(5);
52+
EditorGUILayout.PropertyField(pDisplayInertiaGizmo, new GUIContent("Display Inertia Gizmo"));
53+
GUILayout.Space(5);
54+
55+
EditorGUILayout.PropertyField(pRigidbodyDataSource, new GUIContent("Rigidbody Data Source"));
56+
57+
switch ((UrdfInertial.RigidbodyDataSource) pRigidbodyDataSource.enumValueIndex)
58+
{
59+
case UrdfInertial.RigidbodyDataSource.Manual:
60+
pMass.floatValue = EditorGUILayout.FloatField("Mass",rigidbody.mass);
61+
pCenterOfMass.vector3Value = EditorGUILayout.Vector3Field("Center of Mass", rigidbody.centerOfMass);
62+
pInertiaTensor.vector3Value = EditorGUILayout.Vector3Field("Inertia Tensor", rigidbody.inertiaTensor);
63+
Vector3 angles = EditorGUILayout.Vector3Field("Inertia Tensor Rotation", rigidbody.inertiaTensorRotation.eulerAngles);
64+
pInertiaTensorRotation.quaternionValue = Quaternion.Euler(angles);
65+
break;
66+
67+
default:
68+
EditorGUILayout.LabelField(new GUIContent("Mass"), new GUIContent(rigidbody.mass.ToString()));
69+
EditorGUILayout.LabelField(new GUIContent("Center of Mass"), new GUIContent(rigidbody.centerOfMass.ToString()));
70+
EditorGUILayout.LabelField(new GUIContent("Inertia Tensor"), new GUIContent(rigidbody.inertiaTensor.ToString()));
71+
EditorGUILayout.LabelField(new GUIContent("Inertia Tensor Rotation"), new GUIContent(rigidbody.inertiaTensorRotation.eulerAngles.ToString()));
72+
break;
73+
}
74+
serializedObject.ApplyModifiedProperties();
75+
}
76+
77+
}
78+
}

‎Unity3D/Assets/RosSharp/Scripts/Urdf/Editor/UrdfComponents/UrdfLinkExtensions.cs

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -32,11 +32,11 @@ public static UrdfLink Create(Transform parent, Link link = null, Joint joint =
3232

3333
if (link != null)
3434
urdfLink.ImportLinkData(link, joint);
35-
else
35+
/* else
3636
{
3737
UrdfInertial.Create(linkObject);
3838
UnityEditor.EditorGUIUtility.PingObject(linkObject);
39-
}
39+
}*/
4040

4141
return urdfLink;
4242
}

‎Unity3D/Assets/RosSharp/Scripts/Urdf/UrdfComponents/UrdfInertial.cs

Lines changed: 73 additions & 38 deletions
Original file line numberDiff line numberDiff line change
@@ -23,62 +23,102 @@ namespace RosSharp.Urdf
2323
[RequireComponent(typeof(Rigidbody))]
2424
public class UrdfInertial : MonoBehaviour
2525
{
26+
[SerializeField] private Rigidbody _rigidbody;
2627
public bool DisplayInertiaGizmo;
2728

28-
public bool UseUrdfData;
29+
public enum RigidbodyDataSource { Urdf, Unity, Manual};
30+
public RigidbodyDataSource rigidbodyDataSource;
31+
32+
public float Mass;
2933
public Vector3 CenterOfMass;
3034
public Vector3 InertiaTensor;
3135
public Quaternion InertiaTensorRotation;
3236

37+
public float UrdfMass;
38+
public Vector3 UrdfCenterOfMass;
39+
public Vector3 UrdfInertiaTensor;
40+
public Quaternion UrdfInertiaTensorRotation;
41+
3342
private const int RoundDigits = 10;
3443
private const float MinInertia = 1e-6f;
3544

45+
private bool isCreated;
46+
3647
public static void Create(GameObject linkObject, Link.Inertial inertial = null)
3748
{
3849
UrdfInertial urdfInertial = linkObject.AddComponent<UrdfInertial>();
39-
Rigidbody _rigidbody = urdfInertial.GetComponent<Rigidbody>();
40-
41-
if (inertial != null)
42-
{
43-
_rigidbody.mass = (float)inertial.mass;
50+
urdfInertial.UrdfMass = (float)inertial.mass;
4451

45-
if (inertial.origin != null)
46-
_rigidbody.centerOfMass = UrdfOrigin.GetPositionFromUrdf(inertial.origin);
52+
if (inertial.origin != null)
53+
urdfInertial.UrdfCenterOfMass = UrdfOrigin.GetPositionFromUrdf(inertial.origin);
4754

48-
urdfInertial.ImportInertiaData(inertial.inertia);
55+
urdfInertial.ImportInertiaData(inertial.inertia);
56+
urdfInertial.Initialize();
57+
urdfInertial.isCreated = true;
58+
}
4959

50-
urdfInertial.UseUrdfData = true;
51-
}
60+
private void Initialize()
61+
{
62+
rigidbodyDataSource = RigidbodyDataSource.Urdf;
63+
64+
Mass = UrdfMass;
65+
CenterOfMass = UrdfCenterOfMass;
66+
InertiaTensor = UrdfInertiaTensor;
67+
InertiaTensorRotation = UrdfInertiaTensorRotation;
5268

53-
urdfInertial.DisplayInertiaGizmo = false;
69+
DisplayInertiaGizmo = false;
5470

55-
//Save original rigidbody data from URDF
56-
urdfInertial.CenterOfMass = _rigidbody.centerOfMass;
57-
urdfInertial.InertiaTensor = _rigidbody.inertiaTensor;
58-
urdfInertial.InertiaTensorRotation = _rigidbody.inertiaTensorRotation;
71+
UpdateRigidBodyData();
5972
}
6073

6174
#region Runtime
75+
private void Reset()
76+
{
77+
if(isCreated)
78+
Initialize();
79+
}
6280

63-
private void Start()
81+
private void OnValidate()
6482
{
65-
UpdateRigidBodyData();
83+
if (isCreated)
84+
UpdateRigidBodyData();
6685
}
6786

6887
public void UpdateRigidBodyData()
6988
{
70-
Rigidbody _rigidbody = GetComponent<Rigidbody>();
89+
_rigidbody = GetComponent<Rigidbody>();
7190

72-
if (UseUrdfData)
73-
{
74-
_rigidbody.centerOfMass = CenterOfMass;
75-
_rigidbody.inertiaTensor = InertiaTensor;
76-
_rigidbody.inertiaTensorRotation = InertiaTensorRotation;
77-
}
78-
else
91+
switch (rigidbodyDataSource)
7992
{
80-
_rigidbody.ResetCenterOfMass();
81-
_rigidbody.ResetInertiaTensor();
93+
case RigidbodyDataSource.Urdf:
94+
{
95+
_rigidbody.mass = UrdfMass;
96+
_rigidbody.centerOfMass = UrdfCenterOfMass;
97+
_rigidbody.inertiaTensor = UrdfInertiaTensor;
98+
_rigidbody.inertiaTensorRotation = UrdfInertiaTensorRotation;
99+
return;
100+
}
101+
case RigidbodyDataSource.Unity:
102+
{
103+
_rigidbody.mass = Mass;
104+
bool isKinematic = _rigidbody.isKinematic;
105+
_rigidbody.isKinematic = false;
106+
_rigidbody.ResetCenterOfMass();
107+
_rigidbody.ResetInertiaTensor();
108+
_rigidbody.isKinematic = isKinematic;
109+
CenterOfMass = _rigidbody.centerOfMass;
110+
InertiaTensor = _rigidbody.inertiaTensor;
111+
InertiaTensorRotation = _rigidbody.inertiaTensorRotation;
112+
return;
113+
}
114+
case RigidbodyDataSource.Manual:
115+
{
116+
_rigidbody.mass = Mass;
117+
_rigidbody.centerOfMass = CenterOfMass;
118+
_rigidbody.inertiaTensor = InertiaTensor;
119+
_rigidbody.inertiaTensorRotation = InertiaTensorRotation;
120+
return;
121+
}
82122
}
83123
}
84124

@@ -106,10 +146,10 @@ private void ImportInertiaData(Link.Inertial.Inertia inertia)
106146
Matrix3x3 rotationMatrix = ToMatrix3x3(inertia);
107147
rotationMatrix.DiagonalizeRealSymmetric(out eigenvalues, out eigenvectors);
108148

109-
Rigidbody _rigidbody = GetComponent<Rigidbody>();
110-
111-
_rigidbody.inertiaTensor = ToUnityInertiaTensor(FixMinInertia(eigenvalues));
112-
_rigidbody.inertiaTensorRotation = ToQuaternion(eigenvectors[0], eigenvectors[1], eigenvectors[2]).Ros2Unity();
149+
UrdfInertiaTensor = ToUnityInertiaTensor(FixMinInertia(eigenvalues));
150+
Debug.Log(UrdfInertiaTensor);
151+
UrdfInertiaTensorRotation = ToQuaternion(eigenvectors[0], eigenvectors[1], eigenvectors[2]).Ros2Unity();
152+
Debug.Log(UrdfInertiaTensorRotation);
113153
}
114154

115155
private static Vector3 ToUnityInertiaTensor(Vector3 vector3)
@@ -180,12 +220,7 @@ private static Quaternion ToQuaternion(Vector3 eigenvector0, Vector3 eigenvector
180220
#region Export
181221
public Link.Inertial ExportInertialData()
182222
{
183-
Rigidbody _rigidbody = GetComponent<Rigidbody>();
184-
185-
if (_rigidbody == null)
186-
return null;
187-
188-
UpdateRigidBodyData();
223+
// should we read the data from this clas instead of _rigidbody?
189224
Origin inertialOrigin = new Origin(_rigidbody.centerOfMass.Unity2Ros().ToRoundedDoubleArray(), new double[] { 0, 0, 0 });
190225
Link.Inertial.Inertia inertia = ExportInertiaData(_rigidbody);
191226

‎Unity3D/Assets/RosSharp/Scripts/Urdf/UrdfComponents/UrdfRobot.cs

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -42,7 +42,9 @@ public void SetRigidbodiesIsKinematic(bool isKinematic)
4242
public void SetUseUrdfInertiaData(bool useUrdfData)
4343
{
4444
foreach (UrdfInertial urdfInertial in GetComponentsInChildren<UrdfInertial>())
45-
urdfInertial.UseUrdfData = useUrdfData;
45+
urdfInertial.rigidbodyDataSource = useUrdfData ?
46+
UrdfInertial.RigidbodyDataSource.Urdf :
47+
UrdfInertial.RigidbodyDataSource.Unity;
4648
}
4749

4850
public void SetRigidbodiesUseGravity(bool useGravity)

0 commit comments

Comments
 (0)