ric_arm_rc.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 import roslib; roslib.load_manifest('ric_robot')
00003 import rospy
00004 import math
00005 from ric_robot.msg import *
00006 from std_msgs.msg import Float64
00007 from math import *
00008 from sensor_msgs.msg import JointState
00009 
00010 def rc_callback(data):
00011   global right_finger_pub,right_finger_min,right_finger_max
00012   global left_finger_pub,left_finger_min,left_finger_max
00013   global wrist_pub,wrist_min,wrist_max, wrist_ang
00014   global elbow2_pub,elbow2_min,elbow2_max, elbow2_ang
00015   global elbow1_pub,elbow1_min,elbow1_max, elbow1_ang
00016   global shoulder_pub,shoulder_min,shoulder_max, shoulder_ang
00017   global base_rotation_pub,base_rotation_min,base_rotation_max, base_rotation_ang
00018   global got_links_states
00019   global init
00020   if got_links_states==False:
00021     #rospy.loginfo("Waiting for joint states");
00022     return
00023   if init:  
00024      wrist_pub.publish(wrist_ang)
00025      elbow1_pub.publish(elbow1_ang)
00026      elbow2_pub.publish(elbow2_ang)
00027      shoulder_pub.publish(shoulder_ang)
00028      base_rotation_pub.publish(base_rotation_ang) 
00029      init=False
00030   
00031   #rospy.loginfo("commanding...")
00032   msg=right_finger_min+((right_finger_max-right_finger_min)/(2000.0-1000.0))*(data.RX3-1000.0)
00033   msg=math.pi-msg*2.0*math.pi/4096.0
00034   right_finger_pub.publish(-msg)
00035   
00036   #msg=left_finger_min+((left_finger_max-left_finger_min)/(2000.0-1000.0))*(data.RX3-1000.0)
00037   #msg=math.pi-msg*2.0*math.pi/4096.0
00038   left_finger_pub.publish(+msg)
00039   
00040   
00041   if data.RX4 > 1600:
00042      wrist_ang+=0.02
00043   elif data.RX4 < 1400:
00044      wrist_ang-=0.02
00045   if wrist_ang> math.pi-wrist_min*2.0*math.pi/4096.0:
00046     wrist_ang=math.pi-wrist_min*2.0*math.pi/4096.0
00047   elif wrist_ang< math.pi-wrist_max*2.0*math.pi/4096.0:
00048     wrist_ang=math.pi-wrist_max*2.0*math.pi/4096.0
00049   wrist_pub.publish(wrist_ang)
00050   
00051   
00052   if data.RX6 < 1500:
00053      if data.RX2 > 1600:
00054         elbow2_ang-=0.02
00055      elif data.RX2 < 1400:
00056         elbow2_ang+=0.02
00057      if elbow2_ang> math.pi-elbow2_min*2.0*math.pi/4096.0:
00058        elbow2_ang=math.pi-elbow2_min*2.0*math.pi/4096.0
00059      elif elbow2_ang< math.pi-elbow2_max*2.0*math.pi/4096.0:
00060        elbow2_ang=math.pi-elbow2_max*2.0*math.pi/4096.0
00061      elbow2_pub.publish(elbow2_ang) 
00062   
00063      if data.RX1 > 1650:
00064         elbow1_ang+=0.02
00065      elif data.RX1 < 1400:
00066         elbow1_ang-=0.02
00067      if elbow1_ang> math.pi-elbow1_min*2.0*math.pi/4096.0:
00068        elbow1_ang=math.pi-elbow1_min*2.0*math.pi/4096.0
00069      elif elbow1_ang< math.pi-elbow1_max*2.0*math.pi/4096.0:
00070        elbow1_ang=math.pi-elbow1_max*2.0*math.pi/4096.0
00071      elbow1_pub.publish(elbow1_ang) 
00072   else:
00073      if data.RX2 > 1600:
00074         shoulder_ang-=0.02
00075      elif data.RX2 < 1400:
00076         shoulder_ang+=0.02
00077      if shoulder_ang> -(math.pi-shoulder_max*2.0*math.pi/4096.0):
00078        shoulder_ang=-(math.pi-shoulder_max*2.0*math.pi/4096.0)
00079      elif shoulder_ang< -(math.pi-shoulder_min*2.0*math.pi/4096.0):
00080        shoulder_ang=-(math.pi-shoulder_min*2.0*math.pi/4096.0)
00081      shoulder_pub.publish(shoulder_ang)
00082      
00083      if data.RX1 > 1650:
00084         base_rotation_ang+=0.02
00085      elif data.RX1 < 1400:
00086         base_rotation_ang-=0.02
00087      if base_rotation_ang> math.pi-base_rotation_min*2.0*math.pi/4096.0:
00088        base_rotation_ang=math.pi-base_rotation_min*2.0*math.pi/4096.0
00089      elif base_rotation_ang< math.pi-base_rotation_max*2.0*math.pi/4096.0:
00090        base_rotation_ang=math.pi-base_rotation_max*2.0*math.pi/4096.0
00091      base_rotation_pub.publish(base_rotation_ang) 
00092      
00093   #rospy.loginfo(shoulder_ang)
00094   #rospy.loginfo(math.pi-wrist_max*2.0*math.pi/4096.0)
00095   #rospy.loginfo(math.pi-wrist_min*2.0*math.pi/4096.0)
00096   #rospy.loginfo(data.RX6)
00097   #rospy.loginfo("--")
00098   
00099   
00100 def js_callback(data):
00101   global got_links_states
00102   global  wrist_ang,elbow2_ang,elbow1_ang, shoulder_ang, base_rotation_ang
00103   global init
00104   if got_links_states==False and (len(data.position)==7 or len(data.position)==8) and 'base_rotation' in data.name[0]:
00105     base_rotation_ang=data.position[0]
00106     shoulder_ang=data.position[1]
00107     elbow1_ang=data.position[2]
00108     elbow2_ang=data.position[3]
00109     wrist_ang=data.position[4]
00110     
00111     got_links_states=True
00112     init=True
00113     #rospy.loginfo(shoulder_ang)
00114     rospy.loginfo("RC Control ready!")
00115   
00116 def ric_arm_rc():
00117    global right_finger_pub,right_finger_min,right_finger_max
00118    global left_finger_pub,left_finger_min,left_finger_max
00119    global wrist_pub,wrist_min,wrist_max, wrist_ang
00120    global elbow2_pub,elbow2_min,elbow2_max, elbow2_ang
00121    global elbow1_pub,elbow1_min,elbow1_max, elbow1_ang
00122    global shoulder_pub,shoulder_min,shoulder_max, shoulder_ang
00123    global base_rotation_pub,base_rotation_min,base_rotation_max, base_rotation_ang
00124    global got_links_states
00125    global init
00126    init=False
00127    got_links_states=False;
00128    wrist_ang=0.0;
00129    elbow2_ang=0.0;
00130    elbow1_ang=0.0;
00131    shoulder_ang=0.0;
00132    base_rotation_ang=0.0;
00133    ns=rospy.get_namespace()
00134    rospy.init_node('ric_gui', anonymous=True)
00135    ns=rospy.get_namespace()
00136    right_finger_max = rospy.get_param("right_finger_controller/motor/max")
00137    right_finger_min = rospy.get_param("right_finger_controller/motor/min")
00138    left_finger_max = rospy.get_param("left_finger_controller/motor/max")
00139    left_finger_min = rospy.get_param("left_finger_controller/motor/min")
00140    wrist_max = rospy.get_param("wrist_controller/motor/max")
00141    wrist_min = rospy.get_param("wrist_controller/motor/min")
00142    elbow2_max = rospy.get_param("elbow2_controller/motor/max")
00143    elbow2_min = rospy.get_param("elbow2_controller/motor/min")
00144    elbow1_max = rospy.get_param("elbow1_controller/motor/max")
00145    elbow1_min = rospy.get_param("elbow1_controller/motor/min")
00146    shoulder_max = rospy.get_param("shoulder_controller/motor/max")
00147    shoulder_min = rospy.get_param("shoulder_controller/motor/min")
00148    base_rotation_max = rospy.get_param("base_rotation_controller/motor/max")
00149    base_rotation_min = rospy.get_param("base_rotation_controller/motor/min")
00150    rospy.Subscriber(ns+"RC",ric_rc, rc_callback)
00151    left_finger_pub = rospy.Publisher(ns+"left_finger_controller/command", Float64);
00152    right_finger_pub = rospy.Publisher(ns+"right_finger_controller/command", Float64)
00153    wrist_pub = rospy.Publisher(ns+"wrist_controller/command", Float64)
00154    elbow2_pub = rospy.Publisher(ns+"elbow2_controller/command", Float64)   
00155    elbow1_pub = rospy.Publisher(ns+"elbow1_controller/command", Float64)   
00156    shoulder_pub = rospy.Publisher(ns+"shoulder_controller/command", Float64)   
00157    base_rotation_pub = rospy.Publisher(ns+"base_rotation_controller/command", Float64) 
00158   
00159    rospy.Subscriber("joint_states", JointState, js_callback)
00160   
00161    while not rospy.is_shutdown():
00162      #rospy.loginfo("test")
00163      rospy.sleep(0.1)
00164         
00165 if __name__ == '__main__':
00166    ric_arm_rc()
00167         


ric_robot
Author(s): RoboTiCan
autogenerated on Thu Aug 27 2015 14:40:59