Merge pull request #73 from berkayalpcakal/master

improvements regarding urdf patcher and joint states
siemens · Jun 11, 2018 · df13069 · df13069
2 parents b961811 + b8e9979
commit df13069
Show file tree

Hide file tree

Showing 6 changed files with 35 additions and 21 deletions.
diff --git a/Unity3D/Assets/RosSharp/Editor/UrdfImporter/UrdfJointExtensions.cs b/Unity3D/Assets/RosSharp/Editor/UrdfImporter/UrdfJointExtensions.cs
@@ -27,8 +27,6 @@ public static UnityEngine.Joint Create(this Joint joint, GameObject gameObject,
             if (parentRigidbody == null)
                 return null;
 
-            gameObject.name = gameObject.name + " (" + joint.type + " Joint: " + joint.name + ")";
-
             if (joint.type == "fixed")
                 return joint.CreateFixedJoint(gameObject, parentRigidbody);
             if (joint.type == "continuous" || joint.type == "revolute")
@@ -130,6 +128,9 @@ public static ConfigurableJoint CreatePrismaticJoint(this Joint joint, GameObjec
             {
                 prismaticJoint.lowAngularXLimit = joint.limit.GetLowSoftJointLimit();
                 prismaticJoint.highAngularXLimit = joint.limit.GetHighSoftJointLimit();
+
+                // set linear limit
+                prismaticJoint.linearLimit = joint.limit.GetLinearLimit();
             }
 
             // data:
@@ -237,5 +238,12 @@ public static SoftJointLimit GetHighSoftJointLimit(this Joint.Limit limit)
             softJointLimit.limit = (float)limit.upper * Mathf.Rad2Deg;
             return softJointLimit;
         }
+
+        public static SoftJointLimit GetLinearLimit(this Joint.Limit limit)
+        {
+            SoftJointLimit softJointLimit = new SoftJointLimit();
+            softJointLimit.limit = (float)limit.upper;
+            return softJointLimit;
+        }
     }
 }
diff --git a/Unity3D/Assets/RosSharp/Scripts/MessageHandling/JointStateReader.cs b/Unity3D/Assets/RosSharp/Scripts/MessageHandling/JointStateReader.cs
@@ -78,15 +78,15 @@ public RevoluteJoint(HingeJoint _hingeJoint)
             }
             public float GetPosition()
             {
-                return hingeJoint.angle * Mathf.Deg2Rad;
+                return -hingeJoint.angle * Mathf.Deg2Rad;
             }
             public float GetVelocity()
             {
-                return hingeJoint.velocity * Mathf.Deg2Rad;
+                return -hingeJoint.velocity * Mathf.Deg2Rad;
             }
             public float GetEffort()
             {
-                return hingeJoint.motor.force;
+                return -hingeJoint.motor.force;
             }
         }
     }

diff --git a/Unity3D/Assets/RosSharp/Scripts/MessageHandling/JointStateReceiver.cs b/Unity3D/Assets/RosSharp/Scripts/MessageHandling/JointStateReceiver.cs
@@ -15,14 +15,16 @@ limitations under the License.
 
 using System;
 using System.Collections.Generic;
+using UnityEngine;
 
 namespace RosSharp.RosBridgeClient
 {
     public class JointStateReceiver : MessageReceiver
     {
         public override Type MessageType { get { return (typeof(SensorJointStates)); } }
 
-        public Dictionary<string, JointStateWriter> JointStateWriterDictionary;
+        public List<string> JointNames;
+        public List<JointStateWriter> JointStateWriters;
 
         private SensorJointStates message;
 
@@ -34,8 +36,8 @@ private void Awake()
         private void ReceiveMessage(object sender, MessageEventArgs e)
         {
             message = (SensorJointStates)e.Message;
-            for (int i=0; i< message.name.Length; i++)            
-                JointStateWriterDictionary[message.name[i]].Write(message.position[i]);   
+            for (int i = 0; i < message.name.Length; i++)
+                JointStateWriters[JointNames.IndexOf( message.name[i] )].Write( message.position[i] );
         }
     }
 }

diff --git a/Unity3D/Assets/RosSharp/Scripts/MessageHandling/JointStateWriter.cs b/Unity3D/Assets/RosSharp/Scripts/MessageHandling/JointStateWriter.cs
@@ -23,8 +23,8 @@ public class JointStateWriter : MonoBehaviour
         private Joint joint;
         private JointUrdfDataManager jointUrdfDataManager;
 
-        private float newState; // deg or m
-        private float prevState; // deg or m
+        private float newState;  // rad or m
+        private float prevState; // rad or m
         private bool isNewStateReceived;
 
         private void Start()
@@ -55,12 +55,12 @@ private void WriteHingeJointUpdate()
         {
             Vector3 anchor = transform.TransformPoint(joint.anchor);
             Vector3 axis = transform.TransformDirection(joint.axis);
-            transform.RotateAround(anchor, axis, (prevState - newState) * Mathf.Rad2Deg);
+            transform.RotateAround(anchor, axis, -(newState - prevState) * Mathf.Rad2Deg);
         }
         private void WritePrismaticJointUpdate()
         {
             Vector3 axis = transform.TransformDirection(joint.axis);
-            transform.Translate(axis * (prevState - newState));
+            transform.Translate(axis * (newState - prevState));
         }
 
         public void Write(float state)

diff --git a/Unity3D/Assets/RosSharp/Scripts/MessageHandling/JoyAxisJointTransformWriter.cs b/Unity3D/Assets/RosSharp/Scripts/MessageHandling/JoyAxisJointTransformWriter.cs
@@ -43,7 +43,7 @@ private void SetLimit()
             if (jointUrdfDataManager.IsRevoluteOrContinuous)
             {
                 HingeJoint hingeJoint = (HingeJoint)joint;
-                limit = new Vector2(hingeJoint.limits.min, hingeJoint.limits.max);
+                limit = new Vector2(hingeJoint.limits.min, hingeJoint.limits.max) * Mathf.Deg2Rad;
             }
             else if (jointUrdfDataManager.IsPrismatic)
             {

diff --git a/Unity3D/Assets/RosSharp/Scripts/UrdfComponents/UrdfPatcher.cs b/Unity3D/Assets/RosSharp/Scripts/UrdfComponents/UrdfPatcher.cs
@@ -53,7 +53,8 @@ public void Patch()
                 jointStateProvider.JointStateReaders = AddJointStateReaderComponents();
 
             if (AddJointStateWriters)
-                jointStateReceiver.JointStateWriterDictionary = AddJointStateWriterComponents();
+               AddJointStateWriterComponents(out jointStateReceiver.JointNames, out jointStateReceiver.JointStateWriters);
+
         }
 
         private JointStateReader[] AddJointStateReaderComponents() 
@@ -64,14 +65,17 @@ private JointStateReader[] AddJointStateReaderComponents()
             return jointStateReaders.ToArray();
         }
 
-        private Dictionary<string, JointStateWriter> AddJointStateWriterComponents()
-        {
-            Dictionary<string, JointStateWriter> jointStateWriters = new Dictionary<string, JointStateWriter>();
+        private void AddJointStateWriterComponents(out List<string> jointNames, out List<JointStateWriter> jointStateWriters)
+        {   
+            jointNames = new List<string>();
+            jointStateWriters = new List<JointStateWriter>();
+
             foreach (JointUrdfDataManager jointUrdfDataManager in UrdfModel.GetComponentsInChildren<JointUrdfDataManager>())
-                jointStateWriters.Add(
-                    jointUrdfDataManager.name,
-                    jointUrdfDataManager.gameObject.AddComponent<JointStateWriter>());
-            return jointStateWriters;
+            {
+                jointNames.Add(jointUrdfDataManager.JointName);
+                jointStateWriters.Add(jointUrdfDataManager.gameObject.AddComponent<JointStateWriter>());
+            }
+
         }
 
         private void RemoveExistingComponents()