From 4b9d46eadd1297fccdadaafe8897831a13765d62 Mon Sep 17 00:00:00 2001 From: tanneguy Date: Sun, 28 Jul 2024 20:23:19 +0200 Subject: [PATCH 1/3] enable DH ik (#143) * enable DH * set name at None --------- Co-authored-by: tdevillemagne --- src/ikpy/link.py | 14 +++++++++--- tests/test_dh_UR10.py | 53 +++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 64 insertions(+), 3 deletions(-) create mode 100644 tests/test_dh_UR10.py diff --git a/src/ikpy/link.py b/src/ikpy/link.py index 7619f1c..c91794e 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_dh_UR10.py b/tests/test_dh_UR10.py new file mode 100644 index 0000000..275a251 --- /dev/null +++ b/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() \ No newline at end of file From 9e4b2f8c5797632301a9a55216fbb0ac89d48056 Mon Sep 17 00:00:00 2001 From: "Pierre Manceron (phylliade)" Date: Sun, 28 Jul 2024 20:26:00 +0200 Subject: [PATCH 2/3] Refactor tests and bump version --- src/ikpy/_version.py | 2 +- tests/{test_dh_UR10.py => test_chain_dh.py} | 26 +++++++++++---------- 2 files changed, 15 insertions(+), 13 deletions(-) rename tests/{test_dh_UR10.py => test_chain_dh.py} (71%) 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/tests/test_dh_UR10.py b/tests/test_chain_dh.py similarity index 71% rename from tests/test_dh_UR10.py rename to tests/test_chain_dh.py index 275a251..848741c 100644 --- a/tests/test_dh_UR10.py +++ b/tests/test_chain_dh.py @@ -39,15 +39,17 @@ def create_dh_robot(robot): 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() \ No newline at end of file + +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") From 66872d0bad0422dd6085c4b64f23cf86d56915c8 Mon Sep 17 00:00:00 2001 From: "Pierre Manceron (phylliade)" Date: Sun, 28 Jul 2024 20:28:20 +0200 Subject: [PATCH 3/3] Fix linting --- src/ikpy/link.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/ikpy/link.py b/src/ikpy/link.py index c91794e..db24e18 100644 --- a/src/ikpy/link.py +++ b/src/ikpy/link.py @@ -268,8 +268,8 @@ class DHLink(Link): d: float offset along previous z to the common normal a: float - length of the common normal (do not confuse with alpha). - Assuming a revolute joint, this is the radius about previous z. + 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