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)
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)
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()
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()