596 lines
19 KiB
Python
Executable File
596 lines
19 KiB
Python
Executable File
import urx
|
|
import math3d as m3d
|
|
from scipy.optimize import fsolve
|
|
import math
|
|
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
|
|
from pyModbusTCP.client import ModbusClient
|
|
|
|
|
|
|
|
|
|
class Rob():
|
|
robot = None
|
|
#offset_x, offset_y, offset_z = (0, 0, 0.14) # Tool offset
|
|
#
|
|
def __init__(self, config):
|
|
self.config = config
|
|
armc = config["arm"]
|
|
self.ip = armc["ip"]
|
|
tool = armc["tool"]
|
|
limbs = armc["limbs"]
|
|
self.offset_x, self.offset_y, self.offset_z = (tool["offset_x"], tool["offset_y"], tool["offset_z"])
|
|
self.limb_base = limbs["limb_base"]
|
|
self.limb1 = limbs["limb1"]
|
|
self.limb2 = limbs["limb2"]
|
|
self.limb3 = limbs["limb3"]
|
|
self.limb_wrist = limbs["limb_wrist"]
|
|
#self.init_arm()
|
|
|
|
def init_arm(self):
|
|
#sys.stdout = Logger()
|
|
fprint("Starting UR5 power up...")
|
|
|
|
# power up robot here
|
|
|
|
# wait for power up (this function runs async)
|
|
|
|
|
|
# trigger auto-initialize
|
|
|
|
# wait for auto-initialize
|
|
ip = self.ip
|
|
# init urx
|
|
fprint("Connecting to arm at " + ip)
|
|
trying = True
|
|
while trying:
|
|
try:
|
|
self.robot = urx.Robot(ip)
|
|
trying = False
|
|
except:
|
|
time.sleep(1)
|
|
|
|
# Sets robot arm endpoint offset (x,y,z,rx,ry,rz)
|
|
self.robot.set_tcp((self.offset_x, self.offset_y, self.offset_z, 0, 0, 0))
|
|
|
|
# Set weight
|
|
self.robot.set_payload(2, (0, 0, 0.1))
|
|
#rob.set_payload(2, (0, 0, 0.1))
|
|
time.sleep(0.2)
|
|
fprint("UR5 ready.")
|
|
|
|
def set_pos_abs(robot, x, y, z, xb, yb, zb, threshold=None):
|
|
rob = robot.robot
|
|
new_orientation = m3d.Transform()
|
|
new_orientation.orient.rotate_xb(xb) # Replace rx with the desired rotation around X-axis
|
|
new_orientation.orient.rotate_yb(yb) # Replace ry with the desired rotation around Y-axis
|
|
new_orientation.orient.rotate_zb(zb) # Replace rz with the desired rotation around Z-axis
|
|
|
|
# Get the current pose
|
|
trans = rob.getl()
|
|
# Apply the new orientation while keeping the current position
|
|
new_trans = m3d.Transform(new_orientation.orient, m3d.Vector(trans[0:3]))
|
|
new_trans.pos.x = x
|
|
new_trans.pos.y = y
|
|
new_trans.pos.z = z
|
|
#rob.speedj(0.2, 0.5, 99999)
|
|
rob.set_pose(new_trans, acc=2, vel=2, command="movej", threshold=threshold) # apply the new pose
|
|
|
|
def set_pos_rel_rot_abs(robot, x, y, z, xb, yb, zb):
|
|
rob = robot.robot
|
|
new_orientation = m3d.Transform()
|
|
new_orientation.orient.rotate_xb(xb) # Replace rx with the desired rotation around X-axis
|
|
new_orientation.orient.rotate_yb(yb) # Replace ry with the desired rotation around Y-axis
|
|
new_orientation.orient.rotate_zb(zb) # Replace rz with the desired rotation around Z-axis
|
|
|
|
# Get the current pose
|
|
trans = rob.getl()
|
|
|
|
# Apply the new orientation while keeping the current position
|
|
new_trans = m3d.Transform(new_orientation.orient, m3d.Vector(trans[0:3]))
|
|
new_trans.pos.x += x
|
|
new_trans.pos.y += y
|
|
new_trans.pos.z += z
|
|
#rob.speedj(0.2, 0.5, 99999)
|
|
rob.set_pose(new_trans, acc=0.1, vel=0.4, command="movej") # apply the new pose
|
|
|
|
def set_pos_abs_rot_rel(robot, x, y, z, xb, yb, zb):
|
|
rob = robot.robot
|
|
new_orientation = m3d.Transform()
|
|
new_orientation.orient.rotate_xb(xb) # Replace rx with the desired rotation around X-axis
|
|
new_orientation.orient.rotate_yb(yb) # Replace ry with the desired rotation around Y-axis
|
|
new_orientation.orient.rotate_zb(zb) # Replace rz with the desired rotation around Z-axis
|
|
|
|
# Get the current pose
|
|
trans = rob.getl()
|
|
|
|
# Apply the new orientation while keeping the current position
|
|
new_trans = m3d.Transform(new_orientation.orient, m3d.Vector(trans[0:3]))
|
|
new_trans.pos.x = x
|
|
new_trans.pos.y = y
|
|
new_trans.pos.z = z
|
|
#rob.speedj(0.2, 0.5, 99999)
|
|
rob.set_pose(new_trans, acc=0.1, vel=0.4, command="movej") # apply the new pose
|
|
|
|
|
|
def is_safe_move(start_pose, end_pose, r=0.25):
|
|
start_x, start_y = (start_pose[0], start_pose[1])
|
|
end_x, end_y = (end_pose[0], end_pose[1])
|
|
|
|
try:
|
|
m = (end_y-start_y)/(end_x-start_x)
|
|
b = start_y - m*start_x
|
|
# print('m = y/x =', m)
|
|
# print('b =', b)
|
|
except:
|
|
m = (end_x-start_x)/(end_y-start_y)
|
|
b = start_x - m*start_y
|
|
# print('m = x/y =', m)
|
|
# print('b =', b)
|
|
|
|
return r**2 - b**2 + m**2 * r**2 < 0
|
|
|
|
def cartesian_to_polar(x, y):
|
|
r = np.sqrt(x**2 + y**2)
|
|
theta = np.arctan2(y, x)
|
|
return r, theta
|
|
|
|
def polar_to_cartesian(r, theta):
|
|
x = r * np.cos(theta)
|
|
y = r * np.sin(theta)
|
|
return x, y
|
|
|
|
|
|
def move_to_polar(robot, start_pos, end_pos):
|
|
rob = robot.robot
|
|
|
|
# Convert to polar coordinates
|
|
start_r, start_theta = cartesian_to_polar(start_pos[0], start_pos[1])
|
|
end_r, end_theta = cartesian_to_polar(end_pos[0], end_pos[1])
|
|
|
|
# Interpolate for xy (spiral arc)
|
|
n_points = 30
|
|
r_intermediate = np.linspace(start_r, end_r, n_points)
|
|
theta_intermediate = np.linspace(start_theta, end_theta, n_points)
|
|
|
|
# Interpolate for z (height)
|
|
start_z = start_pos[2]
|
|
end_z = end_pos[2]
|
|
|
|
z_intermediate = np.linspace(start_z, end_z, n_points)
|
|
|
|
# Interpolate for rz (keep tool rotation fixed relative to robot)
|
|
|
|
|
|
curr_rot = rob.getl()
|
|
|
|
theta_delta = theta_intermediate[1]-theta_intermediate[0]
|
|
|
|
rx_intermediate = [curr_rot[5] + theta_delta*i for i in range(n_points)]
|
|
|
|
# curr_rot = rob.getj()
|
|
# start_rz = curr_rot[5]
|
|
# rot = end_theta - start_theta
|
|
# end_base_joint = curr_rot[0]-start_theta + rot
|
|
|
|
# end_rz = curr_rot[0] + rot
|
|
# # rob.movel([*polar_to_cartesian(end_r, end_theta), *rob.getl()[2:]], acc=2, vel=2)
|
|
|
|
# print('start_theta = ', math.degrees(start_theta))
|
|
# print('end_theta = ', math.degrees(curr_rot[0]-start_theta+rot))
|
|
# print('start_rz =', math.degrees(start_rz))
|
|
# print('rot =', math.degrees(rot))
|
|
# print('end_rz =', math.degrees(end_rz))
|
|
|
|
# rz_intermediate = np.linspace(start_rz, end_rz, n_points)
|
|
|
|
# Convert back to cartesian coordinates
|
|
curr_pos = rob.getl()
|
|
|
|
intermediate_points = [[*polar_to_cartesian(r, theta), z, *curr_pos[3:]]
|
|
for r, theta, z, rx in zip(r_intermediate,
|
|
theta_intermediate,
|
|
z_intermediate,
|
|
rx_intermediate)]
|
|
|
|
# Move robot
|
|
rob.movels(intermediate_points, acc=2, vel=2, radius=0.1)
|
|
|
|
|
|
return rx_intermediate
|
|
|
|
def move_to_home(robot):
|
|
rob = robot.robot
|
|
|
|
# Home position in degrees
|
|
home_pos = [0.10421807948612624,
|
|
-2.206111555015423,
|
|
1.710679229503537,
|
|
-1.075834511928354,
|
|
-1.569301366430687,
|
|
1.675098295930943]
|
|
|
|
# Move robot
|
|
rob.movej(home_pos, acc=2, vel=2)
|
|
|
|
def normalize_degree(theta):
|
|
# Normalizes degree theta from -1.5pi to 1.5pi
|
|
multiplier = 1
|
|
normalized_theta = theta % (math.pi * multiplier)
|
|
|
|
# Maintain the negative sign if the original angle is negative
|
|
if theta < 0:
|
|
normalized_theta -= math.pi * multiplier
|
|
|
|
# Return angle
|
|
return normalized_theta
|
|
|
|
def get_joints_from_xyz_rel(robot, 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
|
|
#l3=0.15
|
|
l_bs, l1, l2, l3, l_wt = (robot.limb_base, robot.limb1, robot.limb2, robot.limb3, robot.limb_wrist) # Limb lengths
|
|
l3 += l3offset # add wrist offset, used for gripper angle calculations
|
|
offset_x = robot.offset_x
|
|
offset_y = robot.offset_y
|
|
offset_z = robot.offset_z
|
|
# Calculate base angle and r relative to shoulder joint
|
|
def calculate_theta(x, y, a):
|
|
# Calculate if we need the + or - in our equations
|
|
if (x>-a and y>=0) or (x>a and y<0):
|
|
flip = 1
|
|
elif (x<-a and y>=0) or (x<a and y<0):
|
|
flip = -1
|
|
else:
|
|
# Critical section (x=a, or x=-a). Infinite slope
|
|
# Return 0 or 180 depending on sign
|
|
return math.atan2(y, 0)
|
|
|
|
# Calculate tangent line y = mx + b
|
|
m = (x*y - math.sqrt(x*x*y*y-(x*x-a*a)*(y*y-a*a)))/(x*x-a*a)
|
|
b = flip * a * math.sqrt(1+m*m)
|
|
# Calculate equivalent tangent point on circle
|
|
cx = (-flip*m*b)/(1+m*m)
|
|
cy = m*cx + flip*b
|
|
# Calculate base angle, make angle negative if flip=1
|
|
theta = math.atan2(cy, cx) + (-math.pi if flip==1 else 0)
|
|
return theta
|
|
|
|
base_theta = calculate_theta(x, y, l_bs)
|
|
cx, cy = l_bs*math.cos(base_theta), l_bs*math.sin(base_theta)
|
|
r = math.sqrt((x+offset_x+cx)**2 + (y+offset_y+cy)**2)
|
|
|
|
|
|
# Formulas to find out joint positions for (r, z)
|
|
def inv_kin_r_z(p):
|
|
a, b, c = p
|
|
|
|
return (l1*math.cos(a) + l2*math.cos(a-b) + l3*math.cos(a-b-c) - r, # r
|
|
l1*math.sin(a) + l2*math.sin(a-b) - l3*math.sin(a-b-c) - (l3*math.sin(a-b-c)) - (z + offset_z), # z
|
|
a-b-c) # wrist angle
|
|
|
|
|
|
# Normalize angles
|
|
base, shoulder, elbow, wrist1 = [normalize_degree(deg) for deg in [base_theta, *fsolve(inv_kin_r_z, initial_guess)]]
|
|
wrist1 += rx
|
|
# Return result
|
|
return base, shoulder, elbow, wrist1, ry, rz
|
|
|
|
def get_joints_from_xyz_abs(robot, x, y, z, rx=0, ry=-math.pi/2, rz=math.pi/2, l3offset=0, use_closest_path=True):
|
|
rob = robot.robot
|
|
joints = get_joints_from_xyz_rel(robot, 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 math.degrees(joints[1]) > 137:
|
|
print("CRASH! Shoulder at", joints[1] * 180/math.pi)
|
|
#else:
|
|
#print("Shoulder at", joints[1] * 180/math.pi)
|
|
|
|
# Get adjusted joint positions
|
|
adjusted_joints = [o+j*i for j, o, i in zip(joints, offsets, inverse)]
|
|
|
|
curr_joints = rob.getj()
|
|
def get_complimentary_angle(joint_angle):
|
|
if joint_angle<0:
|
|
new_angle = joint_angle + 2*math.pi
|
|
else:
|
|
new_angle = joint_angle - 2*math.pi
|
|
|
|
if abs(new_angle) > math.radians(350):
|
|
return joint_angle
|
|
else:
|
|
return new_angle
|
|
|
|
# Use closest path (potentially going beyond 180 degrees)
|
|
if use_closest_path:
|
|
if abs(get_complimentary_angle(adjusted_joints[0])-curr_joints[0]) < abs(adjusted_joints[0]-curr_joints[0]):
|
|
adjusted_joints[0] = get_complimentary_angle(adjusted_joints[0])
|
|
|
|
# final_joint_positions = []
|
|
# for curr_joint, adjusted_joint in zip(curr_joints, adjusted_joints):
|
|
# if abs(curr_joint - adjusted_joint) < abs(curr_joint - get_complimentary_angle(adjusted_joint)):
|
|
# final_joint_positions.append(adjusted_joint)
|
|
# else:
|
|
# final_joint_positions.append(get_complimentary_angle(adjusted_joint))
|
|
|
|
# return final_joint_positions
|
|
|
|
return adjusted_joints
|
|
|
|
|
|
def move_arc(robot, x, y, z, rx=0, ry=-math.pi/2, rz=math.pi/2):
|
|
rob = robot.robot
|
|
|
|
start_joints = rob.getj()
|
|
end_joint = get_joints_from_xyz_abs(robot, 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(robot, x, y, z, gripperangle=30, gripperlength=0.20+0.018, flip=False, use_closest_path=True):
|
|
# gripper angle: from vertical
|
|
# gripper length: from joint to start of grip
|
|
# to flip, you can use flip=True or make gripper angle negative
|
|
limb3 = robot.limb3
|
|
# Determine tool rotation depending on gripper angle
|
|
if gripperangle < 0:
|
|
rz = - math.pi / 2
|
|
else:
|
|
rz = math.pi / 2
|
|
|
|
if flip:
|
|
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(robot, x, y, z-grippery, rx=gripperangle + math.radians(180), l3offset=-gripperx, ry=math.pi/2, rz=rz, use_closest_path=use_closest_path)
|
|
|
|
else:
|
|
gripperangle = math.radians(gripperangle)
|
|
grippery = gripperlength - math.cos(gripperangle) * gripperlength
|
|
grippery -= math.sin(gripperangle) * limb3
|
|
gripperx = math.sin(gripperangle) * gripperlength
|
|
gripperx += (1-math.cos(gripperangle)) * limb3
|
|
|
|
return get_joints_from_xyz_abs(robot, x, y, z-grippery, rx=gripperangle, l3offset=-gripperx, rz=rz, use_closest_path=use_closest_path)
|
|
|
|
def goto_holder_index(robot, idx, z=0.05, gripperangle=30, flip=False, use_closest_path=True, verbose=False):
|
|
joint = robot.config["position_map"][idx]
|
|
|
|
if verbose:
|
|
print("Going to cable holder index", joint["index"], "at position", joint["pos"])
|
|
|
|
safe_move(robot, joint["pos"][0]/1000, joint["pos"][1]/1000, z, use_closest_path=use_closest_path)
|
|
#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
|
|
#angles = get_joints_from_xyz_abs(joint["pos"][1]/1000, joint["pos"][0]/1000, 0.05, )
|
|
|
|
def is_flipped(robot):
|
|
rob = robot.robot
|
|
wrist2 = rob.getj()[4]
|
|
|
|
if wrist2>0:
|
|
return True
|
|
else:
|
|
return False
|
|
|
|
def flip(robot):
|
|
rob = robot.robot
|
|
|
|
# A list of safe positions to flip
|
|
safe_positions = [(-0.18, -0.108, 0.35),
|
|
(0.18, -0.108, 0.35)]
|
|
|
|
# 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(robot, *safe_pos, flip=is_flipped(robot)), vel=2, acc=2) # Move to safe position
|
|
rob.movej(offset_gripper_angle(robot, *safe_pos, flip=(not is_flipped(robot))), vel=2, acc=2) # Flip gripper
|
|
|
|
def safe_move(robot, x, y, z, use_closest_path=True):
|
|
rob = robot.robot
|
|
flip_radius = 0.22 # 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(robot)) or (r > flip_radius and not is_flipped(robot)):
|
|
flip(robot)
|
|
|
|
rob.movej(offset_gripper_angle(robot, x, y, z, flip=is_flipped(robot), use_closest_path=use_closest_path), vel=2, acc=2)
|
|
|
|
def pick_up_routine(robot, holder_index, verbose=False):
|
|
rob = robot.robot
|
|
|
|
if verbose:
|
|
print('Pickup routine for index', holder_index)
|
|
|
|
goto_holder_index(robot, holder_index, 0.2, use_closest_path=False)
|
|
open_gripper()
|
|
|
|
curr_pos = rob.getl()
|
|
new_pos = curr_pos
|
|
new_pos[2] = 0.005
|
|
rob.movel(new_pos, vel=0.1, acc=1)
|
|
# goto_holder_index(robot, holder_index, 0.0)
|
|
close_gripper()
|
|
|
|
|
|
|
|
new_pos[2] = 0.2
|
|
rob.movel(new_pos, vel=0.1, acc=1)
|
|
was_flipped = is_flipped(robot)
|
|
|
|
# # Tray position 1
|
|
rob.movej(offset_gripper_angle(robot, -0.15, -0.15, 0.3, flip=is_flipped(robot), use_closest_path=False), vel=2, acc=2)
|
|
rob.movej(get_joints_from_xyz_abs(robot, -0.35, -0.15, 0.0, math.pi/2, 0.1), vel=2, acc=2)
|
|
time.sleep(0.5)
|
|
open_gripper()
|
|
|
|
# Back to safe position
|
|
rob.movej(offset_gripper_angle(robot, -0.205, -0.108, 0.3, flip=True), vel=2, acc=2)
|
|
|
|
def open_gripper():
|
|
print("opening")
|
|
c = ModbusClient(host="192.168.1.20", port=502, auto_open=True, auto_close=False)
|
|
|
|
c.write_single_register(0, 0b0)
|
|
c.write_single_register(435, 0b1)
|
|
time.sleep(0.5)
|
|
c.close()
|
|
|
|
#c.close()
|
|
|
|
def close_gripper():
|
|
print("closing")
|
|
c = ModbusClient(host="192.168.1.20", port=502, auto_open=True, auto_close=False)
|
|
c.write_single_register(435, 0b0)
|
|
c.write_single_register(0, 0b1)
|
|
time.sleep(0.5)
|
|
c.close()
|
|
#
|
|
|
|
if __name__ == "__main__":
|
|
|
|
#rob.movej((0, 0, 0, 0, 0, 0), 0.1, 0.2)
|
|
#rob.movel((x, y, z, rx, ry, rz), a, v)
|
|
#init("192.168.1.145")
|
|
with open('config.yml', 'r') as fileread:
|
|
#global config
|
|
config = yaml.safe_load(fileread)
|
|
|
|
open_gripper()
|
|
#time.sleep(5)
|
|
#close_gripper()
|
|
|
|
#time.sleep(100)
|
|
|
|
|
|
|
|
robot = Rob(config) # robot of type Rob is the custom class above
|
|
robot.init_arm()
|
|
rob = robot.robot # rob is robot.robot is the urx robot class, what we've been using previously
|
|
|
|
print("Current tool pose is: ", rob.getl())
|
|
move_to_home(robot)
|
|
|
|
home_pose = [-0.4999999077032916,
|
|
-0.2000072960336574,
|
|
0.40002172976662786,
|
|
0,
|
|
-3.14152741295329,
|
|
math.radians(62)]
|
|
|
|
# time.sleep(.5)
|
|
|
|
p1 = [0,
|
|
|
|
0.6,
|
|
.4,
|
|
0.2226,
|
|
3.1126,
|
|
0.0510]
|
|
|
|
p2 = [0.171,
|
|
-0.115,
|
|
0.2,
|
|
0.2226,
|
|
3.1126,
|
|
0.0510]
|
|
|
|
curr_pos = rob.getl()
|
|
|
|
|
|
config = None
|
|
joints = []
|
|
for i in np.linspace(-0.2, -0.7, 10):
|
|
joints.append(get_joints_from_xyz_abs(robot, i, 0, 0))
|
|
# rob.movejs(joints, acc=2, vel=2, radius=0.1)
|
|
|
|
# rob.movej(offset_gripper_angle(robot, -0.15, -0.15, 0.3, gripperangle=90, flip=is_flipped(robot)), vel=2, acc=2)
|
|
|
|
# move_arc(0, 0.3, 0.1)
|
|
# move_arc(0, -0.3, 0.3)
|
|
|
|
# rob.movej(get_joints_from_xyz_abs(robot, 0.2, 0, 0.05), vel=0.5, acc=2)
|
|
# goto_holder_index(robot, 27, 0.05)
|
|
|
|
for i in [6,7,8]:
|
|
pick_up_routine(robot, i, verbose=True)
|
|
|
|
|
|
|
|
# goto_holder_index(robot, 7, 0.0)
|
|
|
|
# pick_up_routine(robot, 8)
|
|
|
|
|
|
# rob.movej(get_joints_from_xyz_abs(robot, -0.35, -0.15, 0.0, math.pi/2, 0.1), vel=2, acc=2)
|
|
|
|
|
|
#rob.movej(goto_holder_index(24, 0.2, 0), acc=2, vel=2).
|
|
#joints = []
|
|
|
|
|
|
# angle = 30
|
|
# goto_holder_index(robot, 26, 0.1, angle)
|
|
# time.sleep(1)
|
|
# goto_holder_index(robot, 25, 0.1, angle)
|
|
# time.sleep(1)
|
|
# goto_holder_index(robot, 24, 0.1, angle)
|
|
|
|
|
|
|
|
|
|
#rob.movej(angles, acc=2, vel=2)
|
|
#time.sleep(10)
|
|
#rob.movejs(joints, acc=2, vel=2)
|
|
# joints = []
|
|
# for i in np.linspace(-0.3, -0.7, 50):
|
|
# joints.append(get_joints_from_xyz_abs(i, 0, 0))
|
|
# rob.movejs(joints, acc=2, vel=2)
|
|
|
|
# time.sleep(5)
|
|
|
|
|
|
|
|
# print("Current tool pose is: ", rob.getl())
|
|
# print("getj(): ", rob.getj())
|
|
|
|
# move_to_home()
|
|
|
|
rob.stop()
|
|
os.kill(os.getpid(), 9) # dirty kill of self
|
|
sys.exit(0)
|
|
|