Merge pull request #156 from Algoryx/fix/urdf-import

Small URDF import fixes

AGXUnity/IO/URDF/Model.cs

@@ -612,7 +612,7 @@ private RigidBody CreateRigidBody( GameObject gameObject, Link link )
     {
       var rb = GetOrCreateComponent<RigidBody>( gameObject );
       GetOrCreateComponent<ElementComponent>( gameObject ).SetElement( link );
-      if ( link.IsStatic ) {
+      if ( link.IsStatic || link.IsWorld) {
         rb.MotionControl = agx.RigidBody.MotionControl.STATIC;
         return rb;
       }
@@ -784,6 +784,12 @@ private GameObject AddJoint( UJoint joint, Dictionary<string, GameObject> linkIn
                                                parent.GetComponent<RigidBody>() );
 
         var constraint = constraintGameObject.GetComponent<Constraint>();
+
+        // Since URDF does not have a notion of stiffness and assumes a stiff system we set the default stiffness to be slightly higher than the regular default.
+        constraint.SetCompliance( 1e-10f );
+        foreach ( var controller in constraint.GetElementaryConstraintControllers() )
+          controller.Compliance = 1e-10f;
+
         constraint.CollisionsState = Constraint.ECollisionsState.DisableRigidBody1VsRigidBody2;
         // Note: If this is a mimic joint we're just setting the values,
         //       the actual controllers are disabled.

AGXUnity/IO/URDF/UJoint.cs

@@ -393,6 +393,8 @@ public override void Read( XElement element, bool optional = true )
                JointType.Fixed:
              type == "planar" ?
                JointType.Planar :
+             type == "floating" ?
+               JointType.Floating :
                JointType.Unknown;
       if ( Type == JointType.Unknown )
         throw new UrdfIOException( $"{Utils.GetLineInfo( element )}: Unknown joint type '{type}'." );