Baxter抓取物块——基于单应性矩阵（二）

        之前写了Baxter抓取物块的视觉部分（见一），接下来说一说剩下的、比较简单的模块。

机械臂末端位姿获取：

         Baxter启动后会将自身各坐标系的变换关系发布到 '/tf' 话题中，我们只需要使用TF包（具体参考wiki）即可。定义的current_pose()函数是为了将矩阵转换为ROS的pose Message。

class tf_listener():
	def __init__(self):
		#订阅'/tf'，使各坐标系变换实时更新
		rospy.Subscriber('/tf', TFMessage, self.frame_handler)
		#Baxter的坐标系变换
		self.tf_baxter = tf.TransformerROS()
		
		self.trans_left = None	
		self.trans_right = None		
	

	def frame_handler(self, tf_message):
		temp_msg = TransformStamped()
		#将收到的TFMessage中的各个坐标系的信息进行更新
		for i in range(0,len(tf_message.transforms)):
			temp_msg = tf_message.transforms[i]
			self.tf_baxter.setTransform(temp_msg)
		
		#Baxter中torso与base的坐标系姿态相同,求从left_gripper到基坐标系的变换
		try:
			self.trans_left = self.tf_baxter.lookupTransform('torso','left_gripper',rospy.Time())
			self.trans_right = self.tf_baxter.lookupTransform('torso','right_gripper',rospy.Time())
			
		except:
			rospy.loginfo("正在尝试与tf同步............. \n")
		
	def current_pose(self, name):	
		"""
	该函数用于获得当前左/右抓手的位姿
	"""
		if name == 'left':
			ans = Pose()				
			ans.position.x = self.trans_left[0][0]
			ans.position.y = self.trans_left[0][1]
			ans.position.z = self.trans_left[0][2]
			ans.orientation.x = self.trans_left[1][0]
			ans.orientation.y = self.trans_left[1][1]
			ans.orientation.z = self.trans_left[1][2]
			ans.orientation.w = self.trans_left[1][3]
			return ans
		else:
			ans = Pose()				
			ans.position.x = self.trans_right[0][0]
			ans.position.y = self.trans_right[0][1]
			ans.position.z = self.trans_right[0][2]
			ans.orientation.x = self.trans_right[1][0]
			ans.orientation.y = self.trans_right[1][1]
			ans.orientation.z = self.trans_right[1][2]
			ans.orientation.w = self.trans_right[1][3]
			return ans 

 Baxter内置的逆运动学求解器：

        这一部分参考Baxter sdk wiki，会调用Baxter的服务就行。

#Baxter内置的逆运动学求解器	
class baxter_ik_srv:
	def __init__(self):
		rospy.wait_for_service('/ExternalTools/left/PositionKinematicsNode/IKService')
		self.ik_service = rospy.ServiceProxy('/ExternalTools/left/PositionKinematicsNode/IKService',SolvePositionIK)	#创建服务原型
	def solve(self,pose):
		posestamped = PoseStamped()	#服务要求的输入为PoseStamped（多个，我们只送一个）
		posestamped.pose = pose	#传入的参数类型为Pose
		posestamped.header.stamp = rospy.Time.now()	#将pose打上stamp
		posestamped.header.frame_id = 'base'	
		req = SolvePositionIKRequest()	#请求实例
		req.pose_stamp.append(posestamped)
		rospy.wait_for_service('/ExternalTools/left/PositionKinematicsNode/IKService')	#等待服务可用
		try:
			resp = self.ik_service(req)	#请求服务
			if resp.isValid[0] == True:	#True代表有解，只送一个进去，故索引为0
				return resp
			else:
				rospy.logerr("反解器无解..........\n")
				return None
		except rospy.ServiceException as exc:
			rospy.logerr("请求服务出错：" + str(exc))

 Baxter控制模块：

        设置了抓手的最大抓取力矩，以防止抓手损坏（一个还是挺贵的），函数的用法均可以在Baxter sdk wiki上查找到。这里包装的也很简单，就不多做描述。

class baxter_control():
	def __init__(self):
		#手臂初始化
		self.IK_srv = baxter_ik_srv()	#启动逆运动学求解器
		self.left_arm = Limb('left')    #手臂实例
		self.left_arm.set_joint_position_speed(0.2)	#设置位置控制时的关节速度

		#末端执行器初始化，打开并设置最大力矩
		self.left_gripper = Gripper('left')	#抓手实例
		if self.left_gripper.calibrated() == False:	#若抓手未校准，则校准
			self.left_gripper.calibrate()
		self.left_gripper.open(block=True)
		self.left_gripper.set_velocity(5)	#设置抓手移动时的速度
		self.left_gripper.set_moving_force(10)	#设置抓手移动时的力（最大值）
		self.left_gripper.set_holding_force(5)  #抓住物体时的保持力矩

		#末端红外测距初始化（备用）
		self.left_range = AnalogIO('left_hand_range') 	#红外测距,有效距离30厘米

	def go(self,pose):
		"""
		输入参数——姿态pose
		经逆运动学求解后，控制机械臂抵达目标位置
		"""
		ik_response = self.IK_srv.solve(pose)
		try:
  	  		limb_joints = dict(zip(ik_response.joints[0].name, ik_response.joints[0].position)) #dict zip从两个列表构造字典
   			self.left_arm.move_to_joint_positions(limb_joints)
		except:
			rospy.logerr("无法抵达目标位置")

	def go_start_position(self):
		start_pose = Pose()	#设置起始位置并让手臂抵达
		start_pose.position.x = 0.65
		start_pose.position.y = 0.14
		start_pose.position.z = 0.27
		start_pose.orientation.x = 0.0
		start_pose.orientation.y = 1.0 
		start_pose.orientation.z = 0.0
		start_pose.orientation.w = 0.0
		self.go(start_pose)	#移动

主函数 ：

        下面是一段比较简单的主函数，起始位置->找到物块->移动->下降并抓取。目标位置是由自身位置加上偏差获取的。

def main():
        rospy.init_node('pick_demo_no_moveit')
	baxter_ctrl = baxter_control()	#初始化Baxter移动控制器
	visual_processor = image_converter()	#开启相机图像处理
	pose_processor = tf_listener()	#开启坐标系变换追踪
	rospy.sleep(5)

        baxter_ctrl.go_start_position()
        delta_pose =  visual_processor._get_obj_world_pose('blue')	#得到应在桌面坐标系移动的距离	
        tar = pose_processor.current_pose('left')
        tar.position.x += delta_pose[0]	#根据偏差移动
	tar.position.y += delta_pose[1]
	self.go(tar)
        tar.position.z -= 0.21	#下降（抓取）姿态
	self.go(tar)	#移动
	self.left_gripper.close(block=True)	#抓手抓取

        整个程序仍然有很多地方可以改进，比如识别蓝色以外的物体，图像中有多个物体时如何处理，物体摆放的角度不同时要如何处理等等。注意在进行连续抓取时，单应性矩阵只需进行一次计算（当然每次抓取前都要回到起始位置所在的水平面），而不是每次都要识别棋盘格的存在。

         以上就是Baxter抓取物块的一个简单demo。