Basic move restrictions and flip routine
This commit is contained in:
BlueOceanWave
parent
218303e92b
commit
2ec7906ee4
File diff suppressed because one or more lines are too long
166
ur5_control.py
166
ur5_control.py
@ -6,6 +6,7 @@ import numpy as np
|
||||
import time
|
||||
import os
|
||||
import logging
|
||||
import yaml
|
||||
from urx.robotiq_two_finger_gripper import Robotiq_Two_Finger_Gripper
|
||||
import sys
|
||||
from util import fprint
|
||||
@ -191,11 +192,6 @@ def move_to_polar(start_pos, end_pos):
|
||||
|
||||
return rx_intermediate
|
||||
|
||||
def degtorad(angle):
|
||||
return angle/180.0 * math.pi
|
||||
def radtodeg(angle):
|
||||
return angle*180.0 / math.pi
|
||||
|
||||
def move_to_home():
|
||||
global rob
|
||||
|
||||
@ -222,8 +218,6 @@ def normalize_degree(theta):
|
||||
# Return angle
|
||||
return normalized_theta
|
||||
|
||||
|
||||
|
||||
def get_joints_from_xyz_rel(x, y, z, rx=0, ry=-math.pi/2, rz=0, initial_guess = (math.pi/2, math.pi/2, 0), l3offset=0):
|
||||
# Get limbs and offsets
|
||||
|
||||
@ -276,37 +270,61 @@ def get_joints_from_xyz_rel(x, y, z, rx=0, ry=-math.pi/2, rz=0, initial_guess =
|
||||
def get_joints_from_xyz_abs(x, y, z, rx=0, ry=-math.pi/2, rz=math.pi/2, l3offset=0):
|
||||
joints = get_joints_from_xyz_rel(x, y, z, rx, ry, rz, l3offset=l3offset)
|
||||
|
||||
# Return current positions if coordinates don't make sense
|
||||
if z<0:
|
||||
return rob.getj()
|
||||
|
||||
# Joint offsets
|
||||
# Base, Shoulder, Elbow, Wrist
|
||||
inverse = [1, -1, 1, 1, 1, 1]
|
||||
offsets = [-math.pi/2, 0, 0, -math.pi/2, 0, 0]
|
||||
if radtodeg(joints[1]) > 137:
|
||||
|
||||
if math.degrees(joints[1]) > 137:
|
||||
print("CRASH! Shoulder at", joints[1] * 180/math.pi)
|
||||
#else:
|
||||
#print("Shoulder at", joints[1] * 180/math.pi)
|
||||
|
||||
# Return adjusted joint positions
|
||||
return [o+j*i for j, o, i in zip(joints, offsets, inverse)]
|
||||
|
||||
# gripper angle: from vertical
|
||||
# gripper length: from joint to start of grip
|
||||
# to flip, you can use flip=True or make gripper angle negative
|
||||
def move_arc(x, y, z, rx=0, ry=-math.pi/2, rz=math.pi/2):
|
||||
|
||||
global rob
|
||||
start_joints = rob.getj()
|
||||
end_joint = get_joints_from_xyz_abs(x, y, z, rx, ry, rz)
|
||||
|
||||
n_points = 50
|
||||
intermediate_joints = []
|
||||
for i in range(0, 6):
|
||||
intermediate_joints.append(np.linspace(start_joints[i], end_joint[i], n_points))
|
||||
|
||||
joints = [joint_position for joint_position in zip(*intermediate_joints)]
|
||||
|
||||
rob.movejs(joints, acc=2, vel=2, radius=0.1)
|
||||
|
||||
def offset_gripper_angle(x, y, z, gripperangle=35, gripperlength=0.20+0.018, flip=False):
|
||||
# gripper angle: from vertical
|
||||
# gripper length: from joint to start of grip
|
||||
# to flip, you can use flip=True or make gripper angle negative
|
||||
|
||||
# Determine tool rotation depending on gripper angle
|
||||
if gripperangle < 0:
|
||||
rz = - math.pi / 2
|
||||
else:
|
||||
rz = math.pi / 2
|
||||
|
||||
if flip:
|
||||
gripperangle = -degtorad(gripperangle)
|
||||
gripperangle = -math.radians(gripperangle)
|
||||
grippery = gripperlength - math.cos(gripperangle) * gripperlength
|
||||
grippery += math.sin(gripperangle) * limb3
|
||||
gripperx = math.sin(gripperangle) * gripperlength + limb3 * 2
|
||||
gripperx -= (1-math.cos(gripperangle)) * limb3
|
||||
rz = - math.pi / 2
|
||||
# flip the whole wrist
|
||||
return get_joints_from_xyz_abs(x, y, z-grippery, rx=gripperangle + degtorad(180), l3offset=-gripperx, ry=math.pi/2, rz=rz)
|
||||
return get_joints_from_xyz_abs(x, y, z-grippery, rx=gripperangle + math.radians(180), l3offset=-gripperx, ry=math.pi/2, rz=-rz)
|
||||
|
||||
else:
|
||||
gripperangle = degtorad(gripperangle)
|
||||
gripperangle = math.radians(gripperangle)
|
||||
grippery = gripperlength - math.cos(gripperangle) * gripperlength
|
||||
grippery -= math.sin(gripperangle) * limb3
|
||||
gripperx = math.sin(gripperangle) * gripperlength
|
||||
@ -314,15 +332,59 @@ def offset_gripper_angle(x, y, z, gripperangle=35, gripperlength=0.20+0.018, fli
|
||||
|
||||
return get_joints_from_xyz_abs(x, y, z-grippery, rx=gripperangle, l3offset=-gripperx, rz=rz)
|
||||
|
||||
|
||||
def goto_holder_index(idx, z=0.05, gripperangle=35, flip=False):
|
||||
joint = config["position_map"][idx]
|
||||
print("Going to cable holder index", joint["index"], "at position", joint["pos"])
|
||||
angles = offset_gripper_angle(joint["pos"][1]/1000, joint["pos"][0]/1000, z, gripperangle=gripperangle, flip=flip)
|
||||
|
||||
safe_move(joint["pos"][1]/1000, joint["pos"][0]/1000, z)
|
||||
#angles = offset_gripper_angle(joint["pos"][1]/1000, joint["pos"][0]/1000, z, gripperangle=gripperangle, flip=flip)
|
||||
#rob.movej(angles, acc=2, vel=2)
|
||||
return angles
|
||||
#return angles
|
||||
#angles = get_joints_from_xyz_abs(joint["pos"][1]/1000, joint["pos"][0]/1000, 0.05, )
|
||||
|
||||
def is_flipped():
|
||||
global rob
|
||||
wrist2 = rob.getj()[4]
|
||||
|
||||
if wrist2>0:
|
||||
return True
|
||||
else:
|
||||
return False
|
||||
|
||||
def flip():
|
||||
global rob
|
||||
|
||||
# A list of safe positions to flip
|
||||
safe_positions = [(-0.205, -0.108, 0.3),
|
||||
(0.205, -0.108, 0.3)]
|
||||
|
||||
# Find the closest safe position
|
||||
curr_pos = rob.getl()[:3]
|
||||
def dist_from_robot(pos):
|
||||
x, y, z = pos
|
||||
rx, ry, rz = curr_pos
|
||||
return math.sqrt((rx-x)**2+(ry-y)**2+(rz-z)**2)
|
||||
|
||||
pos_dist_pairs = zip(safe_positions, [dist_from_robot(pos) for pos in safe_positions])
|
||||
safe_pos = min(pos_dist_pairs, key=lambda x:x[1])[0]
|
||||
|
||||
# Flip at safe position
|
||||
rob.movej(offset_gripper_angle(*safe_pos, flip=is_flipped()), vel=2, acc=2) # Move to safe position
|
||||
rob.movej(offset_gripper_angle(*safe_pos, flip=(not is_flipped())), vel=2, acc=2) # Flip gripper
|
||||
|
||||
def safe_move(x, y, z):
|
||||
flip_radius = 0.17 # Min radius on which to flip
|
||||
r = math.sqrt(x**2 + y**2) # Get position radius
|
||||
|
||||
# Flip gripper if needed
|
||||
if (r <= flip_radius and is_flipped()) or (r > flip_radius and not is_flipped()):
|
||||
flip()
|
||||
|
||||
global rob
|
||||
rob.movej(offset_gripper_angle(x, y, z, flip=is_flipped()), vel=2, acc=2)
|
||||
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
||||
#rob.movej((0, 0, 0, 0, 0, 0), 0.1, 0.2)
|
||||
@ -356,59 +418,55 @@ if __name__ == "__main__":
|
||||
0.0510]
|
||||
|
||||
curr_pos = rob.getl()
|
||||
# up/down,
|
||||
# tool rotation
|
||||
# tool angle (shouldn't need)
|
||||
# rob.set_pos(p1[0:3], acc=0.5, vel=0.5)
|
||||
|
||||
# set_pos_abs(*home_pose)
|
||||
|
||||
|
||||
|
||||
# angles = get_joints_from_xyz_abs(-0.2, 0, 0)
|
||||
# rob.movej(angles, acc=2, vel=2)
|
||||
# angles = get_joints_from_xyz_abs(-0.2, -0.2, 0)
|
||||
# rob.movej(angles, acc=2, vel=2)
|
||||
# angles = get_joints_from_xyz_abs(0, -0.6, 0)
|
||||
# rob.movej(angles, acc=2, vel=2)
|
||||
# angles = get_joints_from_xyz_abs(0, -0.5, 0)
|
||||
# rob.movej(angles, acc=2, vel=2)
|
||||
# angles = get_joints_from_xyz_abs(0, -0.4, 0)
|
||||
# rob.movej(angles, acc=2, vel=2)
|
||||
# angles = get_joints_from_xyz_abs(0, -0.3, 0)
|
||||
# rob.movej(angles, acc=2, vel=2)
|
||||
# angles = get_joints_from_xyz_abs(0, -0.2, 0)
|
||||
# rob.movej(angles, acc=2, vel=2)
|
||||
# angles = get_joints_from_xyz_abs(0, -0.13, 0)
|
||||
# rob.movej(angles, acc=2, vel=2)
|
||||
config = None
|
||||
joints = []
|
||||
for i in np.linspace(-0.2, -0.7, 50):
|
||||
for i in np.linspace(-0.2, -0.7, 10):
|
||||
joints.append(get_joints_from_xyz_abs(i, 0, 0))
|
||||
#rob.movejs(joints, acc=2, vel=2)
|
||||
import yaml
|
||||
# rob.movejs(joints, acc=2, vel=2, radius=0.1)
|
||||
|
||||
with open('config.yml', 'r') as fileread:
|
||||
#global config
|
||||
config = yaml.safe_load(fileread)
|
||||
|
||||
# move_arc(0, 0.3, 0.1)
|
||||
# move_arc(0, -0.3, 0.3)
|
||||
|
||||
|
||||
# for i in range(20):
|
||||
# goto_holder_index(i, 0.1)
|
||||
|
||||
flip()
|
||||
flip()
|
||||
|
||||
#rob.movej(goto_holder_index(24, 0.2, 0), acc=2, vel=2)
|
||||
#joints = []
|
||||
|
||||
#for i in np.linspace(0, 340, 340):
|
||||
# joints.append(goto_holder_index(24, 0.5, i))
|
||||
#rob.movejs(joints, acc=1, vel=3)
|
||||
angle = 30
|
||||
rob.movej(goto_holder_index(26, 0.1, angle), acc=2, vel=2)
|
||||
#rob.movej(goto_holder_index(32, 0.2, angle), acc=2, vel=2)
|
||||
#rob.movej(goto_holder_index(38, 0.2, angle), acc=2, vel=2)
|
||||
rob.movej(goto_holder_index(26, 0.1, angle, flip=True), acc=2, vel=2)
|
||||
# angle = 30
|
||||
# rob.movej(goto_holder_index(26, 0.1, angle), acc=2, vel=2)
|
||||
# # rob.movej(goto_holder_index(32, 0.2, angle), acc=2, vel=2)
|
||||
# rob.movej(goto_holder_index(38, 0.2, angle), acc=2, vel=2)
|
||||
# rob.movej(goto_holder_index(26, 0.1, angle, flip=True), acc=2, vel=2)
|
||||
# rob.movej(offset_gripper_angle(-0.3, -0.3, 0.4, flip=True), acc=2, vel=2)
|
||||
|
||||
rob.movej(goto_holder_index(25, 0.2, angle, flip=True), acc=2, vel=2)
|
||||
rob.movej(goto_holder_index(24, 0.0, angle, flip=True), acc=2, vel=2)
|
||||
time.sleep(1)
|
||||
rob.movej(goto_holder_index(25, 0.1, angle, flip=True), acc=2, vel=2)
|
||||
rob.movej(goto_holder_index(49, 0.1, angle), acc=2, vel=2)
|
||||
rob.movej(goto_holder_index(49, 0.1, angle, flip=True), acc=2, vel=2)
|
||||
# safe_move(0.1, 0.1, 0.1)
|
||||
# safe_move(-0.3, -0.3, 0.1)
|
||||
|
||||
# flip()
|
||||
# flip()
|
||||
|
||||
|
||||
|
||||
# rob.movej(goto_holder_index(25, 0.2, angle, flip=True), acc=2, vel=2)
|
||||
# rob.movej(goto_holder_index(24, 0.0, angle, flip=True), acc=2, vel=2)
|
||||
# time.sleep(1)
|
||||
# rob.movej(goto_holder_index(25, 0.1, angle, flip=True), acc=2, vel=2)
|
||||
# rob.movej(goto_holder_index(49, 0.1, angle), acc=2, vel=2)
|
||||
# rob.movej(goto_holder_index(49, 0.1, angle, flip=True), acc=2, vel=2)
|
||||
# rob.movej(goto_holder_index(50, 0.1, angle, flip=True), acc=2, vel=2)
|
||||
# rob.movej(goto_holder_index(51, 0.1, angle, flip=True), acc=2, vel=2)
|
||||
# rob.movej(goto_holder_index(52, 0.1, angle, flip=True), acc=2, vel=2)
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user