enable DH ik (#143)

* enable DH

* set name at None

---------

Co-authored-by: tdevillemagne <[email protected]>

src/ikpy/link.py

@@ -40,6 +40,7 @@ def __init__(self, name, length, bounds=None, is_final=False):
         self.axis_length = length
         self.is_final = is_final
         self.has_rotation = False
+        self.has_translation = False
         self.joint_type = None
 
     def __repr__(self):
@@ -267,7 +268,12 @@ class DHLink(Link):
     d: float
         offset along previous z to the common normal
     a: float
-        offset along previous   to the common normal
+        length of the common normal (do not confuse with alpha). 
+        Assuming a revolute joint, this is the radius about previous z.
+    alpha: float
+        angle about common normal, from old z axis to new z axis
+    theta: float
+        angle about previous z, from old x to new x
     use_symbolic_matrix: bool
         whether the transformation matrix is stored as Numpy array or as a Sympy symbolic matrix.
 
@@ -277,10 +283,12 @@ class DHLink(Link):
         The link object
     """
 
-    def __init__(self, name, d=0, a=0, bounds=None, use_symbolic_matrix=True):
-        Link.__init__(self, use_symbolic_matrix)
+    def __init__(self, name=None, d=0, a=0, alpha=0, theta=0, bounds=None, use_symbolic_matrix=True, length=0):
+        Link.__init__(self, use_symbolic_matrix, length=length)
         self.d = d
         self.a = a
+        self.alpha = alpha
+        self.theta = theta
 
     def get_link_frame_matrix(self, parameters):
         """ Computes the homogeneous transformation matrix for this link. """

tests/test_dh_UR10.py

@@ -0,0 +1,53 @@
+from ikpy.chain import Chain
+from ikpy.link import OriginLink, DHLink
+from ikpy.utils import plot
+
+import matplotlib.pyplot as plt
+
+import numpy as np
+from math import pi
+
+class UR10():
+    def __init__(self):
+        self.robot_name = 'UR10'
+        self.home_config = [0, -pi/2, 0, -pi/2, 0, 0] 
+        self.dh_params = np.array([
+            [  0.1273, 0., pi/2, 0.],
+            [  0., -0.612, 0, 0.],
+            [  0., -0.5723, 0, 0.],
+            [  0.163941, 0.,  pi/2, 0.],
+            [  0.1147, 0.,  -pi/2, 0.],
+            [  0.0922, 0., 0, 0.]])   
+
+        self.joint_limits =  [
+                        (-360, 360),  
+                        (-360, 360),  
+                        (-360, 360),  
+                        (-360, 360),  
+                        (-360, 360),  
+                        (-360, 360)] 
+
+def create_dh_robot(robot):
+    # Create a list of links for the robot
+    links = []
+    for i, dh in enumerate(robot.dh_params):
+        link = DHLink(d=dh[0], a=dh[1], alpha=dh[2], theta=dh[3], length=abs(dh[1]))
+        link.bounds = robot.joint_limits[i]
+        links.append(link)
+
+    # Create a chain using the robot links
+    chain = Chain(links, name=robot.robot_name)
+    return chain
+
+robot = UR10()
+chain = create_dh_robot(robot)
+frame = [[1.112918581, -0.209413742, 0.19382176], [0.0, 1.0, 0.0]]
+target_position, target_orientation = frame
+joint_angles = chain.inverse_kinematics(target_position=target_position, target_orientation=target_orientation, orientation_mode="Z")
+
+print(joint_angles)
+
+fig, ax = plot.init_3d_figure()
+chain.plot(robot.home_config, ax)
+chain.plot(joint_angles, ax)
+plt.show()