Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 import rospy
00019
00020 from sr_robot_commander import SrRobotCommander
00021 from sr_robot_msgs.srv import ForceController
00022 from sr_hand.tactile_receiver import TactileReceiver
00023
00024
00025 class SrHandCommander(SrRobotCommander):
00026 """
00027 Commander class for hand
00028 """
00029
00030 __set_force_srv = {}
00031
00032 def __init__(self, name="right_hand", prefix="rh"):
00033 """
00034 Initialize object
00035 @param name - name of the MoveIt group
00036 @param prefix - prefix used for the tactiles and max_force
00037 """
00038 super(SrHandCommander, self).__init__(name)
00039 self._tactiles = TactileReceiver(prefix)
00040
00041
00042 self._topic_prefix = prefix
00043 if self._topic_prefix and not self._topic_prefix.endswith("/"):
00044 self._topic_prefix += "/"
00045
00046 def get_joints_effort(self):
00047 """
00048 Returns joints effort
00049 @return - dictionary with joints efforts
00050 """
00051 return self._get_joints_effort()
00052
00053 def set_max_force(self, joint_name, value):
00054 """
00055 Set maximum force for hand
00056 @param value - maximum force value
00057 """
00058
00059
00060
00061 if not self.__set_force_srv.get(joint_name):
00062 service_name = "realtime_loop/" + self._topic_prefix + \
00063 "change_force_PID_"+joint_name.upper()
00064 self.__set_force_srv[joint_name] = \
00065 rospy.ServiceProxy(service_name,
00066 ForceController)
00067
00068
00069 motor_settings = None
00070 try:
00071 motor_settings = rospy.get_param(self._topic_prefix +
00072 joint_name.lower() + "/pid")
00073 except KeyError, e:
00074 rospy.logerr("Couldn't get the motor parameters for joint " +
00075 joint_name + " -> " + str(e))
00076
00077
00078 motor_settings["imax"] = value
00079
00080 try:
00081
00082 self.__set_force_srv[joint_name](motor_settings["max_pwm"],
00083 motor_settings["sg_left"],
00084 motor_settings["sg_right"],
00085 motor_settings["f"],
00086 motor_settings["p"],
00087 motor_settings["i"],
00088 motor_settings["d"],
00089 motor_settings["imax"],
00090 motor_settings["deadband"],
00091 motor_settings["sign"])
00092 except rospy.ServiceException, e:
00093 rospy.logerr("Couldn't set the max force for joint " +
00094 joint_name + ": " + str(e))
00095
00096 def get_tactile_type(self):
00097 """
00098 Returns a string indicating the type of tactile sensors present.
00099 Possible values are: PST, biotac, UBI0.
00100 """
00101 return self._tactiles.get_tactile_type()
00102
00103 def get_tactile_state(self):
00104 """
00105 Returns an object containing tactile data. The structure of the
00106 data is different for every tactile_type.
00107 """
00108 return self._tactiles.get_tactile_state()