<s> '''<STR_LIT>''' <EOL> from abc import ABCMeta , abstractmethod <EOL> import copy <EOL> import rospy <EOL> import numpy <EOL> from joint_pose_dict import JointPoseDictionary <EOL> from command_sequence import CommandSequence <EOL> from proxy_depend import ProxyDepends <EOL> class ProxyCommand ( ) : <EOL> key_command_type = "<STR_LIT:type>" <EOL> key_command_spec = "<STR_LIT>" <EOL> key_command_wait_depend = "<STR_LIT>" <EOL> key_command_set_depend = "<STR_LIT>" <EOL> SYS_BEGIN = "<STR_LIT>" <EOL> class BaseProxy ( object ) : <EOL> __metaclass__ = ABCMeta <EOL> _proxy_state_stopped = - <NUM_LIT:1> <EOL> _proxy_state_running = <NUM_LIT:1> <EOL> def __init__ ( self ) : <EOL> '''<STR_LIT>''' <EOL> self . proxy_state = self . _proxy_state_running <EOL> self . arm_num = None <EOL> self . _frame_id = "<STR_LIT>" <EOL> self . positions = None <EOL> self . commands = None <EOL> self . _arm_joint_names = None <EOL> self . _gripper_joint_names = None <EOL> self . _end_effector_link = None <EOL> self . _frame_id = None <EOL> self . _arm_goal = None <EOL> self . _gripper_goal = None <EOL> self . depends_status = ProxyDepends ( self . arm_num ) <EOL> self . locks = { } <EOL> def reset_depend_status ( self ) : <EOL> rospy . loginfo ( "<STR_LIT>" ) <EOL> self . depends_status . reset_database ( ) <EOL> def wait_for_state ( self , state ) : <EOL> while True : <EOL> if self . proxy_state == state : break <EOL> def load_control_plan ( self , path_to_dict_yaml , path_to_cmds_yaml ) : <EOL> self . positions = JointPoseDictionary ( path_to_dict_yaml ) <EOL> rospy . logdebug ( self . positions ) <EOL> self . commands = CommandSequence ( path_to_cmds_yaml ) <EOL> rospy . logdebug ( self . commands ) <EOL> def sleep ( self , rate_obj , fsecs ) : <EOL> rospy . sleep ( fsecs ) <EOL> def wait_for_system_begin ( self ) : <EOL> self . depends_status . wait_for_depend ( ProxyCommand . SYS_BEGIN ) <EOL> def wait_for_depend ( self , cmd ) : <EOL> '''<STR_LIT>''' <EOL> if cmd . has_key ( ProxyCommand . key_command_wait_depend ) : <EOL> wait_depend_name = cmd [ ProxyCommand . key_command_wait_depend ] <EOL> self . depends_status . wait_for_depend ( wait_depend_name ) <EOL> def get_depend_status ( self , name ) : <EOL> return self . depends_status . get_depend_status ( name ) <EOL> def clear_depend ( self , cmd ) : <EOL> '''<STR_LIT>''' <EOL> if cmd . has_key ( ProxyCommand . key_command_wait_depend ) : <EOL> self . depends_status . transmit_update_depend ( cmd [ ProxyCommand . key_command_wait_depend ] , False ) <EOL> def set_depend ( self , cmd ) : <EOL> '''<STR_LIT>''' <EOL> if cmd . has_key ( ProxyCommand . key_command_set_depend ) : <EOL> self . depends_status . transmit_update_depend ( cmd [ ProxyCommand . key_command_set_depend ] , True ) <EOL> @ classmethod <EOL> def measure_euclidean_distance ( cls , x , y ) : <EOL> '''<STR_LIT>''' <EOL> x = numpy . asfarray ( x ) <EOL> y = numpy . asfarray ( y ) <EOL> d = numpy . sqrt ( numpy . sum ( ( x - y ) ** <NUM_LIT:2> ) ) <EOL> return d <EOL> @ classmethod <EOL> def measure_joint_distance_sum ( cls , x , y ) : <EOL> '''<STR_LIT>''' <EOL> x = numpy . asfarray ( x ) <EOL> y = numpy . asfarray ( y ) <EOL> d = numpy . sum ( numpy . abs ( x - y ) ) <EOL> return d <EOL> @ classmethod <EOL> def measure_joint_velocity_ss ( cls , x ) : <EOL> v = numpy . asfarray ( x ) <EOL> d = numpy . sum ( v ** <NUM_LIT:2> ) <EOL> return d <EOL> @ property <EOL> def frame_id ( self ) : <EOL> return self . _frame_id <EOL> @ frame_id . setter <EOL> def frame_id ( self , name ) : <EOL> self . _frame_id = name <EOL> @ property <EOL> def end_effector_link ( self ) : <EOL> return self . _end_effector_link <EOL> @ end_effector_link . setter <EOL> def end_effector_link ( self , name ) : <EOL> self . _end_effector_link = name <EOL> @ property <EOL> def arm_joint_names ( self ) : <EOL> return self . _arm_joint_names <EOL> @ arm_joint_names . setter <EOL> def arm_joint_names ( self , names ) : <EOL> self . _arm_joint_names = copy . deepcopy ( names ) <EOL> @ property <EOL> def gripper_joint_names ( self ) : <EOL> return self . _gripper_joint_names <EOL> @ gripper_joint_names . setter <EOL> def gripper_joint_names ( self , names ) : <EOL> self . _gripper_joint_names = copy . deepcopy ( names ) <EOL> @ abstractmethod <EOL> def plan_arm ( self , pose ) : raise NotImplementedError ( ) <EOL> @ abstractmethod <EOL> def move_arm ( self ) : raise NotImplementedError ( ) <EOL> @ abstractmethod <EOL> def move_gripper ( self , opening_mm ) : raise NotImplementedError ( ) <EOL> @ abstractmethod <EOL> def control_loop ( self ) : raise NotImplementedError ( ) </s>
