def _is_distance_between_frames_similar(self, index1, index2):
"""
Check if distance between frames similar
@param index1 index of the first value
@param index2 index of the second value
@return boolean
"""
transformation1 = self.get_recent_transformation(
self._marker_holder_frames[index1],
self._marker_holder_frames[index2])
transformation2 = self.get_recent_filtered_transformation(
self._camera_markers_ids[index1],
self._camera_markers_ids[index2])
vector1 = TransformationManager.point_from_tf(transformation1)
vector2 = TransformationManager.point_from_tf(transformation2)
norm1 = vector_norm(vector1)
norm2 = vector_norm(vector2)
if math.fabs(norm1 - norm2) > self._acceptable_distance_error:
rospy.logerr("Distance between " + self._camera_markers_ids[
index1] + " and " + self._camera_markers_ids[
index2] + " is not matching appropriate robot "
"marker distances")
rospy.logerr(
"It is " + str(norm2) + " and should be " + str(norm1))
return False
return True
def _compare_transformations(transformation1, transformation2):
"""
Compares two transformations by length of it's translations
@param transformation1 first transformation
@param transformation1 second transformation
@return the same as cmp function for two floating values
"""
norm1 = vector_norm(
TransformationManager.point_from_tf(transformation1))
norm2 = vector_norm(
TransformationManager.point_from_tf(transformation2))
return cmp(norm1, norm2)
def build_noise_quat(noise_vec):
noise_quat = ones((4,1))
noise_mag = tf_math.vector_norm(noise_vec)
if noise_mag != 0.:
noise_quat_vec_part = noise_vec * sin(noise_mag/2.)/noise_mag
else:
noise_quat_vec_part = noise_vec * sin(noise_mag/2.)
noise_quat[0:3,0] = noise_quat_vec_part
noise_quat[3,0] = cos(noise_mag/2.)
return noise_quat
def __init__(self, normal=None, point=None, equation=None):
if equation is None:
self.point = np.array(point)
self.normal = tr.unit_vector(normal)
self.offset = -np.dot(self.normal, self.point)
else:
norm = tr.vector_norm(equation[:3])
self.normal = tr.unit_vector(equation[:3])
self.offset = equation[3] / norm
self.point = -self.offset*self.normal
# Plane origin
self.origin = np.array(self.point)
def estimate_mean(self, transformed_sigmas):
quat_sum = zeros((4,1))
est_mean = zeros(self.kalman_state.shape)
#Compute estimated mean for non-quaternion components
for i in range(0,self.n*2+1):
est_mean += self.weight_mean[i] * transformed_sigmas[i]
#Compute quaternion component mean
for i in range(0, self.n*2+1):
quat_temp = SF9DOF_UKF.recover_quat(transformed_sigmas[i])
quat_sum += self.weight_mean[i] * quat_temp
#This should create a unit quaternion
est_quat = quat_sum / tf_math.vector_norm(quat_sum)
est_mean[0:3,0] = est_quat[0:3,0]
return est_mean
def sendReference(self, vel):
# cap linear speed
l = vel.linear
v = np.array([l.x, l.y, l.z])
speed = vector_norm(v)
if speed > self.max_speed_linear * (1 + self.max_speed_error):
rospy.logwarn('maximum linear speed exceeded with %f m/s, capping to %f m/s' % (speed, self.max_speed_linear))
v = v / speed * self.max_speed_linear
# cap angular speed
a = vel.angular.z
if np.abs(a) > self.max_speed_angular * (1+ self.max_speed_error):
rospy.logwarn('maximum angular speed exceeded with %f rad/s, capping to %f rad/s' % (a, self.max_speed_angular))
a = a / abs(a) * self.max_speed_angular
self.ser.write("<%f,%f,%f>\n"%(v[0], v[1], a))
def sendReference(self, vel):
# cap linear speed
l = vel.linear
v = np.array([l.x, l.y, l.z])
speed = vector_norm(v)
if speed > self.max_speed_linear * (1 + self.max_speed_error):
rospy.logwarn('maximum linear speed exceeded with %f m/s, capping to %f m/s' % (speed, self.max_speed_linear))
v = v / speed * self.max_speed_linear
# cap angular speed
a = vel.angular.z
if np.abs(a) > self.max_speed_angular * (1+ self.max_speed_error):
rospy.logwarn('maximum angular speed exceeded with %f rad/s, capping to %f rad/s' % (a, self.max_speed_angular))
a = a / abs(a) * self.max_speed_angular
self.ser.write("<%f,%f,%f>\n"%(v[0], v[1], a))
def rotate_quat_by_omega(quat, omega, dt):
omega_mag = tf_math.vector_norm(omega)
temp_term = 0.5 * omega_mag * dt
psi_vec = sin(temp_term) * omega
if omega_mag != 0.:
psi_vec = psi_vec / omega_mag
omega_mat = eye(4) * cos(temp_term)
omega_mat[0:3,3] = psi_vec
omega_mat[3,0:3] = -psi_vec
full_quat = zeros((4,1))
full_quat[0:3,0] = quat
full_quat[3,0] = sqrt(1 - dot(quat, quat))
if not alltrue(isreal(full_quat)):
#TODO Should find a better exception class for this
raise Exception("Quat not completely real... this is troubling")
new_quat = dot(omega_mat, full_quat)
new_quat = tf_math.unit_vector(new_quat)
return new_quat[0:3,0]
def get_tags(self, msg):
#pos_x = msg.detections[0].pose.pose.pose.position.x
#print(msg.detections[0].id)
#print("number of detections:",len(msg.detections))
ids = len(msg.detections)
ul_pose = PoseStamped()
ur_pose = PoseStamped()
ll_pose = PoseStamped()
lr_pose = PoseStamped()
#p2 = tf2_geometry_msgs.do_transform_pose(spose, trans)
tf_mat = gettf(msg.detections[0].pose.header.frame_id,
str("panda_link0"))
if ids == 4:
for detection in msg.detections:
#print(detection.id)
if detection.id[0] == self.upleft:
#print(detection.id)
#print('upperleft')
ul_pose.header.stamp = tf_mat.header.stamp
ul_pose.header.frame_id = msg.detections[
0].pose.header.frame_id
ul_pose.pose = detection.pose.pose.pose
ul_pose = tf2_geometry_msgs.do_transform_pose(
ul_pose, tf_mat)
#print("upperleft")
#print(ul_pose)
#print(p2)
#upperleft=detection.pose.pose.pose.position
upperleft = ul_pose.pose.position
elif detection.id[0] == self.upright:
#print(detection.id)
#print('upperright')
ur_pose.header.stamp = tf_mat.header.stamp
ur_pose.header.frame_id = msg.detections[
0].pose.header.frame_id
ur_pose.pose = detection.pose.pose.pose
ur_pose = tf2_geometry_msgs.do_transform_pose(
ur_pose, tf_mat)
upperright = ur_pose.pose.position
#upperright=detection.pose.pose.pose.position
elif detection.id[0] == self.lowleft:
#print(detection.id)
#print("lowerleft")
ll_pose.header.stamp = tf_mat.header.stamp
ll_pose.header.frame_id = msg.detections[
0].pose.header.frame_id
ll_pose.pose = detection.pose.pose.pose
ll_pose = tf2_geometry_msgs.do_transform_pose(
ll_pose, tf_mat)
lowerleft = ll_pose.pose.position
#lowerleft=detection.pose.pose.pose.position
elif detection.id[0] == self.lowright:
lr_pose.header.stamp = tf_mat.header.stamp
lr_pose.header.frame_id = msg.detections[
0].pose.header.frame_id
lr_pose.pose = detection.pose.pose.pose
lr_pose = tf2_geometry_msgs.do_transform_pose(
lr_pose, tf_mat)
#print(detection.id)
#print('lowerright')
lowerright = lr_pose.pose.position
#lowerright=detection.pose.pose.pose.position
x_ax = numpy.array([
ul_pose.pose.position.x - ll_pose.pose.position.x,
ul_pose.pose.position.y - ll_pose.pose.position.y,
ul_pose.pose.position.z - ll_pose.pose.position.z
])
y_ax = numpy.array([
ll_pose.pose.position.x - lr_pose.pose.position.x,
ll_pose.pose.position.y - lr_pose.pose.position.y,
ll_pose.pose.position.z - lr_pose.pose.position.z
])
boxlen = tr.vector_norm(x_ax) #x distance
boxwidth = tr.vector_norm(x_ax) #ydist
x_ax = tr.unit_vector(x_ax)
y_ax = tr.unit_vector(y_ax)
z_ax = numpy.cross(x_ax, y_ax)
#rmat=tr.identity_matrix()
Rx = tr.rotation_matrix(0, x_ax)
Ry = tr.rotation_matrix(0, y_ax)
Rz = tr.rotation_matrix(0, z_ax)
rmat = tr.concatenate_matrices(Rx, Ry, Rz)
aaa = tr.vector_norm(x_ax)
centerquat = tr.quaternion_from_matrix(rmat)
#print(rqq)
#rmat=numpy.asarray(rmat)
#print(rmat)
#R_map_lastbaselink = tr.quaternion_matrix(q_map_lastbaselink)[0:3, 0:3]
#p_lastbaselink_currbaselink = R_map_lastbaselink.transpose().dot(p_map_currbaselink - p_map_lastbaselink)
cx = (lowerleft.x + lowerright.x + upperleft.x +
upperright.x) / 4.0
cy = (lowerleft.y + lowerright.y + upperleft.y +
upperright.y) / 4.0
cz = (lowerleft.z + lowerright.z + upperleft.z +
upperright.z) / 4.0
centerpose = Pose()
centpose = PoseStamped()
#centerpose.orientation=msg.detections[0].pose.pose.pose.orientation
centpose.pose.position.x = cx
centpose.pose.position.y = cy
centpose.pose.position.z = cz
centpose.pose.orientation.x = centerquat[0]
centpose.pose.orientation.y = centerquat[1]
centpose.pose.orientation.z = centerquat[2]
centpose.pose.orientation.w = centerquat[3]
centpose.header.frame_id = lr_pose.header.frame_id
centpose.header.stamp = lr_pose.header.stamp
#print("a:\n",centerpose)
centerpose = centpose.pose
bbox = obb()
bbox.width = boxwidth
bbox.height = boxlen
bbox.depth = cz #pt zero
bbox.pose_orient = centpose
obb_pub.publish(bbox)
#centersp=transformedp.pose
#print(transformedp)
#q_map_baselink = numpy.array([q.x, q.y, q.z, q.w])
#qua1=msg.detections[0].pose.pose.pose.orientation
#qua2=msg.detections[1].pose.pose.pose.orientation
#qua3=msg.detections[2].pose.pose.pose.orientation
#qua4=msg.detections[3].pose.pose.pose.orientation
#qq1=numpy.array([qua1.x,qua1.y,qua1.z,qua1.w])
#qq2=numpy.array([qua2.x,qua2.y,qua2.z,qua2.w])
#qq3=numpy.array([qua3.x,qua3.y,qua3.z,qua3.w])
#qq4=numpy.array([qua4.x,qua4.y,qua4.z,qua4.w])
#r1=transformations.quaternion_matrix(qq1)
#r2=transformations.quaternion_matrix(qq2)
#r3=transformations.quaternion_matrix(qq3)
#r4=transformations.quaternion_matrix(qq4)
#q_last_to_current = tr.quaternion_multiply(tr.quaternion_inverse(qq1), qq2)
#r_d, p_d, yaw_diff = tr.euler_from_quaternion(q_last_to_current)
pub2.publish(centpose)
elif ids == 2:
for detection in msg.detections:
#print(detection.id)
if detection.id[0] == 13 or detection.id[0] == 14:
print("lower 2 detected, move forward")
def recover_quat(mean):
quat_temp = zeros((4,1))
quat_vec = mean[0:3,0]
quat_temp[0:3,0] = quat_vec
quat_temp[3,0] = 1. - tf_math.vector_norm(quat_vec)
return quat_temp
def execute(self, userdata):
first_attamept = True
label = userdata.data['item_to_pick']['label']
suck_level = item_meta[label].get('suction_pressure', 3)
set_suck_level(suck_level)
USE_ANGLED = False
position_limits = POSITION_LIMITS[
userdata.data['picking_from']]['sucker']
# Get the tote weight before the pick
scales_topic = userdata.data['picking_from'] + '_scales/weight'
if item_meta[userdata.data['item_to_pick']
['label']]['grasp_type'] != 'grip_suck':
# If we've just tried sucking, then don't re-weigh
try:
weight = rospy.wait_for_message(scales_topic, Int16, 2.0)
userdata.data['weight_before'] = weight.data
except:
rospy.logerr('Unable to read weight from %s' % scales_topic)
userdata.data['weight_before'] = -1
if item_meta[userdata.data['item_to_pick']
['label']]['grasp_point_type'] == 'rgb_centroid':
rospy.logerr(
"Aborting RGB Centroid pick because there aren't any scales."
)
return 'suck_failed'
for object_global in userdata.data['item_to_pick']['grasp_poses']:
USE_ANGLED = decide_angled(object_global, position_limits)
object_global.position.x += 0.015
object_global.position.y -= 0.03
if USE_ANGLED:
rospy.logerr('ANGLED PICK')
# Make sure we're inside the tote.
edge_offset = 0.0
object_global.position.x = max(
object_global.position.x,
position_limits['x_min'] - edge_offset)
object_global.position.x = min(
object_global.position.x,
position_limits['x_max'] + edge_offset)
object_global.position.y = max(
object_global.position.y,
position_limits['y_min'] - edge_offset)
object_global.position.y = min(
object_global.position.y,
position_limits['y_max'] + edge_offset)
object_global.position.z -= 0.02 # The endpoint is half way through the suction cup, so move up slightly
#t.publish_pose_as_transform(object_global, 'global_xyz_link', 'ORIGINAL', seconds=2)
translation = [
object_global.position.x, object_global.position.y,
object_global.position.z
]
qm = object_global.orientation
q = (qm.x, qm.y, qm.z, qm.w)
R = tft.quaternion_matrix(q)
P = tft.projection_matrix([0, 0, 0], [0, 0, 1]) # XY plane
pose_vec = R[:, 2] # Z portion of matrix.
proj_vec = np.dot(
P,
pose_vec)[:3] # Projected onto XY plane as a unit vector.
proj_vec_unit = tft.unit_vector(proj_vec)
y_unit = [0, 1, 0] # Unit vector in the y-direction.
yaw_angle = np.arccos(np.dot(
proj_vec_unit, y_unit)) # Angle to the grasp around Z.
pitch_angle = np.arcsin(
tft.vector_norm(proj_vec)) # Angle to tilt the suction cup
if pitch_angle > 1.2:
pitch_angle = 1.2
# Only get absolute angle from above. Check the direction of the vector.
if R[0, 2] > 0:
yaw_angle = -1 * yaw_angle
# Create a quaternion with the right angles (note rotations are applied in the rotated frame in zyx order)
q = tft.quaternion_from_euler(yaw_angle, 0, -1 * pitch_angle,
'rzyx')
if yaw_angle > 1.571 or yaw_angle < -1.571:
# Rotate by 180 to normalise to the sucker workspace.
q2 = tft.quaternion_from_euler(0, 0, 3.1415)
q = tft.quaternion_multiply(q, q2)
e = list(tft.euler_from_quaternion(q))
if e[2] > 1.5: # Stop gimbal lock and weird joint configurations.
e[2] = 1.5
if e[2] < -1.5:
e[2] = -1.5
q = tft.quaternion_from_euler(e[0], e[1], e[2])
object_global.orientation.x = q[0]
object_global.orientation.y = q[1]
object_global.orientation.z = q[2]
object_global.orientation.w = q[3]
R = tft.quaternion_matrix(q)
t.publish_tf_quaterion_as_transform(translation,
q,
'global_xyz_link',
'MOVE_HERE',
seconds=0.5)
# Create a pre-grasp pose away from the object.
T = tft.translation_matrix(translation)
T2 = tft.translation_matrix((0, 0, -0.05))
F = tft.concatenate_matrices(T, R, T2)
pre_grasp_t = tft.translation_from_matrix(F)
pre_grasp_q = tft.quaternion_from_matrix(F)
#t.publish_tf_quaterion_as_transform(pre_grasp_t, pre_grasp_q, 'global_xyz_link', 'PRE', seconds=5.0)
grasp_orientation = Quaternion()
grasp_orientation.x = q[0]
grasp_orientation.y = q[1]
grasp_orientation.z = q[2]
grasp_orientation.w = q[3]
rospy.loginfo('MOVE TO TOOL CHANGE HEIGHT')
if first_attamept:
# Move the whole wrist and then select the sucker.
res = m.move_to_global(
min(object_global.position.x, REALSENSE_MAX_GLOBAL_X),
max(
min(object_global.position.y,
SUCKER_MAX_Y_STRAIGHT) - 0.02,
SUCKER_MIN_GLOBAL_Y + 0.02) -
SUCKER_REALSENSE_Y_OFFSET,
TOOL_CHANGE_REALSENSE_GLOBAL_Z, 'realsense')
if not res:
return 'failed'
rospy.loginfo('SELECTING SUCKER')
res = m.move_to_named_pose('wrist_only', 'sucker')
if not res:
return 'failed'
first_attamept = False
else:
# Straighten up the sucker from the last attempt and move into position
straight_q = Quaternion()
straight_q.x = 0
straight_q.y = 0
straight_q.z = 0
straight_q.w = 1
res = m.move_to_global(
min(object_global.position.x, SUCKER_MAX_GLOBAL_X),
max(
min(object_global.position.y,
SUCKER_MAX_Y_STRAIGHT) - 0.02,
SUCKER_MIN_GLOBAL_Y + 0.02),
TOOL_CHANGE_REALSENSE_GLOBAL_Z + 0.35,
'sucker',
orientation=straight_q)
if not res:
return 'failed'
rospy.loginfo("MOVE TO PRE-GRASP")
res = m.move_to_global(pre_grasp_t[0],
pre_grasp_t[1],
min(pre_grasp_t[2],
SUCKER_MAX_GLOBAL_Z),
'sucker',
orientation=grasp_orientation,
velo_scale=0.4)
if not res:
return 'failed'
rospy.loginfo("PUMP ON, MOVING TO OBJECT")
suction.pump_on()
(res, stopped,
bail) = m.move_to_global_monitor_weight_and_suction(
object_global.position.x,
object_global.position.y,
min(object_global.position.z, SUCKER_MAX_GLOBAL_Z),
'sucker',
scales_topic,
orientation=grasp_orientation,
velo_scale=0.3)
if not res:
return 'failed'
else:
# Straight Down Suck
rospy.logerr('STRAIGHT PICK')
# Make sure we're inside the tote.
edge_offset = 0.0
object_global.position.x = max(
object_global.position.x,
position_limits['x_min'] - edge_offset)
object_global.position.x = min(
object_global.position.x,
position_limits['x_max'] + edge_offset)
object_global.position.y = max(
object_global.position.y,
position_limits['y_min'] - edge_offset)
object_global.position.y = min(
object_global.position.y,
position_limits['y_max'] + edge_offset)
object_global.position.z = min(object_global.position.z,
position_limits['z_max'])
t.publish_pose_as_transform(object_global,
'global_xyz_link',
'MOVE_HERE',
seconds=0.5)
rospy.loginfo('MOVE TO TOOL CHANGE HEIGHT')
# We only need to select the sucker on the first attempt.
# Can cause it to break becasue the controllers don't switch properly
# if there's no actual movment and we change controllers too quickly. Not sure why, something to do with timing.
if first_attamept:
res = m.move_to_global(
object_global.position.x,
max(
min(object_global.position.y,
SUCKER_MAX_Y_STRAIGHT),
SUCKER_MIN_Y_STRAIGHT) - SUCKER_REALSENSE_Y_OFFSET,
TOOL_CHANGE_REALSENSE_GLOBAL_Z, 'realsense')
if not res:
return 'failed'
rospy.loginfo('SELECTING SUCKER')
res = m.move_to_named_pose('wrist_only', 'sucker')
if not res:
return 'failed'
first_attamept = False
if item_meta[userdata.data['item_to_pick']
['label']]['grasp_type'] == 'grip_suck':
# Weird async movement bug.
rospy.sleep(1.0)
else:
straight_q = Quaternion()
straight_q.x = 0
straight_q.y = 0
straight_q.z = 0
straight_q.w = 1
res = m.move_to_global(
object_global.position.x,
max(
min(object_global.position.y,
SUCKER_MAX_Y_STRAIGHT), SUCKER_MIN_Y_STRAIGHT),
TOOL_CHANGE_REALSENSE_GLOBAL_Z + 0.35,
'sucker',
orientation=straight_q)
if not res:
return 'failed'
# Correct the rotation if required.
pca_yaw = userdata.data['item_to_pick']['pca_yaw']
q = (pca_yaw.x, pca_yaw.y, pca_yaw.z, pca_yaw.w)
e = list(tft.euler_from_quaternion(q))
if e[2] > 1.57:
e[2] -= 3.14
if e[2] < -1.57:
e[2] += 3.14
if e[2] > 1.5: # Stop gimbal lock and weird joint configurations.
e[2] = 1.5
if e[2] < -1.5:
e[2] = -1.5
q = tft.quaternion_from_euler(0, 0, e[2])
pca_yaw.x = q[0]
pca_yaw.y = q[1]
pca_yaw.z = q[2]
pca_yaw.w = q[3]
rospy.loginfo("PUMP ON, MOVING TO OBJECT")
suction.pump_on()
v_scale = 0.25
if item_meta[userdata.data['item_to_pick']
['label']]['grasp_point_type'] == 'rgb_centroid':
v_scale = 0.1
(res, stopped,
bail) = m.move_to_global_monitor_weight_and_suction(
object_global.position.x,
min(object_global.position.y, SUCKER_MAX_Y_STRAIGHT),
min(object_global.position.z, SUCKER_MAX_GLOBAL_Z),
'sucker',
scales_topic,
orientation=pca_yaw,
velo_scale=v_scale)
if not res:
return 'failed'
if bail:
m.move_relative(0, 0, -0.02, 'sucker_endpoint', 'sucker')
rospy.sleep(0.5)
elif not suction.check_pressure_sensor():
# If it's not already on, give it a sec.
rospy.sleep(1.0)
suction_state = suction.check_pressure_sensor(
) #query pressure sensor for state, has the object been sucked correctly
rospy.loginfo("PRESSURE SENSOR STATUS: %s" % suction_state)
if not suction_state and not bail:
set_suck_level(3)
# Just move down a touch and double check
move_amount = 0.02
if USE_ANGLED:
move_amount = 0.035
if object_global.position.z < (SUCKER_MAX_GLOBAL_Z):
move_amount = min(
move_amount,
SUCKER_MAX_GLOBAL_Z - object_global.position.z)
res = m.move_relative(0,
0,
move_amount,
'sucker_endpoint',
'sucker',
plan_time=0.5)
else:
res = True
if res:
rospy.sleep(1.0)
suction_state = suction.check_pressure_sensor()
if suction_state:
#userdata.data['last_failed'] = None
return 'succeeded'
else:
suction.pump_off()
# Run out of grasp poses.
suction.pump_off()
if item_meta[userdata.data['item_to_pick']
['label']]['grasp_type'] == 'suck_grip':
return 'try_gripper'
userdata.data['last_failed'][userdata.data['item_to_pick']
['label']] = 0
return 'suck_failed'
io_state.configurable_5 = testBit(state.actual_digital_input_bits,
13)
io_state.configurable_6 = testBit(state.actual_digital_input_bits,
14)
io_state.configurable_7 = testBit(state.actual_digital_input_bits,
15)
io_state.tool_0 = testBit(state.actual_digital_input_bits, 16)
io_state.tool_1 = testBit(state.actual_digital_input_bits, 17)
if universe not in ["URsim"]:
io_pub.publish(io_state)
tcp_quat = trans.quaternion_about_axis(
trans.vector_norm([
state.actual_TCP_pose[3], state.actual_TCP_pose[4],
state.actual_TCP_pose[5]
]),
trans.unit_vector([
state.actual_TCP_pose[3], state.actual_TCP_pose[4],
state.actual_TCP_pose[5]
]))
br.sendTransform(
(state.actual_TCP_pose[0],
state.actual_TCP_pose[1], state.actual_TCP_pose[2]), tcp_quat,
rospy.Time.now(), tool_frame, base_frame)
except IOError:
pass
con.send_pause()
con.disconnect()
