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