コード例 #1
0
def stateManager():

    rospy.loginfo("1st begin")
    motion1 = fineTuneClient(4)
    rospy.loginfo("1st success")
    rospy.sleep(10)

    # 	rospy.loginfo("1st begin")
    # 	motion1 = fineTuneClient(3)
    # 	rospy.loginfo("1st success")
    # 	rospy.sleep(10)
    # 	rospy.loginfo("2nd begin")
    # 	motion2 = fineTuneClient(7)
    # 	rospy.loginfo("2nd success")

    # shared code block
    rospy.loginfo("3rd begin")
    motion3 = fineTuneClient(1)
    rospy.loginfo("3rd success")
    rospy.loginfo("4th begin")
    motion4 = fineTuneClient(6)
    rospy.loginfo("4th success")
    rospy.loginfo("4.5 begin")
    motion3 = fineTuneClient(1)
    rospy.loginfo("4.5 success")

    grabBlock = graspClient(1)

    atHome = fineTuneClient(5)
    rospy.wait(5)

    dropped = graspClient(2)
コード例 #2
0
def stateManager():

    rospy.loginfo("1st begin")
    motion1 = fineTuneClient(4)
    rospy.loginfo("1st success")
    rospy.sleep(10)

    #	rospy.loginfo("1st begin")
    #	motion1 = fineTuneClient(3)
    #	rospy.loginfo("1st success")
    #	rospy.sleep(10)
    #	rospy.loginfo("2nd begin")
    #	motion2 = fineTuneClient(7)
    #	rospy.loginfo("2nd success")

    # shared code block
    rospy.loginfo("3rd begin")
    motion3 = fineTuneClient(1)
    rospy.loginfo("3rd success")
    rospy.loginfo("4th begin")
    motion4 = fineTuneClient(6)
    rospy.loginfo("4th success")
    rospy.loginfo("4.5 begin")
    motion3 = fineTuneClient(1)
    rospy.loginfo("4.5 success")

    grabBlock = graspClient(1)

    atHome = fineTuneClient(5)
    rospy.wait(5)

    dropped = graspClient(2)
コード例 #3
0
	def stateCallback(self, data):
		self.newPose = data

		targetLocation = Pose()
		startTime = rospy.get_time()
		i = 0

		os.system("rostopic pub  /gripper_joint/command std_msgs/Float64 -- 1.57 &")
		rospy.sleep(5)

		print 'Move to Flower'
		targetLocation.position.x = 0.0144326423953
		targetLocation.position.y = 0.263114368294
		targetLocation.position.z = 0.554147669158
		targetLocation.orientation.x = -0.15822121316
		targetLocation.orientation.y = 0.110466551696
		targetLocation.orientation.z = 0.804522708772
		targetLocation.orientation.w = 0.561699563586
		self.group.set_pose_target(targetLocation)
		
		
		# Compute plan and visualize if successful
		newPlan = self.group.plan()
		# Perform plan on Robot
		
		self.group.go(wait=True)
		print "Grasp"
		rospy.sleep(5)
		
		
		os.system("rostopic pub  /gripper_joint/command std_msgs/Float64 -- -1.2")
		
		rospy.wait(2)
		rospy.sleep(10)

		print 'Lift flower'
		targetLocation.position.x = 0.152084466078
		targetLocation.position.y = -0.101408893961
		targetLocation.position.z = 0.687939025664
		targetLocation.orientation.x = 0.779572704978
		targetLocation.orientation.y = -0.529587263257
		targetLocation.orientation.z = 0.276585994996
		targetLocation.orientation.w = 0.187893362364
		self.group.set_pose_target(targetLocation)
		# Compute plan and visualize if successful
		newPlan = self.group.plan()
		# Perform plan on Robot
		self.group.go(wait=True)
		rospy.sleep(2)

		print "Release"
		os.system("rostopic pub  /gripper_joint/command std_msgs/Float64 -- 1.57 &")
		rospy.sleep(5)

		# End task
		print 'Rest'
		targetLocation.position.x = 0.0637378729914
		targetLocation.position.y = 0.314422971062
		targetLocation.position.z = 0.585093066776
		targetLocation.orientation.x = -0.0363535046398
		targetLocation.orientation.y = 0.0320124025576
		targetLocation.orientation.z = 0.749608676985
		targetLocation.orientation.w = 0.660106400644
		#os.system("rosrun squirtle_arm_kinematics armPlanning.py");

		self.group.set_pose_target(targetLocation)
		# Compute plan and visualize if successful
		newPlan = self.group.plan()
		# Perform plan on Robot
		self.group.go(wait=True)
		self.group.clear_pose_targets()
		#relax servos
		for servo in self.servos:
			self.relaxers[servo]()
		rospy.sleep(5)
		#print  self.group.get_planning_frame()
		## Adding/Removing Objects and Attaching/Detaching Objects
		## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
		## First, we will define the collision object message
		collision_object = moveit_msgs.msg.CollisionObject()

		## When finished shut down moveit_commander.
		moveit_commander.roscpp_shutdown()
コード例 #4
0
    def stateCallback(self, data):
        self.newPose = data

        targetLocation = Pose()
        startTime = rospy.get_time()
        i = 0

        os.system(
            "rostopic pub  /gripper_joint/command std_msgs/Float64 -- 1.57 &")
        rospy.sleep(5)

        print 'Move to Flower'
        targetLocation.position.x = 0.0144326423953
        targetLocation.position.y = 0.263114368294
        targetLocation.position.z = 0.554147669158
        targetLocation.orientation.x = -0.15822121316
        targetLocation.orientation.y = 0.110466551696
        targetLocation.orientation.z = 0.804522708772
        targetLocation.orientation.w = 0.561699563586
        self.group.set_pose_target(targetLocation)

        # Compute plan and visualize if successful
        newPlan = self.group.plan()
        # Perform plan on Robot

        self.group.go(wait=True)
        print "Grasp"
        rospy.sleep(5)

        os.system(
            "rostopic pub  /gripper_joint/command std_msgs/Float64 -- -1.2")

        rospy.wait(2)
        rospy.sleep(10)

        print 'Lift flower'
        targetLocation.position.x = 0.152084466078
        targetLocation.position.y = -0.101408893961
        targetLocation.position.z = 0.687939025664
        targetLocation.orientation.x = 0.779572704978
        targetLocation.orientation.y = -0.529587263257
        targetLocation.orientation.z = 0.276585994996
        targetLocation.orientation.w = 0.187893362364
        self.group.set_pose_target(targetLocation)
        # Compute plan and visualize if successful
        newPlan = self.group.plan()
        # Perform plan on Robot
        self.group.go(wait=True)
        rospy.sleep(2)

        print "Release"
        os.system(
            "rostopic pub  /gripper_joint/command std_msgs/Float64 -- 1.57 &")
        rospy.sleep(5)

        # End task
        print 'Rest'
        targetLocation.position.x = 0.0637378729914
        targetLocation.position.y = 0.314422971062
        targetLocation.position.z = 0.585093066776
        targetLocation.orientation.x = -0.0363535046398
        targetLocation.orientation.y = 0.0320124025576
        targetLocation.orientation.z = 0.749608676985
        targetLocation.orientation.w = 0.660106400644
        #os.system("rosrun squirtle_arm_kinematics armPlanning.py");

        self.group.set_pose_target(targetLocation)
        # Compute plan and visualize if successful
        newPlan = self.group.plan()
        # Perform plan on Robot
        self.group.go(wait=True)
        self.group.clear_pose_targets()
        #relax servos
        for servo in self.servos:
            self.relaxers[servo]()
        rospy.sleep(5)
        #print  self.group.get_planning_frame()
        ## Adding/Removing Objects and Attaching/Detaching Objects
        ## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
        ## First, we will define the collision object message
        collision_object = moveit_msgs.msg.CollisionObject()

        ## When finished shut down moveit_commander.
        moveit_commander.roscpp_shutdown()