Go to the documentation of this file.00001
00002 import roslib
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
00010
00011
00012
00013
00014
00015
00016
00017
00018 joint_names = ["thj1", "thj2", "thj3", "thj4", "thj5", "ffj0",
00019 "ffj3", "ffj4", "rfj0", "rfj3", "rfj4", "wrj1", "wrj2"]
00020 joint_pubs = {}
00021 for jname in joint_names:
00022 topic = "/sh_" + jname + "_muscle_position_controller/command"
00023 print topic
00024 joint_pubs[jname] = rospy.Publisher(topic, Float64, latch=True)
00025
00026
00027 def sendupdate(joints):
00028 print "Sending:"
00029 for jname in joints:
00030 if jname not in joint_pubs:
00031 print "\tJoint %s not found" % jname
00032 return
00033 msg = Float64()
00034 msg.data = math.radians(float(joints[jname]))
00035 print "\t" + jname + ": " + str(joints[jname])
00036 joint_pubs[jname].publish(msg)
00037
00038 rospy.init_node('example_sendupdate')
00039
00040 start_pose = {'ffj0': 27.0, 'ffj3': 0, 'ffj4': 0,
00041 'rfj0': 40.0, 'rfj3': 0, 'rfj4': 0,
00042 'thj1': 20, 'thj2': 31, 'thj3': 0, 'thj4': 25, 'thj5': -29,
00043 'wrj1': 0, 'wrj2': 0
00044 }
00045
00046 print "Start"
00047 sendupdate(start_pose)
00048 rospy.sleep(2)
00049
00050
00051 sendupdate({'ffj3': 90})
00052 rospy.sleep(3)
00053 sendupdate({'ffj0': 120})
00054 rospy.sleep(2)
00055 sendupdate(start_pose)
00056 rospy.sleep(4)
00057
00058
00059 sendupdate({'rfj0': 120, 'rfj3': 90})
00060 rospy.sleep(4)
00061 sendupdate(start_pose)
00062 rospy.sleep(4)
00063
00064
00065 sendupdate(start_pose)
00066 rospy.sleep(2)
00067
00068
00069 sendupdate({'wrj2': 10})
00070 rospy.sleep(6)
00071 sendupdate({'wrj2': -20})
00072 rospy.sleep(6)
00073 sendupdate({'wrj2': 0})
00074 rospy.sleep(4)
00075
00076
00077 sendupdate(start_pose)
00078
00079 rospy.sleep(2)