def move_to_pose(orion, pose, threshold=10.0): """ move to a pose defined by a list of toolpoint pose [radius, theta, height, attack, claw] returns current position after move """ # calculate inverse kinematics, to get joint angles from pose joints = orion5_math.ikinematics(*pose) return move_to_position(orion, joints, threshold)
def waka_waka(orion, theta_start, theta_change, attack_range, wak_frequency=15.0): """ do the waka waka zig zag camera scan thing theta_start is the theta (turret rotation) position to start in theta_change is the angular range of the waka waka (can be +ve or -ve to set the direction) attack_range is the +ve and -ve motion of the wrist during a waka, should be +ve wak_frequency is the theta movement between each waka """ print( f'doing waka - ts: {theta_start}, tc: {theta_change}, ar: {attack_range}' ) num_waks = int(abs(theta_change // wak_frequency)) wak_frequency = math.copysign(wak_frequency, theta_change) # get the current position, to maintain the claw position joints = orion.getAllJointsPosition() # move to the starting position, attack angle 0 position = orion5_math.ikinematics(180, theta_start, 300, 0, joints[4]) joints = move_to_position(orion, position) sign = 1.5 for i in range(num_waks): # change the turret position by +- wak_frequency position[0] = orion5_math.wrap360f(position[0] + wak_frequency) # change the wrist angle by attack_range, alternating the sign each itteration position[3] = joints[3] + sign * attack_range if sign > 0: sign = -0.75 else: sign = 1.5 move_to_position(orion, position, threshold=15.0)
[210, 270, 110, 270, 250], [210, 270, 200, 270, 250], [175, 0, 200, 270, 250], [175, 0, 50, 270, 80], [280, 0, 140, 348, 250], ] arriveThreshold = 12 waitTime = 0.5 index = 0 # create Orion5 object orion = orion5.Orion5() for index in range(len(path)): joints = orion5_math.ikinematics(path[index][0], path[index][1], path[index][2], path[index][3], path[index][4]) print("Moving to:", joints) # keep writing the desired position until we arrive while not arrived(joints, orion.getAllJointsPosition(), arriveThreshold): orion.setAllJointsPosition(joints) time.sleep(0.1) time.sleep(waitTime) orion.stop()