class AlignedKinematicsTest(unittest.TestCase): """ Tests aligned kinematics """ def setUp(self): print("<Setup START>") links = [26, 22, 12.5] self.kinematics = Kinematics(links, 0, 0, 0, 0) def test_inverse_aligned_kuka_initial(self): """ Tests the following aligned inverse kinematics problem: 1) "rear low" Axis 3 fixed: 0 on base, 1.73 on joint 1, 2.50 on joint 2, -0.75 on joint 3, 0 on joint 4: 35.01157, 0, 25.96866 """ print("<Test aligned kuka initial START>") x, y, z = 35.01157, 0, 25.96866 alignment = 0 results = self.kinematics.inverse_aligned(x, y, z, alignment) print(results) expected = [0, 1.55, -1.55, 0] result = all((abs(x - y) < 0.01 for x, y in zip(results, expected))) self.assertTrue(result) def test_inverse_aligned_rear_high(self): """ Tests the following aligned inverse kinematics problem: 1) "rear low" Axis 3 fixed: 0 on base, 1.6735 on joint 1, -0.8171 on joint 2, -1.0564 on joint 3, 0 on joint 4: 24, 0, 40 """ print("<Test aligned kuka initial START>") x, y, z = 24, 0, 40 alignment = -0.2 results = self.kinematics.inverse_aligned(x, y, z, alignment) print(results) expected = [0, 1.6735, -0.8171, -1.0564] result = all((abs(x - y) < 0.01 for x, y in zip(results, expected))) self.assertTrue(result) def test_inverse_align_too_far(self): """ Tests the following inverse kinematics problem: 11) "Way too far": not reachable 500, 500, 500 """ print("<Test Inverse too far START>") x, y, z = 500, 500, 500 alignment = 0 self.assertRaises(CalculationError, self.kinematics.inverse_aligned, x, y, z, alignment)
class Robotarm: """ Main controller-class for the 00SIRIS Robotarm Hedgehog information: -2000 Steps per Servo """ def __init__(self, client, links, base_offset, x_offset, y_offset, z_offset, min_angles, max_angles, min_servo_pos, max_servo_pos): """ Constructor - used for initialization move_to_init should be called after initialization to move the robotarm to the initial position. Servos can only track position after move_to_init has been called! (=High risk of unexpected behaviour!) :param links: the lengths of all links in an array :param base_offset: the distance in between the center of the base axis and the first joint """ # Params self.client = client self.links = links self.base_offset = base_offset self.x_offset = x_offset self.y_offset = y_offset self.z_offset = z_offset self.min_angles = min_angles self.max_angles = max_angles self.min_servo_pos = min_servo_pos self.max_servo_pos = max_servo_pos # Set to initial position - move_to_init should be called after initialization! # Base, axis 1, axis 2, axis 3, axis 4, grabber self.servo_pos = [0, 0, 0, 0, 0, 0] self.kinematics = Kinematics(self.links, self.base_offset, self.x_offset, self.y_offset, self.z_offset) def move_to_init(self): """ Moves the robotarm to the initial position """ print("Moving to init!") self.move_to_config([0.0, 1.51, -1.51, 0.0, 0.0, 0.0]) print("Successfully moved to init!") def move_to_pos(self, pos, joint): """ Moves the given joint to the given position :param pos: the position :param joint: the joint that will be used :raise OutOfReachError: if the given position cannot be reached """ if 0 <= pos <= 2000: # TODO: maybe this can be changed to servo_min/max but this has to be checked properly self.client.set_servo(joint, True, int(pos)) self.servo_pos[joint] = pos else: raise OutOfReachError("Position out of reach! (Range 0 to 2000): " + str(pos)) def move_to_pos_t(self, pos, joint, duration): """ Moves a joint to the given position in exactly the given duration :param pos: the position that the servo should move to :param joint: the joint that should be moved :param duration: the duration that the movement should take """ start_position = self.servo_pos[joint] movement = pos - start_position print("Starting!") if movement == 0: print("Already at position") return # Already at position print("Starting procedure!") time_per_step = abs(duration / movement) start_time = time.time() while start_time + duration > time.time() and pos != self.servo_pos[joint]: crnt_step = round((time.time() - start_time) / time_per_step) self.move_to_pos(start_position + crnt_step * math.copysign(1, movement), joint) time.sleep(0.001) # To prevent from overload def move_to_multi_pos_t(self, positions, duration): """ Moves the joints to the given position in exactly the given duration. If less than 6 positions are given, the first x joints are moved, where x is the number of given positions :param positions: the positions that the servos should move to :param duration: the duration that the movement should take """ if len(positions) < 1 or len(positions) > 6: raise OutOfReachError("1-6 positions required!") start_positions = self.servo_pos movements = [pos - self.servo_pos[joint] for joint, pos in enumerate(positions)] # If movement = 0 -> Don't move: Set times_per_step to 0 and check for 0 when calculating crnt step times_per_step = [abs(duration / movement) if movement != 0 else 0 for movement in movements] start_time = time.time() while start_time + duration > time.time(): # If time_per_step = 0 -> Movement = 0 -> Stay at position crnt_steps = [ round((time.time() - start_time) / time_per_step) if time_per_step != 0 else 0 for joint, time_per_step in enumerate(times_per_step)] cfg = [(joint, True, int(start_positions[joint] + crnt_step * math.copysign(1, movements[joint]))) for joint, crnt_step in enumerate(crnt_steps)] # TODO: Just using the first 4 axis for now([:4]). Needs a 2nd hedgehog for more self.client.set_multi_servo(cfg[:4]) time.sleep(0.001) # To prevent from overload # Update positions for index in range(len(positions), 6): positions.append(self.servo_pos[index]) self.servo_pos = positions def move_to_angle(self, angle, joint): """ Moves a joint to a certain angle :param angle: the angle :param joint: the joint """ pos = self.angle_to_step(angle, joint) self.move_to_pos(pos, joint) def move_to_cartesian(self, x, y, z, fixed_joint, fj_angle): """ Moves the robotarm to the given cartesian coordinates. :param x: the x-coordinate :param y: the y-coordinate :param z: the z-coordinate :param fixed_joint: the joint that is fixed :param fj_angle: the angle of the fixed joint """ try: angles = self.kinematics.inverse(x, y, z, fixed_joint, fj_angle) self.move_to_config(angles) except KinematicsError as ke: print("Position cannot be reached: " + ke.message) except OutOfReachError as oore: print("The kinematics module was able to calculate a position, but the servos are not able to reach it: " + oore.message) def move_to_cartesian_t(self, x, y, z, fixed_joint, fj_angle, duration): """ Moves the robotarm to the given cartesian coordinates. :param x: the x-coordinate :param y: the y-coordinate :param z: the z-coordinate :param fixed_joint: the joint that is fixed :param fj_angle: the angle of the fixed joint :param duration: the duration of the movement """ try: angles = self.kinematics.inverse(x, y, z, fixed_joint, fj_angle) self.move_to_config_t(angles, duration) except KinematicsError as ke: print("Position cannot be reached: " + ke.message) except OutOfReachError as oore: print("The kinematics module was able to calculate a position, but the servos are not able to reach it: " + oore.message) def move_to_cartesian_aligned(self, x, y, z, alignment): """ Moves the robotarm to the given cartesian coordinates. This one takes the alignment of the grabber towards the x-axis and uses it as the fixed joint. :param x: the x-coordinate :param y: the y-coordinate :param z: the z-coordinate :param alignment: the alignment of the grabber """ try: angles = self.kinematics.inverse_aligned(x, y, z, alignment) self.move_to_config(angles) except KinematicsError as ke: print("Position cannot be reached: " + ke.message) except OutOfReachError as oore: print("The kinematics module was able to calculate a position, but the servos are not able to reach it: " + oore.message) def move_to_cartesian_aligned_t(self, x, y, z, alignment, duration): """ Moves the robotarm to the given cartesian coordinates. This one takes the alignment of the grabber towards the x-axis and uses it as the fixed joint. :param x: the x-coordinate :param y: the y-coordinate :param z: the z-coordinate :param alignment: the alignment of the grabber :param duration: the duration of the movement """ try: angles = self.kinematics.inverse_aligned(x, y, z, alignment) self.move_to_config_t(angles, duration) except KinematicsError as ke: print("Position cannot be reached: " + ke.message) except OutOfReachError as oore: print("The kinematics module was able to calculate a position, but the servos are not able to reach it: " + oore.message) def move_to_config(self, angles): """ Move the robotarm to the given configuration. :param angles: the angles of all joints """ if self.validate_config(angles): positions = [self.angle_to_step(angle, joint) for joint, angle in enumerate(angles)] cfg = [(joint, True, position) for joint, position in enumerate(positions)] # TODO: Just using the first 4 axis for now([:4]). Needs a 2nd hedgehog for more self.client.set_multi_servo(cfg[:4]) # Update positions for index in range(len(positions), 6): positions.append(self.servo_pos[index]) self.servo_pos = positions else: raise OutOfReachError("The given configuration cannot be reached!") def move_to_config_t(self, angles, duration): """ Move the robotarm to the given configuration in the given time. :param angles: the angles of all joints :param duration: the duration of the movement """ if self.validate_config(angles): positions = [self.angle_to_step(angle, joint) for joint, angle in enumerate(angles)] self.move_to_multi_pos_t(positions, duration) # Already updates the servo positions else: raise OutOfReachError("The given configuration cannot be reached!") def move_through_configs_t(self, configs, duration): """ Moves the joints through all the given configurations in the given duration If less than 6 positions are given, the first x joints are moved, where x is the number of given positions :param configs: the configurations as list of list of angles e.g. [[0.0, 1.7, -2.0, 0.3], [0.0, 1.7, -2.1, 0.5], ...] :param duration: the duration that the movement should take """ for index, config in enumerate(configs): if len(config) < 1 or len(config) > 6: raise OutOfReachError("1-6 positions required!") # Pre-validation, to find errors BEFORE the movement starts and not in between: if not self.validate_config(config): raise OutOfReachError("At least one of the given angles is not valid! Index: " + str(index)) time_per_config = duration/len(configs) start_time = time.time() while start_time + duration > time.time(): crnt_step = int((time.time() - start_time) / time_per_config) # Using move_to_config_t to make the movement "smoother" # To leave some room for calculation-time, only 90% of the intended time is used as parameter self.move_to_config_t(configs[crnt_step], time_per_config * 0.9) time.sleep(0.001) # To prevent from overload def move_linear_aligned_t(self, a, b, align_start, align_end, resolution, duration): """ Moves from point a to point b in a given time. :param a: the point a as (x,y,z)-tuple :param b: the point b as (x,y,z)-tuple :param align_start: the alignment of the grabber at the start of the movement :param align_end: the alignment of the grabber at the start of the movement :param resolution: the number of points that will be used for the movement :param duration: the duration of the movement """ points = self.kinematics.linear(a, b, resolution) angles = self.kinematics.linear_aligned(points, align_start, align_end) self.move_through_configs_t(angles, duration) def angle_to_step(self, angle, joint): """ Calculates the step value of the given angle for the given joint. Servos have 2048 steps and this maps the angle to those :param angle: the angle in radiant representation :param joint: the joint that should be used :return: the servo-position in steps """ if not self.validate_angle(angle, joint): raise OutOfReachError if self.min_angles[joint] == self.max_angles[joint]: return 0 # if no min/max angle is set for the servo, position does not matter total_steps = self.max_servo_pos[joint] - self.min_servo_pos[joint] total_angle = self.max_angles[joint] - self.min_angles[joint] """ >Move angle to 0 ("remove" min) >Calculate steps/angle ratio >Add min servo pos (offset) """ pos = int((total_steps / total_angle) * (angle - self.min_angles[joint]) + self.min_servo_pos[joint]) return pos def step_to_angle(self, pos, joint): """ Calculates the angle of the given servo-position (in steps) for the given joint. Servos have 2048 steps and this maps the angle to those :param pos: the given step :param joint: the joint that should be used :return: the servo-position in angle """ # TODO: Reimplement this! return 0 def validate_config(self, angles): """ Validates whether the given configuration is reachable. :param angles: the angles of all joints :return: True if the given configuration is reachable, else: False """ if len(angles) < 4: return False # the K-00SIRIS needs at least 4 joints for a kinematic configuration # Check whether the angles on all joints can be reached for joint, angle in enumerate(angles): if not self.validate_angle(angle, joint): return False return True def validate_angle(self, angle, joint): """ Validates whether the given angle is reachable with the given joint. :param angle: the angle that should be validated :param joint: the joint that the angle should be validated for :return: True if the given angle is reachable with the given joint, else: False """ if self.min_angles[joint] <= angle <= self.max_angles[joint] or self.min_angles[joint] >= angle >= \ self.max_angles[joint]: return True else: return False def get_tool_cs(self): """ Returns the tool coordinate system :return: the tool coordinate system as WorldCoordinateSystem """ angles = [] for joint, pos in enumerate(self.servo_pos): angles[joint] = self.step_to_angle(pos, joint) x, y, z, theta_x, theta_y, theta_z = self.kinematics.direct(angles) return WorldCoordinateSystem(x, y, z, theta_x, theta_y, theta_z) def servos_off(self): """ Turns off all servos """ self.client.set_multi_servo([(0, False, 0), (1, False, 0), (2, False, 0), (3, False, 0)])