arm_home.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 import roslib; roslib.load_manifest('ric_robot')
00003 import rospy
00004 from std_msgs.msg import Float64
00005  
00006  
00007 def move_arm():
00008         pub_base_rotation=rospy.Publisher('/komodo_1/base_rotation_controller/command', Float64)
00009         pub_shoulder = rospy.Publisher('/komodo_1/shoulder_controller/command', Float64)
00010         pub_elbow1 = rospy.Publisher('/komodo_1/elbow1_controller/command', Float64)    
00011         pub_elbow2 = rospy.Publisher('/komodo_1/elbow2_controller/command', Float64)    
00012         pub_wrist = rospy.Publisher('/komodo_1/wrist_controller/command', Float64)
00013         pub_left_finger = rospy.Publisher('/komodo_1/left_finger_controller/command', Float64)
00014         pub_right_finger = rospy.Publisher('/komodo_1/right_finger_controller/command', Float64)
00015         rospy.init_node('move_arm_home')
00016         #while not rospy.is_shutdown():
00017         br = Float64(0.0)
00018         sh = Float64(0.0)
00019         e1 = Float64(0.0)
00020         e2 = Float64(0.0)       
00021         wr = Float64(0.0) 
00022         lf = Float64(-0.3)      
00023         rf = Float64(0.3)
00024         pub_base_rotation.publish(br)
00025         pub_shoulder.publish(sh)
00026         pub_elbow1.publish(e1)
00027         pub_elbow2.publish(e2)
00028         pub_wrist.publish(wr)
00029         pub_left_finger.publish(lf)
00030         pub_right_finger.publish(rf)
00031         rospy.sleep(0.1)
00032         pub_base_rotation.publish(br)
00033         pub_shoulder.publish(sh)
00034         pub_elbow1.publish(e1)
00035         pub_elbow2.publish(e2)
00036         pub_wrist.publish(wr)
00037         pub_left_finger.publish(lf)
00038         pub_right_finger.publish(rf)
00039         rospy.sleep(6.0)
00040         # 2nd instance 
00041         br = Float64(0)
00042         sh = Float64(1.08)
00043         e1 = Float64(0)
00044         e2 = Float64(2.08)      
00045         wr = Float64(0) 
00046         lf = Float64(-1.1)      
00047         rf = Float64(1.1)
00048         pub_base_rotation.publish(br)
00049         pub_shoulder.publish(sh)
00050         pub_elbow1.publish(e1)
00051         pub_elbow2.publish(e2)
00052         pub_wrist.publish(wr)
00053         pub_left_finger.publish(lf)
00054         pub_right_finger.publish(rf)
00055         rospy.sleep(5.0)
00056         # 3rd instance
00057         br = Float64(0)
00058         sh = Float64(1.2)
00059         e1 = Float64(0)
00060         e2 = Float64(1.93)      
00061         wr = Float64(0) 
00062         lf = Float64(-1.1)      
00063         rf = Float64(1.1)
00064         pub_base_rotation.publish(br)
00065         pub_shoulder.publish(sh)
00066         pub_elbow1.publish(e1)
00067         pub_elbow2.publish(e2)
00068         pub_wrist.publish(wr)
00069         pub_left_finger.publish(lf)
00070         pub_right_finger.publish(rf)
00071         
00072  
00073 if __name__ == '__main__':
00074         try:
00075                 move_arm()
00076         except rospy.ROSInterruptException:
00077                 pass


ric_robot
Author(s): RoboTiCan
autogenerated on Fri May 20 2016 03:50:58