shadowhand_commander.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 
00003 # Copyright 2014 Shadow Robot Company Ltd.
00004 #
00005 # This program is free software: you can redistribute it and/or modify it
00006 # under the terms of the GNU General Public License as published by the Free
00007 # Software Foundation, either version 2 of the License, or (at your option)
00008 # any later version.
00009 #
00010 # This program is distributed in the hope that it will be useful, but WITHOUT
00011 # ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
00012 # FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
00013 # more details.
00014 #
00015 # You should have received a copy of the GNU General Public License along
00016 # with this program.  If not, see <http://www.gnu.org/licenses/>.
00017 
00018 import rospy
00019 import threading
00020 
00021 from sr_robot_msgs.msg import sendupdate, joint
00022 from sr_hand.shadowhand_ros import ShadowHand_ROS
00023 from sr_hand.grasps_interpoler import GraspInterpoler
00024 from sr_hand.Grasp import Grasp
00025 
00026 """
00027 Module to provide a quick and easy way to script the Shadow Hand.
00028 This is done via creating a short script with a Commander object then calling
00029 methods on the commander to script the motion.
00030 
00031 Basic script will look like:
00032 
00033     #!/usr/bin/python
00034     import rospy
00035     from sr_hand.shadowhand_commander import Commander
00036 
00037     rospy.init_node('basic_example')
00038     c = Commander()
00039 
00040     c.move_hand({
00041         "THJ1": 0, "THJ2": 6, "THJ3": 10, "THJ4": 37, "THJ5": 9,
00042         "FFJ0": 21, "FFJ3": 26, "FFJ4": 0,
00043         "MFJ0": 18, "MFJ3": 24, "MFJ4": 0,
00044         "RFJ0": 30, "RFJ3": 16, "RFJ4": 0,
00045         "LFJ0": 30, "LFJ3": 0, "LFJ4": -10, "LFJ5": 10
00046     })
00047     rospy.sleep(3.0)
00048 
00049 
00050 See the examples directory in the package sr_examples.
00051 """
00052 
00053 class Commander(object):
00054     def __init__(self):
00055         rospy.logwarn("The class Commander in package sr_hand is deprecated. "
00056                       "Please use SrHandCommander from package sr_robot_commander")
00057 
00058         # Mutex to control thread access to certain operations
00059         self.mutex = threading.Lock()
00060 
00061         # This is used to store the grasp interpolators for the different threads.
00062         self.grasp_interpolators = {}
00063 
00064         # Shadow hand setup
00065         self.hand = ShadowHand_ROS()
00066 
00067         # Period in seconds between interpolated commands (e.g. if we want an interpolation time of 1s and have a period of 0.1,
00068         # the interpoler will use 10 steps
00069         self.hand_interpolation_period = 0.01
00070         if self.hand.check_hand_type() == "gazebo":
00071             # Interpolation is slower under simulation compared to real hand
00072             self.hand_interpolation_period = 0.1
00073 
00074 
00075     def move_hand(self, command):
00076         """
00077         Move the Shadow Hand.
00078 
00079         This call is non blocking. It will return before the motion has finished.
00080 
00081         If called with interpolation time of 0.0 the joints will move to the target position
00082         at maximum velocity (i.e. the max. vel. allowed by the position controller for that joint)
00083 
00084         If called with interpolation time greater than self.hand_interpolation_period,
00085         a separate thread will deal with the interpolation time and will keep
00086         sending targets to the hand until the last target is sent.
00087 
00088         If move_hand() is called again before the previous movements have finished,
00089         the new call will override only the conciding joints. The others will keep moving
00090         to their previous targets at their previous velocity.
00091 
00092         @param command - Dictionary of joint names in the keys and angles in
00093         degrees in the values. The key interpolation_time gives the time in seconds that
00094         the movement will last.
00095         """
00096 
00097         # Copy the dictionary, so that we will not affect the original user command
00098         joints = dict(command)
00099 
00100         interpolation_time = 0.0
00101         if 'interpolation_time' in joints:
00102             interpolation_time = joints['interpolation_time']
00103             del joints['interpolation_time']
00104 
00105         if interpolation_time == 0.0:
00106             self.mutex.acquire()
00107             self._prune_interpolators(joints)
00108             self.mutex.release()
00109             self.hand.sendupdate_from_dict(joints)
00110         else:
00111             threading.Thread(target=self._move_hand_interpolation,
00112                              args=(joints, interpolation_time)).start()
00113 
00114 
00115     def _move_hand_interpolation(self, joints, interpolation_time=1.0):
00116         """
00117         Remove the coinciding joints from the currently running interpolators.
00118         It actually removes the joints from the 'grasp_to' list of the interpolator.
00119 
00120         @param joints - Dictionary of joint names in the keys and angles in
00121         degrees in the values.
00122         @param interpolation_time - A float. Interpolation time in seconds.
00123         """
00124 
00125         rospy.logdebug("call interpolation")
00126         thread_name = threading.current_thread().name
00127         try:
00128             self.mutex.acquire()
00129             self._prune_interpolators(joints)
00130             self.mutex.release()
00131 
00132             rospy.logdebug("start interpolation")
00133             current_grasp = Grasp()
00134             current_grasp.joints_and_positions = self.hand.read_all_current_positions()
00135             target_grasp = Grasp()
00136             target_grasp.joints_and_positions = joints
00137 
00138             interpolator = GraspInterpoler(current_grasp, target_grasp)
00139             self.grasp_interpolators[thread_name] = interpolator
00140 
00141             r = rospy.Rate(1.0 / self.hand_interpolation_period)
00142 
00143             for interpolation in range (1, int(interpolation_time / self.hand_interpolation_period) + 1):
00144                 self.mutex.acquire()
00145                 targets_to_send = interpolator.interpolate(100.0 * interpolation * self.hand_interpolation_period / interpolation_time)
00146                 self.mutex.release()
00147                 self.hand.sendupdate_from_dict(targets_to_send)
00148                 #rospy.loginfo("sent cmd n: %s" %(str(interpolation), ))
00149                 r.sleep()
00150         finally:
00151             self.mutex.acquire()
00152             self.grasp_interpolators.pop(thread_name, None)
00153             self.mutex.release()
00154             rospy.logdebug("end interpolation")
00155 
00156 
00157     def _prune_interpolators(self, joints):
00158         """
00159         Remove the coinciding joints from the currently running interpolators.
00160         It actually removes the joints from the 'grasp_to' list of the interpolator.
00161 
00162         @param joints - Dictionary of joint names in the keys and angles in
00163         degrees in the values.
00164         """
00165         rospy.logdebug("Call prune from thread %s", threading.current_thread().name )
00166         for thread_id in self.grasp_interpolators.keys():
00167             for joint_name in joints.keys():
00168                 self.grasp_interpolators[thread_id].grasp_to.joints_and_positions.pop(joint_name, None)
00169                 rospy.logdebug("Prune joint %s thread %s", joint_name, thread_id )
00170 
00171 
00172     def get_hand_position(self):
00173         """
00174         Returns a dictionary with the position of each joint in degrees.
00175         """
00176         return dict(self.hand.read_all_current_positions())
00177 
00178     def get_hand_velocity(self):
00179         """
00180         Returns a dictionary with the velocity of each joint in degrees/s.
00181         """
00182         return dict(self.hand.read_all_current_velocities())
00183 
00184     def get_hand_effort(self):
00185         """
00186         Returns a dictionary with the effort of each joint. Currently in ADC units, as no calibration is performed on the strain gauges.
00187         """
00188         return dict(self.hand.read_all_current_efforts())
00189 
00190     def get_tactile_type(self):
00191         """
00192         Returns a string indicating the type of tactile sensors present. Possible values are: PST, biotac, UBI0 .
00193         """
00194         return self.hand.get_tactile_type()
00195 
00196     def get_tactile_state(self):
00197         """
00198         Returns an object containing tactile data. The structure of the data is different for every tactile_type .
00199         """
00200         return self.hand.get_tactile_state()
00201 
00202 


sr_hand
Author(s): Ugo Cupcic
autogenerated on Fri Aug 21 2015 12:24:55