def test_ik_spherical_wrist_fk_based(expected_joints, calc_all, dh_melfa_rv_4a, benchmark):
"""
Check that the joints used in the FK can be reconstructed by the IK
:param expected_joints:
:param calc_all:
:param dh_melfa_rv_4a:
:param benchmark:
:return:
"""
expected_joints = np.deg2rad(expected_joints)
# Calculate roboter pose
tform_under_test = forward_kinematics(dh_melfa_rv_4a, expected_joints)
# Calculate pose flags
flags = calculate_pose_flags(dh_melfa_rv_4a, expected_joints)
# Calculate inverse kinematics
if calc_all:
solutions = benchmark(ik_spherical_wrist, dh_melfa_rv_4a, tform_under_test, pose_flags=None)
else:
solutions = benchmark(ik_spherical_wrist, dh_melfa_rv_4a, tform_under_test, pose_flags=flags)
print(f'Solutions found for flags: {sorted(solutions.keys())}')
actual_joints = solutions[flags]
print(f'Actual: {[f"{actual:+.5f}" for actual in actual_joints]}')
print(f'Expect: {[f"{expect:+.5f}" for expect in expected_joints]} Flags: {flags}')
np.testing.assert_allclose(actual_joints, expected_joints, atol=HUNDREDTH_DEGREE)
def test_frr(home):
robot = melfa_rv_4a(rtoff=50, atoff=200)
tuning = [0.005, 0.1, 0.1, 0.1, 0.1, 0.01]
home = [i / 180 * pi for i in home]
new_joints = frr(robot, home, weights=tuning)
tcp_pos = forward_kinematics(robot, home)
tcp_pos_new = forward_kinematics(robot, new_joints)
tcp_pos_dev = tcp_pos_new - tcp_pos
print(f'\nOriginal joints:\t{[f"{i:+.3f}" for i in home]}')
print(f'New joints:\t\t\t{[f"{i:+.3f}" for i in new_joints]}')
print(f'TCP Pos Deviation:\t{[f"{i:+.3f}" for i in tcp_pos_dev[0:3, 3]]}')
print(f'TCP ZDIR Deviation:\t{[f"{i:+.3f}" for i in tcp_pos_dev[0:3, 2]]}')
# Compare rotation around tool axis / z-axis in base frame
np.testing.assert_allclose(np.zeros((3, )), tcp_pos_dev[0:3, 2], atol=1e-3)
# Compare TCP position
np.testing.assert_allclose(np.zeros((3, )), tcp_pos_dev[0:3, 3], atol=1)
def test_ik_spherical_wrist(xdir, ydir, zdir, pos, expected_joints, flags, exc, dh_melfa_rv_4a):
"""
Test that for a given TCP-pose the correct joint angles are determined.
:param xdir: Unit vector of the x-axis of TCP coordinate system expressed in base system
:param ydir: Unit vector of the y-axis of TCP coordinate system expressed in base system
:param zdir: Unit vector of the z-axis of TCP coordinate system expressed in base system
:param pos: TCP position expressed in base system
:param expected_joints: List of expected joint angles in degrees
:param dh_melfa_rv_4a: Joint configuration for Mitsubishi Melfa RV-4A
:return:
"""
# Convert the vectors to a homogeneous matrixy
tform = get_tform(xdir, ydir, zdir, pos)
# Calculate the inverse kinematics
print('\n\nCalculating inverse kinematics...')
if exc is None:
# Regular solution should be possible
solutions = ik_spherical_wrist(dh_melfa_rv_4a, tform, pose_flags=None)
act_joints = solutions[flags]
for key, val in sorted(solutions.items(), key=lambda d: d[0]):
print(f'Solution for flag {key}: {[f"{actual:+.5f}" for actual in val]}')
# Test the solution
expected_joints = np.deg2rad(expected_joints)
print('\nChecking results...')
print(f'Actual ({flags}):\t{[f"{actual:+.5f}" for actual in act_joints]}')
print(f'Expect:\t{[f"{expect:+.5f}" for expect in expected_joints]}')
non_flip = bool((flags & 1) == 1)
up = bool((flags & 2) == 2)
right = bool((flags & 4) == 4)
print(f'EFlags:\t{"R" if right else "L"},{"A" if up else "B"},{"N" if non_flip else "F"}')
# Reconvert the joint angles to a pose
print('\n\nCalculating forward kinematics from solution...')
tform_fk = forward_kinematics(dh_melfa_rv_4a, act_joints)
# Check that the matrices are the same
print('\nChecking results...')
np.testing.assert_allclose(tform_fk, tform, atol=TENTH_MM, err_msg='FK result deviated by > 0.1 mm')
np.testing.assert_allclose(act_joints, expected_joints, atol=ONE_DEGREE, err_msg='IK result off by > 1 deg')
else:
# Singularity expected
with pytest.raises(exc):
act_joints = ik_spherical_wrist(dh_melfa_rv_4a, tform, pose_flags=flags)[flags]
print(f'Actual: {[f"{actual:+.3f}" for actual in act_joints]}')
print('All good!')
def test_tform2euler(dh_melfa_rv_4a, joints_deg, a, b, c):
# Convert to radian
joints_rad = [np.deg2rad(i) for i in joints_deg]
# Calculate actual coordinates
tcp_pose = forward_kinematics(dh_melfa_rv_4a, joints_rad)
# Calculate euler angles
actual_angles = tform2euler(tcp_pose)
actual_angles = np.rad2deg(actual_angles)
a_actual, b_actual, c_actual = actual_angles
digits = 5
print(
f'Actual: A:{a_actual:.{digits}f}° B:{b_actual:.{digits}f}° C:{c_actual:.{digits}f}°'
)
print(f'Expected: A:{a:.{digits}f}° B:{b:.{digits}f}° C:{c:.{digits}f}°')
# Check
diff_to_gimbal_lock = abs(abs(b) - 90)
# Different tolerance depending on distance to gimbal lock
if diff_to_gimbal_lock == 0.0:
# Gimbal lock!
assert round(b - b_actual, 3) % 360 == pytest.approx(0, abs=0.01)
assert round(a - a_actual + c - c_actual, 3) % 360 == pytest.approx(
0, abs=1.5)
else:
if diff_to_gimbal_lock > 0.1:
# Normal
atol = 0.01
else:
# Very close
atol = 1.0
# Rounding to many digits is still necessary to get the modulo right
r = 10
# Angles match or are off by 360 degrees
assert (a == pytest.approx(
a_actual, abs=atol)) or (round(a - a_actual, r) % 360
== pytest.approx(0, abs=atol))
assert (b == pytest.approx(
b_actual, abs=atol)) or (round(b - b_actual, r) % 360
== pytest.approx(0, abs=atol))
assert (c == pytest.approx(
c_actual, abs=atol)) or (round(c - c_actual, r) % 360
== pytest.approx(0, abs=atol))
def test_forward_kinematics_two_translational_joints(
simple_translational_joint, joint_coordinate):
"""
Validate that the correct position and orientation is obtained for transformations for two translational joints.
:param simple_translational_joint:
:param joint_coordinate:
:return:
"""
# Calculate the forward kinematics
transformation = forward_kinematics([simple_translational_joint] * 2,
[joint_coordinate] * 2)
validate_vec('X', transformation, [1, 0, 0])
validate_vec('Y', transformation, [0, 1, 0])
validate_vec('Z', transformation, [0, 0, 1])
validate_vec('pos', transformation, [0, 0, 2 * joint_coordinate])
def _calc_j2_dependants(conf: List[BaseJoint], non_flip: Optional[bool], p14: np.ndarray, flag_sum: int, j1: float,
j2_solutions: Dict[bool, float], tf12: np.ndarray, xdir: np.ndarray, zdir: np.ndarray) \
-> JointSolution:
"""
Calculate all dependant angles (3-6) for given j2 solutions
:param conf: Tuple of joints, offsets are considered
:param non_flip: Non-flip flag. If None all possible configurations are calculated.
:param p14: Vector from frame origin in joint 2 to wrist center
:param flag_sum: Sum of previous levels. All solutions from here are have flag = flagsum + k, k=0..3
:param j1: Value of calculated J1 angle
:param j2_solutions: Dict of J2 solutions. Boolean values of right flag are keys, angles are values.
:param tf12: Transformation matrix from joint 1 to joint 2
:param xdir: TCP frame x-direction unit vector
:param zdir: TCP frame z-direction unit vector
:return: Dictionary of complete IK solutions. Pose flags are key, lists of angles are values.
"""
out = {}
# The solutions of theta 2 contain the corresponding elbow flag
for idx2, (elbow_up, j2) in enumerate(j2_solutions.items()):
# No common setup required
try:
# Calculate theta 3
j3 = _ik_spherical_wrist_joint3(conf, elbow_up, p14)
# Calculate transformation from joint 1 to joint 4
tf24 = forward_kinematics(conf[1:3], [j2, j3],
subtract_offset=True)
tf14 = np.dot(tf12, tf24)
# Calculate theta 5 (requires theta 1 - 3)
j5_solutions = _ik_spherical_wrist_joint5(non_flip, tf14, zdir)
# Add the flag summand of the current level
flag_sum += 2 * elbow_up
# Calculate all angles depending on the j5 solutions
inner_out = _calc_j5_dependants(conf, flag_sum, [j1, j2, j3],
j5_solutions, tf14, xdir, zdir)
# Collect the solutions
out.update(inner_out)
except ValueError:
if len(out) == 0 and idx2 == len(j2_solutions) - 1:
# IK failed for all theta 2 solutions so failed for current theta 1 only
raise
return out
def _calc_j5_dependants(conf: List[BaseJoint], flag_sum: int,
theta123: List[float], j5_solutions: Dict[bool, float],
tf14: np.ndarray, xdir: np.ndarray,
zdir: np.ndarray) -> JointSolution:
"""
Calculate the dependant last joint for given j5 solutions
:param conf: Tuple of joints, offsets are considered
:param flag_sum: Sum of previous levels. All solutions from here are have flag = flagsum + k, k=0..1
:param theta123: List of angles j1-j3
:param j5_solutions: Dict of J5 solutions. Boolean values of right flag are keys, angles are values.
:param tf14: Transformation matrix from joint 1 to joint 4
:param xdir: TCP frame x-direction unit vector
:param zdir: TCP frame z-direction unit vector
:return: Dictionary of complete IK solutions. Pose flags are key, lists of angles are values.
"""
out = {}
# The solutions of theta 5 contain the corresponding non flip flag
for idx5, (non_flip, theta5) in enumerate(j5_solutions.items()):
try:
# Calculate theta 4
theta4 = _ik_spherical_wrist_joint4(non_flip, tf14, zdir)
# Calculate transformation from joint 1 to joint 6
tf46 = forward_kinematics(conf[3:5], [theta4, theta5],
subtract_offset=True)
tf16 = np.dot(tf14, tf46)
# Calculate theta 6 (requires theta 1 - 5)
theta6 = _ik_spherical_wrist_joint6(tf16, xdir)
except ValueError:
# Solution failed
if len(out) == 0 and idx5 == len(j5_solutions) - 1:
# IK failed for all theta 5 solutions so failed for current theta 2 only
raise
else:
# Bundle all the angles and wrap them to (-pi, pi]
theta = theta123 + [theta4, theta5, theta6]
theta = [
wrap_to_pi(angle - joint.zero_offset)
for angle, joint in zip(theta, conf)
]
# Append to solutions using the pose flags as key
out[flag_sum + non_flip] = theta
return out
def _calc_j1_dependants(config: List[BaseJoint], elbow_up: Optional[bool], non_flip: Optional[bool], p04: np.ndarray,
j1_solutions: Dict[bool, float], xdir: np.ndarray, zdir: np.ndarray) \
-> JointSolution:
"""
Calculate all dependant angles (2-6) for given j1 solutions
:param config: Tuple of joints, offsets are considered
:param elbow_up: Elbow-up flag. If None all possible configurations are calculated.
:param non_flip: Non-flip flag. If None all possible configurations are calculated.
:param p04: Wrist center position given in base frame
:param j1_solutions: Dict of J1 solutions. Boolean values of right flag are keys, angles are values.
:param xdir: TCP frame x-direction unit vector
:param zdir: TCP frame z-direction unit vector
:return: Dictionary of complete IK solutions. Pose flags are key, lists of angles are values.
"""
out = {}
# The solutions of theta 1 contain the corresponding flag_sum flag
for idx1, (right, theta1) in enumerate(j1_solutions.items()):
# Get the vector from origin of joint 2 (frame 1) to wrist center
tjoint12 = forward_kinematics([config[0]], [theta1],
subtract_offset=True)
p01 = tjoint12[0:3, 3]
p14 = p04 - p01
try:
# Calculate theta 2 for the current flag_right value
j2_solutions = _ik_spherical_wrist_joint2(config, right, elbow_up,
tjoint12, p14)
# Calculate the flag summand of the current level
flag_sum = 4 * right
# Calculate all angles depending on the j2 solutions
inner_out = _calc_j2_dependants(config, non_flip, p14, flag_sum,
theta1, j2_solutions, tjoint12,
xdir, zdir)
# Collect the solutions
out.update(inner_out)
except ValueError:
# This is only an issue if this is the last iteration of j1 and no solution has been found yet
if len(out) == 0 and idx1 == len(j1_solutions) - 1:
# IK failed for all theta 1 solutions so failed completely
raise
return out
def test_forward_kinematics_rotational_joint(simple_rotational_joint,
joint_coordinate):
"""
Validate that the correct position and orientation is obtained for transformations for one rotational joint.
:param simple_rotational_joint:
:param joint_coordinate:
:return:
"""
# Calculate the forward kinematics
transformation = forward_kinematics([simple_rotational_joint],
[joint_coordinate])
validate_vec('X', transformation,
[cos(joint_coordinate),
sin(joint_coordinate), 0])
validate_vec('Y', transformation,
[-sin(joint_coordinate),
cos(joint_coordinate), 0])
validate_vec('Z', transformation, [0, 0, 1])
validate_vec('pos', transformation, [0, 0, 0])
def is_node_free_and_within(config: List[BaseJoint],
collider: MatlabCollisionChecker,
jcurr: List[float], clim: List[float]) -> bool:
"""
Check whether a node is not in collision and within the cartesian boundaries
:param config: List of joints containing coordinate transformations.
:param collider: Matlab collision checking interface
:param jcurr: Joint coordinates for the given node
:param clim: Cartesian boundaries
:return: Flag to indicate whether the node is free and within the cartesian boundaries
"""
# Check cartesian position
pose = forward_kinematics(config, jcurr)
cviolation = get_violated_boundaries(pose[0:3, 3], clim)
if cviolation:
# Point is outside of allowed cuboid, generate new node
return False
# Check node for collisions
collisions = collider.check_collisions(jcurr, visual=False)
return not collisions[0]
def frr(config: List[BaseJoint], initial_joints: List[float], weights=None, stop_threshold: float = 1e-5) \
-> List[float]:
"""
Functional Redundancy Resolution
:param config: Tuple of joints, offsets are considered
:param initial_joints: Tuple of joint coordinate values (either mm or radian)
:param weights:
:param stop_threshold:
:return:
"""
# Obtain tool column vector as z-axis of endeffector frame
total_tform = forward_kinematics(config, initial_joints)
e = total_tform[0:3, 2]
e = e[:, None]
# Calculate projector onto tool axis
t_proj = e @ e.T
joint_limits = [
-2.7925, 2.7925, -1.5708, 2.4435, +0.2618, 2.9496, -2.7925, 2.7925,
-2.0944, 2.0944, -3.4907, 3.4907
]
median = [(lower + upper) / 2
for lower, upper in zip(joint_limits[::2], joint_limits[1::2])]
# Initialize current joint column vector
current_joints = np.asarray(initial_joints)
current_joints = current_joints[:, None]
# Optimize the joint vector in the orthogonal space
while True:
# Calculate jacobian and its righ-hand pseudo-inverse for current joint values
jac = geometric_jacobian(config, current_joints[:, 0].tolist())
pinv_jac = right_generalized_inverse_jacobian(jac)
# Create the manipulation column vector
manip = [
qm - q for qm, q in zip(median, current_joints[:, 0].tolist())
]
h = np.asarray(manip)
h = h[:, None]
if h.shape != (len(initial_joints), 1):
raise TypeError(f'h must be a {len(initial_joints)}x1 vector.')
# Set weights as diagonal matrix
if weights is None:
weights = [0.1] * len(initial_joints)
elif any((i >= 1 for i in weights)):
raise ValueError(
'Weights need to be < 1 for the algorithm to converge.')
w = np.diag(weights)
# Calculate delta for iteration
effect = np.linalg.multi_dot([t_proj, jac[3:, :], w, h])
delta_joints = pinv_jac @ np.vstack([np.zeros((3, 1)), effect])
current_joints += delta_joints
if np.linalg.norm(delta_joints) < stop_threshold:
# Stop when the correcting effort is below a certain threshold
break
# Return flat list
return current_joints[:, 0].tolist()
return current_joints[:, 0].tolist()
if __name__ == '__main__':
# Define test data
robot = melfa_rv_4a(rtoff=-50, atoff=200)
home = [0, 0, pi / 2, 0, pi / 2, pi]
tuning = [0, 0.1, 0.1, 0.1, 0.1, 0.01]
# Do optimization and print joints
new_joints = frr(robot, home, weights=tuning)
print(f'Original joints:\t{[f"{i:+.3f}" for i in home]}')
print(f'New joints:\t\t\t{[f"{i:+.3f}" for i in new_joints]}')
# Validate new position
tcp_pos = forward_kinematics(robot, home)
tcp_pos_new = forward_kinematics(robot, new_joints)
tcp_pos_dev = tcp_pos_new - tcp_pos
print(f'TCP Pos Deviation:\t{[f"{i:+.3f}" for i in tcp_pos_dev[0:3, 3]]}')
print(f'TCP ZDIR Deviation:\t{[f"{i:+.3f}" for i in tcp_pos_dev[0:3, 2]]}')
print(f'Weights:\t\t\t{tuning}')
def expand_task_trajectory(task_trajectory: List[CartesianTrajSegment],
dphi: float) -> List[CartesianTrajSegment]:
"""
Expand the task trajectory by adding equivalent points obtained by applying constant per segment rotation around
the tool axis.
:param task_trajectory:
:param dphi:
:return:
def test_forward_kinematics_unequal_length(dh_melfa_rv_4a):
with pytest.raises(ValueError):
forward_kinematics(dh_melfa_rv_4a, [0] * (len(dh_melfa_rv_4a) - 1))
with pytest.raises(ValueError):
forward_kinematics(dh_melfa_rv_4a, [0] * (len(dh_melfa_rv_4a) + 1))