class track():
def __init__(self):
self.centerx = 365
self.centery = 120
self.coefx = 0.1/(526-369)
self.coefy = 0.1/(237-90)
self.count = 0
self.hdr = Header(stamp=rospy.Time.now(), frame_id='base')
self.gripper = Gripper("right")
self.gripper.calibrate()
self.gripper.set_moving_force(0.1)
rospy.sleep(0.5)
self.gripper.set_holding_force(0.1)
rospy.sleep(0.5)
self.busy = False
self.gripper_distance = 100
self.subscribe_gripper()
self.robotx = -1
self.roboty = -1
self.framenumber = 0
self.history = np.arange(0,20)*-1
self.newPosition = True
self.bowlcamera = None
self.kin = baxter_kinematics('right')
self.J = self.kin.jacobian()
self.J_inv = self.kin.jacobian_pseudo_inverse()
self.jointVelocity = np.asarray([1,2,3,4,5,6,7],np.float32)
self.control_arm = Limb("right")
self.control_joint_names = self.control_arm.joint_names()
self.dx = 0
self.dy = 0
self.distance = 1000
self.finish = False
self.found = 0
ifr = rospy.Subscriber("/robot/range/right_hand_range/state", Range, self._read_distance)
def _read_distance(self,data):
self.distance = data.range
def vision_request_right(self, controlID, requestID):
try:
rospy.wait_for_service('vision_server_vertical')
vision_server_req = rospy.ServiceProxy('vision_server_vertical', VisionVertical)
return vision_server_req(controlID, requestID)
except (rospy.ServiceException,rospy.ROSInterruptException), e:
print "Service call failed: %s" % e
self.clean_shutdown_service()
class TrajectoryWaypointsRecorder():
def __init__(self, limb_name, file_name):
self.limb = Limb(limb_name)
self.trajectory = []
self.file_name = file_name
self.file_header = self.limb.joint_names()
self.dt = rospy.Duration(secs=2)
self.previous_time = rospy.Time.now()
self.limb_nav = Navigator(limb_name)
self.limb_nav.button2_changed.connect(self.limb_nav_button_pressed)
rospy.on_shutdown(self.save_data)
rospy.loginfo('Press Ctrl + C to exit')
rospy.spin()
def save_data(self):
file_header = ','.join(x for x in self.file_header)
# src: http://wiki.ros.org/Packages#Client_Library_Support
full_path = os.path.join(
rospkg.RosPack().get_path('multiple_kinect_baxter_calibration'),
'files', self.file_name)
np.savetxt(full_path,
self.trajectory,
header=file_header,
delimiter=',',
fmt='%.4f',
comments='')
rospy.loginfo("Trajectory have been successfully saved to \n'%s'\n" %
full_path)
def limb_nav_button_pressed(self, state):
now = rospy.Time.now()
if (now - self.previous_time > self.dt) and state:
self.limb_nav.inner_led = state
self.record()
elif not state:
self.limb_nav.inner_led = state
self.previous_time = now
def record(self):
joint_angles_dict = self.limb.joint_angles()
joint_angles = [
joint_angles_dict[joint_name] for joint_name in self.file_header
]
self.trajectory.append(joint_angles)
rospy.loginfo('%d samples collected.' % len(self.trajectory))
#rospy.sleep(rospy.Duration(5,0))
rs = baxter_interface.RobotEnable(CHECK_VERSION)
rs.enable()
right_gripper = Gripper('right')
right_limb = Limb('right')
right_gripper.calibrate()
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
print "opening gripper"
right_gripper.open()
current_angles = [
right_limb.joint_angle(joint) for joint in right_limb.joint_names()
]
orient_quaternion_components = quaternion_from_euler(
math.pi, 0, math.pi / 2)
orient_down = Quaternion()
orient_down.x = orient_quaternion_components[0]
orient_down.y = orient_quaternion_components[1]
orient_down.z = orient_quaternion_components[2]
orient_down.w = orient_quaternion_components[3]
highPose = PoseStamped(header=hdr,
pose=Pose(position=Point(0, -0.428, -0.57),
orientation=orient_down))
gripPose = PoseStamped(header=hdr,
pose=Pose(position=Point(0, -0.428, -0.71),
class track():
def __init__(self):
self.centerx = 365
self.centery = 120
self.coefx = 0.1/(526-369)
self.coefy = 0.1/(237-90)
self.count = 0
self.hdr = Header(stamp=rospy.Time.now(), frame_id='base')
self.gripper_left = Gripper("left")
self.gripper_left.calibrate()
self.gripper_left.set_moving_force(0.01)
rospy.sleep(0.5)
self.gripper_left.set_holding_force(0.01)
rospy.sleep(0.5)
self.gripper_right = Gripper("right")
self.gripper_right.calibrate()
rospy.sleep(1)
self.busy = False
self.gripper_distance = 100
self.subscribe_gripper()
self.robotx = -1
self.roboty = -1
self.framenumber = 0
self.history = np.arange(0,20)*-1
self.newPosition = True
self.bowlcamera = None
self.kin_right = baxter_kinematics('right')
self.kin_left = baxter_kinematics('left')
self.J = self.kin_right.jacobian()
self.J_inv = self.kin_right.jacobian_pseudo_inverse()
self.jointVelocity = np.asarray([1,2,3,4,5,6,7],np.float32)
self.control_arm = Limb("right")
self.left_arm = Limb("left")
self.control_joint_names = self.control_arm.joint_names()
self.dx = 0
self.dy = 0
self.distance = 1000
self.finish = False
self.found = 0
self.pour_x = 0
self.pour_y = 0
ifr = rospy.Subscriber("/robot/range/right_hand_range/state", Range, self._read_distance)
self.joint_states = {
'observe':{
'right_e0': -0.631,
'right_e1': 0.870,
'right_s0': 0.742,
'right_s1': -0.6087,
'right_w0': 0.508,
'right_w1': 1.386,
'right_w2': -0.5538,
},
'observe_ladle':{
'right_e0': -0.829,
'right_e1': 0.831,
'right_s0': 0.678,
'right_s1': -0.53,
'right_w0': 0.716,
'right_w1': 1.466,
'right_w2': -0.8099,
},
'observe_left':{
'left_w0': 2.761932405432129,
'left_w1': -1.5700293346069336,
'left_w2': -0.8893253607604981,
'left_e0': -0.9805972175354004,
'left_e1': 1.8300390778564455,
'left_s0': 1.4783739826354982,
'left_s1': -0.9503010970092775,
},
'stir':{
'right_e0': -0.179,
'right_e1': 1.403,
'right_s0': 0.381,
'right_s1': -0.655,
'right_w0': 1.3,
'right_w1': 2.04,
'right_w2': 0.612,
},
'observe_vertical':{
'right_e0': 0.699,
'right_e1': 1.451,
'right_s0': -1.689,
'right_s1': 0.516,
'right_w0': 0.204,
'right_w1': 0.935,
'right_w2': -2.706,
},
'observe_midpoint':{
'right_e0': -0.606,
'right_e1': 0.968,
'right_s0': 0.0,
'right_s1': -0.645,
'right_w0': 0.465,
'right_w1': 1.343,
'right_w2': -0.437,
},
'dressing':{
'right_e0': 0.967,
'right_e1': 1.386,
'right_s0': -0.348,
'right_s1': -0.155,
'right_w0': 0.264,
'right_w1': 1.521,
'right_w2': -2.199,
},
}
def _read_distance(self,data):
self.distance = data.range
def vision_request_right(self, controlID, requestID):
try:
rospy.wait_for_service('vision_server_vertical')
vision_server_req = rospy.ServiceProxy('vision_server_vertical', VisionVertical)
return vision_server_req(controlID, requestID)
except (rospy.ServiceException,rospy.ROSInterruptException), e:
print "Service call failed: %s" % e
self.clean_shutdown_service()
class track():
def __init__(self):
self.centerx = 365
self.centery = 120
self.coefx = 0.1/(526-369)
self.coefy = 0.1/(237-90)
self.count = 0
self.hdr = Header(stamp=rospy.Time.now(), frame_id='base')
self.gripper = Gripper("right")
self.gripper.calibrate()
rospy.sleep(2)
# self.gripper.set_moving_force(0.1)
# rospy.sleep(0.5)
# self.gripper.set_holding_force(0.1)
# rospy.sleep(0.5)
self.busy = False
self.gripper_distance = 100
self.subscribe_gripper()
self.robotx = -1
self.roboty = -1
self.framenumber = 0
self.history = np.arange(0,20)*-1
self.newPosition = True
self.bowlcamera = None
self.kin = baxter_kinematics('right')
self.J = self.kin.jacobian()
self.J_inv = self.kin.jacobian_pseudo_inverse()
self.jointVelocity = np.asarray([1,2,3,4,5,6,7],np.float32)
self.control_arm = Limb("right")
self.control_joint_names = self.control_arm.joint_names()
self.dx = 0
self.dy = 0
self.distance = 1000
self.finish = False
self.found = 0
ifr = rospy.Subscriber("/robot/range/right_hand_range/state", Range, self._read_distance)
self.joint_states = {
# 'observe':{
# 'right_e0': -0.365,
# 'right_e1': 1.061,
# 'right_s0': 0.920,
# 'right_s1': -0.539,
# 'right_w0': 0.350,
# 'right_w1': 1.105,
# 'right_w2': -0.221,
# },
'observe':{
'right_e0': -0.631,
'right_e1': 0.870,
'right_s0': 0.742,
'right_s1': -0.6087,
'right_w0': 0.508,
'right_w1': 1.386,
'right_w2': -0.5538,
},
'observe_vertical':{
'right_e0': 0.699,
'right_e1': 1.451,
'right_s0': -1.689,
'right_s1': 0.516,
'right_w0': 0.204,
'right_w1': 0.935,
'right_w2': -2.706,
},
'observe_ladle':{
'right_e0': -0.829,
'right_e1': 0.831,
'right_s0': 0.678,
'right_s1': -0.53,
'right_w0': 0.716,
'right_w1': 1.466,
'right_w2': -0.8099,
},
}
def _read_distance(self,data):
self.distance = data.range
def vision_request_right(self, controlID, requestID):
try:
rospy.wait_for_service('vision_server_vertical')
vision_server_req = rospy.ServiceProxy('vision_server_vertical', VisionVertical)
return vision_server_req(controlID, requestID)
except (rospy.ServiceException,rospy.ROSInterruptException), e:
print "Service call failed: %s" % e
self.clean_shutdown_service()
class track():
def __init__(self):
self.centerx = 365
self.centery = 120
self.coefx = 0.1/(526-369)
self.coefy = 0.1/(237-90)
self.count = 0
self.hdr = Header(stamp=rospy.Time.now(), frame_id='base')
self.gripper = Gripper("right")
self.gripper.calibrate()
rospy.sleep(2)
# self.gripper.set_moving_force(0.1)
# rospy.sleep(0.5)
# self.gripper.set_holding_force(0.1)
# rospy.sleep(0.5)
self.busy = False
self.gripper_distance = 100
self.subscribe_gripper()
self.robotx = -1
self.roboty = -1
self.framenumber = 0
self.history = np.arange(0,20)*-1
self.newPosition = True
self.bowlcamera = None
self.kin = baxter_kinematics('right')
self.J = self.kin.jacobian()
self.J_inv = self.kin.jacobian_pseudo_inverse()
self.jointVelocity = np.asarray([1,2,3,4,5,6,7],np.float32)
self.control_arm = Limb("right")
self.control_joint_names = self.control_arm.joint_names()
self.dx = 0
self.dy = 0
self.distance = 1000
self.finish = False
self.found = 0
ifr = rospy.Subscriber("/robot/range/right_hand_range/state", Range, self._read_distance)
self.joint_states = {
# 'observe':{
# 'right_e0': -0.365,
# 'right_e1': 1.061,
# 'right_s0': 0.920,
# 'right_s1': -0.539,
# 'right_w0': 0.350,
# 'right_w1': 1.105,
# 'right_w2': -0.221,
# },
'observe':{
'right_e0': -0.631,
'right_e1': 0.870,
'right_s0': 0.742,
'right_s1': -0.6087,
'right_w0': 0.508,
'right_w1': 1.386,
'right_w2': -0.5538,
},
'observe_vertical':{
'right_e0': 0.699,
'right_e1': 1.451,
'right_s0': -1.689,
'right_s1': 0.516,
'right_w0': 0.204,
'right_w1': 0.935,
'right_w2': -2.706,
},
'observe_ladle':{
'right_e0': -0.829,
'right_e1': 0.831,
'right_s0': 0.678,
'right_s1': -0.53,
'right_w0': 0.716,
'right_w1': 1.466,
'right_w2': -0.8099,
},
}
def _read_distance(self,data):
self.distance = data.range
def vision_request_right(self, controlID, requestID):
try:
rospy.wait_for_service('vision_server_vertical')
vision_server_req = rospy.ServiceProxy('vision_server_vertical', VisionVertical)
return vision_server_req(controlID, requestID)
except (rospy.ServiceException,rospy.ROSInterruptException), e:
print "Service call failed: %s" % e
self.clean_shutdown_service()
_rate = 500.0
#rospy.sleep(rospy.Duration(5,0))
rs = baxter_interface.RobotEnable(CHECK_VERSION)
rs.enable()
right_gripper = Gripper('right');
right_limb = Limb('right');
right_gripper.calibrate()
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
print "opening gripper"
right_gripper.open()
current_angles = [right_limb.joint_angle(joint) for joint in right_limb.joint_names()]
orient_quaternion_components = quaternion_from_euler(math.pi, 0,math.pi/2)
orient_down = Quaternion()
orient_down.x = orient_quaternion_components[0]
orient_down.y = orient_quaternion_components[1]
orient_down.z = orient_quaternion_components[2]
orient_down.w = orient_quaternion_components[3]
twist_quaternion_components = quaternion_from_euler(math.pi, 0, -math.pi/4)
twist_down = Quaternion()
twist_down.x = twist_quaternion_components[0]
twist_down.y = twist_quaternion_components[1]
twist_down.z = twist_quaternion_components[2]
twist_down.w = twist_quaternion_components[3]
class vision_server():
def __init__(self):
rospy.init_node('vision_server_right')
self.pub = rospy.Publisher('/robot/xdisplay', Image, latch=True)
self.busy = False
self.dx = 0
self.dy = 0
self.avg_dx = -1
self.avg_dy = -1
self.H = homography()
self.framenumber = 0
self.history_x = np.arange(0,10)*-1
self.history_y = np.arange(0,10)*-1
self.bowlcamera = None
self.newPosition = True
self.foundBowl = 0
self.centerx = 400
#self.centerx = 250
self.centery = 150
#self.centery = 190
self.coefx = 0.1/(526-369)
self.coefy = 0.1/(237-90)
self.v_joint = np.arange(7)
self.v_end = np.arange(6)
self.cmd = {}
self.found = False
self.finish = 0
self.distance = 10
self.gripper = Gripper("right")
#self.gripper.calibrate()
cv2.namedWindow('image')
self.np_image = np.zeros((400,640,3), np.uint8)
cv2.imshow('image',self.np_image)
#self._set_threshold()
s = rospy.Service('vision_server_vertical', Vision, self.handle_vision_req)
camera_topic = '/cameras/right_hand_camera/image'
self.right_camera = rospy.Subscriber(camera_topic, Image, self._on_camera)
ifr = rospy.Subscriber("/robot/range/right_hand_range/state", Range, self._read_distance)
print "\nReady to use right hand vision server\n"
self.kin = baxter_kinematics('right')
self.J = self.kin.jacobian()
self.J_inv = self.kin.jacobian_pseudo_inverse()
self.jointVelocity = np.asarray([1,2,3,4,5,6,7],np.float32)
self.control_arm = Limb("right")
self.control_joint_names = self.control_arm.joint_names()
#print np.dot(self.J,self.jointVelocity)
def _read_distance(self,data):
self.distance = data.range
def _set_threshold(self):
cv2.createTrackbar('Min R(H)','image',0,255,nothing)
cv2.createTrackbar('Max R(H)','image',255,255,nothing)
cv2.createTrackbar('Min G(S)','image',0,255,nothing)
cv2.createTrackbar('Max G(S)','image',255,255,nothing)
cv2.createTrackbar('Min B(V)','image',0,255,nothing)
cv2.createTrackbar('Max B(V)','image',255,255,nothing)
def get_joint_velocity(self):
cmd = {}
velocity_list = np.asarray([1,2,3,4,5,6,7],np.float32)
for idx, name in enumerate(self.control_joint_names):
v = self.control_arm.joint_velocity( self.control_joint_names[idx])
velocity_list[idx] = v
cmd[name] = v
return velocity_list
def list_to_dic(self,ls):
cmd = {}
for idx, name in enumerate(self.control_joint_names):
v = ls.item(idx)
cmd[name] = v
return cmd
def PID(self):
Kp = 0.5
vy = -Kp*self.dx
vx = -Kp*self.dy
return vx,vy
def _on_camera(self, data):
self.busy = True
self.framenumber += 1
index = self.framenumber % 10
cv2.namedWindow('image')
cv_image = cv_bridge.CvBridge().imgmsg_to_cv(data, "bgr8")
np_image = np.asarray(cv_image)
image_after_process, mask = self.image_process(np_image)
self.history_x[index] = self.dx*100
self.history_y[index] = self.dy*100
self.avg_dx = np.average(self.history_x)/100
self.avg_dy = np.average(self.history_y)/100
# if abs(self.dx)<0.01 and abs(self.dy)<0.01:
# self.found = True
# else:
# self.found = False
vx, vy = self.PID()
self.v_end = np.asarray([(1-((abs(vx)+abs(vy))*5))*0.03,vy,vx,0,0,0],np.float32)
#self.v_end = np.asarray([0,0,0,0,0,0.05],np.float32)
self.v_joint = np.dot(self.J_inv,self.v_end)
self.cmd = self.list_to_dic(self.v_joint)
self.busy = False
cv2.imshow('image',image_after_process)
cv2.waitKey(1)
def image_process(self,img):
# min_r = cv2.getTrackbarPos('Min R(H)','image')
# max_r = cv2.getTrackbarPos('Max R(H)','image')
# min_g = cv2.getTrackbarPos('Min G(S)','image')
# max_g = cv2.getTrackbarPos('Max G(S)','image')
# min_b = cv2.getTrackbarPos('Min B(V)','image')
# max_b = cv2.getTrackbarPos('Max B(V)','image')
# min_r = 0
# max_r = 57
# min_g = 87
# max_g = 181
# min_b = 37
# max_b = 70
min_r = 0
max_r = 58
min_g = 86
max_g = 255
min_b = 0
max_b = 255
min_color = (min_r, min_g, min_b)
max_color = (max_r, max_g, max_b)
#img = cv2.flip(img, flipCode = -1)
hsv_img = cv2.cvtColor(img,cv2.COLOR_BGR2HSV)
mask = cv2.inRange(hsv_img, min_color, max_color)
result = cv2.bitwise_and(img, img, mask = mask)
mask_bool = np.asarray(mask, bool)
label_img = morphology.remove_small_objects(mask_bool, 1000, connectivity = 1)
objects = measure.regionprops(label_img)
if objects != []:
self.found = True
target = objects[0]
box = target.bbox
cv2.rectangle(img,(box[1],box[0]),(box[3],box[2]),(0,255,0),3)
dx_pixel=target.centroid[1]-self.centerx
dy_pixel=target.centroid[0]-self.centery
dx=dx_pixel*self.coefx
dy=dy_pixel*self.coefy
angle = target.orientation
cv2.circle(img,(int(target.centroid[1]),int(target.centroid[0])),10,(0,0,255),-1)
self.dx = dx
self.dy = dy
#print self.dx,self.dy
else:
self.found = False
self.dx = 0
self.dy = 0
return img, mask_bool
def handle_vision_req(self, req):
#resp = VisionResponse(req.requestID, 0, self.avg_dx, self.avg_dy)
self.finish = 0
if self.found == True:
if self.distance > 0.07:
self.control_arm.set_joint_velocities(self.cmd)
#rospy.sleep(0.02)
else:
self.finish = 1
resp = VisionResponse(req.requestID, self.finish, self.dx, self.dy)
return resp
def clean_shutdown(self):
print "Server finished"
cv2.imwrite('box_img.png', self.blank_image)
rospy.signal_shutdown("Done")
sys.exit()
class vision_server():
def __init__(self):
rospy.init_node('vision_server_right')
self.pub = rospy.Publisher('/robot/xdisplay', Image, latch=True)
self.busy = False
self.dx = 0
self.dy = 0
self.avg_dx = -1
self.avg_dy = -1
self.H = homography()
self.framenumber = 0
self.history_x = np.arange(0, 10) * -1
self.history_y = np.arange(0, 10) * -1
self.bowlcamera = None
self.newPosition = True
self.foundBowl = 0
self.centerx = 400
#self.centerx = 250
self.centery = 150
#self.centery = 190
self.coefx = 0.1 / (526 - 369)
self.coefy = 0.1 / (237 - 90)
self.v_joint = np.arange(7)
self.v_end = np.arange(6)
self.cmd = {}
self.found = False
self.finish = 0
self.distance = 10
self.gripper = Gripper("right")
#self.gripper.calibrate()
cv2.namedWindow('image')
self.np_image = np.zeros((400, 640, 3), np.uint8)
cv2.imshow('image', self.np_image)
#self._set_threshold()
s = rospy.Service('vision_server_vertical', Vision,
self.handle_vision_req)
camera_topic = '/cameras/right_hand_camera/image'
self.right_camera = rospy.Subscriber(camera_topic, Image,
self._on_camera)
ifr = rospy.Subscriber("/robot/range/right_hand_range/state", Range,
self._read_distance)
print "\nReady to use right hand vision server\n"
self.kin = baxter_kinematics('right')
self.J = self.kin.jacobian()
self.J_inv = self.kin.jacobian_pseudo_inverse()
self.jointVelocity = np.asarray([1, 2, 3, 4, 5, 6, 7], np.float32)
self.control_arm = Limb("right")
self.control_joint_names = self.control_arm.joint_names()
#print np.dot(self.J,self.jointVelocity)
def _read_distance(self, data):
self.distance = data.range
def _set_threshold(self):
cv2.createTrackbar('Min R(H)', 'image', 0, 255, nothing)
cv2.createTrackbar('Max R(H)', 'image', 255, 255, nothing)
cv2.createTrackbar('Min G(S)', 'image', 0, 255, nothing)
cv2.createTrackbar('Max G(S)', 'image', 255, 255, nothing)
cv2.createTrackbar('Min B(V)', 'image', 0, 255, nothing)
cv2.createTrackbar('Max B(V)', 'image', 255, 255, nothing)
def get_joint_velocity(self):
cmd = {}
velocity_list = np.asarray([1, 2, 3, 4, 5, 6, 7], np.float32)
for idx, name in enumerate(self.control_joint_names):
v = self.control_arm.joint_velocity(self.control_joint_names[idx])
velocity_list[idx] = v
cmd[name] = v
return velocity_list
def list_to_dic(self, ls):
cmd = {}
for idx, name in enumerate(self.control_joint_names):
v = ls.item(idx)
cmd[name] = v
return cmd
def PID(self):
Kp = 0.5
vy = -Kp * self.dx
vx = -Kp * self.dy
return vx, vy
def _on_camera(self, data):
self.busy = True
self.framenumber += 1
index = self.framenumber % 10
cv2.namedWindow('image')
cv_image = cv_bridge.CvBridge().imgmsg_to_cv(data, "bgr8")
np_image = np.asarray(cv_image)
image_after_process, mask = self.image_process(np_image)
self.history_x[index] = self.dx * 100
self.history_y[index] = self.dy * 100
self.avg_dx = np.average(self.history_x) / 100
self.avg_dy = np.average(self.history_y) / 100
# if abs(self.dx)<0.01 and abs(self.dy)<0.01:
# self.found = True
# else:
# self.found = False
vx, vy = self.PID()
self.v_end = np.asarray(
[(1 - ((abs(vx) + abs(vy)) * 5)) * 0.03, vy, vx, 0, 0, 0],
np.float32)
#self.v_end = np.asarray([0,0,0,0,0,0.05],np.float32)
self.v_joint = np.dot(self.J_inv, self.v_end)
self.cmd = self.list_to_dic(self.v_joint)
self.busy = False
cv2.imshow('image', image_after_process)
cv2.waitKey(1)
def image_process(self, img):
# min_r = cv2.getTrackbarPos('Min R(H)','image')
# max_r = cv2.getTrackbarPos('Max R(H)','image')
# min_g = cv2.getTrackbarPos('Min G(S)','image')
# max_g = cv2.getTrackbarPos('Max G(S)','image')
# min_b = cv2.getTrackbarPos('Min B(V)','image')
# max_b = cv2.getTrackbarPos('Max B(V)','image')
# min_r = 0
# max_r = 57
# min_g = 87
# max_g = 181
# min_b = 37
# max_b = 70
min_r = 0
max_r = 58
min_g = 86
max_g = 255
min_b = 0
max_b = 255
min_color = (min_r, min_g, min_b)
max_color = (max_r, max_g, max_b)
#img = cv2.flip(img, flipCode = -1)
hsv_img = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
mask = cv2.inRange(hsv_img, min_color, max_color)
result = cv2.bitwise_and(img, img, mask=mask)
mask_bool = np.asarray(mask, bool)
label_img = morphology.remove_small_objects(mask_bool,
1000,
connectivity=1)
objects = measure.regionprops(label_img)
if objects != []:
self.found = True
target = objects[0]
box = target.bbox
cv2.rectangle(img, (box[1], box[0]), (box[3], box[2]), (0, 255, 0),
3)
dx_pixel = target.centroid[1] - self.centerx
dy_pixel = target.centroid[0] - self.centery
dx = dx_pixel * self.coefx
dy = dy_pixel * self.coefy
angle = target.orientation
cv2.circle(img, (int(target.centroid[1]), int(target.centroid[0])),
10, (0, 0, 255), -1)
self.dx = dx
self.dy = dy
#print self.dx,self.dy
else:
self.found = False
self.dx = 0
self.dy = 0
return img, mask_bool
def handle_vision_req(self, req):
#resp = VisionResponse(req.requestID, 0, self.avg_dx, self.avg_dy)
self.finish = 0
if self.found == True:
if self.distance > 0.07:
self.control_arm.set_joint_velocities(self.cmd)
#rospy.sleep(0.02)
else:
self.finish = 1
resp = VisionResponse(req.requestID, self.finish, self.dx, self.dy)
return resp
def clean_shutdown(self):
print "Server finished"
cv2.imwrite('box_img.png', self.blank_image)
rospy.signal_shutdown("Done")
sys.exit()
class BaxterController():
def __init__(self, limb_name, trajectory, offset, radius):
# crate a limb instance control the baxter arm
self.limb = Limb(limb_name)
# numpy joint angle trajectory of nx7 shape
self.trajectory = trajectory
# index variable to keep track of current row in numpy array
self.trajectory_index = 0
# store the joint names since
# it will be used later while commanding the limb
self.joint_names = self.limb.joint_names()
# define a service called 'move_arm_to_waypoint'
self.move_arm_to_waypoint_service = rospy.Service(
'move_arm_to_waypoint', Trigger, self.handle_move_arm_to_waypoint)
# define another service called 'get_ee_pose'
self.get_ee_position_service = rospy.Service(
'get_ee_position', GetEEPosition,
self.handle_get_ee_position_service)
# flag to set when trajectory is finished
self.trajectory_finished = False
# define 4x4 transformation for marker wrt end-effector
# considering no rotation in 'marker_wrt_ee' transformation matrix
self.marker_wrt_ee = np.eye(4)
self.marker_wrt_ee[2, -1] = offset + radius # in z direction only
def spin(self):
# let the ros stay awake and serve the request
rate = rospy.Rate(10)
while not rospy.is_shutdown() and not self.trajectory_finished:
rate.sleep()
# wait some time before stopping the node so that the service request returns if any
rospy.sleep(1)
self.get_ee_position_service.shutdown()
self.move_arm_to_waypoint_service.shutdown()
rospy.logdebug('Shutting down the baxter controller node')
rospy.signal_shutdown('User requested')
def handle_get_ee_position_service(self, request):
# create a response object for the 'GetEEPose' service
ee_pose = self.limb.endpoint_pose()
# get rotation matrix from quaternion. 'quaternion_matrix'
# returns 4x4 HTM with translation set to sero
ee_wrt_robot = quaternion_matrix(ee_pose['orientation'])
ee_wrt_robot[:-1, -1] = ee_pose['position'] # update the translation
# marker wrt robot = marker_wrt_ee * ee_wrt_robot
marker_wrt_robot = ee_wrt_robot.dot(self.marker_wrt_ee)
response = GetEEPositionResponse()
response.position.x = marker_wrt_robot[0, -1]
response.position.y = marker_wrt_robot[1, -1]
response.position.z = marker_wrt_robot[2, -1]
return response
def handle_move_arm_to_waypoint(self, request):
# create a response object for the trigger service
response = TriggerResponse()
# check if the trajectory finished
trajectory_finished = self.trajectory_index >= self.trajectory.shape[0]
# if trajectory isn't finished
if not trajectory_finished:
# get the latest joint values from given trajectory
joint_values = trajectory[self.trajectory_index, :]
# create command, i.e., a dictionary of joint names and values
command = dict(zip(self.joint_names, joint_values))
# move the limb to given joint angle
try:
self.limb.move_to_joint_positions(command)
response.message = 'Successfully moved arm to the following waypoint %s' % command
except rospy.exceptions.ROSException:
response.message = 'Error while moving arm to the following waypoint %s' % command
finally:
# increment the counter
self.trajectory_index += 1
else:
response.message = 'Arm trajectory is finished already'
# set the success parameter of the response object
response.success = trajectory_finished
# set the flag just before returning from the function so that it is
# almost certain that the service request is returned successfully
self.trajectory_finished = trajectory_finished
return response
def main():
rospy.init_node('baxter_kinematics')
kin = baxter_kinematics('left')
pos = [0.582583, -0.180819, 0.216003]
rot = [0.03085, 0.9945, 0.0561, 0.0829]
fl = fcntl.fcntl(sys.stdin.fileno(), fcntl.F_GETFL)
fcntl.fcntl(sys.stdin.fileno(), fcntl.F_SETFL, fl | os.O_NONBLOCK)
# Read initial positions:
from sensor_msgs.msg import JointState
from baxter_interface import Limb
right_arm = Limb('right')
left_arm = Limb('left')
joints = left_arm.joint_names()
positions = left_arm.joint_angles()
velocities = left_arm.joint_velocities()
force = convert_dict_to_list('left', left_arm.joint_efforts())
# Initial states
q_previous = positions # Starting joint angles
q_dot_previous = velocities # Starting joint velocities
x_previous = kin.forward_position_kinematics(
) # Starting end effector position
x_dot_previous = np.zeros((6, 1))
# Set model parameters:
m = 1
K = 0.2
C = 15
B = 12
while True:
'''
Following code breaks loop upon user input (enter key)
'''
try:
stdin = sys.stdin.read()
if "\n" in stdin or "\r" in stdin:
return_to_init(joints, 'left')
break
except IOError:
pass
# Gather Jacobian information:
J = kin.jacobian()
J_T = kin.jacobian_transpose()
J_PI = kin.jacobian_pseudo_inverse()
J_T_PI = np.linalg.pinv(J_T)
# Extract sensor data:
from sensor_msgs.msg import JointState
from baxter_interface import Limb
right_arm = Limb('right')
left_arm = Limb('left')
# Information is published at 100Hz by default (every 10ms=.01sec)
time_step = 0.01
joints = left_arm.joint_names()
positions = convert_dict_to_list('left', left_arm.joint_angles())
velocities = convert_dict_to_list('left', left_arm.joint_velocities())
force = convert_dict_to_list('left', left_arm.joint_efforts())
force_mag = np.linalg.norm(force)
print(force_mag)
if (force_mag < 28): # Add deadzone
continue
else:
from sensor_msgs.msg import JointState
from baxter_interface import Limb
right_arm = Limb('right')
left_arm = Limb('left')
joints = left_arm.joint_names()
positions = convert_dict_to_list('left', left_arm.joint_angles())
velocities = convert_dict_to_list('left',
left_arm.joint_velocities())
force = convert_dict_to_list('left', left_arm.joint_efforts())
positions = np.reshape(positions, [7, 1]) # Converts to matrix
velocities = np.reshape(velocities, [7, 1]) # Converts to matrix
force = np.reshape(force, [7, 1]) # Converts to matrix
x = kin.forward_position_kinematics()
x = x[:-1]
x_ref = np.reshape(x, [6, 1]) # Reference position
x_ref_dot = J * velocities # Reference velocities
t_stop = 10
t_end = time.time() + t_stop # Loop timer (in seconds)
# Initial plot parameters
time_plot_cum = 0
time_vec = [time_plot_cum]
pos_vec = [x_ref[1][0]]
# For integral control
x_ctrl_cum = 0
time_cum = 0.00001 # Prevent divide by zero
x_ctrl_prev = 0 # Initial for derivative ctrl
while time.time() < t_end:
from sensor_msgs.msg import JointState
from baxter_interface import Limb
right_arm = Limb('right')
left_arm = Limb('left')
J = kin.jacobian()
J_T = kin.jacobian_transpose()
J_PI = kin.jacobian_pseudo_inverse()
J_T_PI = np.linalg.pinv(J_T)
joints = left_arm.joint_names()
positions = convert_dict_to_list('left',
left_arm.joint_angles())
velocities = convert_dict_to_list('left',
left_arm.joint_velocities())
force = convert_dict_to_list('left', left_arm.joint_efforts())
positions = np.reshape(positions, [7, 1]) # Converts to matrix
velocities = np.reshape(velocities,
[7, 1]) # Converts to matrix
force = np.reshape(force, [7, 1]) # Converts to matrix
x = kin.forward_position_kinematics()
x = x[:-1]
x_current = np.reshape(x, [6, 1])
x_dot_current = J * velocities
# Only interested in y-axis:
x_dot_current[0] = 0
#x_dot_current[1]=0
x_dot_current[2] = 0
x_dot_current[3] = 0
x_dot_current[4] = 0
x_dot_current[5] = 0
x_ddot = derivative(x_dot_previous, x_dot_current, time_step)
# Model goes here
f = J_T_PI * force # spacial force
# Only interested in y-axis:
f[0] = [0]
#f[1]=[0]
f[2] = [0]
f[3] = [0]
f[4] = [0]
f[5] = [0]
x_des = ((B * f - m * x_ddot - C *
(x_dot_current - x_ref_dot)) /
K) + x_ref # Spring with damper
# Control robot
Kp = 0.0004
Ki = 0.00000022
Kd = 0.0000005
x_ctrl = x_current - x_des
# Only interested in y-axis:
x_ctrl[0] = 0
#x_ctrl[1]=0
x_ctrl[2] = 0
x_ctrl[3] = 0
x_ctrl[4] = 0
x_ctrl[5] = 0
q_dot_ctrl = J_T * np.linalg.inv(J * J_T + np.identity(6)) * (
-Kp * x_ctrl - Ki * (x_ctrl_cum) - Kd *
(x_ctrl_prev - x_ctrl) / time_step)
q_dot_ctrl_list = convert_mat_to_list(q_dot_ctrl)
q_dot_ctrl_dict = convert_list_to_dict(joints, q_dot_ctrl_list)
left_arm.set_joint_velocities(
q_dot_ctrl_dict) # Velocity control function
# Update plot info
time_cum += .05
time_vec.append(time_cum)
pos_vec.append(x_current[1][0])
# Update integral control parameters
x_ctrl_cum += x_ctrl * time_step
# Update derivative control parameters
x_ctrl_prev = x_ctrl
# Update values before next loop
x_previous = x_current # Updates previous position for next loop iteration
x_dot_previous = x_dot_current # Updates previous velocity for next loop iteration
print(time_vec)
print(pos_vec)
plt.plot(time_vec, pos_vec)
plt.title('Position over time')
plt.xlabel('Time (sec)')
plt.ylabel('Position')
plt.show()
stop_joints(q_dot_ctrl_dict)
left_arm.set_joint_velocities(q_dot_ctrl_dict)
time.sleep(1)
break
def main():
rospy.init_node('baxter_kinematics')
kin = baxter_kinematics('left')
pos = [0.582583, -0.180819, 0.216003]
rot = [0.03085, 0.9945, 0.0561, 0.0829]
fl = fcntl.fcntl(sys.stdin.fileno(), fcntl.F_GETFL)
fcntl.fcntl(sys.stdin.fileno(), fcntl.F_SETFL, fl | os.O_NONBLOCK)
# Read initial positions:
from sensor_msgs.msg import JointState
from baxter_interface import Limb
right_arm=Limb('right')
left_arm=Limb('left')
joints=left_arm.joint_names()
positions=convert_dict_to_list('left',left_arm.joint_angles())
velocities=convert_dict_to_list('left',left_arm.joint_velocities())
force=convert_dict_to_list('left',left_arm.joint_efforts())
positions=np.reshape(positions,[7,1]) # Converts to matrix
velocities=np.reshape(velocities,[7,1]) # Converts to matrix
force=np.reshape(force,[7,1]) # Converts to matrix
# Initial states
q_previous=positions # Starting joint angles
q_dot_previous=velocities # Starting joint velocities
x_previous=kin.forward_position_kinematics() # Starting end effector position
x_dot_previous=np.zeros((6,1))
# Set model parameters:
C=50
B=1
f_des=np.reshape(np.array([0,15,0,0,0,0]),[6,1])
J=kin.jacobian()
J_T=kin.jacobian_transpose()
J_PI=kin.jacobian_pseudo_inverse()
J_T_PI=np.linalg.pinv(J_T)
x=kin.forward_position_kinematics()
x=x[:-1]
x_ref=np.reshape(x,[6,1]) # Reference position
x_ref_dot=J*velocities # Reference velocities
t_stop=15
t_end=time.time()+t_stop # Loop timer (in seconds)
# Initial plot parameters
time_cum=0
time_vec=[time_cum]
f=J_T_PI*force
force_vec=[convert_mat_to_list(f[1])[0]]
while time.time()<t_end:
from sensor_msgs.msg import JointState
from baxter_interface import Limb
right_arm=Limb('right')
left_arm=Limb('left')
J=kin.jacobian()
J_T=kin.jacobian_transpose()
J_PI=kin.jacobian_pseudo_inverse()
J_T_PI=np.linalg.pinv(J_T)
joints=left_arm.joint_names()
positions=convert_dict_to_list('left',left_arm.joint_angles())
velocities=convert_dict_to_list('left',left_arm.joint_velocities())
force=convert_dict_to_list('left',left_arm.joint_efforts())
positions=np.reshape(positions,[7,1]) # Converts to matrix
velocities=np.reshape(velocities,[7,1]) # Converts to matrix
force=np.reshape(force,[7,1]) # Converts to matrix
x=kin.forward_position_kinematics()
x=x[:-1]
x_current=np.reshape(x,[6,1])
x_dot_current=J*velocities
# Only interested in y-axis:
x_dot_current[0]=0
#x_dot_current[1]=0
x_dot_current[2]=0
x_dot_current[3]=0
x_dot_current[4]=0
x_dot_current[5]=0
# Model goes here
f=J_T_PI*force # spacial force
# Only interested in y-axis:
f[0]=[0]
#f[1]=[0]
f[2]=[0]
f[3]=[0]
f[4]=[0]
f[5]=[0]
x_dot_des=-B*(f_des+f)/C # Impedance control
# Control robot
q_dot_ctrl=J_PI*x_dot_des # Use this for damper system
q_dot_ctrl=np.multiply(q_dot_ctrl,np.reshape(np.array([1,0,0,0,0,0,0]),[7,1])) # y-axis translation corresponds to first shoulder joint rotation
q_dot_ctrl_list=convert_mat_to_list(q_dot_ctrl)
q_dot_ctrl_dict=convert_list_to_dict(joints,q_dot_ctrl_list)
left_arm.set_joint_velocities(q_dot_ctrl_dict) # Velocity control function
# Update values before next loop
x_previous=x_current # Updates previous position for next loop iteration
x_dot_previous=x_dot_current # Updates previous velocity for next loop iteration
# Update plot info
time_cum+=.05
time_vec.append(time_cum)
force_vec.append(convert_mat_to_list(f[1])[0])
print(time_vec)
print(force_vec)
plt.plot(time_vec,force_vec)
plt.title('Force applied over time')
plt.xlabel('Time (sec)')
plt.ylabel('Force (N)')
plt.show()
time.sleep(1)