arm_home.py

Go to the documentation of this file.

#!/usr/bin/env python
import roslib; roslib.load_manifest('komodo_arm')
import rospy
from std_msgs.msg import Float64
 
 
def move_arm():
        pub_base_rotation=rospy.Publisher('/base_rotation_controller/command', Float64)
        pub_shoulder = rospy.Publisher('/shoulder_controller/command', Float64)
        pub_elbow1 = rospy.Publisher('/elbow1_controller/command', Float64)     
        pub_elbow2 = rospy.Publisher('/elbow2_controller/command', Float64)     
        pub_wrist = rospy.Publisher('/wrist_controller/command', Float64)
        pub_left_finger = rospy.Publisher('/left_finger_controller/command', Float64)
        pub_right_finger = rospy.Publisher('/right_finger_controller/command', Float64)
        rospy.init_node('move_arm_home')
        #while not rospy.is_shutdown():
        br = Float64(0.0)
        sh = Float64(0.0)
        e1 = Float64(0.0)
        e2 = Float64(0.0)       
        wr = Float64(0.0) 
        lf = Float64(0.3)       
        rf = Float64(-0.3)
        pub_base_rotation.publish(br)
        pub_shoulder.publish(sh)
        pub_elbow1.publish(e1)
        pub_elbow2.publish(e2)
        pub_wrist.publish(wr)
        pub_left_finger.publish(lf)
        pub_right_finger.publish(rf)
        rospy.sleep(0.1)
        pub_base_rotation.publish(br)
        pub_shoulder.publish(sh)
        pub_elbow1.publish(e1)
        pub_elbow2.publish(e2)
        pub_wrist.publish(wr)
        pub_left_finger.publish(lf)
        pub_right_finger.publish(rf)
        rospy.sleep(6.0)
        # 2nd instance 
        br = Float64(0)
        sh = Float64(1.141281706186917)
        e1 = Float64(0)
        e2 = Float64(2.084679890736586) 
        wr = Float64(0) 
        lf = Float64(0.2)       
        rf = Float64(-0.2)
        pub_base_rotation.publish(br)
        pub_shoulder.publish(sh)
        pub_elbow1.publish(e1)
        pub_elbow2.publish(e2)
        pub_wrist.publish(wr)
        pub_left_finger.publish(lf)
        pub_right_finger.publish(rf)
        rospy.sleep(5.0)
        # 3rd instance
        br = Float64(0)
        sh = Float64(1.141281706186917)
        e1 = Float64(0)
        e2 = Float64(2.084679890736586) 
        wr = Float64(0) 
        lf = Float64(-0.2)      
        rf = Float64(0.2)
        pub_base_rotation.publish(br)
        pub_shoulder.publish(sh)
        pub_elbow1.publish(e1)
        pub_elbow2.publish(e2)
        pub_wrist.publish(wr)
        pub_left_finger.publish(lf)
        pub_right_finger.publish(rf)
        
 
if __name__ == '__main__':
        try:
                move_arm()
        except rospy.ROSInterruptException:
                pass