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