コード例 #1
0
ファイル: cob_actions.py プロジェクト: ipa320/research-camp-5
	def execute(self, component_name, target=PoseStamped(), blocking=True):
		ah = ActionHandle("move_arm_cartesian_planned", component_name, target, blocking)

		rospy.loginfo("Move <<%s>> CARTESIAN PLANNED", component_name)

		mp = MotionPlan()
		mp += MoveArm("arm",[target,['sdh_grasp_link']])
		
		planning_res = mp.plan(2)
		print planning_res

		if planning_res.success:
			for e in mp.execute():
				exec_res = e.wait()
				print exec_res
				if not exec_res.success:
					rospy.logerr("Execution of MotionExecutable %s failed", e.name)
					ah.set_failed(4)
					break
		else:
			rospy.logerr("Planning failed")
			ah.set_failed(4)

		return ah
コード例 #2
0
ファイル: cob_actions.py プロジェクト: ipa320/research-camp-5
	def execute(self, target=PoseStamped(), blocking=True):
		ah = ActionHandle("pick_up", "dummy", "", blocking)
		rospy.loginfo("Picking up object...")
		
		#ah = self.actions.grasp_object(target, blocking)
		wi = WorldInterface()
		wi.reset_attached_objects()
		wi.reset_collision_objects()
		
		# add table
		table_extent = (2.0, 2.0, 1.0)
		table_pose = conversions.create_pose_stamped([ -0.5 - table_extent[0]/2.0, 0 ,table_extent[2]/2.0 ,0,0,0,1], 'base_link')
		wi.add_collision_box(table_pose, table_extent, "table")
		
		mp = MotionPlan()
		mp += CallFunction(sss.move, 'torso','front')
		#mp += MoveArm('arm',['pregrasp'])
		mp += CallFunction(sss.move, 'sdh','cylopen', False)
		
		# OpenIssues:
		# - where to place the sdh_grasp_link to grasp the object located at target?
		# - there is no orientation in the target? -> hardcoded taken from pregrasp
		grasp_pose = PoseStamped()
		grasp_pose = deepcopy(target)
		grasp_pose.header.stamp = rospy.Time.now()
		grasp_pose.pose.orientation.x = 0.220
		grasp_pose.pose.orientation.y = 0.670
		grasp_pose.pose.orientation.z = -0.663
		grasp_pose.pose.orientation.w = -0.253
		mp += MoveArm('arm',[grasp_pose,['sdh_grasp_link']], seed = 'pregrasp')
		
		mp += CallFunction(sss.move, 'sdh','cylclosed', True)
		
		
		#ah = self.actions.lift_object(target, blocking)
		lift_pose = PoseStamped()
		lift_pose = deepcopy(target)
		lift_pose.header.stamp = rospy.Time.now()
		lift_pose.pose.position.z += 0.08
		lift_pose.pose.orientation.x = 0.220
		lift_pose.pose.orientation.y = 0.670
		lift_pose.pose.orientation.z = -0.663
		lift_pose.pose.orientation.w = -0.253
		mp += MoveArm('arm',[lift_pose,['sdh_grasp_link']])
		
		
		
		#ah = self.actions.retrieve_object(target, blocking)
		#mp += MoveArm('arm',['pregrasp'])
		mp += MoveArm('arm',['wavein'])
		
		
		
		
		planning_res = mp.plan(2)
		print planning_res

		if planning_res.success:
			for e in mp.execute():
				exec_res = e.wait()
				print exec_res
				if not exec_res.success:
					#rospy.logerr("Execution of MotionExecutable %s failed", e.name)
					ah.set_failed(4)
					break
			ah.set_succeeded()
		else:
			rospy.logerr("Planning failed")
			ah.set_failed(4)
		
		return ah
コード例 #3
0
ファイル: cob_actions.py プロジェクト: ipa320/research-camp-5
	def execute(self, component_name, target="", blocking=True):
		ah = ActionHandle("move_arm_joint_planned", component_name, target, blocking)

		rospy.loginfo("Move planned <<%s>> to <<%s>>",component_name,target)

		# get joint values from parameter server
		if type(target) is str:
			if not rospy.has_param(self.ns_global_prefix + "/" + component_name + "/" + target):
				rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...",self.ns_global_prefix + "/" + component_name + "/" + target)
				ah.set_failed(2)
				return ah
			param = rospy.get_param(self.ns_global_prefix + "/" + component_name + "/" + target)
		else:
			param = target

		# check trajectory parameters
		if not type(param) is list: # check outer list
				rospy.logerr("no valid parameter for %s: not a list, aborting...",component_name)
				print "parameter is:",param
				ah.set_failed(3)
				return ah

		last_point = param[-1]
		
		if type(last_point) is str:
			if not rospy.has_param(self.ns_global_prefix + "/" + component_name + "/" + last_point):
				rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...",self.ns_global_prefix + "/" + component_name + "/" + last_point)
				ah.set_failed(2)
				return ah
			last_point = rospy.get_param(self.ns_global_prefix + "/" + component_name + "/" + last_point)
			last_point = last_point[0] # \todo TODO: hack because only first point is used, no support for trajectories inside trajectories
			#print last_point
		elif type(last_point) is list:
			rospy.logdebug("last_point is a list")
		else:
			rospy.logerr("no valid parameter for %s: not a list of lists or strings, aborting...",component_name)
			print "parameter is:",param
			ah.set_failed(3)
			return ah

		# here: last_point should be list of floats/ints
		#print last_point
		if not len(last_point) == self.DOF: # check dimension
			rospy.logerr("no valid parameter for %s: dimension should be %d and is %d, aborting...",component_name,self.DOF,len(last_point))
			print "parameter is:",param
			ah.set_failed(3)
			return ah

		for value in last_point:
			#print value,"type2 = ", type(value)
			if not ((type(value) is float) or (type(value) is int)): # check type
				#print type(value)
				rospy.logerr("no valid parameter for %s: not a list of float or int, aborting...",component_name)
				print "parameter is:",param
				ah.set_failed(3)
				return ah
		
			rospy.logdebug("accepted value %f for %s",value,component_name)


		mp = MotionPlan()
		mp += MoveArm("arm", [last_point])
		
		planning_res = mp.plan(2)
		print planning_res

		if planning_res.success:
			for e in mp.execute():
				exec_res = e.wait()
				print exec_res
				if not exec_res.success:
					rospy.logerr("Execution of MotionExecutable %s failed", e.name)
					ah.set_failed(3)
					break
		else:
			rospy.logerr("Planning failed")
			ah.set_failed(3)

		return ah