Go to the documentation of this file.00001
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_home')
00016
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
00041 br = Float64(0)
00042 sh = Float64(1.141281706186917)
00043 e1 = Float64(0)
00044 e2 = Float64(2.084679890736586)
00045 wr = Float64(0)
00046 lf = Float64(0.2)
00047 rf = Float64(-0.2)
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
00057 br = Float64(0)
00058 sh = Float64(1.141281706186917)
00059 e1 = Float64(0)
00060 e2 = Float64(2.084679890736586)
00061 wr = Float64(0)
00062 lf = Float64(-0.2)
00063 rf = Float64(0.2)
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