example_sendupdate.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 import roslib; roslib.load_manifest('sr_edc_muscle_tools')
00003 import rospy
00004 from sr_robot_msgs.msg import sendupdate, joint
00005 from std_msgs.msg import Float64
00006 import math
00007 
00008 #
00009 # Small demo of moving a number of joints together.
00010 # Flexes the 2 fingers and then waves the wrist.
00011 #
00012 
00013 
00014 # We set up all the publishers needed for all joints in a dict and create a simple
00015 # sendupdate function to send joint targets from a dictioary of joint name and angle
00016 # (in degrees)
00017 # Note naist hand is only 2 fingers and a thumb
00018 joint_names = [ "thj1", "thj2", "thj3", "thj4", "thj5", "ffj0", "ffj3", "ffj4", "rfj0", "rfj3", "rfj4", "wrj1", "wrj2" ]
00019 joint_pubs = {}
00020 for jname in joint_names:
00021     topic = "/sh_"+jname+"_muscle_position_controller/command"
00022     print topic
00023     joint_pubs[jname] = rospy.Publisher(topic, Float64, latch=True)
00024 
00025 def sendupdate(joints):
00026     print "Sending:"
00027     for jname in joints:
00028         if not jname in joint_pubs:
00029             print "\tJoint %s not found"%jname
00030             return
00031         msg = Float64()
00032         msg.data = math.radians( float(joints[jname]) )
00033         print "\t" + jname + ": " + str(joints[jname])
00034         joint_pubs[jname].publish(msg)
00035 
00036 rospy.init_node('example_sendupdate')
00037 
00038 start_pose = { 'ffj0': 27.0, 'ffj3': 0, 'ffj4': 0,
00039                'rfj0': 40.0, 'rfj3': 0, 'rfj4': 0,
00040                'thj1': 20, 'thj2': 31, 'thj3': 0, 'thj4': 25, 'thj5':-29,       
00041                'wrj1': 0, 'wrj2': 0 
00042 }
00043 
00044 print "Start"
00045 sendupdate(start_pose)
00046 rospy.sleep(2)
00047 
00048 # FF curl
00049 sendupdate({ 'ffj3': 90 })
00050 rospy.sleep(3)
00051 sendupdate({ 'ffj0': 120 })
00052 rospy.sleep(2)
00053 sendupdate(start_pose)
00054 rospy.sleep(4)
00055 
00056 # RF curl
00057 sendupdate({ 'rfj0': 120, 'rfj3': 90 })
00058 rospy.sleep(4)
00059 sendupdate(start_pose)
00060 rospy.sleep(4)
00061 
00062 # Back to the start
00063 sendupdate(start_pose)
00064 rospy.sleep(2)
00065 
00066 # Wrist wave
00067 sendupdate({ 'wrj2': 10 })
00068 rospy.sleep(6)
00069 sendupdate({ 'wrj2': -20 })
00070 rospy.sleep(6)
00071 sendupdate({ 'wrj2': 0 })
00072 rospy.sleep(4)
00073 
00074 # Back to the start
00075 sendupdate(start_pose)
00076 
00077 rospy.sleep(2)


sr_edc_muscle_tools
Author(s): Shadow Hand
autogenerated on Mon Oct 6 2014 07:36:31