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()