def __init__(self):
self.s = SendMove()
#Start gripper service-server
set_states()
#Set analog output to voltage mode
self.s.sendMove("set_analog_outputdomain(0, 1)")
def __init__(self):
#Initialize manipulation node
rospy.init_node("Manipulation")
#Services
self.scanService = rospy.Service("/move_camera", ManipulationPose,
self.scan_cb)
self.pickService = rospy.Service("/pick", ManipulationAction,
self.pick_cb)
self.placeService = rospy.Service("/place", ManipulationAction,
self.place_cb)
#Instances
self.m = SendMove()
self.t = tf.TransformListener()
self.UR3 = UR()
def __init__(self):
#Instances
self.gripper = Gripper()
self.m = SendMove()
self.t = tf.TransformListener()
self.armPose = ""
#Subscribers
rospy.Subscriber("/tf", TFMessage, self.getToolTF)
rospy.Subscriber("tool_velocity", TwistStamped, self.getToolVelocity)
#Open gripper
self.gripper.moveGripper("Open")
#Go to initial position
self.m.sendMove(self.m.buildMove('j', '', self.m.getPos('test3')))
self.waitForArm()
class Gripper():
def __init__(self):
self.s = SendMove()
#Start gripper service-server
set_states()
#Set analog output to voltage mode
self.s.sendMove("set_analog_outputdomain(0, 1)")
def moveGripper(self,
pos,
objectName=''
): #objectName is a default '' for the open en close state
"""This function can open or close the gripper.
Arguments:
pos {String} -- ["Open" or "Close" or "prepick"]
objectName {String} -- [official object robocup object names]
"""
if pos == "Open":
set_analog_out(0, 0.0) #Open gripper
time.sleep(1)
elif pos == "Close":
set_analog_out(0, 0.45) #Close gripper
time.sleep(1)
elif pos == "prepick":
if (
"20" in objectName
) or objectName == "AXIS" or objectName == "DISTANCE_TUBE" or objectName == "BEARING": #prepick for the middle sized objects
set_analog_out(0, 0.25)
time.sleep(1)
elif (
"40" in objectName
) or objectName == "M30" or objectName == "BEARING_BOX" or objectName == "MOTOR": #prepick for the largesized objects
set_analog_out(0, 0.15)
time.sleep(1)
elif ("HOLDER" in objectName):
set_analog_out(0, 0.45)
time.sleep(1)
pass
else:
rospy.loginfo("Invalid input: %s", str(pos))
class Gripper():
def __init__(self):
self.s = SendMove()
#Start gripper service-server
set_states()
#Set analog output to voltage mode
self.s.sendMove("set_analog_outputdomain(0, 1)")
def moveGripper(self, pos):
"""This function can open or close the gripper.
Arguments:
pos {String} -- ["Open" or "Close"]
"""
if pos == "Open":
set_analog_out(0, 0.4) #Open gripper
time.sleep(1)
elif pos == "Close":
set_analog_out(0, 0.0) #Close gripper
time.sleep(1)
else:
rospy.loginfo("Invalid input: %s", str(pos))
class Manipulation():
def __init__(self):
#Initialize manipulation node
rospy.init_node("Manipulation")
#Services
self.scanService = rospy.Service("/move_camera", ManipulationPose,
self.scan_cb)
self.pickService = rospy.Service("/pick", ManipulationAction,
self.pick_cb)
self.placeService = rospy.Service("/place", ManipulationAction,
self.place_cb)
#Instances
self.m = SendMove()
self.t = tf.TransformListener()
self.UR3 = UR()
# ----------Service Callbacks----------
def scan_cb(self, msg):
self.UR3.gripper.moveGripper("Open")
self.scanTarget = msg.target
scanPose = "scanPosition%s" % self.scanTarget
if self.UR3.armPose is not scanPose:
self.m.sendMove(self.m.buildMove('j', '', self.m.getPos(scanPose)))
rospy.loginfo("Arm is moving to %s", scanPose)
self.UR3.waitForArm()
self.UR3.armPose = scanPose
status = False
else:
rospy.logwarn("Arm is already in %s", scanPose)
status = True
return status
def pick_cb(self, msg):
self.UR3.armPose = ""
self.pickLink = msg.link
if self.t.canTransform("ur3/base", self.pickLink, rospy.Time(0)):
x = self.t.lookupTransform("ur3/base", self.pickLink,
rospy.Time(0))[0][1]
if (self.UR3.tool[1] < 0 and x > 0) or (self.UR3.tool[1] > 0
and x < 0):
self.m.sendMove(
self.m.buildMove('j', '', self.m.getPos("safetyStop")))
self.UR3.waitForArm()
self.UR3.gripper("prepick", self.pickLink)
self.UR3.onTF(self.pickLink, "Pick")
status = 0
elif ("HOLDER" in self.pickLink):
self.UR3.goHolder(self.pickLink, "Pick")
status = 0
else:
status = 1
rospy.logwarn("Invalid link: %s", self.pickLink)
return status
def place_cb(self, msg):
self.UR3.armPose = ""
self.placeLink = msg.link
if self.t.canTransform("ur3/base", self.placeLink, rospy.Time(0)):
x = self.t.lookupTransform("ur3/base", self.placeLink,
rospy.Time(0))[0][1]
if (self.UR3.tool[1] < 0 and x > 0) or (self.UR3.tool[1] > 0
and x < 0):
self.m.sendMove(
self.m.buildMove('j', '', self.m.getPos("safetyStop")))
self.UR3.waitForArm()
self.UR3.onTF(self.placeLink, "Place")
status = 0
elif "HOLDER" in self.placeLink:
self.UR3.goHolder(self.placeLink, "Place")
status = 0
else:
status = 1
rospy.logwarn("Invalid link: %s", self.placeLink)
return status
class UR():
def __init__(self):
#Instances
self.gripper = Gripper()
self.m = SendMove()
self.t = tf.TransformListener()
self.armPose = ""
#Subscribers
rospy.Subscriber("/tf", TFMessage, self.getToolTF)
rospy.Subscriber("tool_velocity", TwistStamped, self.getToolVelocity)
#Open gripper
self.gripper.moveGripper("Open")
#Go to initial position
self.m.sendMove(self.m.buildMove('j', '', self.m.getPos('test3')))
self.waitForArm()
def onTF(self, link, state):
"""[This function is used to do pick and place based on TF positions]
Arguments:
link {String} -- [This is the TF name]
state {String} -- ["Pick" or "Place"]
"""
transform = self.t.lookupTransform("ur3/base", link, rospy.Time(0))
rospy.loginfo_throttle(5, "Tranformation found for %s" % link)
x = transform[0][0]
y = transform[0][1]
z = transform[0][2]
qrx = transform[1][0]
qry = transform[1][1]
qrz = transform[1][2]
qrw = transform[1][3]
[roll, pitch,
yaw] = tf.transformations.euler_from_quaternion([qrx, qry, qrz, qrw])
[rx, ry, rz] = self.m.euler2Rot(roll, pitch, yaw)
blend_radius = 0.01
#self.m.sendMove(self.m.buildMove('j', 'p', [x, y, z + 0.05, rx, ry, rz], blend_radius))
#self.waitForArmBlend([x, y, (z + 0.05), rx, ry, rz], blend_radius+0.02)
#self.moveTool([x, y, z, rx, ry, rz])
self.m.sendMove(
self.m.buildBlendMove("l", "p", [x, y, z + 0.05, rx, ry, rz],
[x, y, z, rx, ry, rz], blend_radius))
self.waitForArmBlend([x, y, z + 0.05, rx, ry, rz], blend_radius)
if state == "Pick":
self.gripper.moveGripper("Close")
elif state == "Place":
self.gripper.moveGripper("Open")
#self.moveTool([x, y, z + 0.05, rx, ry, rz])
#----------Code for object following----------
# pose = [x, y, z + 0.05 * sin(2.3*rospy.get_time()) + 0.05, rx, ry, rz]
# array = list(pose)
# a = 1.0
# v = 1.0
# t = 0.05
# st = "servoj(get_inverse_kin(p%s), a=%s, v=%s, t=%s)"%(array, a, v, t)
# self.m.sendMove(st)
# time.sleep(t)
def moveTool(self, pose):
"""[This function is used to send the tool to a position in linear tool space]
Arguments:
pose {List} -- [x,y,z,rx,ry,rz]
"""
if self.isPoseReachable(pose):
self.m.sendMove(self.m.buildMove('l', 'p', pose))
self.waitForArm()
def goHolder(self, holderPlace, picking):
"""[This function is used to do pick and place task to/from a holder place on Suii]
Arguments:
holderPlace {String} -- ["HOLDER_1", "HOLDER_2", "HOLDER_3"]
picking {String} -- ["Pick" or "Place"]
"""
rospy.loginfo("Going to holderplace %s", holderPlace)
if holderPlace == "HOLDER_1":
self.m.sendMove(
self.m.buildMove('j', '', self.m.getPos('finalPreHolder1')))
self.waitForArm()
self.m.sendMove(
self.m.buildMove('j', '', self.m.getPos('finalHolder1')))
self.waitForArm()
if picking == "Pick":
self.gripper.moveGripper("Close")
elif picking == "Place":
self.gripper.moveGripper("Open")
self.m.sendMove(
self.m.buildMove('j', '', self.m.getPos('finalPreHolder1')))
self.waitForArm()
elif holderPlace == "HOLDER_2":
self.m.sendMove(
self.m.buildMove('j', '', self.m.getPos('finalPreHolder2')))
self.waitForArm()
self.m.sendMove(
self.m.buildMove('j', '', self.m.getPos('finalHolder2')))
self.waitForArm()
if picking == "Pick":
self.gripper.moveGripper("Close")
elif picking == "Place":
self.gripper.moveGripper("Open")
self.m.sendMove(
self.m.buildMove('j', '', self.m.getPos('finalPreHolder2')))
self.waitForArm()
elif holderPlace == "HOLDER_3":
self.m.sendMove(
self.m.buildMove('j', '', self.m.getPos('finalPreHolder3')))
self.waitForArm()
self.m.sendMove(
self.m.buildMove('j', '', self.m.getPos('finalHolder4')))
self.waitForArm()
if picking == "Pick":
self.gripper.moveGripper("Close")
elif picking == "Place":
self.gripper.moveGripper("Open")
self.m.sendMove(
self.m.buildMove('j', '', self.m.getPos('finalPreHolder2')))
self.waitForArm()
def waitForArm(self):
"""[This function waits till the arm reached it's position, by waiting till tool velocity is zero.]
"""
time.sleep(
0.3
) #0.3 value for real ur3 otherwise arm has no time to get speed
while (abs(self.xVel) > 0.0001 or abs(self.yVel) > 0.0001
or abs(self.zVel) > 0.0001) and not rospy.is_shutdown():
rospy.loginfo_throttle(1, "Robot is moving to position")
rospy.loginfo("Robot reached position")
def waitForArmBlend(self, pose, radius):
while abs(
sqrt((self.tool[0] - pose[0])**2 +
(self.tool[1] - pose[1])**2 +
(self.tool[2] - pose[2])**2)) > radius:
rospy.loginfo_throttle(1, "Moving to blend radius")
rospy.loginfo("Robot reached blend radius")
def isPoseReachable(self, pose):
"""[This function checks if the goal position is within reach of UR3]
Arguments:
pose {list} -- [x,y,z,rx,ry,rz]
Returns:
[bool] -- [True if is within reach, else false]
"""
maxReach = 0.538
minReach = 0.064
x = pose[0]
y = pose[1]
z = pose[2]
distance = sqrt(x**2 + y**2 + z**2)
if distance < maxReach and distance > minReach:
rospy.loginfo("Positon in reach. Distance = %s", distance)
return True
else:
rospy.logwarn("Out of reach, command not sended. Distance = %s",
distance)
def getToolVelocity(self, msg):
self.xVel = msg.twist.linear.x
self.yVel = msg.twist.linear.y
self.zVel = msg.twist.linear.z
self.rxVel = msg.twist.angular.x
self.ryVel = msg.twist.angular.y
self.rzVel = msg.twist.angular.z
def getToolTF(self, msg):
# if len(msg.transforms) == 0:
# return
if msg.transforms[-1].child_frame_id == "ur3/tool0_controller":
x = msg.transforms[-1].transform.translation.x
y = msg.transforms[-1].transform.translation.y
z = msg.transforms[-1].transform.translation.z
qx = msg.transforms[-1].transform.rotation.x
qy = msg.transforms[-1].transform.rotation.y
qz = msg.transforms[-1].transform.rotation.z
qw = msg.transforms[-1].transform.rotation.w
[roll, pitch,
yaw] = tf.transformations.euler_from_quaternion([qx, qy, qz, qw])
[rx, ry, rz] = self.m.euler2Rot(roll, pitch, yaw)
self.tool = [x, y, z, rx, ry, rz]
#
import rospy
import cv2 as cv
from SendMove import SendMove
from UR import UR
from post_processing_test_measuring_precision import PostProcessingTest
import time
import tf2_ros
import csv
import os
from std_msgs.msg import String
rospy.init_node('accuracy_test')
stepl = 0.015
m = SendMove()
arm = UR()
vision = PostProcessingTest()
#ls = tf.TransformListener()
#tr = tf.Transformer()
adress = '/home/suii/catkin_ws/src/camera_accuracy_test/results/'
tfBuffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tfBuffer)
world_poses = []
camera_poses = []
ur3Base_poses = []
last_camera = []
def save_data(base_filename):