komodo_arm: arm_demo.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 import roslib; roslib.load_manifest('komodo_arm')
00003 import rospy
00004 from std_msgs.msg import Float64
00005  
00006  
00007 def move_arm():
00008         pub_base_rotation=rospy.Publisher('/base_rotation_controller/command', Float64)
00009         pub_shoulder = rospy.Publisher('/shoulder_controller/command', Float64)
00010         pub_elbow1 = rospy.Publisher('/elbow1_controller/command', Float64)     
00011         pub_elbow2 = rospy.Publisher('/elbow2_controller/command', Float64)     
00012         pub_wrist = rospy.Publisher('/wrist_controller/command', Float64)
00013         pub_left_finger = rospy.Publisher('/left_finger_controller/command', Float64)
00014         pub_right_finger = rospy.Publisher('/right_finger_controller/command', Float64)
00015         rospy.init_node('move_arm')
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.0)       
00023         rf = Float64(0.0)
00024         rospy.loginfo('base_rotation_'+str(br))
00025         rospy.loginfo('shoulder_'+str(sh))
00026         rospy.loginfo('elbow1_'+str(e1))
00027         rospy.loginfo('elbow2_'+str(e2))
00028         rospy.loginfo('wrist_'+str(wr))
00029         rospy.loginfo('left_finger_'+str(lf))
00030         rospy.loginfo('right_finger_'+str(rf))
00031         pub_base_rotation.publish(br)
00032         pub_shoulder.publish(sh)
00033         pub_elbow1.publish(e1)
00034         pub_elbow2.publish(e2)
00035         pub_wrist.publish(wr)
00036         pub_left_finger.publish(lf)
00037         pub_right_finger.publish(rf)
00038         rospy.sleep(5.0)
00039         # 2nd instance
00040         br = Float64(-0.34821363885004053)
00041         sh = Float64(-0.8710098387316108)
00042         e1 = Float64(0.02761165418194154)
00043         e2 = Float64(-1.8438449070385408)       
00044         wr = Float64(0.32673790781964157) 
00045         lf = Float64(-0.0)      
00046         rf = Float64(0.0)
00047         rospy.loginfo('base_rotation_'+str(br))
00048         rospy.loginfo('shoulder_'+str(sh))
00049         rospy.loginfo('elbow1_'+str(e1))
00050         rospy.loginfo('elbow2_'+str(e2))
00051         rospy.loginfo('wrist_'+str(wr))
00052         rospy.loginfo('left_finger_'+str(lf))
00053         rospy.loginfo('right_finger_'+str(rf))
00054         pub_base_rotation.publish(br)
00055         pub_shoulder.publish(sh)
00056         pub_elbow1.publish(e1)
00057         pub_elbow2.publish(e2)
00058         pub_wrist.publish(wr)
00059         pub_left_finger.publish(lf)
00060         pub_right_finger.publish(rf)
00061         rospy.sleep(5.0)
00062         # 3rd instance
00063         br = Float64(-0.34821363885004053)
00064         sh = Float64(-0.8710098387316108)
00065         e1 = Float64(0.02761165418194154)
00066         e2 = Float64(-1.8438449070385408)       
00067         wr = Float64(0.32673790781964157) 
00068         lf = Float64(-0.65)     
00069         rf = Float64(0.65)
00070         rospy.loginfo('base_rotation_'+str(br))
00071         rospy.loginfo('shoulder_'+str(sh))
00072         rospy.loginfo('elbow1_'+str(e1))
00073         rospy.loginfo('elbow2_'+str(e2))
00074         rospy.loginfo('wrist_'+str(wr))
00075         rospy.loginfo('left_finger_'+str(lf))
00076         rospy.loginfo('right_finger_'+str(rf))
00077         pub_base_rotation.publish(br)
00078         pub_shoulder.publish(sh)
00079         pub_elbow1.publish(e1)
00080         pub_elbow2.publish(e2)
00081         pub_wrist.publish(wr)
00082         pub_left_finger.publish(lf)
00083         pub_right_finger.publish(rf)
00084         rospy.sleep(2.0)
00085         # 4th instance
00086         br = Float64(0.5629709491540303)
00087         sh = Float64(0.3574175235773544)
00088         e1 = Float64(0.34514567727426926)
00089         e2 = Float64(0.5108156023659185)        
00090         wr = Float64(0.1718058482431918) 
00091         lf = Float64(-0.65)     
00092         rf = Float64(0.65)
00093         rospy.loginfo('base_rotation_'+str(br))
00094         rospy.loginfo('shoulder_'+str(sh))
00095         rospy.loginfo('elbow1_'+str(e1))
00096         rospy.loginfo('elbow2_'+str(e2))
00097         rospy.loginfo('wrist_'+str(wr))
00098         rospy.loginfo('left_finger_'+str(lf))
00099         rospy.loginfo('right_finger_'+str(rf))
00100         pub_base_rotation.publish(br)
00101         pub_shoulder.publish(sh)
00102         pub_elbow1.publish(e1)
00103         pub_elbow2.publish(e2)
00104         pub_wrist.publish(wr)
00105         pub_left_finger.publish(lf)
00106         pub_right_finger.publish(rf)
00107         rospy.sleep(3.0)
00108         # 5th instance
00109         br = Float64(0.5553010452146021)
00110         sh = Float64(1.6582332317043782)
00111         e1 = Float64(0.3911651009108385)
00112         e2 = Float64(0.9878836273983529)        
00113         wr = Float64(0.16873788666742054) 
00114         lf = Float64(-0.65)     
00115         rf = Float64(0.65)
00116         rospy.loginfo('base_rotation_'+str(br))
00117         rospy.loginfo('shoulder_'+str(sh))
00118         rospy.loginfo('elbow1_'+str(e1))
00119         rospy.loginfo('elbow2_'+str(e2))
00120         rospy.loginfo('wrist_'+str(wr))
00121         rospy.loginfo('left_finger_'+str(lf))
00122         rospy.loginfo('right_finger_'+str(rf))
00123         pub_base_rotation.publish(br)
00124         pub_shoulder.publish(sh)
00125         pub_elbow1.publish(e1)
00126         pub_elbow2.publish(e2)
00127         pub_wrist.publish(wr)
00128         pub_left_finger.publish(lf)
00129         pub_right_finger.publish(rf)
00130         rospy.sleep(5.0)
00131         # 6th instance
00132         br = Float64(0.5553010452146021)
00133         sh = Float64(1.6582332317043782)
00134         e1 = Float64(0.3911651009108385)
00135         e2 = Float64(0.9878836273983529)        
00136         wr = Float64(0.16873788666742054) 
00137         lf = Float64(0)         
00138         rf = Float64(0)
00139         rospy.loginfo('base_rotation_'+str(br))
00140         rospy.loginfo('shoulder_'+str(sh))
00141         rospy.loginfo('elbow1_'+str(e1))
00142         rospy.loginfo('elbow2_'+str(e2))
00143         rospy.loginfo('wrist_'+str(wr))
00144         rospy.loginfo('left_finger_'+str(lf))
00145         rospy.loginfo('right_finger_'+str(rf))
00146         pub_base_rotation.publish(br)
00147         pub_shoulder.publish(sh)
00148         pub_elbow1.publish(e1)
00149         pub_elbow2.publish(e2)
00150         pub_wrist.publish(wr)
00151         pub_left_finger.publish(lf)
00152         pub_right_finger.publish(rf)
00153         rospy.sleep(3.0)
00154         br = Float64(0.0)
00155         sh = Float64(0.0)
00156         e1 = Float64(0.0)
00157         e2 = Float64(0.0)       
00158         wr = Float64(0.0) 
00159         lf = Float64(0.0)       
00160         rf = Float64(0.0)
00161         rospy.loginfo('base_rotation_'+str(br))
00162         rospy.loginfo('shoulder_'+str(sh))
00163         rospy.loginfo('elbow1_'+str(e1))
00164         rospy.loginfo('elbow2_'+str(e2))
00165         rospy.loginfo('wrist_'+str(wr))
00166         rospy.loginfo('left_finger_'+str(lf))
00167         rospy.loginfo('right_finger_'+str(rf))
00168         pub_base_rotation.publish(br)
00169         pub_shoulder.publish(sh)
00170         pub_elbow1.publish(e1)
00171         pub_elbow2.publish(e2)
00172         pub_wrist.publish(wr)
00173         pub_left_finger.publish(lf)
00174         pub_right_finger.publish(rf)
00175  
00176 if __name__ == '__main__':
00177         try:
00178                 move_arm()
00179         except rospy.ROSInterruptException:
00180                 pass