diff --git a/src/ikpy/_version.py b/src/ikpy/_version.py index 6c61f23..36f7773 100644 --- a/src/ikpy/_version.py +++ b/src/ikpy/_version.py @@ -1 +1 @@ -__version__ = '3.3.4' +__version__ = '3.4' diff --git a/src/ikpy/link.py b/src/ikpy/link.py index 7619f1c..db24e18 100644 --- a/src/ikpy/link.py +++ b/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. """ diff --git a/tests/test_chain_dh.py b/tests/test_chain_dh.py new file mode 100644 index 0000000..848741c --- /dev/null +++ b/tests/test_chain_dh.py @@ -0,0 +1,55 @@ +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 + + +def test_dh_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.savefig("out/UR10.png")