Merge pull request #42 from maximkulkin/improve-urdf-loading

Improve urdf loading. Fixes #32, #34, #39
Phylliade · Mar 17, 2019 · f3ec04a · f3ec04a
2 parents 25c6fa8 + e9756b5
commit f3ec04a
Show file tree

Hide file tree

Showing 2 changed files with 32 additions and 10 deletions.
diff --git a/src/ikpy/URDF_utils.py b/src/ikpy/URDF_utils.py
@@ -150,22 +150,43 @@ def get_urdf_parameters(urdf_file, base_elements=["base_link"], last_link_vector
 
     # Save the joints in the good format
     for joint in joints:
-        translation = joint.find("origin").attrib["xyz"].split()
-        orientation = joint.find("origin").attrib["rpy"].split()
-        rotation = joint.find("axis").attrib['xyz'].split()
+        translation = [0, 0, 0]
+        orientation = [0, 0, 0]
+        rotation = [1, 0, 0]
+        bounds = [None, None]
+
+        origin = joint.find("origin")
+        if origin is not None:
+            if origin.attrib["xyz"]:
+                translation = [float(x) for x in origin.attrib["xyz"].split()]
+            if origin.attrib["rpy"]:
+                orientation = [float(x) for x in origin.attrib["rpy"].split()]
+
+        axis = joint.find("axis")
+        if axis is not None:
+            rotation = [float(x) for x in axis.attrib["xyz"].split()]
+
+        limit = joint.find("limit")
+        if limit is not None:
+            if limit.attrib["lower"]:
+                bounds[0] = float(limit.attrib["lower"])
+            if limit.attrib["upper"]:
+                bounds[1] = float(limit.attrib["upper"])
+
         parameters.append(lib_link.URDFLink(
-            translation_vector=[float(translation[0]), float(translation[1]), float(translation[2])],
-            orientation=[float(orientation[0]), float(orientation[1]), float(orientation[2])],
-            rotation=[float(rotation[0]), float(rotation[1]), float(rotation[2])],
-            name=joint.attrib["name"]
+            name=joint.attrib["name"],
+            bounds=tuple(bounds),
+            translation_vector=translation,
+            orientation=orientation,
+            rotation=rotation,
         ))
 
     # Add last_link_vector to parameters
     if last_link_vector is not None:
         parameters.append(lib_link.URDFLink(
             translation_vector=last_link_vector,
             orientation=[0, 0, 0],
-            rotation=[0, 0, 0],
+            rotation=[1, 0, 0],
             name="last_joint"
         ))
 

diff --git a/src/ikpy/link.py b/src/ikpy/link.py
@@ -27,7 +27,7 @@ def __init__(self, name, bounds=(None, None)):
         self.name = name
 
     def __repr__(self):
-        return("Link name={}".format(self.name))
+        return("Link name={} bounds={}".format(self.name, self.bounds))
 
     def _get_rotation_axis(self):
         # Defaults to None
@@ -90,9 +90,10 @@ def __init__(self, name, translation_vector, orientation, rotation, bounds=(None
 
     def __str__(self):
         return("""URDF Link {} :
+    Bounds : {}
     Translation : {}
     Orientation : {}
-    Rotation : {}""".format(self.name, self.translation_vector, self.orientation, self.rotation))
+    Rotation : {}""".format(self.name, self.bounds, self.translation_vector, self.orientation, self.rotation))
 
     def _get_rotation_axis(self):
         return (np.dot(geometry_utils.homogeneous_translation_matrix(*self.translation_vector), np.dot(geometry_utils.cartesian_to_homogeneous(geometry_utils.rpy_matrix(*self.orientation)), geometry_utils.cartesian_to_homogeneous_vectors(self.rotation * self._axis_length))))