def init_thok(self): if self.thok: return print "Init THOK" self.hok = hs.Hokuyo('utm',self.config.thok_hoknum,flip=False) self.thok = ths.tilt_hokuyo(self.config.thok_devname,self.config.thok_servonum,self.hok,l1=self.config.thok_l1,l2=self.config.thok_l2) print "Init THOK done"
def init_thok(self): if self.thok: return print "Init THOK" self.hok = hs.Hokuyo('utm', self.config.thok_hoknum, flip=False) self.thok = ths.tilt_hokuyo(self.config.thok_devname, self.config.thok_servonum, self.hok, l1=self.config.thok_l1, l2=self.config.thok_l2) print "Init THOK done"
def __init__(self, move_segway=False, use_right_arm=True, use_left_arm=True, set_wrist_theta_gc=False, end_effector_length=0.12818): self.mech_kinematics_lock = RLock() self.fit_circle_lock = RLock() if use_right_arm: # stiffness in Nm/rad: [20,50,15,25,2.5] #settings_r = hr.MekaArmSettings(stiffness_list=[0.1939,0.6713,0.748,0.7272,0.75]) # stiffness in Nm/rad: [20,50,20,25,2.5] settings_r = hr.MekaArmSettings(stiffness_list=[0.1939,0.6713,0.997,0.7272,0.75]) #settings_r = hr.MekaArmSettings(stiffness_list=[0.7,0.7,0.9,0.9,0.7]) # for RIP project else: settings_r = None if use_left_arm: if set_wrist_theta_gc: settings_l = hr.MekaArmSettings(stiffness_list=[0.1939,0.6713,0.748,0.7272,.8,.3,1.], control_mode = 'wrist_theta_gc') else: settings_l = hr.MekaArmSettings(stiffness_list=[0.1939,0.6713,0.748,0.7272,0.75]) print 'left arm assigned' #settings_l = hr.MekaArmSettings(stiffness_list=[0.7,0.7,0.9,0.9,0.7]) # for RIP project else: settings_l = None print 'left arm NOT assigned' self.firenze = hr.M3HrlRobot(connect = True, left_arm_settings = settings_l, right_arm_settings = settings_r, end_effector_length = end_effector_length) rospy.init_node('compliant_trajectory', anonymous = False) rospy.Subscriber('mechanism_kinematics_rot', MechanismKinematicsRot, self.mechanism_kinematics_rot_cb) rospy.Subscriber('mechanism_kinematics_jac', MechanismKinematicsJac, self.mechanism_kinematics_jac_cb) self.mech_traj_pub = rospy.Publisher('mechanism_trajectory', Point32) self.hok = hs.Hokuyo('utm', 0, flip = True, ros_init_node = False) self.thok = ths.tilt_hokuyo('/dev/robot/servo0',5,self.hok,l1=0.,l2=-0.055) self.cam = camera.Camera('mekabotUTM') self.set_camera_settings() self.z = zenither.Zenither(robot='HRL2', pose_read_thread=True) self.zenither_client = zc.ZenitherClient() self.zenither_list = [] self.zenither_moving = False self.move_segway_flag = move_segway self.segway_trajectory = at.PlanarTajectory() self.move_segway_lock = RLock() self.segway_motion_tup = (0.0,0.0,0.0) self.new_segway_command = False self.dist_boundary = 0.05 self.eq_pt_close_to_bndry = False # used in segway_motion_local self.workspace_dict = ut.load_pickle(os.environ['HRLBASEPATH']+'/src/projects/equilibrium_point_control/pkls/workspace_dict_2010Feb20_225427.pkl') if move_segway: self.segway_command_node = sc.SegwayCommand(vel_topic='/hrl/segway/command', pose_local_topic='/hrl/segway/pose_local', pose_global_topic='/hrl/segway/pose_global', stop_topic='/hrl/segway/stop', max_vel_topic='/hrl/segway/max_vel', ros_init_node=False) #self.segway_pose = vop.vo_pose('/hrl/segway/pose',ros_init_node=False) self.segway_pose = vop.vo_pose('pose',ros_init_node=False) self.eq_pt_not_moving_counter = 0