class Ctrl(): def __init__(self, topic="tmobile"): self.sub = rospy.Subscriber(topic, direction, self.callback, None, 1) print 'start subscribing to topic', topic try: print 'initialize platform node' rospy.init_node("platform", anonymous=False) except rospy.ROSException, e: pass # Robotis servo self.tilt = rs.robotis_servo('/dev/robot/servo0', 5, baudrate=57600) self.pan = rs.robotis_servo('/dev/robot/servo0', 25, baudrate=57600) # self.tilt = rs.robotis_servo('/dev/robot/servo0', 26, baudrate=57600) self.angle1 = 0 self.angle2 = 0 self.reset = 0 # Zenither self.z = zenither.Zenither(robot='HRL2') self.z_dir = 0 self.init_height = 1. #initial zenither height = 1m # Segway Omni self.mec = segway.Mecanum(init_ros_node=False) self.xvel = 0. self.yvel = 0. self.avel = 0. self.lock = 0.
def initialize_zenither(self, mode): # print 'Initializing Zenither' rospy.loginfo('Initializing Zenither') self.z = zenither.Zenither(robot='test_rig') self.time_since_last_cb = rospy.Time.now() self.force_threshold_exceeded_pub = rospy.Publisher( '/ft_threshold_exceeded', Bool, queue_size=1) self.force_threshold_exceeded_sub = rospy.Subscriber( '/ft_threshold_exceeded', Bool, self.ft_threshold_cb) self.ft_sleeve_sub = rospy.Subscriber('/force_torque_sleeve', WrenchStamped, self.ft_sleeve_cb) # self.ft_arm_sub = rospy.Subscriber('/force_torque_arm', WrenchStamped, self.ft_arm_cb) if mode == 'calib': self.zenither_calibrate() # print 'Calibrating Zenither' rospy.loginfo('Calibrating Zenither') if not self.z.calibrated: self.zenither_calibrate() # print 'Calibrating Zenither' rospy.loginfo('Calibrating Zenither') # print 'Zenither ready for action!' rospy.loginfo('Zenither ready for action!') self.repeated_movements()
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
from pygame.locals import * if __name__=='__main__': p = optparse.OptionParser() p.add_option('-z', action='store_true', dest='zenither', help='control the zenither also') opt, args = p.parse_args() zenither_flag = opt.zenither if zenither_flag: import zenither.zenither as zenither z = zenither.Zenither(robot='HRL2') cmd_node = sc.SegwayCommand() max_xvel = 0.18 max_yvel = 0.15 max_speed = 0.18 # don't exceed 0.18 under any condition. max_avel = 0.18 xvel = 0.0 yvel = 0.0 avel = 0.0 #init pygame pygame.init()
dest='sleeve', help='Move actuator to pull sleeve on arm.') p.add_option('--sine_expt', action='store_true', dest='sine_expt', help='run the sinusoid experiment.') p.add_option('--cmp_sine_pkl', action='store', dest='cmp_sine_pkl', type='string', default='', help='pkl saved by test_sine.') opt, args = p.parse_args() z = zenither.Zenither(robot='test_rig') if opt.calib: calibrate(z) if not z.calibrated: calibrate(z) if opt.test_lin: test_linear(z) if opt.test_sine: A0 = 0.5 A = 0.05 #0.02 freq = 0.1 #0.50 test_sinusoid(z, A0, A, freq)
self.apply_torque) self.stmp = rospy.Service('/zenither/torque_move_position', Float_Int, self.torque_move_position) def move_position(self, msg): print 'move_position is UNTESTED' #self.zenither.move_position( msg.value ) return True def apply_torque(self, req): self.zenither.nadir(req.value) return True def estop(self, req): self.zenither.estop() return True def torque_move_position(self, req): self.zenither.torque_move_position(req.value) return True if __name__ == '__main__': import zenither.zenither as zenither z = zenither.Zenither('HRL2', pose_read_thread = True) zs = ZenitherServer(z) rospy.spin() # rosservice call /zenither/move_position 0.3