def get_target_3d(d):
gp = geometry_msgs.msg.Pose()
gp.position.x = -d[0]
gp.position.y = -d[1]
gp.position.z = d[2]
gp.orientation.w = 1
q = tft.quaternion_from_euler(d[3], d[4], d[5])
gp.orientation.x = q[0]
gp.orientation.y = q[1]
gp.orientation.z = q[2]
gp.orientation.w = q[3]
# Convert to base frame, add the angle in (ensures planar grasp, camera isn't guaranteed to be perpendicular).
gp_base = convert_pose(gp, 'camera_link', 'j2n6s300_link_base')
if gp_base == None:
av = np.array([0, 0, 0, 0, 0, 0, 0])
return av
gpbo = gp_base.orientation
# e = tft.euler_from_quaternion([gpbo.x, gpbo.y, gpbo.z, gpbo.w])
# Only really care about rotation about x (e[0]). update is mean function
# av = pose_averager.update(np.array([gp_base.position.x, gp_base.position.y, gp_base.position.z, e[0],e[1],e[2]]))
av = np.array([
gp_base.position.x, gp_base.position.y, gp_base.position.z, gpbo.x,
gpbo.y, gpbo.z, gpbo.w
])
return av
def execute(self):
# Get the bottle pose from the camera.
rospy.loginfo('Moveing to view')
m.move_to_named_pose('whole_arm', 'view_checkerboard')
rospy.sleep(4)
poses = []
qt = tf.transformations.quaternion_from_euler(0.0, 0.0,
-3.141592653 / 2)
for i in range(16):
rospy.loginfo('GET OBJECT POSE')
b_req = cartesian_calibration.srv.GetACoorRequest()
b_req.data_set = "measured"
b_req.object_idx = i
b_res = self.pose_service.call(b_req)
p = b_res.object_pose.pose
# p.orientation.x = qt[0]
# p.orientation.y = qt[1]
# p.orientation.z = qt[2]
# p.orientation.w = qt[3]
object_global = t.convert_pose(p, self.camera_reference_frame,
'global_xyz_link')
# object_global.position.z = min(0.80,p.position.z)
poses.append(object_global)
m.move_to_named_pose('realsense', 'tool_change_position')
m.move_to_named_pose('wrist_only', 'sucker')
for p in poses:
if p.position.x == 0 and p.position.y == 0 and p.position.z == 0:
rospy.logerr('Failed to find an object. %s' % i)
# return 'failed'
continue
print("MOVE SUCKER TO ABOVE OBJECT")
p = t.align_pose_orientation_to_frame(p, 'global_xyz_link',
'sucker_endpoint')
t.publish_pose_as_transform(p, 'global_xyz_link', 'MOVE HERE', 0.5)
res = m.move_to_global(p.position.x - 0.02,
p.position.y + 0.01,
p.position.z - 0.02,
'sucker',
orientation=p.orientation)
rospy.sleep(4)
def move_pos():
g_pose1 = geometry_msgs.msg.Pose()
g_pose1.orientation.w = 1
end_effector = convert_pose(g_pose1, 'j2n6s300_end_effector',
'j2n6s300_link_base')
end_effector_list = [
end_effector.orientation.x, end_effector.orientation.y,
end_effector.orientation.z, end_effector.orientation.w
]
q = tft.quaternion_from_euler(
np.pi, np.pi / 2, np.pi / 2
) # 绕j2n6s300_base_link's fixed x y z轴转动到正的位置; np.pi, 0, np.pi/2 to left; np.pi/2, 0, np.pi/2 to forward;np.pi, np.pi/2, np.pi/2 to down
q1 = tft.quaternion_from_euler(np.pi / 2, 0, 0) #将手指竖起来
q = tft.quaternion_multiply(q1, q)
end_effector_inverse = tft.quaternion_inverse(end_effector_list)
pgo = tft.quaternion_multiply(q, end_effector_inverse)
q1 = [pgo[0], pgo[1], pgo[2], pgo[3]]
# q1 = [pgo.x, pgo.y, pgo.z, pgo.w]
e = tft.euler_from_quaternion(q1)
dr = 1 * e[0]
dp = 1 * e[1]
dyaw = 1 * e[2]
vx = max(min(0 * 2.5, MAX_VELO_X), -1.0 * MAX_VELO_X)
vy = max(min(0 * 2.5, MAX_VELO_Y), -1.0 * MAX_VELO_Y)
vz = max(min(0, MAX_VELO_Z), -1.0 * MAX_VELO_Z)
# Apply a nonlinearity to the velocity
v = np.array([vx, vy, vz])
vc = np.dot(v, VELO_COV)
CURRENT_VELOCITY[0] = vc[0]
CURRENT_VELOCITY[1] = vc[1]
CURRENT_VELOCITY[2] = vc[2]
CURRENT_VELOCITY[3] = 1 * dr #x: end effector self rotate
CURRENT_VELOCITY[4] = 1 * dp #y: up and down rotate
CURRENT_VELOCITY[
5] = 1 * dyaw #max(min(1 * dyaw, MAX_ROTATION), -1 * MAX_ROTATION) #z: left and right rotate
def command_callback(target_pose, target_quaternion):
global SERVO
global CURR_Z, MIN_Z
global CURR_DEPTH
global pose_averager
global GOAL_Z
global GRIP_WIDTH_MM
global VELO_COV
#CURR_DEPTH = msg.data[5]
if SERVO:
# d = list(msg.data)
# PBVS Method.
#if d[2] > 0.150: # Min effective range of the realsense.
# Convert width in pixels to mm.
# 0.07 is distance from end effector (CURR_Z) to camera.
# 0.1 is approx degrees per pixel for the realsense.
# if d[2] > 0.25:
# GRIP_WIDTH_PX = msg.data[4]
# GRIP_WIDTH_MM = 2 * ((CURR_Z + 0.07)) * np.tan(0.1 * GRIP_WIDTH_PX / 2.0 / 180.0 * np.pi) * 1000
# Construct the Pose in the frame of the camera.
# gp = geometry_msgs.msg.Pose()
# gp.position.x = d[0]
# gp.position.y = d[1]
# gp.position.z = d[2]
# q = tft.quaternion_from_euler(0, 0, -1 * d[3])
# gp.orientation.x = q[0]
# gp.orientation.y = q[1]
# gp.orientation.z = q[2]
# gp.orientation.w = q[3]
# Calculate Pose of Grasp in Robot Base Link Frame
# Average over a few predicted poses to help combat noise.
# x: 0.645162225459
# y: -0.318927784776
# z: 0.694286052858
# w: -0.00420092196819
# x: 0.81874143089
# y: -0.506091943919
# z: 0.374129838746
# gp_base = convert_pose(gp, 'camera_link', 'j2n6s300_link_base')
# print(gp_base.orientation)
# print(gp_base.position)
#gpbo = gp_base.orientation
e = tft.euler_from_quaternion(target_quaternion)
# Only really care about rotation about x (e[0]). update is mean function
av = pose_averager.update(np.array(target_pose + [e[0], e[1], e[2]]))
# else:
# gp_base = geometry_msgs.msg.Pose()
# av = pose_averager.evaluate()
# compute end_effector to j2n6s300_link_base's orientation and euler
g_pose1 = geometry_msgs.msg.Pose()
g_pose1.orientation.w = 1
try:
end_effector = convert_pose(g_pose1, 'j2n6s300_end_effector',
'j2n6s300_link_base')
end_effector_list = [
end_effector.orientation.x, end_effector.orientation.y,
end_effector.orientation.z, end_effector.orientation.w
]
except Exception as ex:
template = "An exception of type {0} occurred. Arguments:{1!r}"
message = template.format(type(ex).__name__, ex.args)
print(message)
return
e1 = tft.euler_from_quaternion(end_effector_list)
# Average pick pose in base frame.
gp_base = geometry_msgs.msg.Pose()
gp_base.position.x = av[0]
gp_base.position.y = av[1]
gp_base.position.z = av[2]
GOAL_Z = av[2]
ang = av[3] - np.pi / 2 # We don't want to align, we want to grip.
q = tft.quaternion_from_euler(
np.pi, 0, np.pi / 2
) # 绕j2n6s300_base_link's fixed x y z轴转动到正的位置; np.pi, 0, np.pi/2 to left; np.pi/2, 0, np.pi/2 to forward;np.pi, np.pi/2, np.pi/2 to down
q1 = tft.quaternion_from_euler(np.pi / 2, 0, 0) #将手指竖起来
q = tft.quaternion_multiply(q1, q)
gp_base.orientation.x = q[0]
gp_base.orientation.y = q[1]
gp_base.orientation.z = q[2]
gp_base.orientation.w = q[3]
# Get the Position of the End Effector in the frame of the Robot base Link
g_pose = geometry_msgs.msg.Pose()
g_pose.position.z = 0.03 # Offset from the end_effector frame to the actual position of the fingers.
g_pose.orientation.w = 1
p_gripper = geometry_msgs.msg.Pose()
try:
p_gripper = convert_pose(g_pose, 'j2n6s300_end_effector',
'j2n6s300_link_base')
except Exception as ex:
template = "An exception of type {0} occurred. Arguments:{1!r}"
message = template.format(type(ex).__name__, ex.args)
print(message)
return
publish_pose_as_transform(gp_base, 'j2n6s300_link_base', 'G', 0.0)
# Calculate Position Error. pick pose - finger pose in base_frame
dx = (gp_base.position.x - p_gripper.position.x)
dy = (gp_base.position.y - p_gripper.position.y)
dz = (gp_base.position.z - p_gripper.position.z)
# Orientation velocity control is done in the frame of the gripper,
# so figure out the rotation offset in the end effector frame.
end_effector_inverse = tft.quaternion_inverse(end_effector_list)
pgo = tft.quaternion_multiply(q, end_effector_inverse)
q1 = [pgo[0], pgo[1], pgo[2], pgo[3]]
# q1 = [pgo.x, pgo.y, pgo.z, pgo.w]
e = tft.euler_from_quaternion(q1)
dr = 1 * e[0]
dp = 1 * e[1]
dyaw = 1 * e[2]
vx = max(min(dx * 2.5, MAX_VELO_X), -1.0 * MAX_VELO_X)
vy = max(min(dy * 2.5, MAX_VELO_Y), -1.0 * MAX_VELO_Y)
vz = max(min(dz - 0.04, MAX_VELO_Z), -1.0 * MAX_VELO_Z)
# Apply a nonlinearity to the velocity
v = np.array([vx, vy, vz])
vc = np.dot(v, VELO_COV)
CURRENT_VELOCITY[0] = vc[0]
CURRENT_VELOCITY[1] = vc[1]
CURRENT_VELOCITY[2] = vc[2]
CURRENT_VELOCITY[3] = 1 * dr #x: end effector self rotate
CURRENT_VELOCITY[4] = 1 * dp #y: up and down rotate
CURRENT_VELOCITY[
5] = 1 * dyaw #max(min(1 * dyaw, MAX_ROTATION), -1 * MAX_ROTATION) #z: left and right rotate
def execute_grasp(cont, N, p):
width = p[6*N+4]
pred=[[0.52,-0.346, 0.6, 0, 0, -math.pi/4],
[0.52, -0.346, 0.37, 0, 0, -math.pi/4],
[0.52,-0.346, 0.14,0,0,-math.pi/4],
[0.565,-0.31, 0.6, 0,0,-math.pi/4],
[0.565,-0.31, 0.37,0,0, -math.pi/4],
[0.565,-0.31, 0.14,0,0,-math.pi/4],
[0.52, -0.346, 0.39, 0, 0, -math.pi/4],
[0.52, -0.346, 0.39, 0, 0, -math.pi/4],
[0.52, -0.346, 0.39, 0, 0, -math.pi/4],
[0.565,-0.31, 0.62, 0,0,-math.pi/4],
[0.565,-0.31, 0.39,0,0, -math.pi/4],
[0.565,-0.31, 0.16, 0, 0, -math.pi/4]]
pred2= [[0.69,-0.516, 0.6,0,0,-math.pi/4],
[0.69,-0.516, 0.37,0,0,-math.pi/4],
[0.69,-0.516, 0.14,0,0,-math.pi/4],
[0.735,-0.48, 0.60,0,0,-math.pi/4],
[0.735,-0.48, 0.39,0,0,-math.pi/4],
[0.735,-0.48, 0.16,0,0,-math.pi/4],
[0.69,-0.516, 0.39,0,0,-math.pi/4],
[0.69,-0.516, 0.39,0,0,-math.pi/4],
[0.69,-0.516, 0.39,0,0,-math.pi/4],
[0.735,-0.48, 0.62,0,0,-math.pi/4],
[0.735,-0.48, 0.39,0,0,-math.pi/4],
[0.735,-0.48, 0.16,0,0,-math.pi/4]]
mov = p[6*N+5]
punto =to_pose(p[6*N+0],p[6*N+1],p[6*N+2],0,0,0,1)
punto1 = geometry_msgs.msg.Pose()
punto2 = geometry_msgs.msg.Pose()
punto1 = convert_pose(punto,"cam","world")
eu = [0, 1.5, p[6*N+3]]
q = tft.quaternion_from_euler(0, 1.5, p[6*N+3]*math.pi/180)
lista=PoseRPYarray()
puntorpy=PoseRPY()
puntorpy.position.x= punto1.position.x
puntorpy.position.y=punto1.position.y
puntorpy.position.z = Z_OFF
puntorpy.rpy.roll = 0
puntorpy.rpy.pitch = 1.57
puntorpy.rpy.yaw = p[6*N+3]
print(puntorpy)
lista.poses.append(copy.deepcopy(puntorpy))
#enviar arriba del objeto
puntorpy.position.z= 0.02
#puntorpy.position.z= 0.26
lista.poses.append(copy.deepcopy(puntorpy))
pub = rospy.Publisher('robot_commander/cmd_path', PoseRPYarray, queue_size=10)
rate = rospy.Rate(1) # 10hz
rate.sleep()
pub.publish(lista)
msg=rospy.wait_for_message('/robot_commander/path_done',std_msgs.msg.String)
print('crrar')
set_gripper_position(width)
rospy.sleep(0.2)
set_gripper_position(width)
print('crro')
rospy.sleep(1)
lista=[]
lista=PoseRPYarray()
print(lista)
#enviar de regreso a la pos predefinida arriba del objeto
puntorpy.position.z= 0.35
lista.poses.append(copy.deepcopy(puntorpy))
# puntorpy.rpy.pitch=0
# lista.poses.append(copy.deepcopy(puntorpy))
puntorpy.position.x= pred[cont][0]
puntorpy.position.y= pred[cont][1]
puntorpy.position.z= pred[cont][2]
puntorpy.rpy.roll = pred[cont][3]
puntorpy.rpy.pitch = pred[cont][4]
puntorpy.rpy.yaw = pred[cont][5]
lista.poses.append(copy.deepcopy(puntorpy))
puntorpy.position.x= pred2[cont][0]
puntorpy.position.y= pred2[cont][1]
puntorpy.position.z= pred2[cont][2]
puntorpy.rpy.roll = pred2[cont][3]
puntorpy.rpy.pitch = pred2[cont][4]
puntorpy.rpy.yaw = pred2[cont][5]
lista.poses.append(copy.deepcopy(puntorpy))
pub = rospy.Publisher('robot_commander/cmd_path', PoseRPYarray, queue_size=10)
rate = rospy.Rate(1) # 10hz
rate.sleep()
pub.publish(lista)
msg=rospy.wait_for_message('/robot_commander/path_done',std_msgs.msg.String)
print('abr')
set_gripper_position(width+45)
print('siiiiiii')
rospy.sleep(1)
lista=[]
lista=PoseRPYarray()
print(lista)
puntorpy.position.x= pred[cont][0]
puntorpy.position.y= pred[cont][1]
puntorpy.position.z= pred[cont][2]
puntorpy.rpy.roll = pred[cont][3]
puntorpy.rpy.pitch = pred[cont][4]
puntorpy.rpy.yaw = pred[cont][5]
lista.poses.append(copy.deepcopy(puntorpy))
#enviar a la posicion pred al frente del estante
print lista
pub = rospy.Publisher('robot_commander/cmd_path', PoseRPYarray, queue_size=10)
rate = rospy.Rate(1) # 10hz
rate.sleep()
pub.publish(lista)
msg=rospy.wait_for_message('/robot_commander/path_done',std_msgs.msg.String)
pubJo.publish(home)
msg=rospy.wait_for_message('/robot_commander/joint_done',std_msgs.msg.String)
print('finalizo mov')
#joint_commander.move_to_jointspose(home)
flag=True
return flag
def push_object(cont, N, p):
width = p[6*N+4]
mov = p[6*N+5]
punto =to_pose(p[6*N+0],p[6*N+1],p[6*N+2],0,0,0,1)
punto1 = geometry_msgs.msg.Pose()
punto2 = geometry_msgs.msg.Pose()
punto1 = convert_pose(punto,"cam","world")
eu = [0, 1.5, p[6*N+3]]
q = tft.quaternion_from_euler(0, 1.5, p[6*N+3]*math.pi/180)
lista=PoseRPYarray()
puntorpy=PoseRPY()
puntorpy.position.x= punto1.position.x
puntorpy.position.y=punto1.position.y
puntorpy.position.z = Z_OFF
puntorpy.rpy.roll = 0
puntorpy.rpy.pitch = 1.57
puntorpy.rpy.yaw = p[6*N+3]
lista.poses.append(copy.deepcopy(puntorpy))
#enviar arriba del objeto
set_gripper_position(0)
puntorpy.position.z= punto1.position.z
lista.poses.append(copy.deepcopy(puntorpy))
#enviar de regreso a la pos predefinida arriba del objeto
if mov == 3:
puntorpy.position.x= puntorpy.position.x-0.1
elif mov == 4:
puntorpy.position.x= puntorpy.position.x+0.1
elif mov == 1:
puntorpy.position.x= puntorpy.position.y-0.1
elif mov == 2:
puntorpy.position.x= puntorpy.position.y+0.1
lista.poses.append(copy.deepcopy(puntorpy))
puntorpy.position.z= 0.35
# puntorpy.rpy.pitch=0
lista.poses.append(copy.deepcopy(puntorpy))
#enviar a la posicion pred al frente del estante
print lista
pub = rospy.Publisher('robot_commander/cmd_path', PoseRPYarray, queue_size=10)
rate = rospy.Rate(1) # 10hz
rate.sleep()
pub.publish(lista)
#msg=rospy.wait_for_message('/robot_commander/path_done',std_msgs.msg.String)
pubJo.publish(home)
msg=rospy.wait_for_message('/robot_commander/joint_done',std_msgs.msg.String)
print('finalizo mov')
#joint_commander.move_to_jointspose(home)
flag=True
return flag
rospy.sleep(1)
p = list(msg.data)
punto =to_pose(p[6*N+0],p[6*N+1],p[6*N+2],0,0,0,1)
#invertidos porque si
#punto.position.x=y
#punto.position.y=x
#punto.position.z=-z
#print punto
#print punto
#x =0.5
#y=-0.1
#z=0.6
w = 1
punto1 = geometry_msgs.msg.Pose()
punto2 = geometry_msgs.msg.Pose()
punto1 = convert_pose(punto,"cam","world")
print('punto1', punto1)
print "este es el punto"
print(p)
if p[5]!=6:
if p[6*N+5]==0.0:
flag = execute_grasp(cont, N, p)
N=N+1
pubJo.publish(home)
elif p[6*N+5] != 0.0:
push_object(cont, N, p)
elif p[5]==6:
print('No hay objetos u hipotesis de grasping')
#joint_commander.move_to_jointspose(home)
def command_callback(msg):
global SERVO
global CURR_Z, MIN_Z
global CURR_DEPTH
global pose_averager
global GOAL_Z
global GRIP_WIDTH_MM
global VELO_COV
CURR_DEPTH = msg.data[5]
if SERVO:
d = list(msg.data)
# PBVS Method.
if d[2] > 0.150: # Min effective range of the realsense.
# Convert width in pixels to mm.
# 0.07 is distance from end effector (CURR_Z) to camera.
# 0.1 is approx degrees per pixel for the realsense.
if d[2] > 0.25:
GRIP_WIDTH_PX = msg.data[4]
GRIP_WIDTH_MM = 2 * ((CURR_Z + 0.07)) * np.tan(
0.1 * GRIP_WIDTH_PX / 2.0 / 180.0 * np.pi) * 1000
# Construct the Pose in the frame of the camera.
gp = geometry_msgs.msg.Pose()
gp.position.x = d[0]
gp.position.y = d[1]
gp.position.z = d[2]
q = tft.quaternion_from_euler(0, 0, -1 * d[3])
gp.orientation.x = q[0]
gp.orientation.y = q[1]
gp.orientation.z = q[2]
gp.orientation.w = q[3]
# Calculate Pose of Grasp in Robot Base Link Frame
# Average over a few predicted poses to help combat noise.
gp_base = convert_pose(gp, 'camera_depth_frame',
'j2n6s300_link_base')
gpbo = gp_base.orientation
e = tft.euler_from_quaternion([gpbo.x, gpbo.y, gpbo.z, gpbo.w])
# Only really care about rotation about z (e[2]).
av = pose_averager.update(
np.array([
gp_base.position.x, gp_base.position.y, gp_base.position.z,
e[2]
]))
else:
gp_base = geometry_msgs.msg.Pose()
av = pose_averager.evaluate()
# Average pose in base frame.
gp_base.position.x = av[0]
gp_base.position.y = av[1]
gp_base.position.z = av[2]
GOAL_Z = av[2]
ang = av[3] - np.pi / 2 # We don't want to align, we want to grip.
q = tft.quaternion_from_euler(np.pi, 0, ang)
gp_base.orientation.x = q[0]
gp_base.orientation.y = q[1]
gp_base.orientation.z = q[2]
gp_base.orientation.w = q[3]
# Get the Position of the End Effector in the frame fo the Robot base Link
g_pose = geometry_msgs.msg.Pose()
g_pose.position.z = 0.03 # Offset from the end_effector frame to the actual position of the fingers.
g_pose.orientation.w = 1
p_gripper = convert_pose(g_pose, 'j2n6s300_end_effector',
'j2n6s300_link_base')
publish_pose_as_transform(gp_base, 'j2n6s300_link_base', 'G', 0.0)
# Calculate Position Error.
dx = (gp_base.position.x - p_gripper.position.x)
dy = (gp_base.position.y - p_gripper.position.y)
dz = (gp_base.position.z - p_gripper.position.z)
# Orientation velocity control is done in the frame of the gripper,
# so figure out the rotation offset in the end effector frame.
gp_gripper = convert_pose(gp_base, 'j2n6s300_link_base',
'j2n6s300_end_effector')
pgo = gp_gripper.orientation
q1 = [pgo.x, pgo.y, pgo.z, pgo.w]
e = tft.euler_from_quaternion(q1)
dr = 1 * e[0]
dp = 1 * e[1]
dyaw = 1 * e[2]
vx = max(min(dx * 2.5, MAX_VELO_X), -1.0 * MAX_VELO_X)
vy = max(min(dy * 2.5, MAX_VELO_Y), -1.0 * MAX_VELO_Y)
vz = max(min(dz - 0.04, MAX_VELO_Z), -1.0 * MAX_VELO_Z)
# Apply a nonlinearity to the velocity
v = np.array([vx, vy, vz])
vc = np.dot(v, VELO_COV)
CURRENT_VELOCITY[0] = vc[0]
CURRENT_VELOCITY[1] = vc[1]
CURRENT_VELOCITY[2] = vc[2]
CURRENT_VELOCITY[3] = -1 * dp
CURRENT_VELOCITY[4] = 1 * dr
CURRENT_VELOCITY[5] = max(min(1 * dyaw, MAX_ROTATION),
-1 * MAX_ROTATION)
print(CURRENT_VELOCITY)
def execute_grasp():
# Execute a grasp.
global MOVING
global CURR_Z
global start_force_srv
global stop_force_srv
# Get the positions.
msg = rospy.wait_for_message('/ggcnn/out/command',
std_msgs.msg.Float32MultiArray)
d = list(msg.data)
# Calculate the gripper width.
grip_width = d[4]
# Convert width in pixels to mm.
# 0.07 is distance from end effector (CURR_Z) to camera.
# 0.1 is approx degrees per pixel for the realsense.
#g_width = 2 * ((CURR_Z + 0.07)) * np.tan(0.1 * grip_width / 2.0 / 180.0 * np.pi) * 1000
# Convert into motor positions.
#g = min((1 - (min(g_width, 70)/70)) * (6800-4000) + 4000, 5500)
#set_finger_positions([g, g])
rospy.sleep(0.5)
# Pose of the grasp (position only) in the camera frame.
gp = geometry_msgs.msg.Pose()
gp.position.x = d[0]
gp.position.y = d[1]
gp.position.z = d[2]
gp.orientation.w = 1
# Convert to base frame, add the angle in (ensures planar grasp, camera isn't guaranteed to be perpendicular).
gp_base = convert_pose(gp, 'camera_depth_optical_frame',
'm1n6s200_link_base')
q = tft.quaternion_from_euler(np.pi, 0, d[3])
gp_base.orientation.x = q[0]
gp_base.orientation.y = q[1]
gp_base.orientation.z = q[2]
gp_base.orientation.w = q[3]
publish_pose_as_transform(gp_base, 'm1n6s200_link_base', 'G', 0.5)
# Offset for initial pose.
initial_offset = 0.20
gp_base.position.z += initial_offset
# Disable force control, makes the robot more accurate.
stop_force_srv.call(kinova_msgs.srv.StopRequest())
move_to_pose(gp_base)
rospy.sleep(0.1)
# Start force control, helps prevent bad collisions.
start_force_srv.call(kinova_msgs.srv.StartRequest())
rospy.sleep(0.25)
# Reset the position
gp_base.position.z -= initial_offset
# Flag to check for collisions.
MOVING = True
# Generate a nonlinearity for the controller.
cart_cov = generate_cartesian_covariance(0)
# Move straight down under velocity control.
velo_pub = rospy.Publisher('/m1n6s200_driver/in/cartesian_velocity',
kinova_msgs.msg.PoseVelocity,
queue_size=1)
while MOVING and CURR_Z - 0.02 > gp_base.position.z:
dz = gp_base.position.z - CURR_Z - 0.03 # Offset by a few cm for the fingertips.
MAX_VELO_Z = 0.08
dz = max(min(dz, MAX_VELO_Z), -1.0 * MAX_VELO_Z)
v = np.array([0, 0, dz])
vc = list(np.dot(v, cart_cov)) + [0, 0, 0]
velo_pub.publish(kinova_msgs.msg.PoseVelocity(*vc))
rospy.sleep(1 / 100.0)
MOVING = False
# close the fingers.
rospy.sleep(0.1)
set_finger_positions([8000, 8000])
rospy.sleep(0.5)
# Move back up to initial position.
gp_base.position.z += initial_offset
gp_base.orientation.x = 1
gp_base.orientation.y = 0
gp_base.orientation.z = 0
gp_base.orientation.w = 0
move_to_pose(gp_base)
stop_force_srv.call(kinova_msgs.srv.StopRequest())
return
def execute(self, userdata):
rospy.loginfo('Finding best segment.')
segments = []
ignored_segments = []
move_mode_seen_wanted_items = []
for i in range(len(userdata.data['identified_objects']['labels'])):
l = userdata.data['identified_objects']['labels'][i]
if l == 'tote':
continue
if userdata.data['task'] == 'stow':
# If we're nearing the end of the stow task, we might be checking
# for recalssified items, so ignore them until they're bumped out of the list.
reclass_item = False
for _, to_item in userdata.data['weight_reclassifications']:
if l == to_item:
reclass_item = True
break
if reclass_item:
rospy.logerr("Ignoring %s as reclassified item" % l)
continue
if userdata.data.get('move_objects_mode', False):
# Don't filter wanted items.
if l in userdata.data['moved_objects']:
continue
elif userdata.data[
'wanted_items'] is not None and l not in userdata.data[
'wanted_items']:
continue
s = userdata.data['identified_objects']['point_clouds'][i]
ad = userdata.data['identified_objects']['aligned_dimensions'][i]
if (ad.width.data < 0.01 and
ad.height.data < 0.01) and self.failed_last_time == False:
rospy.logwarn(
'Ignoring %s because the point cloud is too small' % l)
continue
if userdata.data['identified_objects']['segment_certainties'][
i] < 6 and self.failed_last_time == False:
rospy.logwarn(
'Ignoring %s because the certainty isn\'t high enough' % l)
continue
# Round to the nearest xx cm
round_cm = 3.0
height_sort = round(ad.average_z.data *
(100.0 / round_cm)) / (100.0 / round_cm)
if userdata.data.get('move_objects_mode', False):
# sort by size (negative one to sort smallest to largest.)
item_weight = ad.width.data * ad.height.data
# Weight it higher if it intersects an object that we wanted in older views.
found_intersection = False
for view in userdata.data['previous_views']:
if l in view['labels']:
rospy.logerr('%s is in previsous view' % l)
# Figure out if the view contained any objects that we're trying to find.
seen_wanted_items = [
label for label in view['labels']
if label in userdata.data['wanted_items']
]
if not seen_wanted_items:
rospy.logerr("No wanted items in that view though")
continue
rospy.logerr(
"There were also wanted items in that view.")
item_mask = (view['masked_classifications'] == (
view['labels'].index(l) + 1)).astype(np.uint8)
for wanted_item_label in seen_wanted_items:
wanted_item_id = view['labels'].index(
wanted_item_label) + 1
wanted_item_mask = (view['masked_classifications']
== wanted_item_id).astype(
np.uint8)
wanted_item_mask = ndimage.morphology.binary_dilation(
wanted_item_mask)
intersection = item_mask * wanted_item_mask
if intersection.max() > 0:
rospy.logerr("Found intersection with %s" %
wanted_item_label)
found_intersection = True
item_weight += 10
break
if found_intersection:
break
segments.append((-1 * item_weight, s, l, ad))
else:
# Sort by height in the tote and certainty of the classification.
segments.append(
((height_sort, 255 - userdata.data['identified_objects']
['segment_certainties'][i]), s, l, ad))
if len(segments) == 0:
rospy.loginfo("No Segments")
if userdata.data['move_objects_mode'] == True:
userdata.data['moved_objects'] = []
if userdata.data['task'] == 'pick':
# We use multiple viewpoints for the pick task.
userdata.data['failed_views'][
userdata.data['chosen_bin']].append(
userdata.data['camera_location'])
else:
self.failed_last_time = True
return 'failed'
# Sort by size, biggest first.
segments.sort()
grasp_poses = []
grasp_point_type = ''
last_failed = userdata.data['last_failed']
ignored_segments = 1
pick_task_break = False
# If we don't have grasp poses, but have ignored segments,
# Then repeat the process, because eventually one of the ignored ones will become active again.
IGNORED_ITEMS_RESET = 3
if userdata.data['task'] == 'stow':
IGNORED_ITEMS_RESET = 4
while (not grasp_poses
and ignored_segments > 0) and not pick_task_break:
if userdata.data['task'] == 'pick':
# Stop being in a loop of trying the same object, ingoring and and not moving.
pick_task_break = True
rospy.logerr(last_failed)
ignored_segments = 0
to_remove = []
for l in last_failed:
last_failed[l] += 1
if last_failed[l] > IGNORED_ITEMS_RESET:
to_remove.append(l)
for l in to_remove:
del last_failed[l]
for s in segments:
grasp_poses = []
item_label = s[2]
grasp_point_type = item_meta[item_label]['grasp_point_type']
if item_label == 'mesh_cup' and len(segments) > 1:
continue
if item_label in userdata.data['last_failed']:
# Ignore it for now.
ignored_segments += 1
continue
rospy.loginfo(
"\033[92mI WANT TO PICK ITEM: %s, GRASP POINT TYPE: %s\033[0m"
% (item_label, grasp_point_type))
normal_succeeded = False
centroid_succeeded = False
rgb_succeeded = False
# Try and get fancy grasp poses.
if grasp_point_type == 'normal':
try:
gc_res = self.grasp_candidate_service.call(
s[1], 0.01, False) # False = Don't use centroid.
normal_poses = gc_res.grasp_candidates.grasp_poses.poses
self.pc_pub.publish(s[1])
if len(normal_poses) > 0:
normal_poses = rank_grasp_poses(
normal_poses,
gc_res.grasp_candidates.grasp_utilities)
grasp_poses.extend(normal_poses[:2])
normal_succeeded = True
else:
rospy.logerr(
'Failed to find any normal points for object.')
except rospy.ServiceException:
rospy.logerr(
'Failed to find any normal grasp points for object.'
)
# Get the centroid grasp pose.
if grasp_point_type in ['normal', 'centroid']:
try:
gc_res = self.grasp_candidate_service.call(
s[1], 0.01, True) # True = Use Centroid.
centroid_poses = gc_res.grasp_candidates.grasp_poses.poses
if len(centroid_poses) > 0:
cp = centroid_poses[0]
# Make it straight up and down.
cp.orientation.x = 0
cp.orientation.y = 0
cp.orientation.z = 0
cp.orientation.w = 1
grasp_poses.append(cp)
centroid_succeeded = True
else:
rospy.logerr(
'Failed to find any centroid points for object.'
)
except IndexError:
rospy.logerr(
'Failed to find any centroid points for object.')
if grasp_point_type == 'rgb_centroid' or \
(grasp_point_type in ['normal', 'centroid'] and centroid_succeeded == False):
try:
# Calculate the real world location of the RGB centroid.
index = userdata.data['identified_objects'][
'labels'].index(item_label)
class_mask = np.equal(
index + 1,
self.cb.imgmsg_to_cv2(
userdata.data['identified_objects']
['masked_classifications'],
'mono8')).astype(np.uint8)
com = ndimage.measurements.center_of_mass(class_mask)
img_y = Int32(com[0])
img_x = Int32(com[1])
res = self.image_to_world_service.call(
userdata.data['point_cloud'], img_x, img_y)
p = Pose()
p.position.x = res.world_x.data
p.position.y = res.world_y.data
p.position.z = 1.2 # big number, will get overridden to the storage bottom.
p.orientation.w = 1
grasp_poses.append(p)
rgb_succeeded = True
except IndexError:
rospy.logerr(
'Failed to calculate RGB Centroid grasp point for object'
)
if normal_succeeded == False and centroid_succeeded == False and rgb_succeeded == False:
continue
break
# Make sure we actually got something.
if not grasp_poses:
rospy.logerr('Didn\'t find grasp points for any objects')
if userdata.data['move_objects_mode'] == True:
userdata.data['moved_objects'] = []
if userdata.data['task'] == 'pick':
userdata.data['failed_views'][
userdata.data['chosen_bin']].append(
userdata.data['camera_location'])
else:
self.failed_last_time = True
return 'failed'
self.failed_last_time = False
# Convert the grasp poses to the global reference frame
converted_poses = []
for p in grasp_poses:
converted_poses.append(
t.convert_pose(p, self.camera_reference_frame,
'global_xyz_link'))
# Get the PCA Rotation of the RGB segment because its more reliable than the point cloud.
index = userdata.data['identified_objects']['labels'].index(item_label)
class_mask = np.equal(
index + 1,
self.cb.imgmsg_to_cv2(
userdata.data['identified_objects']['masked_classifications'],
'mono8')).astype(np.uint8)
y, x = np.nonzero(class_mask.squeeze())
x = x - np.mean(x)
y = y - np.mean(y)
coords = np.vstack([x, y])
cov = np.cov(coords)
evals, evecs = np.linalg.eig(cov)
sort_indices = np.argsort(evals)[::-1]
evec1, evec2 = evecs[:, sort_indices]
eig_x, eig_y = evec2
angle = np.arctan2(eig_y, eig_x)
if abs(eig_y) > abs(eig_x):
angle *= -1.0
if angle > 1.57:
angle -= 3.14
if angle < -1.57:
angle += 3.14
q = tft.quaternion_from_euler(0, 0, angle)
pca_rotation = Quaternion()
pca_rotation.x = q[0]
pca_rotation.y = q[1]
pca_rotation.z = q[2]
pca_rotation.w = q[3]
userdata.data['item_to_pick'] = {
'label': item_label,
'grasp_poses': converted_poses,
'pointcloud': s[1],
'grasp_point_type': grasp_point_type,
'pca_yaw': pca_rotation,
'aligned_dimensions': s[3]
}
if (userdata.data['move_objects_mode']
or userdata.data['move_objects_between_bins_mode'] is not None
) and userdata.data[
'wanted_items'] is not None and item_label in userdata.data[
'wanted_items']:
# Just in case we somehow now pick something we want.
userdata.data['move_objects_mode'] = False
userdata.data['move_objects_between_bins_mode'] = None
else:
userdata.data['moved_objects'].append(item_label)
rospy.loginfo('Generated %s grasp poses for object.' %
len(grasp_poses))
# Publish the point cloud segment for debugging purposes.
self.pc_pub.publish(s[1])
# Select sucking or grasping.
grasp_type = item_meta[item_label]['grasp_type']
if grasp_type.startswith('grip'):
return 'succeeded_gripper'
elif grasp_type.startswith('suck'):
return 'succeeded_sucker'
else:
rospy.logerr("UNKNOWN GRASP TYPE %s for %s" %
(grasp_type, item_label))
return 'succeeded_sucker'
return 'failed'
def execute_grasp():
# 执行抓取。
global MOVING
global CURR_Z
global start_force_srv
global stop_force_srv
# 得到位置。
msg = rospy.wait_for_message('/ggcnn/out/command',
std_msgs.msg.Float32MultiArray)
d = list(msg.data)
# 计算抓爪宽度。
grip_width = d[4]
# 将以像素为单位的宽度转换为毫米。
# 0.07是从末端执行器（CURR_Z）到摄像机的距离。
#对于实感，每个像素约0.1度。
g_width = 2 * ((CURR_Z + 0.07)) * np.tan(
0.1 * grip_width / 2.0 / 180.0 * np.pi) * 1000
# 转换为电机位置。
g = min((1 - (min(g_width, 70) / 70)) * (6800 - 4000) + 4000, 5500)
set_finger_positions([g, g])
rospy.sleep(0.5)
# 握把的姿势（仅位置）在相机框架中。
gp = geometry_msgs.msg.Pose()
gp.position.x = d[0]
gp.position.y = d[1]
gp.position.z = d[2]
gp.orientation.w = 1
# 转换为基本框架，并添加角度（以确保平面抓地力，不保证相机垂直）。
gp_base = convert_pose(gp, 'camera_depth_optical_frame',
'm1n6s200_link_base')
q = tft.quaternion_from_euler(np.pi, 0, d[3])
gp_base.orientation.x = q[0]
gp_base.orientation.y = q[1]
gp_base.orientation.z = q[2]
gp_base.orientation.w = q[3]
publish_pose_as_transform(gp_base, 'm1n6s200_link_base', 'G', 0.5)
# 初始姿势的偏移量。
initial_offset = 0.20
gp_base.position.z += initial_offset
# 禁用力控制，使机器人更加精确。
stop_force_srv.call(kinova_msgs.srv.StopRequest())
move_to_pose(gp_base)
rospy.sleep(0.1)
# 启动力控制，有助于防止不良碰撞。
start_force_srv.call(kinova_msgs.srv.StartRequest())
rospy.sleep(0.25)
# 重设位置
gp_base.position.z -= initial_offset
# 标记以检查碰撞。
MOVING = True
# 为控制器生成非线性。
cart_cov = generate_cartesian_covariance(0)
# 在速度控制下直线向下移动。
velo_pub = rospy.Publisher('/m1n6s200_driver/in/cartesian_velocity',
kinova_msgs.msg.PoseVelocity,
queue_size=1)
while MOVING and CURR_Z - 0.02 > gp_base.position.z:
dz = gp_base.position.z - CURR_Z - 0.03 # Offset by a few cm for the fingertips.
MAX_VELO_Z = 0.08
dz = max(min(dz, MAX_VELO_Z), -1.0 * MAX_VELO_Z)
v = np.array([0, 0, dz])
vc = list(np.dot(v, cart_cov)) + [0, 0, 0]
velo_pub.publish(kinova_msgs.msg.PoseVelocity(*vc))
rospy.sleep(1 / 100.0)
MOVING = False
# 闭合手指。
rospy.sleep(0.1)
set_finger_positions([8000, 8000])
rospy.sleep(0.5)
# 移回初始位置。
gp_base.position.z += initial_offset
gp_base.orientation.x = 1
gp_base.orientation.y = 0
gp_base.orientation.z = 0
gp_base.orientation.w = 0
move_to_pose(gp_base)
stop_force_srv.call(kinova_msgs.srv.StopRequest())
return
def open_servo(av):
global SERVO
global CURR_Z, MIN_Z
global CURR_DEPTH
global pose_averager
global GOAL_Z
global GRIP_WIDTH_MM
global VELO_COV
#CURR_DEPTH = msg.data[5]
if SERVO:
# Average pick pose in base frame.
gp_base = geometry_msgs.msg.Pose()
gp_base.position.x = av[0]
gp_base.position.y = av[1]
gp_base.position.z = av[2]
gp_base.orientation.x = av[3]
gp_base.orientation.y = av[4]
gp_base.orientation.z = av[5]
gp_base.orientation.w = av[6]
# publish_pose_as_transform(gp_base, 'j2n6s300_link_base', 'G4', 1.0)
# Get the Position of the End Effector in the frame of the Robot base Link
g_pose = geometry_msgs.msg.Pose()
g_pose.position.z = 0.05 # Offset from the end_effector frame to the actual position of the fingers.
g_pose.orientation.w = 1
p_gripper = geometry_msgs.msg.Pose()
try:
p_gripper = convert_pose(g_pose, 'j2n6s300_end_effector',
'j2n6s300_link_base')
end_effector_list = [
p_gripper.orientation.x, p_gripper.orientation.y,
p_gripper.orientation.z, p_gripper.orientation.w
]
# publish_pose_as_transform(p_gripper, 'j2n6s300_link_base', 'G', 1)
except Exception as ex:
template = "An exception of type {0} occurred. Arguments:{1!r}"
message = template.format(type(ex).__name__, ex.args)
print(message)
return
# Calculate Position Error. pick pose - finger pose in base_frame
dx = (gp_base.position.x - p_gripper.position.x) #forward
dy = (gp_base.position.y - p_gripper.position.y) #left
dz = (gp_base.position.z - p_gripper.position.z) #up
# Orientation velocity control is done in the frame of the gripper,
# so figure out the rotation offset in the end effector frame.
end_effector_inverse = tft.quaternion_inverse(end_effector_list)
pgo = tft.quaternion_multiply(av[3:], end_effector_inverse)
q1 = [pgo[0], pgo[1], pgo[2], pgo[3]]
e = tft.euler_from_quaternion(q1)
if (np.sqrt(dx * dx + dy * dy + dz * dz) < 0.001):
vx = 0
vy = 0
vz = 0
dm = max(max(abs(e[0]), abs(e[1])), abs(e[2]))
if dm > 0.001:
dr = e[0] / dm * MAX_VELO
dp = e[1] / dm * MAX_VELO
dyaw = e[2] / dm * MAX_VELO
else:
return
else:
dm = max(max(abs(dx), abs(dy)), abs(dz))
vx = dx / dm * MAX_VELO
vy = dy / dm * MAX_VELO
vz = dz / dm * MAX_VELO
t = dm / MAX_VELO
dr = 1 * e[0] / t
dp = 1 * e[1] / t
dyaw = 1 * e[2] / t
# Apply a nonlinearity to the velocity
v = np.array([vx, vy, vz])
vc = np.dot(v, VELO_COV)
CURRENT_VELOCITY[0] = vc[0] # forward
CURRENT_VELOCITY[1] = vc[1] # left
CURRENT_VELOCITY[2] = vc[2] # up
CURRENT_VELOCITY[
3] = 1 * dr #x: end effector self rotate,positive is shun
CURRENT_VELOCITY[4] = 1 * dp #y: up and down rotate (positive is down)
CURRENT_VELOCITY[
5] = 1 * dyaw #z: left and right rotate,positive is left
