def __init__(self):
# Sets the arm that is used for the servoing
limb = 'right'
self._baxter = BaxterVS(limb)
# Initializes the marker that the arm should track
target_marker = 0
self._apriltag_client = AprilTagClient(target_marker)
self._visual_servo = VisualServoing(False)
cv2.namedWindow("Detected tags", 0)
def __init__(self):
# Baxter specific code. You don't need this if you're using another robot.
# Sets the arm that is used for the servoing
limb = 'right'
self._baxter = BaxterVS(limb)
# AprilTag specific code. You don't need this if you're using another tracking system.
# Initializes the marker that the arm should track
target_marker = 0
self._apriltag_client = AprilTagClient(target_marker)
self._visual_servo = VisualServoing(ibvs=True)
def __init__(self):
# Baxter specific code. You don't need this if you're using another robot.
# Sets the arm that is used for the servoing
limb='left'
self._baxter = BaxterVS(limb)
self.gripper = baxter_interface.Gripper(limb)
self.gripper.calibrate()
self.golf_ball_x = 0.85 # x = front back
self.golf_ball_y = 0.10 # y = left right
self.golf_ball_z = 0.0 # z = up down
self.roll = -1.0 * math.pi # roll = horizontal
self.pitch = 0.0 * math.pi # pitch = vertical
self.yaw = 0.0 * math.pi # yaw = rotation
self.pose = (self.golf_ball_x,
self.golf_ball_y,
self.golf_ball_z,
self.roll,
self.pitch,
self.yaw)
print 'ready to move'
self.baxter_ik_move(limb, self.pose)
# self.golf_ball_x = self.golf_ball_x + 0.010
# # self.golf_ball_y = self.golf_ball_y - 0.10
# # self.golf_ball_z = self.golf_ball_z + 0.10
# self.pitch = self.pitch - 0.2 * math.pi
# self.pose = (self.golf_ball_x,
# self.golf_ball_y,
# self.golf_ball_z,
# self.roll,
# self.pitch,
# self.yaw)
# print 'ready to move'
# self.baxter_ik_move(limb, self.pose)
# AprilTag specific code. You don't need this if you're using another tracking system.
# Initializes the marker that the arm should track
target_marker = 0
# self._apriltag_client = AprilTagClient(target_marker)
self._visual_servo = VisualServoing(ibvs=True)
# camera parameters (NB. other parameters in open_camera)
self.cam_calib = 0.0025 # meters per pixel at 1 meter
self.cam_x_offset = -0.01 # camera gripper offset
self.cam_y_offset = -0.035
self.width = 960 # Camera resolution
self.height = 600
# callback image
self.cv_image = cv.CreateImage((self.width, self.height), 8, 3)
# create image publisher to head monitor
self.pub = rospy.Publisher('/robot/xdisplay', Image, latch=True)
self.subscribe_to_camera(limb)
def __init__(self):
# Sets the arm that is used for the servoing
limb='right'
self._baxter = BaxterVS(limb)
# Initializes the marker that the arm should track
target_marker = 0
self._apriltag_client = AprilTagClient(target_marker)
self._visual_servo = VisualServoing(False)
cv2.namedWindow("Detected tags",0)
def __init__(self):
# Baxter specific code. You don't need this if you're using another robot.
# Sets the arm that is used for the servoing
limb='right'
self._baxter = BaxterVS(limb)
# AprilTag specific code. You don't need this if you're using another tracking system.
# Initializes the marker that the arm should track
target_marker = 0
self._apriltag_client = AprilTagClient(target_marker)
self._visual_servo = VisualServoing(ibvs=True)
def __init__(self):
# Sets the arm that is used for the servoing
limb='left'
self._baxter = BaxterVS(limb)
self.gripper = baxter_interface.Gripper(limb)
self.gripper.calibrate()
# Initializes the marker that the arm should track
target_marker = 0
# self._apriltag_client = AprilTagClient(target_marker)
self._visual_servo = VisualServoing(False)
# cv2.namedWindow("Detected tags",0)
self.marker_t=None
self.marker_R=None
self.target_obj_name = "expo_dry_erase_board_eraser"
simtrack_topic = "/simtrack/" + self.target_obj_name
print 'simtrack_topic: ', simtrack_topic
self.subscribe_to_simtrack(simtrack_topic)
class IbvsEih(object):
"""
Performs eye in hand (eih) image based visual servoing (ibvs).
"""
def __init__(self):
# Baxter specific code. You don't need this if you're using another robot.
# Sets the arm that is used for the servoing
limb='left'
self._baxter = BaxterVS(limb)
self.golf_ball_x = 0.8 # x = front back
self.golf_ball_y = 0.25 # y = left right
self.golf_ball_z = -0.1 # z = up down
self.roll = -1.0 * math.pi # roll = horizontal
self.pitch = 0.0 * math.pi # pitch = vertical
self.yaw = 0.0 * math.pi # yaw = rotation
self.pose = (self.golf_ball_x,
self.golf_ball_y,
self.golf_ball_z,
self.roll,
self.pitch,
self.yaw)
print 'ready to move'
self.baxter_ik_move(limb, self.pose)
# AprilTag specific code. You don't need this if you're using another tracking system.
# Initializes the marker that the arm should track
target_marker = 0
self._apriltag_client = AprilTagClient(target_marker)
self._visual_servo = VisualServoing(ibvs=True)
# camera parameters (NB. other parameters in open_camera)
self.cam_calib = 0.0025 # meters per pixel at 1 meter
self.cam_x_offset = -0.01 # camera gripper offset
self.cam_y_offset = -0.035
self.width = 960 # Camera resolution
self.height = 600
# callback image
self.cv_image = cv.CreateImage((self.width, self.height), 8, 3)
# create image publisher to head monitor
self.pub = rospy.Publisher('/robot/xdisplay', Image, latch=True)
self.subscribe_to_camera(limb)
def new_image_arrived(self):
"""
Boolean to test if a new image has arrived.
"""
print "waiting for new image"
if self._apriltag_client.corners is not None:
self._apriltag_client.corners = None
return True
return False
# def _get_detected_corners(self):
# """
# Returns the most recently detected corners in the image.
# """
# # This method currently uses AprilTag detections, so replace with
# # your own method if otherwise.
# # print "looking for corners"
# return self._apriltag_client.corners
def _command_velocity(self,cam_vel,error_norm):
"""
Move the camera at the specified v and omega in vel (6x1 vector).
"""
# This method currently commands the Baxter robot from Rethink Robotics.
#Replace with your own method if using another manipulator.
hand_vel_in_base = self._baxter.cam_to_body(cam_vel)
# print 'baxter_vel: ', baxter_vel
self._baxter.set_hand_vel(hand_vel_in_base,error_norm)
def set_target(self,final_camera_depth,desired_corners):
"""
Sets the final camera depth (Z) and desired position for the tracked features
at the goal position.
"""
Z = final_camera_depth
ideal_cam_pose = np.array([0,0,Z])
# print 'ideal_cam_pose: ', ideal_cam_pose
self._visual_servo.set_target(ideal_cam_pose,None,desired_corners)
def move_to_position(self,final_camera_depth,desiredImg, desiredCircles, desired_centers, dist_tol):
"""
Runs one instance of the visual servoing control law. Call when a new
image has arrived.
"""
# self.set_target(final_camera_depth,desired_corners)
# r = rospy.Rate(60)
error_norm = 1000
while error_norm>dist_tol and not rospy.is_shutdown():
# if not self.new_image_arrived():
# # print " no new_image_arrived"
# continue
# Continue if no corners detected
# marker_corners = self._get_detected_corners()
# img_array = cv2array(self.cv_image)
# print self.cv_image.shape
# cv.WaitKey(1000)
print type(self.cv_image)
# currentI = cv2array(self.cv_image)
currentI = numpy.asarray(self.cv_image[:,:])
print type(currentI)
gray = cv2.cvtColor(currentI, cv2.COLOR_BGR2GRAY)
# cv2.imshow("input", gray)
# cv2.waitKey(0)
# detect circles in the image
currentCircles = cv2.HoughCircles(gray, cv2.cv.CV_HOUGH_GRADIENT, 2, 60)
# print 'desiredCircles: ', desiredCircles
# print 'currentCircles: ', currentCircles
FoundThreeCircles = False
if currentCircles is not None:
# convert the (x, y) coordinates and radius of the circles to integers
currentCircles = np.round(currentCircles[0, :]).astype("int")
number_of_circles =3
circle_x = np.zeros(number_of_circles)
circle_y = np.zeros(number_of_circles)
current_centers = np.zeros(number_of_circles*2)
i = 0
for (x, y, r) in currentCircles:
print 'r: ', r
if r < 1100 and r > 30 :
# draw the circle in the output image, then draw a rectangle
# corresponding to the center of the circle
cv2.circle(currentI, (x, y), r, (0, 255, 0), 4)
cv2.rectangle(currentI, (x - 5, y - 5), (x + 5, y + 5), (0, 128, 255), -1)
circle_x[i] = x
circle_y[i] = y
i = i+1
if i == number_of_circles:
FoundThreeCircles = True
break
index = np.argsort(circle_x)
for i in range(0,number_of_circles):
current_centers[2*i] = circle_x[index[i]]
current_centers[2*i+1] = circle_y[index[i]]
print 'desired_centers: ', desired_centers
print 'current_centers: ', current_centers
# plt.figure(1);
# plt.show(plt.imshow(img))
msg = cv_bridge.CvBridge().cv2_to_imgmsg(currentI, encoding="bgr8")
self.pub.publish(msg)
# plt.figure(1);
# plt.show(plt.imshow(img))
# show the output image
cv2.imshow("output", currentI)
# cv2.waitKey(0)
# ratio=0.75
# reprojThresh=4.0
# # print 'desiredkps_npa: ', desiredkps_npa
# # print 'desiredFeatures: ', desiredFeatures
# (matches, H, status) = self.matchKeypoints(desiredkps_npa, detectedkps_npa, desiredFeatures, detectedFeatures, ratio, reprojThresh)
# selected_matches_id = np.where(status != 0)[0]
# if matches is None:
# # print "no marker corners"
# continue
if not FoundThreeCircles:
print "no enough matching"
continue
# selected_matches_id = selected_matches_id[0:3]
# print 'mathces:', len(selected_matches_id)
# print 'desiredkps_npa: ', desiredkps_npa
# print 'status: ', status
# cv2.waitKey(0)
# print desiredkps_npa.shape
# print detectedkps_npa.shape
# print detectedkps_npa
# print 'mathces:', matches
# print 'mathces[1]:', matches[1][0]
# desired_corners = np.zeros(len(selected_matches_id)*2)
# detected_corners = np.zeros(len(selected_matches_id)*2)
# # selected_matches = [(0,0),(0,0),(0,0)]
# for i in range(0,len(selected_matches_id)):
# desired_m = desiredkps_npa[matches[selected_matches_id[i]][1]]
# desired_corners[2*i] = desired_m[0]
# desired_corners[2*i+1] = desired_m[1]
# detected_m = detectedkps_npa[matches[selected_matches_id[i]][0]]
# detected_corners[2*i] = detected_m[0]
# detected_corners[2*i+1] = detected_m[1]
# # selected_matches[i] = matches[selected_matches_id[i]]
# print 'desired_corners: ', desired_corners
# print 'detected_corners: ', detected_corners
# vis = self.drawMatches(desiredImg, currentI, desiredkps_npa, detectedkps_npa, matches, status)
# print 'selected_matches: ', selected_matches
# Don't move if the target hasn't been set
if not self._visual_servo._target_set:
self.set_target(final_camera_depth,desired_centers)
print "the target hasn't been set"
continue
# Get control law velocity and transform to body frame, then send to robot
cam_vel, error = self._visual_servo.get_next_vel(corners = current_centers)
error_norm = np.linalg.norm(error)
self._command_velocity(cam_vel, error_norm)
# print 'servo_vel: ', servo_vel
# print 'before sleep'
# print rospy.is_shutdown()
# r.sleep()
# cv.WaitKey(1000)
# vis = self.drawMatches(desiredImg, currentI, desiredkps_npa, detectedkps_npa, matches, status)
# # vis = -self.drawMatches(desiredImg, currentI, desiredkps_npa, detectedkps_npa, selected_matches, np.ones(3))
# cv2.imshow("Keypoint Matches", vis)
# cv2.waitKey(0)
rospy.sleep(0.8)
def baxter_ik_move(self, limb, rpy_pose):
quaternion_pose = conversions.list_to_pose_stamped(rpy_pose, "base")
node = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
ik_service = rospy.ServiceProxy(node, SolvePositionIK)
ik_request = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id="base")
ik_request.pose_stamp.append(quaternion_pose)
try:
rospy.wait_for_service(node, 5.0)
ik_response = ik_service(ik_request)
except (rospy.ServiceException, rospy.ROSException), error_message:
rospy.logerr("Service request failed: %r" % (error_message,))
sys.exit("ERROR - baxter_ik_move - Failed to append pose")
if ik_response.isValid[0]:
print("PASS: Valid joint configuration found")
# convert response to joint position control dictionary
limb_joints = dict(zip(ik_response.joints[0].name, ik_response.joints[0].position))
# move limb
# if self.limb == limb:
limb_interface = baxter_interface.Limb(limb)
limb_interface.move_to_joint_positions(limb_joints)
print 'moving'
# else:
# self.other_limb_interface.move_to_joint_positions(limb_joints)
else:
# display invalid move message on head display
self.splash_screen("Invalid", "move")
# little point in continuing so exit with error message
print "requested move =", rpy_pose
sys.exit("ERROR - baxter_ik_move - No valid joint configuration found")
# if self.limb == limb: # if working arm
quaternion_pose = limb_interface.endpoint_pose()
position = quaternion_pose['position']
# if working arm remember actual (x,y) position achieved
self.pose = [position[0], position[1], \
self.pose[2], self.pose[3], self.pose[4], self.pose[5]]
class PbvsEih(object):
"""
Performs eye in hand (eih) image based visual servoing (ibvs).
"""
def __init__(self):
# Sets the arm that is used for the servoing
limb='left'
self._baxter = BaxterVS(limb)
self.gripper = baxter_interface.Gripper(limb)
self.gripper.calibrate()
# Initializes the marker that the arm should track
target_marker = 0
# self._apriltag_client = AprilTagClient(target_marker)
self._visual_servo = VisualServoing(False)
# cv2.namedWindow("Detected tags",0)
self.marker_t=None
self.marker_R=None
self.target_obj_name = "expo_dry_erase_board_eraser"
simtrack_topic = "/simtrack/" + self.target_obj_name
print 'simtrack_topic: ', simtrack_topic
self.subscribe_to_simtrack(simtrack_topic)
def get_pose(self, data):
# print 'cam_pose: ', data.pose
(self.marker_t, self.marker_R) = get_t_R(data.pose)
def subscribe_to_simtrack(self, simtrack_topic):
print '\n subscribing'
sub = rospy.Subscriber(simtrack_topic, PoseStamped, self.get_pose)
print '\n end subscribing'
def _main_iter(self):
"""
Runs one instance of the visual servoing control law. Call in a loop.
"""
# key = -2
# image = self._apriltag_client.image
# if image is None:
# print 'no image'
# return
# Draw the ideal position if selected
# if self._visual_servo._target_set:
# print 'target set'
# corners = self._visual_servo._ideal_corners
# for i in range(0,4):
# # Camera intrinsics currently hard coded, tune as needed,
# # or add a subscriber to the camera_info message.
# cv2.circle(image,(int(corners[i*2]*407.19+317.22),int(corners[i*2+1]*407.195786+206.752)),5,(255,0,0),5)
# cv2.imshow("Detected tags",image)
# key = cv2.waitKey(5)
# pose:
# position:
# x: 0.0703210349954
# y: 0.0207515639124
# z: 0.100353679031
# orientation:
# x: 0.0305494043199
# y: 0.996535070586
# z: 0.0757659684332
# w: 0.0156238604684
# position:
# x: 0.125581991683
# y: 0.056925803143
# z: 0.167849532136
# orientation:
# x: 0.00578534021301
# y: 0.999924139472
# z: 0.00808789160022
# w: 0.00726850397684
# pose:
# position:
# x: 0.0798907744327
# y: 0.0393022484119
# z: 0.107615683339
# orientation:
# x: 0.0300291017366
# y: 0.999237092819
# z: 0.0247878443813
# w: 0.00300801503595
Success = False
error_norm = 1
error_y = 1
dist_tol = 0.008
DesiredPose = Pose()
DesiredPose.position.x = .125581991683
DesiredPose.position.y = .056925803143
DesiredPose.position.z = .167849532136
DesiredPose.orientation.x = 0.00578534021301
DesiredPose.orientation.y = 0.999924139472
DesiredPose.orientation.z = 0.00808789160022
DesiredPose.orientation.w = 0.00726850397684
while error_norm > dist_tol or error_y > 0.005 :
# Return if no tag detected
marker_pose = self.marker_t
marker_rot = self.marker_R
# marker_corners = self._apriltag_client.corners
print 'marker_pose: ', marker_pose
# print 'marker_rot: ', marker_rot
if marker_pose is None:
return
# Don't reprocess detections
# if self._visual_servo._target_set:
# self._apriltag_client.marker_t = None
# self._apriltag_client.marker_R = None
# self._apriltag_client.corners = None
# Press any key to set a new target position for the servo control law.
# print 'key: ', key
if not self._visual_servo._target_set: #key !=-1:
rospy.loginfo("Setting new target")
print 'desired_pose: ', marker_pose
# desired_corners = marker_corners
# desired_pose = marker_pose
# desired_rot = marker_rot
(desired_pose, desired_rot) = get_t_R(DesiredPose)
self._visual_servo.set_target(desired_pose,desired_rot)
# self._visual_servo.set_target(desired_pose,desired_rot,desired_corners)
if not self._visual_servo._target_set:
return
# Get control law velocity and transform to body frame, then send to baxter
servo_vel, error = self._visual_servo.get_next_vel(marker_pose,marker_rot)
error_norm = np.linalg.norm(error)/6
print 'error_norm: ', error_norm
# print 'servo_vel: ', servo_vel
baxter_vel = self._baxter.cam_to_body(servo_vel)
# print 'baxter_vel: ', baxter_vel
self._baxter.set_hand_vel(baxter_vel, error_norm)
error_y = np.abs(marker_pose[1]-DesiredPose.position.y)
print 'error_y: ', error_y
rospy.sleep(0.5)
hand_pose = baxter.get_arm_pose('left')
print 'hand_pose:', hand_pose
self.golf_ball_x = hand_pose.position.x # x = front back
self.golf_ball_y = hand_pose.position.y # y = left right
self.golf_ball_z = hand_pose.position.z -0.02 # z = up down
self.orientation = hand_pose.orientation
self.pose = (self.golf_ball_x,
self.golf_ball_y,
self.golf_ball_z,
self.orientation.x,
self.orientation.y,
self.orientation.z,
self.orientation.w)
print 'ready to move'
self.baxter_ik_move('left', self.pose)
self.golf_ball_z = self.golf_ball_z -0.02 # z = up down
self.pose = (self.golf_ball_x,
self.golf_ball_y,
self.golf_ball_z,
self.orientation.x,
self.orientation.y,
self.orientation.z,
self.orientation.w)
print 'ready to move'
self.baxter_ik_move('left', self.pose)
self.golf_ball_z = self.golf_ball_z -0.02 # z = up down
self.pose = (self.golf_ball_x,
self.golf_ball_y,
self.golf_ball_z,
self.orientation.x,
self.orientation.y,
self.orientation.z,
self.orientation.w)
print 'ready to move'
self.baxter_ik_move('left', self.pose)
rospy.sleep(1)
self.gripper.close()
rospy.sleep(1)
hand_pose = baxter.get_arm_pose('left')
self.golf_ball_x = hand_pose.position.x # x = front back
self.golf_ball_y = hand_pose.position.y # y = left right
self.golf_ball_z = hand_pose.position.z+0.03 # z = up down
self.orientation = hand_pose.orientation
self.pose = (self.golf_ball_x,
self.golf_ball_y,
self.golf_ball_z,
self.orientation.x,
self.orientation.y,
self.orientation.z,
self.orientation.w)
print 'ready to move'
self.baxter_ik_move('left', self.pose)
Success = True
return Success
def baxter_ik_move(self, limb, rpy_pose):
quaternion_pose = conversions.list_to_pose_stamped(rpy_pose, "base")
node = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
ik_service = rospy.ServiceProxy(node, SolvePositionIK)
ik_request = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id="base")
ik_request.pose_stamp.append(quaternion_pose)
try:
rospy.wait_for_service(node, 5.0)
ik_response = ik_service(ik_request)
except (rospy.ServiceException, rospy.ROSException), error_message:
rospy.logerr("Service request failed: %r" % (error_message,))
sys.exit("ERROR - baxter_ik_move - Failed to append pose")
if ik_response.isValid[0]:
print("PASS: Valid joint configuration found")
# convert response to joint position control dictionary
limb_joints = dict(zip(ik_response.joints[0].name, ik_response.joints[0].position))
# move limb
# if self.limb == limb:
limb_interface = baxter_interface.Limb(limb)
limb_interface.move_to_joint_positions(limb_joints)
print 'moving'
# else:
# self.other_limb_interface.move_to_joint_positions(limb_joints)
else:
# display invalid move message on head display
# self.splash_screen("Invalid", "move")
# little point in continuing so exit with error message
print "requested move =", rpy_pose
sys.exit("ERROR - baxter_ik_move - No valid joint configuration found")
# if self.limb == limb: # if working arm
quaternion_pose = limb_interface.endpoint_pose()
position = quaternion_pose['position']
# if working arm remember actual (x,y) position achieved
self.pose = [position[0], position[1], \
self.pose[2], self.pose[3], self.pose[4], self.pose[5]]
class PbvsEih(object):
"""
Performs eye in hand (eih) image based visual servoing (ibvs).
"""
def __init__(self):
# Sets the arm that is used for the servoing
limb = 'right'
self._baxter = BaxterVS(limb)
# Initializes the marker that the arm should track
target_marker = 0
self._apriltag_client = AprilTagClient(target_marker)
self._visual_servo = VisualServoing(False)
cv2.namedWindow("Detected tags", 0)
def _main_iter(self):
"""
Runs one instance of the visual servoing control law. Call in a loop.
"""
key = -1
image = self._apriltag_client.image
if image is None:
return
# Draw the ideal position if selected
if self._visual_servo._target_set:
corners = self._visual_servo._ideal_corners
for i in range(0, 4):
# Camera intrinsics currently hard coded, tune as needed,
# or add a subscriber to the camera_info message.
cv2.circle(image,
(int(corners[i * 2] * 407.19 + 317.22),
int(corners[i * 2 + 1] * 407.195786 + 206.752)), 5,
(255, 0, 0), 5)
cv2.imshow("Detected tags", image)
key = cv2.waitKey(5)
# Return if no tag detected
marker_pose = self._apriltag_client.marker_t
marker_rot = self._apriltag_client.marker_R
marker_corners = self._apriltag_client.corners
if marker_pose is None:
return
# Don't reprocess detections
if self._visual_servo._target_set:
self._apriltag_client.marker_t = None
self._apriltag_client.marker_R = None
self._apriltag_client.corners = None
# Press any key to set a new target position for the servo control law.
if key != -1:
rospy.loginfo("Setting new target")
desired_corners = marker_corners
desired_pose = marker_pose
desired_rot = marker_rot
self._visual_servo.set_target(desired_pose, desired_rot,
desired_corners)
if not self._visual_servo._target_set:
return
# Get control law velocity and transform to body frame, then send to baxter
servo_vel = self._visual_servo.get_next_vel(marker_pose, marker_rot)
baxter_vel = self._baxter.cam_to_body(servo_vel)
self._baxter.set_hand_vel(baxter_vel)
class PbvsEih(object):
"""
Performs eye in hand (eih) image based visual servoing (ibvs).
"""
def __init__(self):
# Sets the arm that is used for the servoing
limb='right'
self._baxter = BaxterVS(limb)
# Initializes the marker that the arm should track
target_marker = 0
self._apriltag_client = AprilTagClient(target_marker)
self._visual_servo = VisualServoing(False)
cv2.namedWindow("Detected tags",0)
def _main_iter(self):
"""
Runs one instance of the visual servoing control law. Call in a loop.
"""
key = -1
image = self._apriltag_client.image
if image is None:
return
# Draw the ideal position if selected
if self._visual_servo._target_set:
corners = self._visual_servo._ideal_corners
for i in range(0,4):
# Camera intrinsics currently hard coded, tune as needed,
# or add a subscriber to the camera_info message.
cv2.circle(image,(int(corners[i*2]*407.19+317.22),int(corners[i*2+1]*407.195786+206.752)),5,(255,0,0),5)
cv2.imshow("Detected tags",image)
key = cv2.waitKey(5)
# Return if no tag detected
marker_pose = self._apriltag_client.marker_t
marker_rot = self._apriltag_client.marker_R
marker_corners = self._apriltag_client.corners
if marker_pose is None:
return
# Don't reprocess detections
if self._visual_servo._target_set:
self._apriltag_client.marker_t = None
self._apriltag_client.marker_R = None
self._apriltag_client.corners = None
# Press any key to set a new target position for the servo control law.
if key !=-1:
rospy.loginfo("Setting new target")
desired_corners = marker_corners
desired_pose = marker_pose
desired_rot = marker_rot
self._visual_servo.set_target(desired_pose,desired_rot,desired_corners)
if not self._visual_servo._target_set:
return
# Get control law velocity and transform to body frame, then send to baxter
servo_vel = self._visual_servo.get_next_vel(marker_pose,marker_rot)
baxter_vel = self._baxter.cam_to_body(servo_vel)
self._baxter.set_hand_vel(baxter_vel)
class IbvsEih(object):
"""
Performs eye in hand (eih) image based visual servoing (ibvs).
"""
def __init__(self):
# Baxter specific code. You don't need this if you're using another robot.
# Sets the arm that is used for the servoing
limb='right'
self._baxter = BaxterVS(limb)
# AprilTag specific code. You don't need this if you're using another tracking system.
# Initializes the marker that the arm should track
target_marker = 0
self._apriltag_client = AprilTagClient(target_marker)
self._visual_servo = VisualServoing(ibvs=True)
def new_image_arrived(self):
"""
Boolean to test if a new image has arrived.
"""
if self._apriltag_client.corners is not None:
self._apriltag_client.corners = None
return True
return False
def _get_detected_corners(self):
"""
Returns the most recently detected corners in the image.
"""
# This method currently uses AprilTag detections, so replace with
# your own method if otherwise.
return self._apriltag_client.corners
def _command_velocity(self,vel):
"""
Move the camera at the specified v and omega in vel (6x1 vector).
"""
# This method currently commands the Baxter robot from Rethink Robotics.
#Replace with your own method if using another manipulator.
baxter_vel = self._baxter.cam_to_body(servo_vel)
self._baxter.set_hand_vel(baxter_vel)
def set_target(self,final_camera_depth,desired_corners):
"""
Sets the final camera depth (Z) and desired position for the tracked features
at the goal position.
"""
ideal_cam_pose = np.array([0,0,Z])
self._vision_servo.set_target(ideal_cam_pose,None,desired_corners)
def move_to_position(self,final_camera_depth,desired_corners,dist_tol):
"""
Runs one instance of the visual servoing control law. Call when a new
image has arrived.
"""
self.set_target(final_camera_depth,desired_corners)
r = rospy.Rate(60)
while np.linalg.norm(error)>dist_tol and not rospy.is_shutdown():
if not self.new_image_arrived():
continue
# Continue if no corners detected
marker_corners = self._get_detected_corners()
if marker_corners is None:
continue
# Don't move if the target hasn't been set
if not self._visual_servo._target_set:
continue
# Get control law velocity and transform to body frame, then send to robot
servo_vel = self._visual_servo.get_next_vel(corners = marker_corners)
self._command_velocity(servo_vel)
r.sleep()
class IbvsEih(object):
"""
Performs eye in hand (eih) image based visual servoing (ibvs).
"""
def __init__(self):
# Baxter specific code. You don't need this if you're using another robot.
# Sets the arm that is used for the servoing
limb = 'right'
self._baxter = BaxterVS(limb)
# AprilTag specific code. You don't need this if you're using another tracking system.
# Initializes the marker that the arm should track
target_marker = 0
self._apriltag_client = AprilTagClient(target_marker)
self._visual_servo = VisualServoing(ibvs=True)
def new_image_arrived(self):
"""
Boolean to test if a new image has arrived.
"""
if self._apriltag_client.corners is not None:
self._apriltag_client.corners = None
return True
return False
def _get_detected_corners(self):
"""
Returns the most recently detected corners in the image.
"""
# This method currently uses AprilTag detections, so replace with
# your own method if otherwise.
return self._apriltag_client.corners
def _command_velocity(self, vel):
"""
Move the camera at the specified v and omega in vel (6x1 vector).
"""
# This method currently commands the Baxter robot from Rethink Robotics.
#Replace with your own method if using another manipulator.
baxter_vel = self._baxter.cam_to_body(servo_vel)
self._baxter.set_hand_vel(baxter_vel)
def set_target(self, final_camera_depth, desired_corners):
"""
Sets the final camera depth (Z) and desired position for the tracked features
at the goal position.
"""
ideal_cam_pose = np.array([0, 0, Z])
self._vision_servo.set_target(ideal_cam_pose, None, desired_corners)
def move_to_position(self, final_camera_depth, desired_corners, dist_tol):
"""
Runs one instance of the visual servoing control law. Call when a new
image has arrived.
"""
self.set_target(final_camera_depth, desired_corners)
r = rospy.Rate(60)
while np.linalg.norm(error) > dist_tol and not rospy.is_shutdown():
if not self.new_image_arrived():
continue
# Continue if no corners detected
marker_corners = self._get_detected_corners()
if marker_corners is None:
continue
# Don't move if the target hasn't been set
if not self._visual_servo._target_set:
continue
# Get control law velocity and transform to body frame, then send to robot
servo_vel = self._visual_servo.get_next_vel(corners=marker_corners)
self._command_velocity(servo_vel)
r.sleep()
class IbvsEih(object):
"""
Performs eye in hand (eih) image based visual servoing (ibvs).
"""
def __init__(self):
# Baxter specific code. You don't need this if you're using another robot.
# Sets the arm that is used for the servoing
limb='left'
self._baxter = BaxterVS(limb)
self.gripper = baxter_interface.Gripper(limb)
self.gripper.calibrate()
self.golf_ball_x = 0.85 # x = front back
self.golf_ball_y = 0.10 # y = left right
self.golf_ball_z = 0.0 # z = up down
self.roll = -1.0 * math.pi # roll = horizontal
self.pitch = 0.0 * math.pi # pitch = vertical
self.yaw = 0.0 * math.pi # yaw = rotation
self.pose = (self.golf_ball_x,
self.golf_ball_y,
self.golf_ball_z,
self.roll,
self.pitch,
self.yaw)
print 'ready to move'
self.baxter_ik_move(limb, self.pose)
# self.golf_ball_x = self.golf_ball_x + 0.010
# # self.golf_ball_y = self.golf_ball_y - 0.10
# # self.golf_ball_z = self.golf_ball_z + 0.10
# self.pitch = self.pitch - 0.2 * math.pi
# self.pose = (self.golf_ball_x,
# self.golf_ball_y,
# self.golf_ball_z,
# self.roll,
# self.pitch,
# self.yaw)
# print 'ready to move'
# self.baxter_ik_move(limb, self.pose)
# AprilTag specific code. You don't need this if you're using another tracking system.
# Initializes the marker that the arm should track
target_marker = 0
# self._apriltag_client = AprilTagClient(target_marker)
self._visual_servo = VisualServoing(ibvs=True)
# camera parameters (NB. other parameters in open_camera)
self.cam_calib = 0.0025 # meters per pixel at 1 meter
self.cam_x_offset = -0.01 # camera gripper offset
self.cam_y_offset = -0.035
self.width = 960 # Camera resolution
self.height = 600
# callback image
self.cv_image = cv.CreateImage((self.width, self.height), 8, 3)
# create image publisher to head monitor
self.pub = rospy.Publisher('/robot/xdisplay', Image, latch=True)
self.subscribe_to_camera(limb)
def new_image_arrived(self):
"""
Boolean to test if a new image has arrived.
"""
print "waiting for new image"
if self._apriltag_client.corners is not None:
self._apriltag_client.corners = None
return True
return False
# def _get_detected_corners(self):
# """
# Returns the most recently detected corners in the image.
# """
# # This method currently uses AprilTag detections, so replace with
# # your own method if otherwise.
# # print "looking for corners"
# return self._apriltag_client.corners
def _command_velocity(self,cam_vel,error_norm):
"""
Move the camera at the specified v and omega in vel (6x1 vector).
"""
# This method currently commands the Baxter robot from Rethink Robotics.
#Replace with your own method if using another manipulator.
hand_vel_in_base = self._baxter.cam_to_body(cam_vel)
# print 'baxter_vel: ', baxter_vel
self._baxter.set_hand_vel(hand_vel_in_base, error_norm)
def set_target(self,final_camera_depth,desired_corners):
"""
Sets the final camera depth (Z) and desired position for the tracked features
at the goal position.
"""
Z = final_camera_depth
ideal_cam_pose = np.array([0,0,Z])
# print 'ideal_cam_pose: ', ideal_cam_pose
self._visual_servo.set_target(ideal_cam_pose,None,desired_corners)
def move_to_position(self,final_camera_depth,desiredImg, desiredkps, desiredkps_npa, desiredFeatures,dist_tol):
"""
Runs one instance of the visual servoing control law. Call when a new
image has arrived.
"""
# self.set_target(final_camera_depth,desired_corners)
# r = rospy.Rate(60)
error_norm = 1000
hand_pose = baxter.get_arm_pose('left')
count = 1
while error_norm>dist_tol and not rospy.is_shutdown() and not hand_pose.position.z < -0.371:
# if not self.new_image_arrived():
# # print " no new_image_arrived"
# continue
# Continue if no corners detected
# marker_corners = self._get_detected_corners()
# img_array = cv2array(self.cv_image)
# print self.cv_image.shape
# cv.WaitKey(1000)
currentI = self.cv_image
(detectedkps, detectedkps_npa, detectedFeatures) = self.detectAndDescribe(currentI)
img=cv2.drawKeypoints(currentI,detectedkps)
# plt.figure(1);
# plt.show(plt.imshow(img))
# plt.figure(1);
# plt.show(plt.imshow(img))
ratio=0.75
reprojThresh=4.0
# print 'desiredkps_npa: ', desiredkps_npa
# print 'desiredFeatures: ', desiredFeatures
(matches, H, status) = self.matchKeypoints(desiredkps_npa, detectedkps_npa, desiredFeatures, detectedFeatures, ratio, reprojThresh)
selected_matches_id = np.where(status != 0)[0]
# if matches is None:
# # print "no marker corners"
# continue
if len(selected_matches_id) < 4:
print "no enough matching"
continue
# selected_matches_id = selected_matches_id[0:3]
print 'mathces:', len(selected_matches_id)
# print 'desiredkps_npa: ', desiredkps_npa
# print 'status: ', status
# cv2.waitKey(0)
# print desiredkps_npa.shape
# print detectedkps_npa.shape
# print detectedkps_npa
# print 'mathces:', matches
# print 'mathces[1]:', matches[1][0]
desired_corners = np.zeros(len(selected_matches_id)*2)
detected_corners = np.zeros(len(selected_matches_id)*2)
# selected_matches = [(0,0),(0,0),(0,0)]
for i in range(0,len(selected_matches_id)):
desired_m = desiredkps_npa[matches[selected_matches_id[i]][1]]
desired_corners[2*i] = desired_m[0]
desired_corners[2*i+1] = desired_m[1]
detected_m = detectedkps_npa[matches[selected_matches_id[i]][0]]
detected_corners[2*i] = detected_m[0]
detected_corners[2*i+1] = detected_m[1]
# selected_matches[i] = matches[selected_matches_id[i]]
# print 'desired_corners: ', desired_corners
# print 'detected_corners: ', detected_corners
# vis = self.drawMatches(desiredImg, currentI, desiredkps_npa, detectedkps_npa, matches, status)
# print 'selected_matches: ', selected_matches
self.set_target(final_camera_depth,desired_corners)
# Don't move if the target hasn't been set
if not self._visual_servo._target_set:
print "the target hasn't been set"
continue
# Get control law velocity and transform to body frame, then send to robot
cam_vel, error = self._visual_servo.get_next_vel(corners = detected_corners)
error_norm = np.linalg.norm(error)/(len(detected_corners)/2)
self._command_velocity(cam_vel, error_norm)
# print 'servo_vel: ', servo_vel
# print 'before sleep'
# print 'error: ', np.linalg.norm(error)
# print rospy.is_shutdown()
# r.sleep()
# cv.WaitKey(1000)
vis = self.drawMatches(desiredImg, currentI, desiredkps_npa, detectedkps_npa, matches, status)
# vis = -self.drawMatches(desiredImg, currentI, desiredkps_npa, detectedkps_npa, selected_matches, np.ones(3))
cv2.imshow("Keypoint Matches", vis)
filename = '/home/pracsys/shaojun/visual_servoing_ws/'+str(count) +'.jpg'
cv2.imwrite(filename, vis)
# cv2.waitKey(0)
msg = cv_bridge.CvBridge().cv2_to_imgmsg(vis, encoding="bgr8")
self.pub.publish(msg)
rospy.sleep(0.8)
print 'count: ', count
count = count + 1
hand_pose = baxter.get_arm_pose('left')
print 'hand_pose:', hand_pose
self.golf_ball_x = hand_pose.position.x + 0.05 # x = front back
self.golf_ball_y = hand_pose.position.y # y = left right
self.golf_ball_z = hand_pose.position.z # z = up down
self.orientation = hand_pose.orientation
self.pose = (self.golf_ball_x,
self.golf_ball_y,
self.golf_ball_z,
self.orientation.x,
self.orientation.y,
self.orientation.z,
self.orientation.w)
print
print 'ready to move'
self.baxter_ik_move('left', self.pose)
time.sleep(1)
self.gripper.close()
time.sleep(1)
hand_pose = baxter.get_arm_pose('left')
self.golf_ball_x = hand_pose.position.x # x = front back
self.golf_ball_y = hand_pose.position.y # y = left right
self.golf_ball_z = hand_pose.position.z+0.03 # z = up down
self.orientation = hand_pose.orientation
self.pose = (self.golf_ball_x,
self.golf_ball_y,
self.golf_ball_z,
self.orientation.x,
self.orientation.y,
self.orientation.z,
self.orientation.w)
print 'ready to move'
self.baxter_ik_move('left', self.pose)
hand_pose = baxter.get_arm_pose('left')
self.golf_ball_x = hand_pose.position.x-0.15 # x = front back
self.golf_ball_y = hand_pose.position.y # y = left right
self.golf_ball_z = hand_pose.position.z # z = up down
self.orientation = hand_pose.orientation
self.pose = (self.golf_ball_x,
self.golf_ball_y,
self.golf_ball_z,
self.orientation.x,
self.orientation.y,
self.orientation.z,
self.orientation.w)
print
print 'ready to move'
self.baxter_ik_move('left', self.pose)
def baxter_ik_move(self, limb, rpy_pose):
quaternion_pose = conversions.list_to_pose_stamped(rpy_pose, "base")
node = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
ik_service = rospy.ServiceProxy(node, SolvePositionIK)
ik_request = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id="base")
ik_request.pose_stamp.append(quaternion_pose)
try:
rospy.wait_for_service(node, 5.0)
ik_response = ik_service(ik_request)
except (rospy.ServiceException, rospy.ROSException), error_message:
rospy.logerr("Service request failed: %r" % (error_message,))
sys.exit("ERROR - baxter_ik_move - Failed to append pose")
if ik_response.isValid[0]:
print("PASS: Valid joint configuration found")
# convert response to joint position control dictionary
limb_joints = dict(zip(ik_response.joints[0].name, ik_response.joints[0].position))
# move limb
# if self.limb == limb:
limb_interface = baxter_interface.Limb(limb)
limb_interface.move_to_joint_positions(limb_joints)
print 'moving'
# else:
# self.other_limb_interface.move_to_joint_positions(limb_joints)
else:
# display invalid move message on head display
# self.splash_screen("Invalid", "move")
# little point in continuing so exit with error message
print "requested move =", rpy_pose
sys.exit("ERROR - baxter_ik_move - No valid joint configuration found")
# if self.limb == limb: # if working arm
quaternion_pose = limb_interface.endpoint_pose()
position = quaternion_pose['position']
# if working arm remember actual (x,y) position achieved
self.pose = [position[0], position[1], \
self.pose[2], self.pose[3], self.pose[4], self.pose[5]]
