def initialize_joints(self, right_arm_settings, left_arm_settings):
self.proxy = m3p.M3RtProxy()
self.proxy.start(True, True) # the second true enables ROS (needed for the skin patch)
for c in ['m3pwr_pwr003','m3loadx6_ma1_l0','m3arm_ma1','m3loadx6_ma2_l0','m3arm_ma2']:
if not self.proxy.is_component_available(c):
raise m3t.M3Exception('Component '+c+' is not available.')
self.joint_list_dict = {}
right_l = []
for c in ['m3joint_ma1_j0','m3joint_ma1_j1','m3joint_ma1_j2',
'm3joint_ma1_j3','m3joint_ma1_j4','m3joint_ma1_j5',
'm3joint_ma1_j6']:
if not self.proxy.is_component_available(c):
raise m3t.M3Exception('Component '+c+' is not available.')
right_l.append(m3f.create_component(c))
self.joint_list_dict['right_arm'] = right_l
left_l = []
for c in ['m3joint_ma2_j0','m3joint_ma2_j1','m3joint_ma2_j2',
'm3joint_ma2_j3','m3joint_ma2_j4','m3joint_ma2_j5',
'm3joint_ma2_j6']:
if not self.proxy.is_component_available(c):
raise m3t.M3Exception('Component '+c+' is not available.')
left_l.append(m3f.create_component(c))
self.joint_list_dict['left_arm'] = left_l
for arm,arm_settings in zip(['right_arm','left_arm'],[right_arm_settings,left_arm_settings]):
if arm_settings == None:
continue
for comp in self.joint_list_dict[arm]:
self.proxy.subscribe_status(comp)
self.proxy.publish_command(comp)
self.set_arm_settings(right_arm_settings,left_arm_settings)
right_fts=m3.loadx6.M3LoadX6('m3loadx6_ma1_l0')
self.proxy.subscribe_status(right_fts)
left_fts=m3.loadx6.M3LoadX6('m3loadx6_ma2_l0')
self.proxy.subscribe_status(left_fts)
self.fts = {'right_arm':right_fts,'left_arm':left_fts}
#self.pwr=m3w.M3Pwr('m3pwr_pwr003')
self.pwr=m3f.create_component('m3pwr_pwr003')
self.proxy.subscribe_status(self.pwr)
self.proxy.publish_command(self.pwr)
self.arms = {}
self.arms['right_arm']=m3.arm.M3Arm('m3arm_ma1')
self.proxy.subscribe_status(self.arms['right_arm'])
self.arms['left_arm']=m3.arm.M3Arm('m3arm_ma2')
self.proxy.subscribe_status(self.arms['left_arm'])
self.proxy.step()
self.proxy.step()
def calibrate(self, proxy):
"""
Calibrates Omnibase casters if necessary.
:param proxy: running proxy
:type proxy: M3RtProxy
"""
need_to_calibrate = False
for i in range(self.num_casters):
if (not self.is_calibrated(i)):
need_to_calibrate = True
if need_to_calibrate:
print '------------------------------------------------'
print 'All casters not calibrated. Do calibration [y]?'
if m3t.get_yes_no('y'):
print 'Note: Orientations are facing robot'
print "Turn power on to robot and press any key."
raw_input()
self.set_mode_caster()
proxy.step()
time.sleep(4)
caster_names = [
'FrontRight', 'RearRight', 'RearLeft', 'FrontLeft'
]
wiggle = [1, 2, 1, 2]
last_calib = -1
repeat_calib = 0
while need_to_calibrate:
for i in [1, 2, 3, 0]:
if (not self.is_calibrated(i)):
print '-------------------------------------------'
print 'Calibrating caster: ', caster_names[i], '..'
#print 'Manual assist required in CCW direction'
if i == last_calib:
repeat_calib += 1
if repeat_calib == 0:
wiggle = [1, 2, 1, 2]
self.home(i, proxy, wiggle[i])
elif repeat_calib == 1:
wiggle = [3, 0, 3, 0]
self.home(i, proxy, wiggle[i])
elif repeat_calib == 2:
wiggle = [2, 3, 0, 1]
self.home(i, proxy, wiggle[i])
elif repeat_calib >= 3:
raise m3t.M3Exception(
'Error calibrating. Please reposition base and try again.'
)
last_calib = i
need_to_calibrate = False
for i in range(self.num_casters):
if (not self.is_calibrated(i)):
need_to_calibrate = True
self.set_mode_caster_off(range(self.num_casters))
self.set_mode_off()
else:
print "Skipping Calibration.."
def __init__(self, q_desired, qdot_avg):
self.q_desired = q_desired
self.qdot_avg = qdot_avg
for i in range(len(qdot_avg)):
if qdot_avg[i] <= 0:
raise m3t.M3Exception('qdot_avg must be greater than zero')