From d5a185a899d5f14b3ebea2af44d73e1749a0e28e Mon Sep 17 00:00:00 2001
From: Maxim Kulkin <maxim.kulkin@gmail.com>
Date: Wed, 13 Jun 2018 02:04:18 -0700
Subject: [PATCH 1/2] Improve URDF loading of optional joint
 elements/attributes

---
 src/ikpy/URDF_utils.py | 28 ++++++++++++++++++++--------
 1 file changed, 20 insertions(+), 8 deletions(-)

diff --git a/src/ikpy/URDF_utils.py b/src/ikpy/URDF_utils.py
index cafe203..17fd6ec 100644
--- a/src/ikpy/URDF_utils.py
+++ b/src/ikpy/URDF_utils.py
@@ -136,14 +136,26 @@ 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]
+
+        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()]
+
         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"],
+            translation_vector=translation,
+            orientation=orientation,
+            rotation=rotation,
         ))
 
     # Add last_link_vector to parameters
@@ -151,7 +163,7 @@ def get_urdf_parameters(urdf_file, base_elements=["base_link"], last_link_vector
         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"
         ))
 

From b79239d1b47fa1ba9db4cf0b2eb0ef77ccc1c535 Mon Sep 17 00:00:00 2001
From: Maxim Kulkin <maxim.kulkin@gmail.com>
Date: Wed, 13 Jun 2018 02:05:15 -0700
Subject: [PATCH 2/2] Update URDF loading to support loading joint limits

---
 src/ikpy/URDF_utils.py | 9 +++++++++
 src/ikpy/link.py       | 5 +++--
 2 files changed, 12 insertions(+), 2 deletions(-)

diff --git a/src/ikpy/URDF_utils.py b/src/ikpy/URDF_utils.py
index 17fd6ec..c1d2fd0 100644
--- a/src/ikpy/URDF_utils.py
+++ b/src/ikpy/URDF_utils.py
@@ -139,6 +139,7 @@ def get_urdf_parameters(urdf_file, base_elements=["base_link"], last_link_vector
         translation = [0, 0, 0]
         orientation = [0, 0, 0]
         rotation = [1, 0, 0]
+        bounds = [None, None]
 
         origin = joint.find("origin")
         if origin is not None:
@@ -151,8 +152,16 @@ def get_urdf_parameters(urdf_file, base_elements=["base_link"], last_link_vector
         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(
             name=joint.attrib["name"],
+            bounds=tuple(bounds),
             translation_vector=translation,
             orientation=orientation,
             rotation=rotation,
diff --git a/src/ikpy/link.py b/src/ikpy/link.py
index 12b5e16..f1f6fa9 100644
--- a/src/ikpy/link.py
+++ b/src/ikpy/link.py
@@ -24,7 +24,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
@@ -87,9 +87,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))))