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()