def start(self, ctype):
#Return true if success
#ctype. e.g.,: {'comp_ec':'m3actuator_ec','comp_rt':'m3actuator'}
self.proxy = m3p.M3RtProxy()
self.proxy.start()
print 'Select component'
ac = self.proxy.get_available_components(ctype['comp_rt'])
if len(ac) == 0:
print 'Required components not available'
return False
self.name_rt = m3t.user_select_components_interactive(ac,
single=True)[0]
self.name_ec = self.name_rt.replace(ctype['comp_rt'], ctype['comp_ec'])
self.comp_ec = m3f.create_component(self.name_ec)
self.comp_rt = m3f.create_component(self.name_rt)
if self.proxy.is_component_available(self.name_ec):
self.proxy.subscribe_status(self.comp_ec)
self.proxy.publish_command(self.comp_ec)
self.proxy.publish_param(self.comp_ec)
self.proxy.make_operational(self.name_ec)
if self.proxy.is_component_available(self.name_rt):
self.proxy.subscribe_status(self.comp_rt)
pwr_ec = self.proxy.get_available_components('m3pwr_ec')
pwr_rt = self.proxy.get_available_components('m3pwr')
if len(pwr_rt):
pr = m3f.create_component(pwr_rt[0])
self.proxy.publish_command(pr)
self.proxy.make_operational(pwr_rt[0])
pr.set_motor_power_on()
if len(pwr_ec):
self.proxy.make_operational(pwr_ec[0])
self.step()
return True
def start(self):
self.proxy.start()
names=self.proxy.get_available_components('m3loadx6_ec')
if len(names):
ec_name=m3t.user_select_components_interactive(names)[0]
name=ec_name.replace('m3loadx6_ec','m3loadx6')
self.comp_ec=cf.create_component(ec_name)
self.proxy.subscribe_status(self.comp_ec)
self.proxy.make_operational(ec_name)
else:
print 'No loadx6_ec component available'
return
if self.proxy.is_component_available(name):
self.comp_rt=cf.create_component(name)
self.proxy.subscribe_status(self.comp_rt)
self.proxy.make_operational(name)
else:
self.comp_rt=None
#Create gui
self.fx=[0]
self.fy=[0]
self.fz=[0]
self.tx=[0]
self.ty=[0]
self.tz=[0]
self.status_dict=self.proxy.get_status_dict()
self.gui.add('M3GuiTree', 'Status', (self,'status_dict'),[],[],m3g.M3GuiRead,column=1)
self.gui.add('M3GuiSliders','Fx (g)', (self,'fx'),range(1),[-7500,7500],m3g.M3GuiRead)
self.gui.add('M3GuiSliders','Fy (g)', (self,'fy'),range(1),[-7500,7500],m3g.M3GuiRead)
self.gui.add('M3GuiSliders','Fz (g)', (self,'fz'),range(1),[-7500,7500],m3g.M3GuiRead)
self.gui.add('M3GuiSliders','Tx (mNm)', (self,'tx'),range(1),[-7500,7500],m3g.M3GuiRead)
self.gui.add('M3GuiSliders','Ty (mNm)', (self,'ty'),range(1),[-7500,7500],m3g.M3GuiRead)
self.gui.add('M3GuiSliders','Tz (mNm)', (self,'tz'),range(1),[-7500,7500],m3g.M3GuiRead)
self.gui.start(self.step)
def start(self,ctype):
#Return true if success
#ctype. e.g.,: {'comp_ec':'m3actuator_ec','comp_rt':'m3actuator'}
self.proxy=m3p.M3RtProxy()
self.proxy.start()
print 'Select component'
ac=self.proxy.get_available_components(ctype['comp_rt'])
if len(ac)==0:
print 'Required components not available'
return False
self.name_rt=m3t.user_select_components_interactive(ac,single=True)[0]
self.name_ec=self.name_rt.replace(ctype['comp_rt'],ctype['comp_ec'])
self.comp_ec=m3f.create_component(self.name_ec)
self.comp_rt=m3f.create_component(self.name_rt)
if self.proxy.is_component_available(self.name_ec):
self.proxy.subscribe_status(self.comp_ec)
self.proxy.publish_command(self.comp_ec)
self.proxy.publish_param(self.comp_ec)
self.proxy.make_operational(self.name_ec)
if self.proxy.is_component_available(self.name_rt):
self.proxy.subscribe_status(self.comp_rt)
pwr_ec=self.proxy.get_available_components('m3pwr_ec')
pwr_rt=self.proxy.get_available_components('m3pwr')
if len(pwr_rt):
pr=m3f.create_component(pwr_rt[0])
self.proxy.publish_command(pr)
self.proxy.make_operational(pwr_rt[0])
pr.set_motor_power_on()
if len(pwr_ec):
self.proxy.make_operational(pwr_ec[0])
self.step()
return True
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 start(self):
self.proxy.start()
cnames=self.proxy.get_available_components('m3actuator_ec')
pwr_ec=self.proxy.get_available_components('m3pwr_ec')
pwr_rt=self.proxy.get_available_components('m3pwr')
print 'Select two ACTUATOR_EC components'
self.names=m3t.user_select_components_interactive(cnames)
if len(self.names)!=2:
print 'Incorrect selection'
return
self.actuator_ec=[]
for name in self.names:
self.actuator_ec.append(m3f.create_component(name))
self.proxy.subscribe_status(self.actuator_ec[-1])
self.proxy.publish_command(self.actuator_ec[-1])
self.proxy.publish_param(self.actuator_ec[-1])
self.proxy.make_operational(name)
if len(pwr_rt):
pr=m3f.create_component(pwr_rt[0])
self.proxy.publish_command(pr)
self.proxy.make_operational(pwr_rt[0])
self.proxy.make_operational(pwr_ec[0])
pr.set_motor_power_on()
tmax=[x.param.t_max for x in self.actuator_ec]
tmin=[x.param.t_min for x in self.actuator_ec]
self.proxy.step()
for c in self.actuator_ec:
self.bias.append(c.status.adc_torque)
tl=min(tmin)-self.bias[0]
tu=max(tmax)-self.bias[0]
self.cycle=False
self.cycle_last=False
self.step_period=[2000.0]*len(self.actuator_ec)
#Create gui
self.mode=[0]*len(self.actuator_ec)
self.t_desire=[0]*len(self.actuator_ec)
self.pwm_desire_a=[0]*len(self.actuator_ec)
self.pwm_desire_b=[0]*len(self.actuator_ec)
self.save=False
self.save_last=False
self.status_dict=self.proxy.get_status_dict()
self.param_dict=self.proxy.get_param_dict()
self.gui.add('M3GuiTree', 'Status', (self,'status_dict'),[],[],m3g.M3GuiRead,column=2)
self.gui.add('M3GuiTree', 'Param', (self,'param_dict'),[],[],m3g.M3GuiWrite,column=3)
self.gui.add('M3GuiModes', 'Mode', (self,'mode'),range(len(self.actuator_ec)),[['Off','Pwm','PID'],1],m3g.M3GuiWrite)
self.gui.add('M3GuiSliders','tqDesire', (self,'t_desire'),range(len(self.actuator_ec)),[tl,tu],m3g.M3GuiWrite)
self.gui.add('M3GuiSliders','pwmDesireA', (self,'pwm_desire_a'),range(len(self.actuator_ec)),[-3200,3200],m3g.M3GuiWrite)
self.gui.add('M3GuiSliders','pwmDesireB', (self,'pwm_desire_b'),range(len(self.actuator_ec)),[-3200,3200],m3g.M3GuiWrite)
self.gui.add('M3GuiSliders','StepPeriod (ms) ', (self,'step_period'),range(len(self.actuator_ec)),[0,4000],m3g.M3GuiWrite)
self.gui.add('M3GuiToggle', 'Cycle', (self,'cycle'),[],[['On','Off']],m3g.M3GuiWrite)
self.gui.add('M3GuiToggle', 'Save', (self,'save'),[],[['On','Off']],m3g.M3GuiWrite)
self.gui.start(self.step)
def start(self):
self.ts=time.time()
self.proxy.start()
chain_names=self.proxy.get_available_components('m3ledx2_ec')
led_name=m3t.user_select_components_interactive(chain_names,single=True)[0]
pwr_name=self.proxy.get_available_components('m3pwr')[0]
pwr_ec_name=self.proxy.get_available_components('m3pwr_ec')[0]
self.pwr=m3f.create_component(pwr_name)
self.proxy.publish_command(self.pwr)
self.led=m3f.create_component(led_name)
self.proxy.publish_command(self.led)
self.proxy.subscribe_status(self.led)
self.proxy.make_operational(led_name)
self.proxy.make_operational(pwr_name)
self.proxy.make_operational(pwr_ec_name)
self.proxy.subscribe_status(self.pwr)
self.pwr.set_motor_power_on()
self.branch_a_board_a_r=[0]
self.branch_a_board_a_g=[0]
self.branch_a_board_a_b=[0]
self.branch_a_board_b_r=[0]
self.branch_a_board_b_g=[0]
self.branch_a_board_b_b=[0]
self.branch_b_board_a_r=[0]
self.branch_b_board_a_g=[0]
self.branch_b_board_a_b=[0]
self.branch_b_board_b_r=[0]
self.branch_b_board_b_g=[0]
self.branch_b_board_b_b=[0]
self.slew=[10000]
#Create gui
self.run=False
self.run_last=False
self.enable=False
self.pulse=False
self.status_dict=self.proxy.get_status_dict()
self.gui.add('M3GuiTree', 'Status', (self,'status_dict'),[],[],m3g.M3GuiRead,column=3)
self.gui.add('M3GuiToggle', 'Enable', (self,'enable'),[],[['On','Off']],m3g.M3GuiWrite,column=1)
self.gui.add('M3GuiToggle', 'Pulse', (self,'pulse'),[],[['On','Off']],m3g.M3GuiWrite,column=1)
self.gui.add('M3GuiSliders','Slew', (self,'slew'),range(1),[0,25500],m3g.M3GuiWrite,column=1)
self.gui.add('M3GuiSliders','BranchA.BoardA.R', (self,'branch_a_board_a_r'),range(1),[0,1023],m3g.M3GuiWrite,column=1)
self.gui.add('M3GuiSliders','BranchA.BoardA.G', (self,'branch_a_board_a_g'),range(1),[0,1023],m3g.M3GuiWrite,column=1)
self.gui.add('M3GuiSliders','BranchA.BoardA.B', (self,'branch_a_board_a_b'),range(1),[0,1023],m3g.M3GuiWrite,column=1)
self.gui.add('M3GuiSliders','BranchA.BoardB.R', (self,'branch_a_board_b_r'),range(1),[0,1023],m3g.M3GuiWrite,column=1)
self.gui.add('M3GuiSliders','BranchA.BoardB.G', (self,'branch_a_board_b_g'),range(1),[0,1023],m3g.M3GuiWrite,column=1)
self.gui.add('M3GuiSliders','BranchA.BoardB.B', (self,'branch_a_board_b_b'),range(1),[0,1023],m3g.M3GuiWrite,column=1)
self.gui.add('M3GuiSliders','BranchB.BoardA.R', (self,'branch_b_board_a_r'),range(1),[0,1023],m3g.M3GuiWrite,column=1)
self.gui.add('M3GuiSliders','BranchB.BoardA.G', (self,'branch_b_board_a_g'),range(1),[0,1023],m3g.M3GuiWrite,column=1)
self.gui.add('M3GuiSliders','BranchB.BoardA.B', (self,'branch_b_board_a_b'),range(1),[0,1023],m3g.M3GuiWrite,column=1)
self.gui.add('M3GuiSliders','BranchB.BoardB.R', (self,'branch_b_board_b_r'),range(1),[0,1023],m3g.M3GuiWrite,column=1)
self.gui.add('M3GuiSliders','BranchB.BoardB.G', (self,'branch_b_board_b_g'),range(1),[0,1023],m3g.M3GuiWrite,column=1)
self.gui.add('M3GuiSliders','BranchB.BoardB.B', (self,'branch_b_board_b_b'),range(1),[0,1023],m3g.M3GuiWrite,column=1)
self.gui.start(self.step)
def do_log(proxy):
print 'Starting log...select component to log'
proxy.start(start_data_svc=False)
comp_name = m3t.user_select_components_interactive(
proxy.get_available_components())[0]
comp = mcf.create_component(comp_name)
proxy.register_log_component(comp)
print 'Enter log name [foo]'
try_again = True
while try_again:
logname = m3t.get_string('foo')
if not os.path.isdir(m3t.get_m3_log_path() + logname):
try_again = False
else:
print 'Log directory already exists. Please enter another name.'
print 'Enter sample frequence (0-X Hz) [250.0]'
freq = m3t.get_float(250.0)
default_samps = int(2 * freq / 5)
print 'Enter samples per file [' + str(default_samps) + ']'
samples = m3t.get_int(default_samps)
print 'Enter sample time (s) [5.0]'
duration = m3t.get_float(5.0)
print 'Hit enter to begin log...'
raw_input()
proxy.start_log_service(logname,
sample_freq_hz=freq,
samples_per_file=samples)
ts = time.time()
while time.time() - ts < duration:
time.sleep(0.25)
print 'Logging ', logname, ' Time: ', time.time() - ts
proxy.stop_log_service()
def do_log(proxy):
print 'Starting log...select component to log'
proxy.start(start_data_svc=False)
comp_name=m3t.user_select_components_interactive(proxy.get_available_components())[0]
comp=mcf.create_component(comp_name)
proxy.register_log_component(comp)
print 'Enter log name [foo]'
try_again = True
while try_again:
logname=m3t.get_string('foo')
if not os.path.isdir(m3t.get_m3_log_path()+logname):
try_again = False
else:
print 'Log directory already exists. Please enter another name.'
print 'Enter sample frequence (0-X Hz) [250.0]'
freq=m3t.get_float(250.0)
default_samps = int(2*freq/5)
print 'Enter samples per file ['+str(default_samps) +']'
samples=m3t.get_int(default_samps)
print 'Enter sample time (s) [5.0]'
duration=m3t.get_float(5.0)
print 'Hit enter to begin log...'
raw_input()
proxy.start_log_service(logname,sample_freq_hz=freq,samples_per_file=samples)
ts=time.time()
while time.time()-ts<duration:
time.sleep(0.25)
print 'Logging ',logname,' Time: ',time.time()-ts
proxy.stop_log_service()
def __init__(self):
# M3-specific stuff
self.omni = m3o.MekaOmnibaseControl('meka_omnibase_control_mb2', 'meka_omnibase_control')
self.proxy = m3p.M3RtProxy()
self.proxy.start()
self.proxy.make_operational_all()
self.proxy.subscribe_status(self.omni)
self.proxy.publish_command(self.omni)
# TODO: Use the method associated with omnibase instead.
pwr_name = self.proxy.get_available_components('m3pwr')
self.pwr = m3f.create_component(pwr_name[1])
self.proxy.subscribe_status(self.pwr)
self.proxy.publish_command(self.pwr)
self.pwr.set_motor_power_on()
self.proxy.step()
# ROS stuff
self.sub_cmd_vel = rospy.Subscriber("cmd_vel", Twist, self.cmd_vel_cb, queue_size=1)
self.pub_odom = rospy.Publisher("odom", Odometry, queue_size=1)
self.tf_bc = tf.TransformBroadcaster()
self.max_lin = rospy.get_param("~max_lin_vel", 0.30)
self.max_ang = rospy.get_param("~max_ang_vel", 1.00)
self.cmd_vel = Twist()
self.last_cmd = rospy.Time(0)
self.timeout = rospy.Duration(0.25)
self.diag_i = 0
def __init__(self, name): self._action_name = name self._as = actionlib.SimpleActionServer(self._action_name, simple_traj_server.msg.TrajAction, execute_cb=self.execute_cb) self.proxy = m3p.M3RtProxy() self.proxy.start() bot_name=m3t.get_robot_name() self.bot=m3f.create_component(bot_name) self.chains = bot.get_available_chains() self.bot.initialize(proxy)
def __init__(self, proxy, bot, r_hand_ua_num=None, stride_ms=100):
self.p = subprocess.Popen(
['roslaunch', 'm3_defs_ros', 'm3_launch.launch'])
rospy.init_node("joint_state_publisher")
pub = rospy.Publisher("/joint_states", JointState)
time.sleep(4.0)
self.bot = bot
self.proxy = proxy
self.pub = pub
self.sleep_time = stride_ms / 1000.0
self.joints = []
self.positions = []
self.chain_names = self.bot.get_available_chains()
self.use_sim = False
self.r_hand_ua_present = False
self.ndof_finger = 3
self.flex_factor_index = [0.3] * self.ndof_finger
self.flex_factor_ring = [0.3] * self.ndof_finger
self.flex_factor_pinky = [0.3] * self.ndof_finger
self.flex_factor_thumb = [0.3] * 2
for chain in self.chain_names:
self.positions += list(self.bot.get_theta_rad(chain))
self.joints += self.bot.get_joint_names(chain)
self.ndof_hand_ua = 12
hand_names = self.proxy.get_available_components('m3hand')
self.hands = []
self.hand_nums = []
for i in range(len(hand_names)):
self.hands.append(m3f.create_component(hand_names[i]))
self.proxy.subscribe_status(self.hands[i])
#self.proxy.publish_command(self.hands[i])
if hand_names[i][-2].isdigit():
self.hand_nums.append(hand_names[i][-2:])
else:
self.hand_nums.append(hand_names[i][-1])
#r_hand_ua_num = 14
self.ndof_hand_ua = 12
for j in range(len(self.hands)):
for i in range(self.ndof_hand_ua):
self.positions.append(0.0)
self.joints.append('m3joint_ua_mh' + str(self.hand_nums[j]) +
'_j' + str(i))
# Thumb: J0,J1,J2
# Index: J3, J4, J5
# Ring: J6,J7,J8
# Pinkie: J9, J10, J11
'''if not r_hand_ua_num is None:
def __init__ (self,proxy,bot,r_hand_ua_num=None,stride_ms=100):
self.p = subprocess.Popen(['roslaunch', 'm3_defs_ros', 'm3_launch.launch'])
rospy.init_node("joint_state_publisher")
pub = rospy.Publisher("/joint_states", JointState)
time.sleep(4.0)
self.bot=bot
self.proxy=proxy
self.pub = pub
self.sleep_time = stride_ms/1000.0
self.joints = []
self.positions = []
self.chain_names = self.bot.get_available_chains()
self.use_sim = False
self.r_hand_ua_present = False
self.ndof_finger = 3
self.flex_factor_index = [0.3] * self.ndof_finger
self.flex_factor_ring = [0.3] * self.ndof_finger
self.flex_factor_pinky = [0.3] * self.ndof_finger
self.flex_factor_thumb = [0.3] * 2
for chain in self.chain_names:
self.positions += list(self.bot.get_theta_rad(chain))
self.joints += self.bot.get_joint_names(chain)
self.ndof_hand_ua = 12
hand_names=self.proxy.get_available_components('m3hand')
self.hands = []
self.hand_nums = []
for i in range(len(hand_names)):
self.hands.append(m3f.create_component(hand_names[i]))
self.proxy.subscribe_status(self.hands[i])
#self.proxy.publish_command(self.hands[i])
if hand_names[i][-2].isdigit():
self.hand_nums.append(hand_names[i][-2:])
else:
self.hand_nums.append(hand_names[i][-1])
#r_hand_ua_num = 14
self.ndof_hand_ua = 12
for j in range(len(self.hands)):
for i in range(self.ndof_hand_ua):
self.positions.append(0.0)
self.joints.append('m3joint_ua_mh'+str(self.hand_nums[j])+'_j'+str(i))
# Thumb: J0,J1,J2
# Index: J3, J4, J5
# Ring: J6,J7,J8
# Pinkie: J9, J10, J11
'''if not r_hand_ua_num is None:
def start(self):
self.proxy.start()
names = self.proxy.get_available_components('m3loadx6_ec')
if len(names):
ec_name = m3t.user_select_components_interactive(names)[0]
name = ec_name.replace('m3loadx6_ec', 'm3loadx6')
self.comp_ec = cf.create_component(ec_name)
self.proxy.subscribe_status(self.comp_ec)
self.proxy.make_operational(ec_name)
else:
print 'No loadx6_ec component available'
return
if self.proxy.is_component_available(name):
self.comp_rt = cf.create_component(name)
self.proxy.subscribe_status(self.comp_rt)
self.proxy.make_operational(name)
else:
self.comp_rt = None
#Create gui
self.fx = [0]
self.fy = [0]
self.fz = [0]
self.tx = [0]
self.ty = [0]
self.tz = [0]
self.status_dict = self.proxy.get_status_dict()
self.gui.add('M3GuiTree',
'Status', (self, 'status_dict'), [], [],
m3g.M3GuiRead,
column=1)
self.gui.add('M3GuiSliders', 'Fx (g)', (self, 'fx'), range(1),
[-7500, 7500], m3g.M3GuiRead)
self.gui.add('M3GuiSliders', 'Fy (g)', (self, 'fy'), range(1),
[-7500, 7500], m3g.M3GuiRead)
self.gui.add('M3GuiSliders', 'Fz (g)', (self, 'fz'), range(1),
[-7500, 7500], m3g.M3GuiRead)
self.gui.add('M3GuiSliders', 'Tx (mNm)', (self, 'tx'), range(1),
[-7500, 7500], m3g.M3GuiRead)
self.gui.add('M3GuiSliders', 'Ty (mNm)', (self, 'ty'), range(1),
[-7500, 7500], m3g.M3GuiRead)
self.gui.add('M3GuiSliders', 'Tz (mNm)', (self, 'tz'), range(1),
[-7500, 7500], m3g.M3GuiRead)
self.gui.start(self.step)
def __init__(self, name):
self._action_name = name
self._as = actionlib.SimpleActionServer(
self._action_name,
simple_traj_server.msg.TrajAction,
execute_cb=self.execute_cb)
self.proxy = m3p.M3RtProxy()
self.proxy.start()
bot_name = m3t.get_robot_name()
self.bot = m3f.create_component(bot_name)
self.chains = bot.get_available_chains()
self.bot.initialize(proxy)
def start(self):
self.proxy.start()
bot_name = m3t.get_robot_name()
if bot_name == "":
print 'Error: no robot components found:', bot_names
return
self.bot = m3f.create_component(bot_name)
self.bot.initialize(self.proxy)
csp_name = self.proxy.get_available_components('m3head_s2csp_ctrl')
if len(csp_name) != 1:
print 'CSP component not available'
exit()
self.csp_rt = m3csp.M3HeadS2CSPCtrl(csp_name[0])
self.proxy.subscribe_status(self.csp_rt)
self.proxy.publish_command(self.csp_rt)
self.proxy.publish_param(self.csp_rt)
self.mode = [0]
self.lat = [0]
self.longi = [0]
self.theta_j2 = [0]
self.ts_rand = 0
self.la = 0
self.lo = 0
self.joints = range(7)
self.save = False
self.save_last = False
self.status_dict = self.proxy.get_status_dict()
self.param_dict = self.proxy.get_param_dict()
self.gui.add('M3GuiTree',
'Status', (self, 'status_dict'), [], [],
m3g.M3GuiRead,
column=1)
self.gui.add('M3GuiTree',
'Param', (self, 'param_dict'), [], [],
m3g.M3GuiWrite,
column=1)
self.gui.add('M3GuiModes', 'Mode', (self, 'mode'), range(1),
[['Off', 'CSP', 'CSPRandom'], 1], m3g.M3GuiWrite)
self.gui.add('M3GuiSliders',
'Left/Right (Deg)', (self, 'longi'), [0], [-90, 90],
m3g.M3GuiWrite,
column=1)
self.gui.add('M3GuiSliders',
'Up/Down (Deg)', (self, 'lat'), [0], [-90, 90],
m3g.M3GuiWrite,
column=1)
self.gui.add('M3GuiSliders',
'Head Roll (Deg)', (self, 'theta_j2'), [0], [-15, 15],
m3g.M3GuiWrite,
column=1)
self.gui.add('M3GuiToggle', 'Save', (self, 'save'), [],
[['On', 'Off']], m3g.M3GuiWrite)
self.gui.start(self.step)
def test_sensor_in_range(self, config, proxy):
err_cnt = 0
err_msg = []
for k in config:
#print 'Starting', k, '..'
for j in range(len(config[k])):
method = config[k][j]['method']
mmin = config[k][j]['min']
mmax = config[k][j]['max']
nsteps = config[k][j]['nsteps']
comp = m3f.create_component(k)
proxy.subscribe_status(comp, verbose=False)
proxy.publish_param(comp)
proxy.step()
fp = True
for i in range(nsteps):
error = False
proxy.step()
fcn = getattr(comp, method)
value = fcn()
if value < mmin or value > mmax:
error = True
msg = comp.name + ' ' + method + ' val: ' + str(
value) + ' min: ' + str(mmin) + ' max: ' + str(
mmax) + ' ' + str(i)
res = {
'Test': 'SensorInRange',
'Error': True,
'Msg': msg
}
if fp:
m = 'Test: SensorInRange: FAIL: \t' + comp.name + ' : \t' + method + ' : \t' + "%3.2f" % value
err_msg.append(m)
print err_msg[-1]
err_cnt = err_cnt + 1
fp = False
else:
msg = comp.name + ' ' + method + ' val: ' + str(
value) + ' min: ' + str(mmin) + ' max: ' + str(
mmax) + ' ' + str(i)
res = {
'Test': 'SensorInRange',
'Error': False,
'Msg': msg
}
if fp:
print 'Test: SensorInRange: Pass: \t', comp.name, ' : \t', method, ' : \t', "%3.2f" % value
fp = False
self.log.append(res)
return err_cnt, err_msg
def get_fields(self, req):
"""
Callback for the get_fields service request
"""
request_name = req.component
if request_name not in self.comps.keys():
try:
comp = mcf.create_component(str(request_name))
self.rt_proxy.subscribe_status(comp)
except AttributeError, e:
rospy.logwarn("No fields available for %s", request_name)
return
self.comps[request_name] = comp
def do_plot(proxy):
print 'Starting plot...'
print 'Enter log name [foo]'
logname=m3t.get_string('foo')
ns=proxy.get_log_num_samples(logname)
if ns==0:
return
print '-------- Available components --------'
names=proxy.get_log_component_names(logname)
for i in range(len(names)):
print names[i]
print '--------------------------------------'
print 'Select component: [',names[0],']'
name=m3t.get_string(names[0])
print '--------------------------------------'
print 'Num samples available: ',ns
print 'Enter start sample idx: [0]'
start=max(0,m3t.get_int(0))
print 'Enter end sample idx: [',ns-1,']'
end=min(ns-1,m3t.get_int(ns-1))
print '--------------------------------------'
print ' Select field to plot...'
comp=mcf.create_component(name)
proxy.register_log_component(comp)
field=m3t.user_select_msg_field(comp.status)
repeated = False
idx = 0
if hasattr(m3t.get_msg_field_value(comp.status,field),'__len__'):
repeated = True
print 'Select index of repeated field to monitor: [0]'
idx = m3t.get_int(0)
print 'Fetching data...'
data=[]
for i in range(start,end):
proxy.load_log_sample(logname,i)
if repeated:
v=m3t.get_msg_field_value(comp.status,field)[idx]
else:
v=m3t.get_msg_field_value(comp.status,field)
data.append(v)
# m3t.plot(data)
m3t.mplot(data,range(len(data)))
def do_plot(proxy):
print 'Starting plot...'
print 'Enter log name [foo]'
logname = m3t.get_string('foo')
ns = proxy.get_log_num_samples(logname)
if ns == 0:
return
print '-------- Available components --------'
names = proxy.get_log_component_names(logname)
for i in range(len(names)):
print names[i]
print '--------------------------------------'
print 'Select component: [', names[0], ']'
name = m3t.get_string(names[0])
print '--------------------------------------'
print 'Num samples available: ', ns
print 'Enter start sample idx: [0]'
start = max(0, m3t.get_int(0))
print 'Enter end sample idx: [', ns - 1, ']'
end = min(ns - 1, m3t.get_int(ns - 1))
print '--------------------------------------'
print ' Select field to plot...'
comp = mcf.create_component(name)
proxy.register_log_component(comp)
field = m3t.user_select_msg_field(comp.status)
repeated = False
idx = 0
if hasattr(m3t.get_msg_field_value(comp.status, field), '__len__'):
repeated = True
print 'Select index of repeated field to monitor: [0]'
idx = m3t.get_int(0)
print 'Fetching data...'
data = []
for i in range(start, end):
proxy.load_log_sample(logname, i)
if repeated:
v = m3t.get_msg_field_value(comp.status, field)[idx]
else:
v = m3t.get_msg_field_value(comp.status, field)
data.append(v)
# m3t.plot(data)
m3t.mplot(data, range(len(data)))
def start(self):
self.proxy.start()
cnames = self.proxy.get_available_components()
print 'Available components:', cnames
self.names = m3t.user_select_components_interactive(cnames)
self.components = []
for name in self.names:
self.components.append(m3f.create_component(name))
self.proxy.subscribe_status(self.components[-1])
self.proxy.publish_command(self.components[-1])
self.proxy.publish_param(self.components[-1])
self.proxy.make_operational_all()
self.proxy.step()
def start(self):
self.proxy.start()
cnames=self.proxy.get_available_components()
print 'Available components:', cnames
self.names=m3t.user_select_components_interactive(cnames)
self.components=[]
for name in self.names:
self.components.append(m3f.create_component(name))
self.proxy.subscribe_status(self.components[-1])
self.proxy.publish_command(self.components[-1])
self.proxy.publish_param(self.components[-1])
self.proxy.make_operational_all()
self.proxy.step()
def do_scope(proxy):
print 'Starting scope...'
print 'Enter log name [foo]'
logname=m3t.get_string('foo')
ns=proxy.get_log_num_samples(logname)
print 'Num samples available: ',ns
if ns==0:
return
print '-------- Available components --------'
names=proxy.get_log_component_names(logname)
for i in range(len(names)):
print names[i]
print '--------------------------------------'
print 'Select component: [',names[0],']'
name=m3t.get_string(names[0])
comp=mcf.create_component(name)
proxy.register_log_component(comp)
scope=m3t.M3Scope(xwidth=ns-1)
print '--------------------------------------'
print 'Num samples available: ',ns
print 'Enter start sample idx: [0]'
start=max(0,m3t.get_int(0))
print 'Enter end sample idx: [',ns-1,']'
end=min(ns-1,m3t.get_int(ns-1))
print '--------------------------------------'
print 'Select field to monitor'
field=m3t.user_select_msg_field(comp.status)
repeated = False
idx = 0
if hasattr(m3t.get_msg_field_value(comp.status,field),'__len__'):
repeated = True
print 'Select index of repeated field to monitor: [0]'
idx = m3t.get_int(0)
for i in range(start,end):
proxy.load_log_sample(logname,i)
if repeated:
v=m3t.get_msg_field_value(comp.status,field)[idx]
else:
v=m3t.get_msg_field_value(comp.status,field)
scope.plot(v)
time.sleep(0.05)
def do_scope(proxy):
print 'Starting scope...'
print 'Enter log name [foo]'
logname = m3t.get_string('foo')
ns = proxy.get_log_num_samples(logname)
print 'Num samples available: ', ns
if ns == 0:
return
print '-------- Available components --------'
names = proxy.get_log_component_names(logname)
for i in range(len(names)):
print names[i]
print '--------------------------------------'
print 'Select component: [', names[0], ']'
name = m3t.get_string(names[0])
comp = mcf.create_component(name)
proxy.register_log_component(comp)
scope = m3t.M3Scope(xwidth=ns - 1)
print '--------------------------------------'
print 'Num samples available: ', ns
print 'Enter start sample idx: [0]'
start = max(0, m3t.get_int(0))
print 'Enter end sample idx: [', ns - 1, ']'
end = min(ns - 1, m3t.get_int(ns - 1))
print '--------------------------------------'
print 'Select field to monitor'
field = m3t.user_select_msg_field(comp.status)
repeated = False
idx = 0
if hasattr(m3t.get_msg_field_value(comp.status, field), '__len__'):
repeated = True
print 'Select index of repeated field to monitor: [0]'
idx = m3t.get_int(0)
for i in range(start, end):
proxy.load_log_sample(logname, i)
if repeated:
v = m3t.get_msg_field_value(comp.status, field)[idx]
else:
v = m3t.get_msg_field_value(comp.status, field)
scope.plot(v)
time.sleep(0.05)
def start(self, process_cb):
self.proxy = m3p.M3RtProxy()
self.proxy.start()
cnames = self.proxy.get_available_components('m3tactile_pps22_ec')
if len(cnames) == 0:
print 'No PPS22 sensor present'
return
if len(cnames) > 1:
name = m3t.user_select_components_interactive(cnames)[0]
else:
name = cnames[0]
self.pps = m3f.create_component(name)
self.proxy.subscribe_status(self.pps)
print 'Place sensor in unloaded state.Hit return when ready'
raw_input()
self.proxy.step()
self.zero = self.pps.get_taxels()
self.process_cb = process_cb
gtk.main()
def start(self,process_cb):
self.proxy = m3p.M3RtProxy()
self.proxy.start()
cnames=self.proxy.get_available_components('m3tactile_pps22_ec')
if len(cnames)==0:
print 'No PPS22 sensor present'
return
if len(cnames)>1:
name=m3t.user_select_components_interactive(cnames)[0]
else:
name=cnames[0]
self.pps=m3f.create_component(name)
self.proxy.subscribe_status(self.pps)
print 'Place sensor in unloaded state.Hit return when ready'
raw_input()
self.proxy.step()
self.zero=self.pps.get_taxels()
self.process_cb=process_cb
gtk.main()
def test_sensor_in_range(self, config, proxy):
err_cnt=0
err_msg=[]
for k in config:
#print 'Starting', k, '..'
for j in range(len(config[k])):
method = config[k][j]['method']
mmin = config[k][j]['min']
mmax = config[k][j]['max']
nsteps = config[k][j]['nsteps']
comp = m3f.create_component(k)
proxy.subscribe_status(comp,verbose=False)
proxy.publish_param(comp)
proxy.step()
fp=True
for i in range(nsteps):
error = False
proxy.step()
fcn = getattr(comp,method)
value = fcn()
if value < mmin or value > mmax:
error = True
msg=comp.name + ' ' + method + ' val: ' + str(value) + ' min: ' + str(mmin) + ' max: ' + str(mmax) + ' ' + str(i)
res={'Test':'SensorInRange','Error':True,'Msg':msg}
if fp:
m= 'Test: SensorInRange: FAIL: \t'+comp.name+' : \t'+method+' : \t'+"%3.2f" % value
err_msg.append(m)
print err_msg[-1]
err_cnt=err_cnt+1
fp=False
else:
msg=comp.name + ' ' + method + ' val: ' + str(value) + ' min: ' + str(mmin) + ' max: ' + str(mmax) + ' ' + str(i)
res={'Test':'SensorInRange','Error':False,'Msg':msg}
if fp:
print 'Test: SensorInRange: Pass: \t',comp.name,' : \t',method,' : \t',"%3.2f" % value
fp=False
self.log.append(res)
return err_cnt,err_msg
def start(self):
self.proxy.start()
bot_name=m3t.get_robot_name()
if bot_name == "":
print 'Error: no robot components found:', bot_names
return
self.bot=m3f.create_component(bot_name)
self.bot.initialize(self.proxy)
csp_name=self.proxy.get_available_components('m3head_s2csp_ctrl')
if len(csp_name)!=1:
print 'CSP component not available'
exit()
self.csp_rt=m3csp.M3HeadS2CSPCtrl(csp_name[0])
self.proxy.subscribe_status(self.csp_rt)
self.proxy.publish_command(self.csp_rt)
self.proxy.publish_param(self.csp_rt)
self.mode=[0]
self.lat=[0]
self.longi=[0]
self.theta_j2=[0]
self.ts_rand=0
self.la=0
self.lo=0
self.joints=range(7)
self.save=False
self.save_last=False
self.status_dict=self.proxy.get_status_dict()
self.param_dict=self.proxy.get_param_dict()
self.gui.add('M3GuiTree', 'Status', (self,'status_dict'),[],[],m3g.M3GuiRead,column=1)
self.gui.add('M3GuiTree', 'Param', (self,'param_dict'),[],[],m3g.M3GuiWrite,column=1)
self.gui.add('M3GuiModes', 'Mode', (self,'mode'),range(1),[['Off','CSP','CSPRandom'],1],m3g.M3GuiWrite)
self.gui.add('M3GuiSliders','Left/Right (Deg)', (self,'longi'),[0],[-90,90],m3g.M3GuiWrite,column=1)
self.gui.add('M3GuiSliders','Up/Down (Deg)', (self,'lat'),[0],[-90,90],m3g.M3GuiWrite,column=1)
self.gui.add('M3GuiSliders','Head Roll (Deg)', (self,'theta_j2'),[0],[-15,15],m3g.M3GuiWrite,column=1)
self.gui.add('M3GuiToggle', 'Save', (self,'save'),[],[['On','Off']],m3g.M3GuiWrite)
self.gui.start(self.step)
def start(self):
self.proxy.start()
self.ndof_finger = 3
self.flex_factor_index = [0.3] * self.ndof_finger
self.flex_factor_ring = [0.3] * self.ndof_finger
self.flex_factor_pinky = [0.3] * self.ndof_finger
self.flex_factor_thumb = [0.3] * 2
self.p = subprocess.Popen(['roslaunch', 'm3_defs_ros', 'm3_launch.launch'])
rospy.init_node("joint_state_publisher")
self.pub = rospy.Publisher("/joint_states", JointState)
time.sleep(4.0)
hand_names=self.proxy.get_available_components('m3hand')
self.hands = []
self.hand_nums = []
for i in range(len(hand_names)):
self.hands.append(m3f.create_component(hand_names[i]))
self.proxy.subscribe_status(self.hands[i])
#self.proxy.publish_command(self.hands[i])
if hand_names[i][-2].isdigit():
self.hand_nums.append(hand_names[i][-2:])
else:
self.hand_nums.append(hand_names[i][-1])
#r_hand_ua_num = 14
self.ndof_hand_ua = 12
self.positions = []
self.joints = []
for j in range(len(self.hands)):
for i in range(self.ndof_hand_ua):
self.positions.append(0.0)
self.joints.append('m3joint_ua_mh'+str(self.hand_nums[j])+'_j'+str(i))
# Thumb: J0,J1,J2
# Index: J3, J4, J5
# Ring: J6,J7,J8
# Pinkie: J9, J10, J11
print 'Starting hand viz.'
while(True):
self.positions = []
self.proxy.step()
for i in range(len(self.hands)):
th =self.hands[i].get_theta_rad()
#Thumb
self.positions.append(-th[0]+1.57) #0
self.positions.append(th[1] * self.flex_factor_thumb[0])
self.positions.append(th[1] * self.flex_factor_thumb[1])
#Index
self.positions.append(th[2] * self.flex_factor_index[0])
self.positions.append(th[2] * self.flex_factor_index[1])
self.positions.append(th[2] * self.flex_factor_index[2])
#Ring
self.positions.append(th[3] * self.flex_factor_ring[0])
self.positions.append(th[3] * self.flex_factor_ring[1])
self.positions.append(th[3] * self.flex_factor_ring[2])
#Pinkie
self.positions.append(th[4] * self.flex_factor_pinky[0])
self.positions.append(th[4] * self.flex_factor_pinky[1])
self.positions.append(th[4] * self.flex_factor_pinky[2])
if self.pub is not None and not rospy.is_shutdown():
header = Header(0,rospy.Time.now(),'0')
self.pub.publish(JointState(header, self.joints, self.positions, [0]*len(self.positions), [0]*len(self.positions)))
else:
print 'Error...exiting.'
break
time.sleep(0.1)
def main():
proxy = m3p.M3RtProxy()
proxy.start()
base_name = proxy.get_available_components('m3omnibase')
if len(base_name) != 1:
print 'Invalid number of base components available'
proxy.stop()
exit()
omni = m3o.M3OmniBase(base_name[0])
proxy.publish_param(omni) # we need this for calibration
proxy.subscribe_status(omni)
proxy.publish_command(omni)
pwr_name = [m3t.get_omnibase_pwr_component_name(base_name[0])]
#proxy.get_available_components('m3pwr')
#if len(pwr_name)>1:
#pwr_name=m3t.user_select_components_interactive(pwr_name,single=True)
pwr = m3f.create_component(pwr_name[0])
proxy.subscribe_status(pwr)
proxy.publish_command(pwr)
proxy.make_operational(pwr_name[0])
proxy.step()
omni.set_mode_off()
pwr.set_motor_power_on()
proxy.make_operational_all()
proxy.step()
time.sleep(0.5)
proxy.step()
omni.calibrate(proxy)
time.sleep(0.5)
print "Turn power on to robot and press any key."
raw_input()
omni.set_local_position(0, 0, 0, proxy)
omni.set_global_position(0, 0, 0, proxy)
omni.set_max_linear_accel(max_lin_acc)
omni.set_max_linear_velocity(max_lin_vel)
omni.set_max_rotation_velocity(max_rot_vel)
omni.set_max_rotation_accel(max_rot_acc)
proxy.step()
'''p = omni.get_global_position()
print 'Pos:', p'''
omni.set_mode_traj_goal()
omni.set_traj_goal(0, 0, 0)
proxy.step()
time.sleep(4)
print 'Bus voltage:', omni.get_bus_voltage()
print 'Press any key to begin pacing.'
raw_input()
try:
while True:
omni.set_traj_goal(2.0, 0, 180)
proxy.step()
time.sleep(0.1)
proxy.step()
while not omni.is_traj_goal_reached():
proxy.step()
p = omni.get_global_position()
print 'Pos:', p
time.sleep(0.1)
omni.set_traj_goal(0, 0, 0)
proxy.step()
time.sleep(0.1)
proxy.step()
while not omni.is_traj_goal_reached():
proxy.step()
p = omni.get_global_position()
print 'Pos:', p
time.sleep(0.1)
except KeyboardInterrupt:
pass
omni.set_mode_off()
pwr.set_motor_power_off()
proxy.step()
proxy.stop()
def start(self):
self.proxy.start()
self.proxy.make_operational_all()
chain_names=self.proxy.get_available_components('m3hand')
if len(chain_names)>1:
hand_name=m3t.user_select_components_interactive(chain_names,single=True)
else:
hand_name=chain_names
pwr_name=self.proxy.get_available_components('m3pwr')
if len(pwr_name)>1:
pwr_name=m3t.user_select_components_interactive(pwr_name,single=True)
print 'Position arm [y]?'
if m3t.get_yes_no('y'):
arm_names=self.proxy.get_available_components('m3arm')
if len(arm_names)>1:
print 'Select arm: '
arm_name=m3t.user_select_components_interactive(arm_names,single=True)[0]
else:
arm_name=arm_names[0]
self.arm=m3f.create_component(arm_name)
self.proxy.publish_command(self.arm)
self.arm.set_mode_theta_gc()
self.arm.set_theta_deg([30,0,0,110,0,0,0])
self.arm.set_stiffness(0.5)
self.arm.set_slew_rate_proportion([0.75]*7)
self.chain=m3f.create_component(hand_name[0])
self.proxy.publish_command(self.chain)
self.proxy.subscribe_status(self.chain)
self.pwr=m3f.create_component(pwr_name[0])
self.proxy.publish_command(self.pwr)
self.pwr.set_motor_power_on()
#Force safe-op of robot if present
hum=self.proxy.get_available_components('m3humanoid')
if len(hum)>0:
self.proxy.make_safe_operational(hum[0])
#Setup postures
self.posture_filename=m3t.get_m3_animation_path()+self.chain.name+'_postures.yml'
f=file(self.posture_filename,'r')
self.data= yaml.safe_load(f.read())
self.param=self.data['param']
f.close()
self.theta_desire=[0,0,0,0,0]
self.mode=[1,1,1,1,1]
#Create gui
self.run=False
self.run_last=False
self.running=False
self.grasp=False
self.grasp_last=False
self.grasp_off=False
self.grasp_off_ts=time.time()
self.status_dict=self.proxy.get_status_dict()
self.gui.add('M3GuiTree', 'Status', (self,'status_dict'),[],[],m3g.M3GuiRead,column=3)
self.gui.add('M3GuiTree', 'Param', (self,'param'),[],[],m3g.M3GuiWrite,column=3)
self.gui.add('M3GuiToggle', 'Animation', (self,'run'),[],[['Run','Stop']],m3g.M3GuiWrite,column=1)
self.gui.add('M3GuiModes', 'Joint', (self,'mode'),range(5),[['Off','Enabled'],1],m3g.M3GuiWrite,column=2)
self.gui.add('M3GuiSliders','Theta (Deg)', (self,'theta_desire'),range(5),[0,300],m3g.M3GuiWrite,column=2)
self.gui.add('M3GuiToggle', 'Power Grasp', (self,'grasp'),[],[['Run','Stop']],m3g.M3GuiWrite,column=2)
self.gui.start(self.step)
def start(self):
self.proxy.start()
self.proxy.make_operational_all()
chain_names = self.proxy.get_available_components('m3hand')
if len(chain_names) > 1:
hand_name = m3t.user_select_components_interactive(chain_names,
single=True)
else:
hand_name = chain_names
pwr_name = self.proxy.get_available_components('m3pwr')
if len(pwr_name) > 1:
pwr_name = m3t.user_select_components_interactive(pwr_name,
single=True)
print 'Position arm [y]?'
if m3t.get_yes_no('y'):
arm_names = self.proxy.get_available_components('m3arm')
if len(arm_names) > 1:
print 'Select arm: '
arm_name = m3t.user_select_components_interactive(
arm_names, single=True)[0]
else:
arm_name = arm_names[0]
self.arm = m3f.create_component(arm_name)
self.proxy.publish_command(self.arm)
self.arm.set_mode_theta_gc()
self.arm.set_theta_deg([30, 0, 0, 110, 0, 0, 0])
self.arm.set_stiffness(0.5)
self.arm.set_slew_rate_proportion([0.75] * 7)
self.chain = m3f.create_component(hand_name[0])
self.proxy.publish_command(self.chain)
self.proxy.subscribe_status(self.chain)
self.pwr = m3f.create_component(pwr_name[0])
self.proxy.publish_command(self.pwr)
self.pwr.set_motor_power_on()
#Force safe-op of robot if present
hum = self.proxy.get_available_components('m3humanoid')
if len(hum) > 0:
self.proxy.make_safe_operational(hum[0])
#Setup postures
self.posture_filename = m3t.get_m3_animation_path(
) + self.chain.name + '_postures.yml'
f = file(self.posture_filename, 'r')
self.data = yaml.safe_load(f.read())
self.param = self.data['param']
f.close()
self.theta_desire = [0, 0, 0, 0, 0]
self.mode = [1, 1, 1, 1, 1]
#Create gui
self.run = False
self.run_last = False
self.running = False
self.grasp = False
self.grasp_last = False
self.grasp_off = False
self.grasp_off_ts = time.time()
self.status_dict = self.proxy.get_status_dict()
self.gui.add('M3GuiTree',
'Status', (self, 'status_dict'), [], [],
m3g.M3GuiRead,
column=3)
self.gui.add('M3GuiTree',
'Param', (self, 'param'), [], [],
m3g.M3GuiWrite,
column=3)
self.gui.add('M3GuiToggle',
'Animation', (self, 'run'), [], [['Run', 'Stop']],
m3g.M3GuiWrite,
column=1)
self.gui.add('M3GuiModes',
'Joint', (self, 'mode'),
range(5), [['Off', 'Enabled'], 1],
m3g.M3GuiWrite,
column=2)
self.gui.add('M3GuiSliders',
'Theta (Deg)', (self, 'theta_desire'),
range(5), [0, 300],
m3g.M3GuiWrite,
column=2)
self.gui.add('M3GuiToggle',
'Power Grasp', (self, 'grasp'), [], [['Run', 'Stop']],
m3g.M3GuiWrite,
column=2)
self.gui.start(self.step)
def start(self):
self.proxy.start()
cnames = self.proxy.get_available_components("m3actuator_ec")
self.names = m3t.user_select_components_interactive(cnames)
if len(self.names) == 0:
return
self.actuator_ec = []
for name in self.names:
self.actuator_ec.append(m3f.create_component(name))
self.proxy.subscribe_status(self.actuator_ec[-1])
self.proxy.publish_command(self.actuator_ec[-1])
self.proxy.publish_param(self.actuator_ec[-1])
self.proxy.make_operational(name)
# pwr_ec=self.proxy.get_available_components('m3pwr_ec')
# pwr_rt=self.proxy.get_available_components('m3pwr')
# print 'A',pwr_rt[0],pwr_ec[0]
# if len(pwr_rt):
# pr=m3f.create_component(pwr_rt[0])
# self.proxy.publish_command(pr)
# self.proxy.make_operational(pwr_rt[0])
# self.proxy.make_operational(pwr_ec[0])
# pr.set_motor_power_on()
pwr_rt = m3t.get_actuator_ec_pwr_component_name(self.names[0])
pwr_ec = pwr_rt.replace("m3pwr", "m3pwr_ec")
pr = m3f.create_component(pwr_rt)
self.proxy.publish_command(pr)
self.proxy.make_operational(pwr_rt)
self.proxy.make_operational(pwr_ec)
pr.set_motor_power_on()
tmax = [x.param.t_max for x in self.actuator_ec]
tmin = [x.param.t_min for x in self.actuator_ec]
self.proxy.step()
for c in self.actuator_ec:
self.bias.append(c.status.adc_torque)
tl = min(tmin) - self.bias[0]
tu = max(tmax) - self.bias[0]
self.cycle_pwm = False
self.cycle_last_pwm = False
self.cycle_tq = False
self.cycle_last_tq = False
self.step_period = [2000.0] * len(self.actuator_ec)
self.brake = [0]
# Create gui
self.mode = [0] * len(self.actuator_ec)
self.t_desire_a = [0] * len(self.actuator_ec)
self.t_desire_b = [0] * len(self.actuator_ec)
self.pwm_desire_a = [0] * len(self.actuator_ec)
self.pwm_desire_b = [0] * len(self.actuator_ec)
self.current_desire_a = [0] * len(self.actuator_ec)
self.current_desire_b = [0] * len(self.actuator_ec)
self.save = False
self.save_last = False
self.do_scope_torque = False
self.scope_torque = None
self.status_dict = self.proxy.get_status_dict()
self.param_dict = self.proxy.get_param_dict()
self.gui.add("M3GuiTree", "Status", (self, "status_dict"), [], [], m3g.M3GuiRead, column=2)
self.gui.add("M3GuiTree", "Param", (self, "param_dict"), [], [], m3g.M3GuiWrite, column=3)
self.gui.add(
"M3GuiModes",
"Mode",
(self, "mode"),
range(len(self.actuator_ec)),
[["Off", "Pwm", "PID", "CURRENT"], 1],
m3g.M3GuiWrite,
)
self.gui.add("M3GuiModes", "Brake", (self, "brake"), range(1), [["Enabled", "Disabled"], 1], m3g.M3GuiWrite)
self.gui.add(
"M3GuiSliders", "tqDesire", (self, "t_desire_a"), range(len(self.actuator_ec)), [tl, tu], m3g.M3GuiWrite
)
self.gui.add(
"M3GuiSliders", "tqDesire", (self, "t_desire_b"), range(len(self.actuator_ec)), [tl, tu], m3g.M3GuiWrite
)
self.gui.add(
"M3GuiSliders",
"pwmDesireA",
(self, "pwm_desire_a"),
range(len(self.actuator_ec)),
[-3200, 3200],
m3g.M3GuiWrite,
)
self.gui.add(
"M3GuiSliders",
"pwmDesireB",
(self, "pwm_desire_b"),
range(len(self.actuator_ec)),
[-3200, 3200],
m3g.M3GuiWrite,
)
self.gui.add(
"M3GuiSliders",
"currentDesireA",
(self, "current_desire_a"),
range(len(self.actuator_ec)),
[-100, 100],
m3g.M3GuiWrite,
)
self.gui.add(
"M3GuiSliders",
"currentDesireB",
(self, "current_desire_b"),
range(len(self.actuator_ec)),
[-3200, 3200],
m3g.M3GuiWrite,
)
self.gui.add(
"M3GuiSliders",
"StepPeriod (ms) ",
(self, "step_period"),
range(len(self.actuator_ec)),
[0, 4000],
m3g.M3GuiWrite,
)
self.gui.add("M3GuiToggle", "CyclePwm", (self, "cycle_pwm"), [], [["On", "Off"]], m3g.M3GuiWrite)
self.gui.add("M3GuiToggle", "CycleTq", (self, "cycle_tq"), [], [["On", "Off"]], m3g.M3GuiWrite)
self.gui.add("M3GuiToggle", "Save", (self, "save"), [], [["On", "Off"]], m3g.M3GuiWrite)
self.gui.add("M3GuiToggle", "Scope", (self, "do_scope_torque"), [], [["On", "Off"]], m3g.M3GuiWrite)
self.gui.start(self.step)
def start(self):
print '--------------------------'
print 'm: Target M3 arm'
print 'v: Target RVIZ'
print 'b: Target Both M3 and RVIZ'
print 'q: quit'
print '--------------'
print
self.k = 'a'
while self.k!='m' and self.k!='v' and self.k!='b' and self.k!='q':
self.k=m3t.get_keystroke()
if self.k=='q':
return
self.proxy = m3p.M3RtProxy()
if self.k=='m' or self.k=='b':
self.proxy.start()
bot_name=m3t.get_robot_name()
if bot_name == "":
print 'Error: no robot components found:', bot_names
return
self.bot=m3f.create_component(bot_name)
arm_names = ['right_arm', 'left_arm']
self.arm_name = m3t.user_select_components_interactive(arm_names,single=True)[0]
if self.arm_name == 'right_arm':
self.center = [0.450, -0.25, -0.1745]
else:
self.center = [0.450, 0.25, -0.1745]
avail_chains = self.bot.get_available_chains()
for c in avail_chains:
if c == 'torso':
self.center[2] += 0.5079
if self.k=='v' or self.k=='b':
self.viz = m3v.M3Viz(self.proxy, self.bot)
self.viz_launched = True
self.viz.turn_sim_on()
if self.k=='v':
self.theta_0[:] = self.bot.get_theta_sim_deg(self.arm_name)[:]
if self.k=='m' or self.k=='b':
self.proxy.subscribe_status(self.bot)
self.proxy.publish_command(self.bot)
self.proxy.make_operational_all()
self.proxy.step()
self.theta_0[:] = self.bot.get_theta_deg(self.arm_name)[:]
self.m3_launched = True
self.theta_soln_deg = [0.]*self.bot.get_num_dof(self.arm_name)
self.thetadot_0 = [0.]*self.bot.get_num_dof(self.arm_name)
self.bot.set_slew_rate_proportion(self.arm_name, [1.0]*7)
while True:
print '--------------------------'
print 'g: generate vias'
print 'd: display vias'
print 'v: set avg velocity (Current ',self.vel_avg,')'
print 's: set stiffness (Current',self.stiffness,')'
if self.k=='b' or self.k=='m':
print 'e: execute vias'
if self.k=='b' or self.k=='v':
print 't: test vias in visualizer'
print 'q: quit'
print '--------------'
print
m=m3t.get_keystroke()
if m=='q':
return
if m=='v':
print 'Enter avg velocity (0-60 Deg/S) [',self.vel_avg,']'
self.vel_avg=max(0,min(60,m3t.get_float(self.vel_avg)))
if m=='s':
print 'Enter stiffness (0-1.0) [',self.stiffness,']'
self.stiffness=max(0,min(1.0,m3t.get_float(self.stiffness)))
if m == 'g':
self.vias=[]
print
print '(s)quare or (c)ircle?'
shape = None
while shape != 's' and shape != 'c':
shape=m3t.get_keystroke()
length_m = 0.0
if shape == 's':
print
print 'Length of square side in cm (10-25) [25]'
length_cm = nu.float(max(10,min(25,m3t.get_int(25))))
length_m = length_cm / 100.0
diameter_m = 0.0
if shape == 'c':
print
print 'Diameter of circle in cm (10-25) [25]'
diameter_cm = nu.float(max(10,min(25,m3t.get_int(25))))
diameter_m = diameter_cm / 100.0
print
print 'Enter shape resolution (1-20 vias/side) [20]'
resolution = max(1,min(20,m3t.get_int(20)))
if self.m3_launched:
self.proxy.step()
x = self.center[0]
if shape == 's':
y_left = self.center[1] + length_m/2.0
y_right = self.center[1] - length_m/2.0
z_top = self.center[2] + length_m/2.0
z_bottom = self.center[2] - length_m/2.0
dy = (y_left - y_right) / nu.float(resolution)
dz = (z_top - z_bottom) / nu.float(resolution)
if self.arm_name=='right_arm':
# first add start point
self.ik_vias.append([x, y_left, z_top, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
# add top line
for i in range(resolution):
self.ik_vias.append([x, y_left - (i+1)*dy, z_top, self.axis_demo[0],self.axis_demo[1], self.axis_demo[2]])
# add right line
for i in range(resolution):
self.ik_vias.append([x, y_right, z_top - (i+1)*dz, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
# add bottom line
for i in range(resolution):
self.ik_vias.append([x, y_right + (i+1)*dy, z_bottom, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
# add left line
for i in range(resolution):
self.ik_vias.append([x, y_left, z_bottom + (i+1)*dz, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
else:
# first add start point
self.ik_vias.append([x, y_right, z_top, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
# add top line
for i in range(resolution):
self.ik_vias.append([x, y_right + (i+1)*dy, z_top, self.axis_demo[0],self.axis_demo[1], self.axis_demo[2]])
# add right line
for i in range(resolution):
self.ik_vias.append([x, y_left, z_top - (i+1)*dz, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
# add bottom line
for i in range(resolution):
self.ik_vias.append([x, y_left - (i+1)*dy, z_bottom, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
# add left line
for i in range(resolution):
self.ik_vias.append([x, y_right, z_bottom + (i+1)*dz, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
if shape == 'c':
for i in range(resolution*4 + 1):
dt = 2*nu.pi/(nu.float(resolution)*4)
t = (nu.pi/2) + i*dt
if t > nu.pi:
t -= 2*nu.pi
y = self.center[1] + (diameter_m/2.0) * nu.cos(t)
z = self.center[2] + (diameter_m/2.0) * nu.sin(t)
self.ik_vias.append([x, y, z, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
self.vias.append(self.theta_0[:])
# use zero position as reference for IK solver
ref=[0]*self.bot.get_num_dof(self.arm_name)
# use holdup position as reference
ref= [30,0,0,40,0,0,0]
self.bot.set_theta_sim_deg(self.arm_name,ref)
for ikv in self.ik_vias:
theta_soln = []
print 'solving for ik via:', ikv
if self.bot.get_tool_axis_2_theta_deg_sim(self.arm_name, ikv[:3], ikv[3:], theta_soln):
self.vias.append(theta_soln)
self.bot.set_theta_sim_deg(self.arm_name,theta_soln)
else:
print 'WARNING: no IK solution found for via ', ikv
self.bot.set_theta_sim_deg(self.arm_name,ref)
if self.viz_launched:
self.viz.step()
self.vias.append(self.theta_0[:])
if m=='d':
print
print '--------- IK Vias (', len(self.ik_vias), ')--------'
print '---------------[end_xyz[3], end_axis[3]]-----------'
for ikv in self.ik_vias:
print ikv
print
print '--------- Joint Vias (', len(self.vias), ')--------'
for v in self.vias:
print v
if m == 'e' or m=='t':
if len(self.vias) != 0:
for v in self.vias:
#print 'Adding via',v
self.jt.add_via_deg(v, [self.vel_avg]*self.ndof)
self.jt.start(self.theta_0[:], self.thetadot_0[:])
print
print '--------- Splines (', len(self.jt.splines), ')--------'
print '------------q_0, q_f, qdot_0, qdot_f, tf--------------'
for s in self.jt.splines:
print s.q_0, s.q_f, s.qdot_0, s.qdot_f, s.tf
print
print 'Hit any key to start or (q) to quit execution'
p=m3t.get_keystroke()
if p != 'q':
if self.m3_launched and m=='e':
self.bot.set_motor_power_on()
self.bot.set_mode_theta_gc(self.arm_name)
self.bot.set_stiffness(self.arm_name, [self.stiffness]*self.bot.get_num_dof(self.arm_name))
while not self.jt.is_splined_traj_complete():
q = self.jt.step()
if self.viz_launched and m=='t':
self.bot.set_theta_sim_deg(self.arm_name, q)
self.viz.step()
time.sleep(0.1)
elif self.m3_launched and m=='e':
self.bot.set_theta_deg(self.arm_name, q)
self.proxy.step()
self.ik_vias=[]
def start(self):
self.proxy.start()
self.ndof_finger = 3
self.flex_factor_index = [0.3] * self.ndof_finger
self.flex_factor_ring = [0.3] * self.ndof_finger
self.flex_factor_pinky = [0.3] * self.ndof_finger
self.flex_factor_thumb = [0.3] * 2
self.p = subprocess.Popen(
['roslaunch', 'm3_defs_ros', 'm3_launch.launch'])
rospy.init_node("joint_state_publisher")
self.pub = rospy.Publisher("/joint_states", JointState)
time.sleep(4.0)
hand_names = self.proxy.get_available_components('m3hand')
self.hands = []
self.hand_nums = []
for i in range(len(hand_names)):
self.hands.append(m3f.create_component(hand_names[i]))
self.proxy.subscribe_status(self.hands[i])
#self.proxy.publish_command(self.hands[i])
if hand_names[i][-2].isdigit():
self.hand_nums.append(hand_names[i][-2:])
else:
self.hand_nums.append(hand_names[i][-1])
#r_hand_ua_num = 14
self.ndof_hand_ua = 12
self.positions = []
self.joints = []
for j in range(len(self.hands)):
for i in range(self.ndof_hand_ua):
self.positions.append(0.0)
self.joints.append('m3joint_ua_mh' + str(self.hand_nums[j]) +
'_j' + str(i))
# Thumb: J0,J1,J2
# Index: J3, J4, J5
# Ring: J6,J7,J8
# Pinkie: J9, J10, J11
print 'Starting hand viz.'
while (True):
self.positions = []
self.proxy.step()
for i in range(len(self.hands)):
th = self.hands[i].get_theta_rad()
#Thumb
self.positions.append(-th[0] + 1.57) #0
self.positions.append(th[1] * self.flex_factor_thumb[0])
self.positions.append(th[1] * self.flex_factor_thumb[1])
#Index
self.positions.append(th[2] * self.flex_factor_index[0])
self.positions.append(th[2] * self.flex_factor_index[1])
self.positions.append(th[2] * self.flex_factor_index[2])
#Ring
self.positions.append(th[3] * self.flex_factor_ring[0])
self.positions.append(th[3] * self.flex_factor_ring[1])
self.positions.append(th[3] * self.flex_factor_ring[2])
#Pinkie
self.positions.append(th[4] * self.flex_factor_pinky[0])
self.positions.append(th[4] * self.flex_factor_pinky[1])
self.positions.append(th[4] * self.flex_factor_pinky[2])
if self.pub is not None and not rospy.is_shutdown():
header = Header(0, rospy.Time.now(), '0')
self.pub.publish(
JointState(header, self.joints, self.positions,
[0] * len(self.positions),
[0] * len(self.positions)))
else:
print 'Error...exiting.'
break
time.sleep(0.1)
def start(self):
self.proxy.start()
#print 'Enable RVIZ [n]?'
self.rviz = False
#if m3t.get_yes_no('n'):
# self.rviz = True
chain_names=self.proxy.get_chain_components()
self.chain=[]
if len(chain_names)>0:
print 'Select kinematic chain'
self.chain_names=m3t.user_select_components_interactive(chain_names,single=True)
self.chain.append(m3f.create_component(self.chain_names[0]))
self.proxy.subscribe_status(self.chain[-1])
self.limb=m3t.get_chain_limb_name(self.chain_names[0])
joint_names=m3t.get_chain_joint_names(self.chain_names[0])
print 'Select joint'
joint_names=m3t.user_select_components_interactive(joint_names,single=True)
self.joint=[]
self.actuator=[]
self.actuator_ec=[]
acutator_names=[]
for n in joint_names:
self.joint.append(m3f.create_component(n))
actuator_name = m3t.get_joint_actuator_component_name(n)
actuator_ec_name = m3t.get_actuator_ec_component_name(actuator_name)
self.actuator.append(m3f.create_component(actuator_name))
self.actuator_ec.append(m3f.create_component(actuator_ec_name))
self.proxy.subscribe_status(self.joint[-1])
self.proxy.publish_param(self.joint[-1])
self.proxy.subscribe_status(self.actuator[-1])
self.proxy.publish_param(self.actuator[-1])
if self.actuator_ec[0] is not None:
self.proxy.subscribe_status(self.actuator_ec[-1])
self.proxy.publish_param(self.actuator_ec[-1])
#kine_names=self.proxy.get_available_components('m3dynamatics')
self.kine = []
#if len(kine_names)>0:
#print 'Select dynamatics controller'
#kine_names=m3t.user_select_components_interactive(kine_names)
#for n in kine_names:
#self.kine.append(m3f.create_component(n))
#self.proxy.subscribe_status(self.kine[-1])
bot_name=m3t.get_robot_name()
if bot_name == "":
print 'Error: no robot components found:', bot_names
return
self.bot=m3f.create_component(bot_name)
if self.rviz:
self.viz = m3v.M3Viz(self.proxy, self.bot)
#self.proxy.publish_param(self.bot) #allow to set payload
#self.proxy.subscribe_status(self.bot)
#self.proxy.publish_command(self.bot)
#self.proxy.make_operational_all()
self.bot.initialize(self.proxy)
#self.chain_names = self.bot.get_available_chains()
#Create gui
self.mode=[0]
self.posture=[0]
self.theta_desire_a=[0]*self.bot.get_num_dof(self.limb)
self.theta_desire_b=[0]*self.bot.get_num_dof(self.limb)
self.stiffness=[50.0]*self.bot.get_num_dof(self.limb)
self.thetadot=[10.0]*self.bot.get_num_dof(self.limb)
#print 'Selected: ',self.chain_names[0],self.limb,self.bot.get_num_dof(self.limb)
#self.slew=[0]
self.save=False
self.save_last=False
self.status_dict=self.proxy.get_status_dict()
self.param_dict=self.proxy.get_param_dict()
self.gui.add('M3GuiModes', 'Mode', (self,'mode'),range(1),[['Off','Pwm','Torque','Theta','Torque_GC','Theta_GC','Theta_MJ', 'Theta_GC_MJ'],1],m3g.M3GuiWrite)
self.gui.add('M3GuiTree', 'Status', (self,'status_dict'),[],[],m3g.M3GuiRead,column=1)
self.gui.add('M3GuiTree', 'Param', (self,'param_dict'),[],[],m3g.M3GuiWrite,column=1)
self.gui.add('M3GuiSliders','ThetaA (Deg)', (self,'theta_desire_a'),range(len(self.theta_desire_a)),[-45,140],m3g.M3GuiWrite,column=2)
self.gui.add('M3GuiModes', 'Posture', (self,'posture'),range(1),[['A','B','Cycle'],1],m3g.M3GuiWrite,column=2)
self.gui.add('M3GuiSliders','ThetaB (Deg)', (self,'theta_desire_b'),range(len(self.theta_desire_b)),[-45,140],m3g.M3GuiWrite,column=2)
self.gui.add('M3GuiSliders','Stiffness ', (self,'stiffness'),range(len(self.stiffness)),[0,100],m3g.M3GuiWrite,column=3)
self.gui.add('M3GuiToggle', 'Save', (self,'save'),[],[['On','Off']],m3g.M3GuiWrite,column=3)
self.gui.add('M3GuiSliders','ThetaDot ', (self,'thetadot'),range(len(self.thetadot)),[0,100],m3g.M3GuiWrite,column=3)
#self.gui.add('M3GuiSliders','Stiffness ', (self,'stiffness'),range(len(self.sea_joint)),[0,100],m3g.M3GuiWrite,column=3)
self.gui.start(self.step)
print 'Omnibase not found. Proceeding...'
else:
omni=m3o.M3OmniBase(base_name[0])
proxy.publish_param(omni) # we need this for calibration
proxy.subscribe_status(omni)
proxy.publish_command(omni)
if omni==None and zlift==None:
exit()
if zlift is not None:
pwr_rt=m3t.get_joint_pwr_component_name(zlift_names[0])
else:
pwr_rt=m3t.get_omnibase_pwr_component_name(base_name[0])
pwr=m3f.create_component(pwr_rt)
proxy.publish_command(pwr)
pwr.set_motor_power_on()
proxy.make_operational_all()
proxy.step()
time.sleep(0.5)
proxy.step()
zlift_shm_names=proxy.get_available_components('m3joint_zlift_shm')
if len(zlift_shm_names) > 0:
proxy.make_safe_operational(zlift_shm_names[0])
omnibase_shm_names=proxy.get_available_components('m3omnibase_shm')
if len(omnibase_shm_names) > 0:
proxy.make_safe_operational(omnibase_shm_names[0])
import m3.toolbox as m3t
import m3.component_factory as mcf
import numpy.numarray as na
import math
proxy = m3p.M3RtProxy()
proxy.start()
cnames=proxy.get_available_components()
print 'All actuator_ec [n]?'
comps=[]
if m3t.get_yes_no('n'):
cnames=[q for q in cnames if q.find('actuator_ec')!=-1]
all_aec=True
for c in cnames:
comps.append(mcf.create_component(c))
else:
all_aec=False
print '------- Components ------'
for i in range(len(cnames)):
print i,' : ',cnames[i]
print '-------------------------'
print 'Enter component id'
cid=m3t.get_int()
name=cnames[cid]
comps.append(mcf.create_component(name))
if len(comps)==0:
print 'No components selected'
exit()
for c in comps:
robot_name = 'm3humanoid_bimanual_mr1'
chain_names = ['m3arm_ma1', 'm3arm_ma2']
dynamatics_nms = ['m3dynamatics_ma1', 'm3dynamatics_ma2']
proxy.make_safe_operational(robot_name)
for c in chain_names:
proxy.make_safe_operational(c)
for d in dynamatics_nms:
proxy.make_safe_operational(d)
proxy = m3p.M3RtProxy()
proxy.start()
pwr_nm = 'm3pwr_pwr003'
pwr = m3f.create_component(pwr_nm)
proxy.publish_command(pwr)
joint_names = [
'm3joint_ma1_j0', 'm3joint_ma1_j1', 'm3joint_ma1_j2', 'm3joint_ma1_j3',
'm3joint_ma1_j4', 'm3joint_ma1_j5', 'm3joint_ma1_j6'
]
comp_list = []
stiff_list = [0.2, 0.67, 1., 0.7, 0.75, 0.5, 0.5]
for i, c in enumerate(joint_names):
comp = m3f.create_component(c)
comp_list.append(comp)
proxy.publish_command(comp)
if i < 5:
comp.set_control_mode(THETA_GC)
def do_dump(proxy):
print 'Starting dump...'
print 'Enter log name [foo]'
logname = m3t.get_string('foo')
ns = proxy.get_log_num_samples(logname)
if ns == 0:
return
print '-------- Available components --------'
names = proxy.get_log_component_names(logname)
for i in range(len(names)):
print names[i]
print '--------------------------------------'
print 'Select component: [', names[0], ']'
name = m3t.get_string(names[0])
print '--------------------------------------'
print 'Num samples available: ', ns
print 'Enter start sample idx: [0]'
start = max(0, m3t.get_int(0))
print 'Enter end sample idx: [', ns - 1, ']'
end = min(ns - 1, m3t.get_int(ns - 1))
print '--------------------------------------'
comp = mcf.create_component(name)
proxy.register_log_component(comp)
fields = []
print 'Dump all data?[n]'
if m3t.get_yes_no('n'):
'''print 'Fetching data...'
fn=m3t.get_m3_log_path()+logname+'/'+logname+'_dump.yaml'
f=file(fn,'w')
print 'Saving...',fn
f.write(yaml.safe_dump(comp.status, default_flow_style=False,width=200))
f.close()'''
fields = comp.status.DESCRIPTOR.fields_by_name.keys()
print fields
else:
print ' Select fields to plot...'
while True:
fields.append(m3t.user_select_msg_field(comp.status))
print 'Another field [n]?'
if not m3t.get_yes_no('n'):
break
print 'Fetching data...'
data = {}
for k in fields:
data[k] = []
print '---------------------------'
print k
print m3t.get_msg_field_value(comp.status, k)
print dir(m3t.get_msg_field_value(comp.status, k))
if hasattr(m3t.get_msg_field_value(comp.status, k), '__len__'):
for j in range(len(m3t.get_msg_field_value(comp.status, k))):
data[k].append([])
for i in range(start, end):
proxy.load_log_sample(logname, i)
for k in fields:
repeated = False
# skip protobuf subclasses (like base) for now. note we'll want this for humanoid
if hasattr(m3t.get_msg_field_value(comp.status, k),
'__metaclass__'):
pass
elif hasattr(m3t.get_msg_field_value(comp.status, k), '__len__'):
for j in range(len(m3t.get_msg_field_value(comp.status, k))):
data[k][j].append(
m3t.get_msg_field_value(comp.status, k)[j])
else:
data[k].append(m3t.get_msg_field_value(comp.status, k))
fn = m3t.get_m3_log_path() + logname + '/' + logname + '_dump.yaml'
f = file(fn, 'w')
print 'Saving...', fn
print data
f.write(yaml.safe_dump(data, default_flow_style=False, width=200))
f.close()
import m3.rt_proxy as m3p
import meka_omnibase_control as m3o
import m3.component_factory as m3f
import math
import numpy
import time
WHEEL_COUNT = 4
if __name__=='__main__':
proxy = m3p.M3RtProxy()
proxy.start()
pwr = m3f.create_component("m3pwr_pwr029")
omni = m3o.MekaOmnibaseControl('meka_omnibase_control_mb2', 'meka_omnibase_control')
proxy.make_operational_all()
proxy.subscribe_status(omni)
proxy.publish_command(omni)
proxy.subscribe_status(pwr)
proxy.publish_command(pwr)
#pwr.set_motor_power_on()
proxy.step()
print "Omnibase calibration - Please slowly move the mobile base so that", \
def start(self):
# ######## Setup Proxy and Components #########################
self.proxy.start()
self.current_first = True
bot_name=m3t.get_robot_name()
if bot_name == "":
print 'Error: no robot components found:', bot_names
return
self.bot=m3.humanoid.M3Humanoid(bot_name)
arm_names = self.bot.get_available_chains()
arm_names = [x for x in arm_names if x.find('arm')!=-1]
if len(arm_names)==0:
print 'No arms found'
return
if len(arm_names)==1:
self.arm_name=arm_names[0]
else:
self.arm_name = m3t.user_select_components_interactive(arm_names,single=True)[0]
self.jnts = self.bot.get_joint_names(self.arm_name)
comp={}
for c in self.jnts:
comp[c]={'comp_rt':None,'comp_j':None,'torque_act':[],'torque_joint':[],'torque_gravity':[],'is_wrist':False}
if (c.find('j5')>=0 or c.find('j6')>=0):
comp[c]['is_wrist']=True
for c in self.jnts:
comp[c]['comp_j']=mcf.create_component(c)
comp[c]['comp_rt']=mcf.create_component(c.replace('joint','actuator'))
self.proxy.subscribe_status(comp[c]['comp_rt'])
self.proxy.subscribe_status(comp[c]['comp_j'])
# ####### Setup Proxy #############
self.proxy.subscribe_status(self.bot)
self.proxy.publish_command(self.bot)
self.proxy.make_operational_all()
self.bot.set_motor_power_on()
self.ndof=self.bot.get_num_dof(self.arm_name)
humanoid_shm_names=self.proxy.get_available_components('m3humanoid_shm')
if len(humanoid_shm_names) > 0:
self.proxy.make_safe_operational(humanoid_shm_names[0])
self.bot.set_mode_off(self.arm_name)
print 'This script will calibrate the zero-torque of the A2 arm while posed using current torque_gravity estimate'
print '----------------------------------------------------------------------------------------------------------'
print 'With E-Stop down, pose the arm in desired position to calibrate torque zeroes around.'
print 'Press any key when ready posed.'
raw_input()
time.sleep(0.5)
self.proxy.step()
self.theta_curr = self.bot.get_theta_deg(self.arm_name)[:]
self.proxy.step()
print 'Posed position set.'
print 'Release E-stop and press any key for arm to hold pose.'
raw_input()
self.bot.set_mode_theta_gc(self.arm_name)
self.bot.set_theta_deg(self.arm_name,self.theta_curr)
self.bot.set_stiffness(self.arm_name,[1.0]*7)
self.bot.set_slew_rate_proportion(self.arm_name,[1.0]*self.ndof)
self.proxy.step()
print 'Press any key to start torque calibration for all joints.'
raw_input()
self.proxy.step()
# ###########################
ns=30
for i in range(ns):
self.proxy.step()
print '---------'
for c in comp.keys():
tqj=comp[c]['comp_j'].get_torque_mNm()
tqg=comp[c]['comp_j'].get_torque_gravity_mNm()/1000.0
tqa=comp[c]['comp_rt'].get_torque_mNm()
comp[c]['torque_act'].append(tqa)
comp[c]['torque_joint'].append(tqj)
comp[c]['torque_gravity'].append(tqg)
if comp[c]['is_wrist']:
print c,':joint',tqj,':gravity',tqg,':actuator',tqa
else:
print c,':joint',tqj,':gravity',tqg,
time.sleep(0.05)
do_query = True
# ###########################
for c in comp.keys():
print '--------',c,'---------'
tqg=float(na.array(comp[c]['torque_gravity'],na.Float32).mean())
tqj=float(na.array(comp[c]['torque_joint'],na.Float32).mean())
tqa=float(na.array(comp[c]['torque_act'],na.Float32).mean())
if not comp[c]['is_wrist']:
bias=tqa+tqg
torque=M3TorqueSensor(comp[c]['comp_rt'].config['calib']['torque']['type'])
print 'Measured torque:',tqa,'Torque gravity:', tqg
print 'Delta of',bias,'mNm'
comp[c]['comp_rt'].config['calib']['torque']['cb_bias']=comp[c]['comp_rt'].config['calib']['torque']['cb_bias']-bias
comp[c]['comp_rt'].config['calibration_date']=time.asctime()
if do_query:
print 'Save calibration? [y]'
if m3t.get_yes_no('y'):
comp[c]['comp_rt'].write_config()
else:
comp[c]['comp_rt'].write_config()
else:
print 'Wrist joint...'
if c.find('j5')!=-1: #do j5/j6 at once
cc=None
for x in comp.keys():
if x.find('j6')!=-1:
cc=x
if cc is None:
print 'Did not find coupled joint to',c
tqg_c=float(na.array(comp[cc]['torque_gravity'],na.Float32).mean())
tqj_c=float(na.array(comp[cc]['torque_joint'],na.Float32).mean())
tqa_c=float(na.array(comp[cc]['torque_act'],na.Float32).mean())
x=comp[c]['comp_j'].config['transmission']['tqj_to_tqa'][0] #Joint to actuator matrix
y=comp[c]['comp_j'].config['transmission']['tqj_to_tqa'][1]
m=comp[cc]['comp_j'].config['transmission']['tqj_to_tqa'][0]
n=comp[cc]['comp_j'].config['transmission']['tqj_to_tqa'][1]
tqg_a5= x*tqg+y*tqg_c
tqg_a6= m*tqg_c+n*tqg
bias_5=tqa+tqg_a5
bias_6=tqa_c+tqg_a6
torque_5=M3TorqueSensor(comp[c]['comp_rt'].config['calib']['torque']['type'])
torque_6=M3TorqueSensor(comp[cc]['comp_rt'].config['calib']['torque']['type'])
print '------------'
print 'J5: Previous joint torque',tqj,'with joint torque gravity', tqg
print 'J5: Previous actuator torque',tqa,'with actuator torque gravity', tqg_a5
print 'J5: Actuator delta of',bias_5,'mNm'
print '------------'
print 'J6: Previous joint torque',tqj_c,'with joint torque gravity', tqg_c
print 'J6: Previous actuator torque',tqa_c,'with actuator torque gravity', tqg_a6
print 'J6: Actuator delta of',bias_6,'mNm'
print '------------'
comp[c]['comp_rt'].config['calib']['torque']['cb_bias']=comp[c]['comp_rt'].config['calib']['torque']['cb_bias']-bias_5
comp[c]['comp_rt'].config['calibration_date']=time.asctime()
comp[cc]['comp_rt'].config['calib']['torque']['cb_bias']=comp[cc]['comp_rt'].config['calib']['torque']['cb_bias']-bias_6
comp[cc]['comp_rt'].config['calibration_date']=time.asctime()
if do_query:
print 'Save calibration? [y]'
if m3t.get_yes_no('y'):
comp[c]['comp_rt'].write_config()
comp[cc]['comp_rt'].write_config()
else:
comp[c]['comp_rt'].write_config()
comp[cc]['comp_rt'].write_config()
self.bot.set_mode_off(self.arm_name)
self.proxy.stop()
print 'Omnibase not found. Proceeding...'
else:
omni = m3o.M3OmniBase(base_name[0])
proxy.publish_param(omni) # we need this for calibration
proxy.subscribe_status(omni)
proxy.publish_command(omni)
if omni == None and zlift == None:
exit()
if zlift is not None:
pwr_rt = m3t.get_joint_pwr_component_name(zlift_names[0])
else:
pwr_rt = m3t.get_omnibase_pwr_component_name(base_name[0])
pwr = m3f.create_component(pwr_rt)
proxy.publish_command(pwr)
pwr.set_motor_power_on()
proxy.make_operational_all()
proxy.step()
time.sleep(0.5)
proxy.step()
zlift_shm_names = proxy.get_available_components('m3joint_zlift_shm')
if len(zlift_shm_names) > 0:
proxy.make_safe_operational(zlift_shm_names[0])
omnibase_shm_names = proxy.get_available_components('m3omnibase_shm')
if len(omnibase_shm_names) > 0:
proxy.make_safe_operational(omnibase_shm_names[0])
def main():
proxy = m3p.M3RtProxy()
proxy.start()
base_name=proxy.get_available_components('m3omnibase')
omni=None
if len(base_name)!=1:
print 'Omnibase not available. Proceeding without it...'
else:
print 'Use OmniBase [y]?'
if m3t.get_yes_no('y'):
omni=m3o.M3OmniBase(base_name[0])
proxy.publish_param(omni) # we need this for calibration
proxy.subscribe_status(omni)
proxy.publish_command(omni)
pwr_name=[m3t.get_omnibase_pwr_component_name(base_name[0])]
#proxy.get_available_components('m3pwr')
#if len(pwr_name)>1:
#pwr_name=m3t.user_select_components_interactive(pwr_name,single=True)
pwr=m3f.create_component(pwr_name[0])
proxy.subscribe_status(pwr)
proxy.publish_command(pwr)
zlift_names=proxy.get_available_components('m3joint_zlift')
zl=None
if len(zlift_names)==1:
print 'Use Zlift [y]?'
if m3t.get_yes_no('y'):
zl=m3z.M3JointZLift(zlift_names[0])
proxy.subscribe_status(zl)
proxy.publish_command(zl)
proxy.publish_param(zl)
proxy.make_operational(pwr_name[0])
proxy.step()
if omni is not None:
omni.set_mode_off()
pwr.set_motor_power_on()
proxy.make_operational_all()
zlift_shm_names=proxy.get_available_components('m3joint_zlift_shm')
if len(zlift_shm_names) > 0:
proxy.make_safe_operational(zlift_shm_names[0])
omnibase_shm_names=proxy.get_available_components('m3omnibase_shm')
if len(omnibase_shm_names) > 0:
proxy.make_safe_operational(omnibase_shm_names[0])
humanoid_shm_names=proxy.get_available_components('m3humanoid_shm')
if len(humanoid_shm_names) > 0:
proxy.make_safe_operational(humanoid_shm_names[0])
m3ledx2xn_ec_shm_names=proxy.get_available_components('m3ledx2xn_ec_shm')
if len(m3ledx2xn_ec_shm_names) > 0:
proxy.make_safe_operational(m3ledx2xn_ec_shm_names[0])
m3led_matrix_ec_shm_names=proxy.get_available_components('m3led_matrix_ec_shm')
if len(m3led_matrix_ec_shm_names) > 0:
proxy.make_safe_operational(m3led_matrix_ec_shm_names[0])
proxy.step()
time.sleep(0.5)
if omni is not None:
proxy.step()
omni.calibrate(proxy)
time.sleep(0.5)
if zl is not None:
if not zl.calibrate(proxy):
zl=None
print 'ZLift failed to calibrate'
if omni is None and zl is None:
exit()
print "Turn motor power on to Omnibase and press any key."
raw_input()
joy=m3to.M3OmniBaseJoy()
joy.start(proxy,omni,zl)
k = 0
try:
while True:
joy.step()
#print 'Bus Current:', pwr.get_bus_torque()
p = omni.get_local_position()
#omni.set_op_space_forces(f.jx*200.0, f.jy*200.0, f.jyaw*50.0)
k += 1
if k == 100:
print '-----------Local Pos-------'
print 'X:', p[0]
print 'Y:', p[1]
print 'Yaw:', p[2]
#print '---------------------------'
k = 0
'''print '-------Joystick Pos-------'
print 'jx:',f.jx
print 'jy:', f.jy
print 'jyaw:', f.jyaw
print 'button:', f.jbutton
print '---------------------------'
#print 'Bus voltage',omni.get_bus_voltage()'''
'''tqs = omni.get_motor_torques()
print tqs[0], tqs[2], tqs[4], tqs[6]'''
#print omni.get_steer_torques()
proxy.step()
except KeyboardInterrupt:
pass
joy.stop()
if omni is not None:
omni.set_mode_off()
pwr.set_motor_power_off()
proxy.step()
proxy.stop()
def start(self):
self.proxy.start()
joint_names = self.proxy.get_joint_components()
if len(joint_names) == 0:
print 'No joint components available'
self.proxy.stop()
exit()
joint_names = m3t.user_select_components_interactive(joint_names)
actuator_ec_names = []
actuator_names = []
ctrl_names = []
for n in joint_names:
ctrl = m3t.get_joint_ctrl_component_name(n)
if ctrl != "":
ctrl_names.append(ctrl)
actuator = m3t.get_joint_actuator_component_name(n)
if actuator != "":
actuator_names.append(actuator)
actuator_ec = m3t.get_actuator_ec_component_name(actuator)
if actuator_ec != "":
actuator_ec_names.append(actuator_ec)
self.joint = []
self.actuator_ec = []
self.actuator = []
self.ctrl = []
for n in actuator_ec_names:
c = m3f.create_component(n)
if c is not None:
try:
self.actuator_ec.append(c)
self.proxy.subscribe_status(self.actuator_ec[-1])
self.proxy.publish_param(self.actuator_ec[-1])
except:
print 'Component', n, 'not available'
for n in actuator_names:
c = m3f.create_component(n)
if c is not None:
self.actuator.append(c)
self.proxy.subscribe_status(self.actuator[-1])
self.proxy.publish_param(self.actuator[-1])
for n in ctrl_names:
c = m3f.create_component(n)
if c is not None:
self.ctrl.append(c)
self.proxy.subscribe_status(self.ctrl[-1])
self.proxy.publish_param(self.ctrl[-1])
for n in joint_names:
c = m3f.create_component(n)
if c is not None:
self.joint.append(c)
self.proxy.subscribe_status(self.joint[-1])
self.proxy.publish_command(self.joint[-1])
self.proxy.publish_param(self.joint[-1])
#Enable motor power
pwr_rt = m3t.get_actuator_ec_pwr_component_name(actuator_ec_names[0])
self.pwr = m3f.create_component(pwr_rt)
if self.pwr is not None:
self.proxy.publish_command(self.pwr)
self.pwr.set_motor_power_on()
#Start them all up
self.proxy.make_operational_all()
#Force safe-op of robot, etc are present
types = ['m3humanoid', 'm3hand', 'm3gripper']
for t in types:
cc = self.proxy.get_available_components(t)
for ccc in cc:
self.proxy.make_safe_operational(ccc)
#Force safe-op of chain so that gravity terms are computed
self.chain = None
if len(joint_names) > 0:
for j in joint_names:
chain_name = m3t.get_joint_chain_name(j)
if chain_name != "":
self.proxy.make_safe_operational(chain_name)
#self.chain=m3f.create_component(chain_name)
#self.proxy.publish_param(self.chain) #allow to set payload
#Force safe-op of chain so that gravity terms are computed
dynamatics_name = m3t.get_chain_dynamatics_component_name(
chain_name)
if dynamatics_name != "":
self.proxy.make_safe_operational(dynamatics_name)
self.dyn = m3f.create_component(dynamatics_name)
self.proxy.publish_param(
self.dyn) #allow to set payload
#Force safe-op of robot so that gravity terms are computed
robot_name = m3t.get_robot_name()
if robot_name != "":
try:
self.proxy.make_safe_operational(robot_name)
self.robot = m3f.create_component(robot_name)
self.proxy.subscribe_status(self.robot)
self.proxy.publish_param(self.robot) #allow to set payload
except:
print 'Component', robot_name, 'not available'
tmax = max([x.param.max_tq for x in self.actuator])
tmin = min([x.param.min_tq for x in self.actuator])
qmax = max([x.param.max_q for x in self.joint])
qmin = min([x.param.min_q for x in self.joint])
## Plots
self.scope_torque = []
self.scope_theta = []
self.scope_thetadot = []
self.scope_thetadotdot = []
self.scope_torquedot = []
for i, name in zip(xrange(len(joint_names)), joint_names):
self.scope_torque.append(
m3t.M3ScopeN(xwidth=100, yrange=None, title='Torque'))
self.scope_theta.append(
m3t.M3ScopeN(xwidth=100, yrange=None, title='Theta'))
self.scope_thetadot.append(
m3t.M3ScopeN(xwidth=100, yrange=None, title='ThetaDot'))
self.scope_thetadotdot.append(
m3t.M3ScopeN(xwidth=100, yrange=None, title='ThetaDotDot'))
self.scope_torquedot.append(
m3t.M3ScopeN(xwidth=100, yrange=None, title='TorqueDot'))
#Create gui
self.mode = [0] * len(self.joint)
self.tq_desire_a = [0] * len(self.joint)
self.tq_desire_b = [0] * len(self.joint)
self.pwm_desire = [0] * len(self.joint)
self.theta_desire_a = [0] * len(self.joint)
self.theta_desire_b = [0] * len(self.joint)
self.thetadot_desire = [0] * len(self.joint)
self.stiffness = [0] * len(self.joint)
self.slew = [0] * len(self.joint)
self.step_period = [2000.0] * len(self.joint)
self.cycle_theta = False
self.cycle_last_theta = False
self.cycle_thetadot = False
self.cycle_last_thetadot = False
self.cycle_torque = False
self.cycle_last_torque = False
self.save = False
self.do_scope_torque = False
self.do_scope_torquedot = False
self.do_scope_theta = False
self.do_scope_thetadot = False
self.do_scope_thetadotdot = False
self.brake = False
self.save_last = False
self.status_dict = self.proxy.get_status_dict()
self.param_dict = self.proxy.get_param_dict()
self.gui.add('M3GuiTree',
'Status', (self, 'status_dict'), [], [],
m3g.M3GuiRead,
column=2)
self.gui.add('M3GuiTree',
'Param', (self, 'param_dict'), [], [],
m3g.M3GuiWrite,
column=3)
self.gui.add('M3GuiModes', 'Mode', (self, 'mode'),
range(len(self.joint)), [[
'Off', 'Pwm', 'Torque', 'Theta', 'Torque_GC',
'Theta_GC', 'Theta_MJ', 'Theta_GC_MJ', 'Pose',
'Torque_GRAV_MODEL', 'ThetaDot_GC', 'ThetaDot'
], 1], m3g.M3GuiWrite)
self.gui.add('M3GuiSliders', 'TorqueA (mNm)', (self, 'tq_desire_a'),
range(len(self.joint)), [tmin, tmax], m3g.M3GuiWrite)
self.gui.add('M3GuiSliders', 'TorqueB (mNm)', (self, 'tq_desire_b'),
range(len(self.joint)), [tmin, tmax], m3g.M3GuiWrite)
self.gui.add('M3GuiSliders', 'Pwm', (self, 'pwm_desire'),
range(len(self.joint)), [-3200, 3200], m3g.M3GuiWrite)
self.gui.add('M3GuiSliders',
'Theta A(Deg)', (self, 'theta_desire_a'),
range(len(self.joint)), [qmin, qmax],
m3g.M3GuiWrite,
column=2)
self.gui.add('M3GuiSliders',
'Theta B(Deg)', (self, 'theta_desire_b'),
range(len(self.joint)), [qmin, qmax],
m3g.M3GuiWrite,
column=2)
self.gui.add('M3GuiSliders',
'Thetadot (Deg)', (self, 'thetadot_desire'),
range(len(self.joint)), [-120.0, 120.0],
m3g.M3GuiWrite,
column=2)
self.gui.add('M3GuiSliders',
'Stiffness ', (self, 'stiffness'),
range(len(self.joint)), [0, 100],
m3g.M3GuiWrite,
column=3)
self.gui.add('M3GuiSliders',
'Slew ', (self, 'slew'),
range(len(self.joint)), [0, 100],
m3g.M3GuiWrite,
column=3)
self.gui.add('M3GuiToggle', 'Save', (self, 'save'), [],
[['On', 'Off']], m3g.M3GuiWrite)
self.gui.add('M3GuiToggle', 'ScopeTorque', (self, 'do_scope_torque'),
[], [['On', 'Off']], m3g.M3GuiWrite)
self.gui.add('M3GuiToggle', 'ScopeTorqueDot',
(self, 'do_scope_torquedot'), [], [['On', 'Off']],
m3g.M3GuiWrite)
self.gui.add('M3GuiToggle', 'ScopeTheta', (self, 'do_scope_theta'), [],
[['On', 'Off']], m3g.M3GuiWrite)
self.gui.add('M3GuiToggle', 'ScopeThetaDot',
(self, 'do_scope_thetadot'), [], [['On', 'Off']],
m3g.M3GuiWrite)
self.gui.add('M3GuiToggle', 'ScopeThetaDotDot',
(self, 'do_scope_thetadotdot'), [], [['On', 'Off']],
m3g.M3GuiWrite)
self.gui.add('M3GuiSliders', 'StepPeriod (ms) ', (self, 'step_period'),
range(len(self.joint)), [0, 8000], m3g.M3GuiWrite)
self.gui.add('M3GuiToggle', 'CycleTheta', (self, 'cycle_theta'), [],
[['On', 'Off']], m3g.M3GuiWrite)
self.gui.add('M3GuiToggle', 'CycleThetaDot', (self, 'cycle_thetadot'),
[], [['On', 'Off']], m3g.M3GuiWrite)
self.gui.add('M3GuiToggle', 'CycleTorque', (self, 'cycle_torque'), [],
[['On', 'Off']], m3g.M3GuiWrite)
self.gui.add('M3GuiToggle', 'Brake', (self, 'brake'), [],
[['On', 'Off']], m3g.M3GuiWrite)
self.gui.start(self.step)
def main():
proxy = m3p.M3RtProxy()
proxy.start()
base_name=proxy.get_available_components('m3omnibase')
if len(base_name)!=1:
print 'Invalid number of base components available'
proxy.stop()
exit()
omni=m3o.M3OmniBase(base_name[0])
proxy.publish_param(omni) # we need this for calibration
proxy.subscribe_status(omni)
proxy.publish_command(omni)
pwr_name=[m3t.get_omnibase_pwr_component_name(base_name[0])]
#proxy.get_available_components('m3pwr')
#if len(pwr_name)>1:
#pwr_name=m3t.user_select_components_interactive(pwr_name,single=True)
pwr=m3f.create_component(pwr_name[0])
proxy.subscribe_status(pwr)
proxy.publish_command(pwr)
proxy.make_operational(pwr_name[0])
proxy.step()
pwr.set_motor_power_on()
proxy.step()
proxy.make_operational_all()
proxy.step()
time.sleep(0.5)
proxy.step()
omni.calibrate(proxy)
time.sleep(0.5)
omni.set_local_position(0,0,0,proxy)
omni.set_global_position(0,0,0,proxy)
omni.set_mode_traj_via()
'''omni.add_via(0, 1.2, -180)
omni.add_via(1, 1.2, 0)
omni.add_via(1, 0, 180)
omni.add_via(0, 0, 0)'''
omni.add_via(0, 0.2, 0)
omni.add_via(0.2, 0.2, 0)
omni.add_via(0.2, 0, 0)
omni.add_via(0, 0, 0)
proxy.step()
time.sleep(0.2)
proxy.step()
'''while not omni.is_traj_goal_reached():
proxy.step()'''
try:
while True:
proxy.step()
p = omni.get_global_position()
d = omni.get_desired_position()
print '-----------Local Pos-------'
print 'X:', p[0]
print 'Y:', p[1]
print 'Yaw:', p[2]
print '---------------------------'
print '-----------Desired Pos-------'
print 'dX:', d[0]
print 'dY:', d[1]
print 'dYaw:', d[2]
print '---------------------------'
time.sleep(0.1)
except (KeyboardInterrupt):
pass
stop()
print '------- Components ------'
for i in range(len(comps)):
print i, ' : ', comps[i]
print '-------------------------'
print 'Enter component id'
cid = m3t.get_int()
print 'Select Y-Range? [n]'
yrange = None
if m3t.get_yes_no('n'):
yrange = []
print 'Min?'
yrange.append(m3t.get_int())
print 'Max?'
yrange.append(m3t.get_int())
name = comps[cid]
comp = mcf.create_component(name)
proxy.subscribe_status(comp)
#proxy.publish_param(comp)
field = m3t.user_select_msg_field(comp.status)
repeated = False
idx = 0
if hasattr(m3t.get_msg_field_value(comp.status, field), '__len__'):
repeated = True
print 'Select index of repeated field to monitor: [0]'
idx = m3t.get_int(0)
scope = m3t.M3Scope(xwidth=100, yrange=yrange)
try:
ts = time.time()
while True:
proxy.step()
import pylab as pyl
import time
import m3.rt_proxy as m3p
import m3.toolbox as m3t
import m3.component_factory as mcf
import numpy.numarray as na
import math
proxy = m3p.M3RtProxy()
proxy.start()
cnames = proxy.get_available_components()
cnames = [q for q in cnames if q.find('actuator_ec') != -1]
cnames = [q for q in cnames if q.find('mh') == -1]
comps = []
for c in cnames:
comps.append(mcf.create_component(c))
proxy.subscribe_status(comps[-1])
last_period = []
last_rollover = []
offset_period = []
offset_rollover = []
for i in range(len(comps)):
last_period.append(0)
last_rollover.append(0)
offset_period.append(0)
offset_rollover.append(0)
try:
ts = time.time()
terr_q = 0
terr_tq = 0
while True:
print 'Position (mm)',gripper.get_pos_mm()
time.sleep(0.2)
proxy = m3p.M3RtProxy()
proxy.start()
proxy.make_operational_all()
#Setup Components
chain_names=proxy.get_available_components('m3gripper')
gripper_name=m3t.user_select_components_interactive(chain_names,single=True)
gripper=m3h.M3Gripper(gripper_name[0])
proxy.publish_command(gripper)
proxy.subscribe_status(gripper)
if len(proxy.get_available_components('m3pwr')):
pwr_name=proxy.get_available_components('m3pwr')[0]
pwr=m3f.create_component(pwr_name)
proxy.publish_command(pwr)
pwr.set_motor_power_on()
while True:
proxy.step()
print '--------------'
print 'f: grip'
print 'o: open'
print 'd: demo'
print 'q: quit'
print '--------------'
print
k=m3t.get_keystroke()
if k=='q':
break
print 'No components found'
exit()
print 'Using components: '
for i in range(len(cnames)):
print i, ':', cnames[i]
print 'Continue [y]?'
if not m3t.get_yes_no('y'):
exit()
print 'Query to save each calibration [y]?'
do_query = m3t.get_yes_no('y')
for c in comp.keys():
comp[c]['comp_rt'] = mcf.create_component(c.replace('_ec', ''))
comp[c]['comp_j'] = mcf.create_component(c.replace('actuator_ec', 'joint'))
proxy.subscribe_status(comp[c]['comp_rt'])
proxy.subscribe_status(comp[c]['comp_j'])
#proxy.publish_param(comp[c]['comp_j'])
proxy.make_operational_all()
proxy.step()
# ###########################
ns = 30
for i in range(ns):
proxy.step()
print '---------'
for c in comp.keys():
tqj = comp[c]['comp_j'].get_torque_mNm()
if lt == 3 or lt == 4:
#POSSIBILITY OF SUCH DAMAGE.
import time
import m3.rt_proxy as m3p
import m3.toolbox as m3t
import m3.component_factory as m3f
import Numeric as nu
import m3.humanoid
# ######################################################
proxy = m3p.M3RtProxy()
proxy.start()
bot_name = m3t.get_robot_name()
if bot_name == "":
print 'Error: no robot components found:', bot_name
bot = m3f.create_component(bot_name)
proxy.publish_param(bot) #allow to set payload
proxy.subscribe_status(bot)
proxy.publish_command(bot)
proxy.make_operational_all()
bot.set_motor_power_on()
chains = bot.get_available_chains()
print 'Select chain'
chains = m3t.user_select_components_interactive(chains, single=True)
for c in chains:
ndof = bot.get_num_dof(c)
bot.set_mode_theta_gc(c)
bot.set_theta_deg(c, [0.0] * ndof)
bot.set_stiffness(c, [0.0] * ndof)
try:
while True:
def start(self):
self.proxy.start()
cnames=self.proxy.get_available_components('m3actuator_ec')
self.names=m3t.user_select_components_interactive(cnames)
if len(self.names)==0:
return
self.actuator_ec=[]
for name in self.names:
self.actuator_ec.append(m3f.create_component(name))
self.proxy.subscribe_status(self.actuator_ec[-1])
self.proxy.publish_command(self.actuator_ec[-1])
self.proxy.publish_param(self.actuator_ec[-1])
self.proxy.make_operational(name)
#pwr_ec=self.proxy.get_available_components('m3pwr_ec')
#pwr_rt=self.proxy.get_available_components('m3pwr')
#print 'A',pwr_rt[0],pwr_ec[0]
#if len(pwr_rt):
#pr=m3f.create_component(pwr_rt[0])
#self.proxy.publish_command(pr)
#self.proxy.make_operational(pwr_rt[0])
#self.proxy.make_operational(pwr_ec[0])
#pr.set_motor_power_on()
pwr_rt=m3t.get_actuator_ec_pwr_component_name(self.names[0])
pwr_ec=pwr_rt.replace('m3pwr','m3pwr_ec')
pr=m3f.create_component(pwr_rt)
self.proxy.publish_command(pr)
self.proxy.make_operational(pwr_rt)
self.proxy.make_operational(pwr_ec)
self.proxy.subscribe_status(pr)
pr.set_motor_power_on()
tmax=[x.param.t_max for x in self.actuator_ec]
tmin=[x.param.t_min for x in self.actuator_ec]
self.proxy.step()
for c in self.actuator_ec:
self.bias.append(c.status.adc_torque)
tl=min(tmin)-self.bias[0]
tu=max(tmax)-self.bias[0]
#Create gui
self.mode=[0]
self.pwm_desire=[0]
self.current_desire=[0]
self.save=False
self.save_last=False
self.do_scope=False
self.scope = None
self.scope_keys=m3t.get_msg_fields(self.actuator_ec[0].status)
self.scope_keys.sort()
self.scope_keys=['None']+self.scope_keys
self.scope_field1=[0]
self.scope_field2=[0]
self.status_dict=self.proxy.get_status_dict()
self.param_dict=self.proxy.get_param_dict()
self.gui.add('M3GuiTree', 'Status', (self,'status_dict'),[],[],m3g.M3GuiRead,column=2)
self.gui.add('M3GuiTree', 'Param', (self,'param_dict'),[],[],m3g.M3GuiWrite,column=3)
self.gui.add('M3GuiModes', 'Mode', (self,'mode'),range(len(self.actuator_ec)),[['off','pwm','torque','current','brake'],1],m3g.M3GuiWrite)
self.gui.add('M3GuiSliders','pwmDesire', (self,'pwm_desire'),range(len(self.actuator_ec)),[-1000,1000],m3g.M3GuiWrite)
self.gui.add('M3GuiSliders','currentDesire', (self,'current_desire'),range(len(self.actuator_ec)),[-10000,10000],m3g.M3GuiWrite)
self.gui.add('M3GuiToggle', 'Save', (self,'save'),[],[['On','Off']],m3g.M3GuiWrite)
self.gui.add('M3GuiToggle', 'Scope', (self,'do_scope'),[],[['On','Off']],m3g.M3GuiWrite)
self.gui.add('M3GuiModes', 'Scope1', (self,'scope_field1'),range(1),[self.scope_keys,1],m3g.M3GuiWrite)
self.gui.add('M3GuiModes', 'Scope2', (self,'scope_field2'),range(1),[self.scope_keys,1],m3g.M3GuiWrite)
self.gui.start(self.step)
def start(self):
self.proxy.start()
#print 'Enable RVIZ [n]?'
self.rviz = False
#if m3t.get_yes_no('n'):
# self.rviz = True
chain_names = self.proxy.get_chain_components()
self.chain = []
if len(chain_names) > 0:
print 'Select kinematic chain'
self.chain_names = m3t.user_select_components_interactive(
chain_names, single=True)
self.chain.append(m3f.create_component(self.chain_names[0]))
self.proxy.subscribe_status(self.chain[-1])
self.limb = m3t.get_chain_limb_name(self.chain_names[0])
joint_names = m3t.get_chain_joint_names(self.chain_names[0])
print 'Select joint'
joint_names = m3t.user_select_components_interactive(joint_names,
single=True)
self.joint = []
self.actuator = []
self.actuator_ec = []
acutator_names = []
for n in joint_names:
self.joint.append(m3f.create_component(n))
actuator_name = m3t.get_joint_actuator_component_name(n)
actuator_ec_name = m3t.get_actuator_ec_component_name(
actuator_name)
self.actuator.append(m3f.create_component(actuator_name))
self.actuator_ec.append(m3f.create_component(actuator_ec_name))
self.proxy.subscribe_status(self.joint[-1])
self.proxy.publish_param(self.joint[-1])
self.proxy.subscribe_status(self.actuator[-1])
self.proxy.publish_param(self.actuator[-1])
if self.actuator_ec[0] is not None:
self.proxy.subscribe_status(self.actuator_ec[-1])
self.proxy.publish_param(self.actuator_ec[-1])
#kine_names=self.proxy.get_available_components('m3dynamatics')
self.kine = []
#if len(kine_names)>0:
#print 'Select dynamatics controller'
#kine_names=m3t.user_select_components_interactive(kine_names)
#for n in kine_names:
#self.kine.append(m3f.create_component(n))
#self.proxy.subscribe_status(self.kine[-1])
bot_name = m3t.get_robot_name()
if bot_name == "":
print 'Error: no robot components found:', bot_names
return
self.bot = m3f.create_component(bot_name)
if self.rviz:
self.viz = m3v.M3Viz(self.proxy, self.bot)
#self.proxy.publish_param(self.bot) #allow to set payload
#self.proxy.subscribe_status(self.bot)
#self.proxy.publish_command(self.bot)
#self.proxy.make_operational_all()
self.bot.initialize(self.proxy)
#self.chain_names = self.bot.get_available_chains()
#Create gui
self.mode = [0]
self.posture = [0]
self.theta_desire_a = [0] * self.bot.get_num_dof(self.limb)
self.theta_desire_b = [0] * self.bot.get_num_dof(self.limb)
self.stiffness = [50.0] * self.bot.get_num_dof(self.limb)
self.thetadot = [10.0] * self.bot.get_num_dof(self.limb)
#print 'Selected: ',self.chain_names[0],self.limb,self.bot.get_num_dof(self.limb)
#self.slew=[0]
self.save = False
self.save_last = False
self.status_dict = self.proxy.get_status_dict()
self.param_dict = self.proxy.get_param_dict()
self.gui.add('M3GuiModes', 'Mode', (self, 'mode'), range(1), [[
'Off', 'Pwm', 'Torque', 'Theta', 'Torque_GC', 'Theta_GC',
'Theta_MJ', 'Theta_GC_MJ'
], 1], m3g.M3GuiWrite)
self.gui.add('M3GuiTree',
'Status', (self, 'status_dict'), [], [],
m3g.M3GuiRead,
column=1)
self.gui.add('M3GuiTree',
'Param', (self, 'param_dict'), [], [],
m3g.M3GuiWrite,
column=1)
self.gui.add('M3GuiSliders',
'ThetaA (Deg)', (self, 'theta_desire_a'),
range(len(self.theta_desire_a)), [-45, 140],
m3g.M3GuiWrite,
column=2)
self.gui.add('M3GuiModes',
'Posture', (self, 'posture'),
range(1), [['A', 'B', 'Cycle'], 1],
m3g.M3GuiWrite,
column=2)
self.gui.add('M3GuiSliders',
'ThetaB (Deg)', (self, 'theta_desire_b'),
range(len(self.theta_desire_b)), [-45, 140],
m3g.M3GuiWrite,
column=2)
self.gui.add('M3GuiSliders',
'Stiffness ', (self, 'stiffness'),
range(len(self.stiffness)), [0, 100],
m3g.M3GuiWrite,
column=3)
self.gui.add('M3GuiToggle',
'Save', (self, 'save'), [], [['On', 'Off']],
m3g.M3GuiWrite,
column=3)
self.gui.add('M3GuiSliders',
'ThetaDot ', (self, 'thetadot'),
range(len(self.thetadot)), [0, 100],
m3g.M3GuiWrite,
column=3)
#self.gui.add('M3GuiSliders','Stiffness ', (self,'stiffness'),range(len(self.sea_joint)),[0,100],m3g.M3GuiWrite,column=3)
self.gui.start(self.step)
def start(self):
# ######## Setup Proxy and Components #########################
self.proxy.start()
self.current_first = True
bot_name = m3t.get_robot_name()
if bot_name == "":
print 'Error: no robot components found:', bot_names
return
self.bot = m3.humanoid.M3Humanoid(bot_name)
arm_names = self.bot.get_available_chains()
arm_names = [x for x in arm_names if x.find('arm') != -1]
if len(arm_names) == 0:
print 'No arms found'
return
if len(arm_names) == 1:
self.arm_name = arm_names[0]
else:
self.arm_name = m3t.user_select_components_interactive(
arm_names, single=True)[0]
self.jnts = self.bot.get_joint_names(self.arm_name)
comp = {}
for c in self.jnts:
comp[c] = {
'comp_rt': None,
'comp_j': None,
'torque_act': [],
'torque_joint': [],
'torque_gravity': [],
'is_wrist': False
}
if (c.find('j5') >= 0 or c.find('j6') >= 0):
comp[c]['is_wrist'] = True
for c in self.jnts:
comp[c]['comp_j'] = mcf.create_component(c)
comp[c]['comp_rt'] = mcf.create_component(
c.replace('joint', 'actuator'))
self.proxy.subscribe_status(comp[c]['comp_rt'])
self.proxy.subscribe_status(comp[c]['comp_j'])
# ####### Setup Proxy #############
self.proxy.subscribe_status(self.bot)
self.proxy.publish_command(self.bot)
self.proxy.make_operational_all()
self.bot.set_motor_power_on()
self.ndof = self.bot.get_num_dof(self.arm_name)
humanoid_shm_names = self.proxy.get_available_components(
'm3humanoid_shm')
if len(humanoid_shm_names) > 0:
self.proxy.make_safe_operational(humanoid_shm_names[0])
self.bot.set_mode_off(self.arm_name)
print 'This script will calibrate the zero-torque of the A2 arm while posed using current torque_gravity estimate'
print '----------------------------------------------------------------------------------------------------------'
print 'With E-Stop down, pose the arm in desired position to calibrate torque zeroes around.'
print 'Press any key when ready posed.'
raw_input()
time.sleep(0.5)
self.proxy.step()
self.theta_curr = self.bot.get_theta_deg(self.arm_name)[:]
self.proxy.step()
print 'Posed position set.'
print 'Release E-stop and press any key for arm to hold pose.'
raw_input()
self.bot.set_mode_theta_gc(self.arm_name)
self.bot.set_theta_deg(self.arm_name, self.theta_curr)
self.bot.set_stiffness(self.arm_name, [1.0] * 7)
self.bot.set_slew_rate_proportion(self.arm_name, [1.0] * self.ndof)
self.proxy.step()
print 'Press any key to start torque calibration for all joints.'
raw_input()
self.proxy.step()
# ###########################
ns = 30
for i in range(ns):
self.proxy.step()
print '---------'
for c in comp.keys():
tqj = comp[c]['comp_j'].get_torque_mNm()
tqg = comp[c]['comp_j'].get_torque_gravity_mNm() / 1000.0
tqa = comp[c]['comp_rt'].get_torque_mNm()
comp[c]['torque_act'].append(tqa)
comp[c]['torque_joint'].append(tqj)
comp[c]['torque_gravity'].append(tqg)
if comp[c]['is_wrist']:
print c, ':joint', tqj, ':gravity', tqg, ':actuator', tqa
else:
print c, ':joint', tqj, ':gravity', tqg,
time.sleep(0.05)
do_query = True
# ###########################
for c in comp.keys():
print '--------', c, '---------'
tqg = float(na.array(comp[c]['torque_gravity'], na.Float32).mean())
tqj = float(na.array(comp[c]['torque_joint'], na.Float32).mean())
tqa = float(na.array(comp[c]['torque_act'], na.Float32).mean())
if not comp[c]['is_wrist']:
bias = tqa + tqg
torque = M3TorqueSensor(
comp[c]['comp_rt'].config['calib']['torque']['type'])
print 'Measured torque:', tqa, 'Torque gravity:', tqg
print 'Delta of', bias, 'mNm'
comp[c]['comp_rt'].config['calib']['torque']['cb_bias'] = comp[
c]['comp_rt'].config['calib']['torque']['cb_bias'] - bias
comp[c]['comp_rt'].config['calibration_date'] = time.asctime()
if do_query:
print 'Save calibration? [y]'
if m3t.get_yes_no('y'):
comp[c]['comp_rt'].write_config()
else:
comp[c]['comp_rt'].write_config()
else:
print 'Wrist joint...'
if c.find('j5') != -1: #do j5/j6 at once
cc = None
for x in comp.keys():
if x.find('j6') != -1:
cc = x
if cc is None:
print 'Did not find coupled joint to', c
tqg_c = float(
na.array(comp[cc]['torque_gravity'],
na.Float32).mean())
tqj_c = float(
na.array(comp[cc]['torque_joint'], na.Float32).mean())
tqa_c = float(
na.array(comp[cc]['torque_act'], na.Float32).mean())
x = comp[c]['comp_j'].config['transmission']['tqj_to_tqa'][
0] #Joint to actuator matrix
y = comp[c]['comp_j'].config['transmission']['tqj_to_tqa'][
1]
m = comp[cc]['comp_j'].config['transmission'][
'tqj_to_tqa'][0]
n = comp[cc]['comp_j'].config['transmission'][
'tqj_to_tqa'][1]
tqg_a5 = x * tqg + y * tqg_c
tqg_a6 = m * tqg_c + n * tqg
bias_5 = tqa + tqg_a5
bias_6 = tqa_c + tqg_a6
torque_5 = M3TorqueSensor(
comp[c]['comp_rt'].config['calib']['torque']['type'])
torque_6 = M3TorqueSensor(
comp[cc]['comp_rt'].config['calib']['torque']['type'])
print '------------'
print 'J5: Previous joint torque', tqj, 'with joint torque gravity', tqg
print 'J5: Previous actuator torque', tqa, 'with actuator torque gravity', tqg_a5
print 'J5: Actuator delta of', bias_5, 'mNm'
print '------------'
print 'J6: Previous joint torque', tqj_c, 'with joint torque gravity', tqg_c
print 'J6: Previous actuator torque', tqa_c, 'with actuator torque gravity', tqg_a6
print 'J6: Actuator delta of', bias_6, 'mNm'
print '------------'
comp[c]['comp_rt'].config['calib']['torque'][
'cb_bias'] = comp[c]['comp_rt'].config['calib'][
'torque']['cb_bias'] - bias_5
comp[c]['comp_rt'].config[
'calibration_date'] = time.asctime()
comp[cc]['comp_rt'].config['calib']['torque'][
'cb_bias'] = comp[cc]['comp_rt'].config['calib'][
'torque']['cb_bias'] - bias_6
comp[cc]['comp_rt'].config[
'calibration_date'] = time.asctime()
if do_query:
print 'Save calibration? [y]'
if m3t.get_yes_no('y'):
comp[c]['comp_rt'].write_config()
comp[cc]['comp_rt'].write_config()
else:
comp[c]['comp_rt'].write_config()
comp[cc]['comp_rt'].write_config()
self.bot.set_mode_off(self.arm_name)
self.proxy.stop()
print 'Use RVIZ? (y/n)'
print '--------------------------'
print
k=m3t.get_keystroke()
rviz = False
pub = None
if k == 'y':
rviz = True
bot_name=m3t.get_robot_name()
if bot_name == "":
print 'Error: no botanoid components found:', bot_names
proxy.stop()
sys.exit()
bot=m3f.create_component(bot_name)
viz = None
if rviz == True:
viz = m3v.M3Viz(proxy, bot)
proxy.publish_param(bot) #allow to set payload
proxy.subscribe_status(bot)
proxy.publish_command(bot)
proxy.make_operational_all()
humanoid_shm_names=proxy.get_available_components('m3humanoid_shm')
if len(humanoid_shm_names) > 0:
proxy.make_safe_operational(humanoid_shm_names[0])
def start(self):
print '--------------------------'
print 'm: Target M3 arm'
print 'v: Target RVIZ'
print 'b: Target Both M3 and RVIZ'
print 'q: quit'
print '--------------'
print
self.k = 'a'
while self.k != 'm' and self.k != 'v' and self.k != 'b' and self.k != 'q':
self.k = m3t.get_keystroke()
if self.k == 'q':
return
self.proxy = m3p.M3RtProxy()
if self.k == 'm' or self.k == 'b':
self.proxy.start()
bot_name = m3t.get_robot_name()
if bot_name == "":
print 'Error: no robot components found:', bot_names
return
self.bot = m3f.create_component(bot_name)
arm_names = ['right_arm', 'left_arm']
self.arm_name = m3t.user_select_components_interactive(arm_names,
single=True)[0]
if self.arm_name == 'right_arm':
self.center = [0.450, -0.25, -0.1745]
else:
self.center = [0.450, 0.25, -0.1745]
avail_chains = self.bot.get_available_chains()
for c in avail_chains:
if c == 'torso':
self.center[2] += 0.5079
if self.k == 'v' or self.k == 'b':
self.viz = m3v.M3Viz(self.proxy, self.bot)
self.viz_launched = True
self.viz.turn_sim_on()
if self.k == 'v':
self.theta_0[:] = self.bot.get_theta_sim_deg(self.arm_name)[:]
if self.k == 'm' or self.k == 'b':
self.proxy.subscribe_status(self.bot)
self.proxy.publish_command(self.bot)
self.proxy.make_operational_all()
self.proxy.step()
self.theta_0[:] = self.bot.get_theta_deg(self.arm_name)[:]
self.m3_launched = True
self.theta_soln_deg = [0.] * self.bot.get_num_dof(self.arm_name)
self.thetadot_0 = [0.] * self.bot.get_num_dof(self.arm_name)
self.bot.set_slew_rate_proportion(self.arm_name, [1.0] * 7)
while True:
print '--------------------------'
print 'g: generate vias'
print 'd: display vias'
print 'v: set avg velocity (Current ', self.vel_avg, ')'
print 's: set stiffness (Current', self.stiffness, ')'
if self.k == 'b' or self.k == 'm':
print 'e: execute vias'
if self.k == 'b' or self.k == 'v':
print 't: test vias in visualizer'
print 'q: quit'
print '--------------'
print
m = m3t.get_keystroke()
if m == 'q':
return
if m == 'v':
print 'Enter avg velocity (0-60 Deg/S) [', self.vel_avg, ']'
self.vel_avg = max(0, min(60, m3t.get_float(self.vel_avg)))
if m == 's':
print 'Enter stiffness (0-1.0) [', self.stiffness, ']'
self.stiffness = max(0, min(1.0,
m3t.get_float(self.stiffness)))
if m == 'g':
self.vias = []
print
print '(s)quare or (c)ircle?'
shape = None
while shape != 's' and shape != 'c':
shape = m3t.get_keystroke()
length_m = 0.0
if shape == 's':
print
print 'Length of square side in cm (10-25) [25]'
length_cm = nu.float(max(10, min(25, m3t.get_int(25))))
length_m = length_cm / 100.0
diameter_m = 0.0
if shape == 'c':
print
print 'Diameter of circle in cm (10-25) [25]'
diameter_cm = nu.float(max(10, min(25, m3t.get_int(25))))
diameter_m = diameter_cm / 100.0
print
print 'Enter shape resolution (1-20 vias/side) [20]'
resolution = max(1, min(20, m3t.get_int(20)))
if self.m3_launched:
self.proxy.step()
x = self.center[0]
if shape == 's':
y_left = self.center[1] + length_m / 2.0
y_right = self.center[1] - length_m / 2.0
z_top = self.center[2] + length_m / 2.0
z_bottom = self.center[2] - length_m / 2.0
dy = (y_left - y_right) / nu.float(resolution)
dz = (z_top - z_bottom) / nu.float(resolution)
if self.arm_name == 'right_arm':
# first add start point
self.ik_vias.append([
x, y_left, z_top, self.axis_demo[0],
self.axis_demo[1], self.axis_demo[2]
])
# add top line
for i in range(resolution):
self.ik_vias.append([
x, y_left - (i + 1) * dy, z_top,
self.axis_demo[0], self.axis_demo[1],
self.axis_demo[2]
])
# add right line
for i in range(resolution):
self.ik_vias.append([
x, y_right, z_top - (i + 1) * dz,
self.axis_demo[0], self.axis_demo[1],
self.axis_demo[2]
])
# add bottom line
for i in range(resolution):
self.ik_vias.append([
x, y_right + (i + 1) * dy, z_bottom,
self.axis_demo[0], self.axis_demo[1],
self.axis_demo[2]
])
# add left line
for i in range(resolution):
self.ik_vias.append([
x, y_left, z_bottom + (i + 1) * dz,
self.axis_demo[0], self.axis_demo[1],
self.axis_demo[2]
])
else:
# first add start point
self.ik_vias.append([
x, y_right, z_top, self.axis_demo[0],
self.axis_demo[1], self.axis_demo[2]
])
# add top line
for i in range(resolution):
self.ik_vias.append([
x, y_right + (i + 1) * dy, z_top,
self.axis_demo[0], self.axis_demo[1],
self.axis_demo[2]
])
# add right line
for i in range(resolution):
self.ik_vias.append([
x, y_left, z_top - (i + 1) * dz,
self.axis_demo[0], self.axis_demo[1],
self.axis_demo[2]
])
# add bottom line
for i in range(resolution):
self.ik_vias.append([
x, y_left - (i + 1) * dy, z_bottom,
self.axis_demo[0], self.axis_demo[1],
self.axis_demo[2]
])
# add left line
for i in range(resolution):
self.ik_vias.append([
x, y_right, z_bottom + (i + 1) * dz,
self.axis_demo[0], self.axis_demo[1],
self.axis_demo[2]
])
if shape == 'c':
for i in range(resolution * 4 + 1):
dt = 2 * nu.pi / (nu.float(resolution) * 4)
t = (nu.pi / 2) + i * dt
if t > nu.pi:
t -= 2 * nu.pi
y = self.center[1] + (diameter_m / 2.0) * nu.cos(t)
z = self.center[2] + (diameter_m / 2.0) * nu.sin(t)
self.ik_vias.append([
x, y, z, self.axis_demo[0], self.axis_demo[1],
self.axis_demo[2]
])
self.vias.append(self.theta_0[:])
# use zero position as reference for IK solver
ref = [0] * self.bot.get_num_dof(self.arm_name)
# use holdup position as reference
ref = [30, 0, 0, 40, 0, 0, 0]
self.bot.set_theta_sim_deg(self.arm_name, ref)
for ikv in self.ik_vias:
theta_soln = []
print 'solving for ik via:', ikv
if self.bot.get_tool_axis_2_theta_deg_sim(
self.arm_name, ikv[:3], ikv[3:], theta_soln):
self.vias.append(theta_soln)
self.bot.set_theta_sim_deg(self.arm_name, theta_soln)
else:
print 'WARNING: no IK solution found for via ', ikv
self.bot.set_theta_sim_deg(self.arm_name, ref)
if self.viz_launched:
self.viz.step()
self.vias.append(self.theta_0[:])
if m == 'd':
print
print '--------- IK Vias (', len(self.ik_vias), ')--------'
print '---------------[end_xyz[3], end_axis[3]]-----------'
for ikv in self.ik_vias:
print ikv
print
print '--------- Joint Vias (', len(self.vias), ')--------'
for v in self.vias:
print v
if m == 'e' or m == 't':
if len(self.vias) != 0:
for v in self.vias:
#print 'Adding via',v
self.jt.add_via_deg(v, [self.vel_avg] * self.ndof)
self.jt.start(self.theta_0[:], self.thetadot_0[:])
print
print '--------- Splines (', len(
self.jt.splines), ')--------'
print '------------q_0, q_f, qdot_0, qdot_f, tf--------------'
for s in self.jt.splines:
print s.q_0, s.q_f, s.qdot_0, s.qdot_f, s.tf
print
print 'Hit any key to start or (q) to quit execution'
p = m3t.get_keystroke()
if p != 'q':
if self.m3_launched and m == 'e':
self.bot.set_motor_power_on()
self.bot.set_mode_theta_gc(self.arm_name)
self.bot.set_stiffness(
self.arm_name, [self.stiffness] *
self.bot.get_num_dof(self.arm_name))
while not self.jt.is_splined_traj_complete():
q = self.jt.step()
if self.viz_launched and m == 't':
self.bot.set_theta_sim_deg(self.arm_name, q)
self.viz.step()
time.sleep(0.1)
elif self.m3_launched and m == 'e':
self.bot.set_theta_deg(self.arm_name, q)
self.proxy.step()
self.ik_vias = []
def start(self):
self.proxy.start()
self.get_component('m3actuator')
print "starting components"
self.start_components(['ctrl','act','act_ec'],None)
print "done starting components"
# for k,v in self.comps.items():
# # accomplishes this: self.act=m3s.M3Actuator(self.comp_name)
# setattr(self, k, v['type'](v['name']) )
# self.comps[k]['comp'] = getattr(self,k)
# self.proxy.subscribe_status(getattr(self,k))
# self.proxy.publish_command(getattr(self,k))
# self.proxy.publish_param(getattr(self,k))
# self.proxy.make_operational(v['name'])
#
pwr_rt=m3t.get_actuator_ec_pwr_component_name(self.comps['act_ec']['name'])
pwr_ec=pwr_rt.replace('m3pwr','m3pwr_ec')
pr=m3f.create_component(pwr_rt)
self.proxy.publish_command(pr)
self.proxy.make_operational(pwr_rt)
self.proxy.make_operational(pwr_ec)
pr.set_motor_power_on()
self.proxy.step()
#Create gui
self.mode = [0]
self.traj = [0]
self.current = [0]
self.theta = [0]
self.torque = [0]
self.torque_lc = [0]
self.save = False
self.save_last = False
self.do_scope = False
self.scope = None
# extract status fields
self.status_dict = self.proxy.get_status_dict()
self.ctrl_scope_keys = m3t.get_msg_fields(self.ctrl.status,prefix='',exclude=['ethercat','base'])
self.ctrl_scope_keys.sort()
self.ctrl_scope_keys = ['None']+self.ctrl_scope_keys
print self.ctrl_scope_keys
self.ctrl_scope_field1 = [0]
self.ctrl_scope_field2 = [0]
self.ec_scope_field1 = [0]
current_max = 3.5
theta_max = 100.0
torque_max = 40.0
self.param_dict=self.proxy.get_param_dict()
print str(self.param_dict)
self.modes = mec._CTRL_SIMPLE_MODE.values_by_number
self.mode_names = [self.modes[i].name for i in sorted(self.modes.keys())]
self.trajs = mec._CTRL_SIMPLE_TRAJ_MODE.values_by_number
self.traj_names = [self.trajs[i].name for i in sorted(self.trajs.keys())]
self.gui.add('M3GuiTree', 'Status', (self,'status_dict'),[],[],m3g.M3GuiRead,column=2)
self.gui.add('M3GuiTree', 'Param', (self,'param_dict'),[],[],m3g.M3GuiWrite,column=3)
self.gui.add('M3GuiModes', 'Mode', (self,'mode'),range(1),[self.mode_names,1],m3g.M3GuiWrite)
self.gui.add('M3GuiModes', 'Traj', (self,'traj'),range(1),[self.traj_names,1],m3g.M3GuiWrite)
# self.gui.add('M3GuiModes', 'Mode', (self,'mode'),range(1),[['Off','Current','Theta','Torque','Torque_LC'],1],m3g.M3GuiWrite)
# self.gui.add('M3GuiModes', 'Traj', (self,'traj'),range(1),[['Off','Current Square','Current Sine','Theta Square','Theta Sine','Torque Square','Torque Sine','TorqueLC Square','TorqueLC Sine'],1],m3g.M3GuiWrite)
self.gui.add('M3GuiSliders','Current', (self,'current'),range(1), [-current_max,current_max],m3g.M3GuiWrite)
self.gui.add('M3GuiSliders','Theta(deg)', (self,'theta'),range(1),[-theta_max,theta_max],m3g.M3GuiWrite)
self.gui.add('M3GuiSliders','Torque', (self,'torque'),range(1),[-torque_max,torque_max],m3g.M3GuiWrite)
self.gui.add('M3GuiSliders','TorqueLC', (self,'torque_lc'),range(1),[-torque_max,torque_max],m3g.M3GuiWrite)
self.gui.add('M3GuiToggle', 'Save', (self,'save'),[],[['On','Off']],m3g.M3GuiWrite)
self.gui.add('M3GuiModes', 'Ctrl Scope1', (self,'ctrl_scope_field1'),range(1),[self.ctrl_scope_keys,1],m3g.M3GuiWrite)
self.gui.add('M3GuiModes', 'Ctrl Scope2', (self,'ctrl_scope_field2'),range(1),[self.ctrl_scope_keys,1],m3g.M3GuiWrite)
self.gui.add('M3GuiToggle', 'Scope', (self,'do_scope'),[],[['On','Off']],m3g.M3GuiWrite)
self.gui.start(self.step)
def start(self):
self.proxy.start()
joint_names=self.proxy.get_joint_components()
if len(joint_names)==0:
print 'No joint components available'
self.proxy.stop()
exit()
joint_names=m3t.user_select_components_interactive(joint_names)
actuator_ec_names=[]
actuator_names=[]
ctrl_names=[]
for n in joint_names:
ctrl = m3t.get_joint_ctrl_component_name(n)
if ctrl != "":
ctrl_names.append(ctrl)
actuator = m3t.get_joint_actuator_component_name(n)
if actuator != "":
actuator_names.append(actuator)
actuator_ec = m3t.get_actuator_ec_component_name(actuator)
if actuator_ec != "":
actuator_ec_names.append(actuator_ec)
self.joint=[]
self.actuator_ec=[]
self.actuator=[]
self.ctrl=[]
for n in actuator_ec_names:
c=m3f.create_component(n)
if c is not None:
try:
self.actuator_ec.append(c)
self.proxy.subscribe_status(self.actuator_ec[-1])
self.proxy.publish_param(self.actuator_ec[-1])
except:
print 'Component',n,'not available'
for n in actuator_names:
c=m3f.create_component(n)
if c is not None:
self.actuator.append(c)
self.proxy.subscribe_status(self.actuator[-1])
self.proxy.publish_param(self.actuator[-1])
for n in ctrl_names:
c=m3f.create_component(n)
if c is not None:
self.ctrl.append(c)
self.proxy.subscribe_status(self.ctrl[-1])
self.proxy.publish_param(self.ctrl[-1])
for n in joint_names:
c=m3f.create_component(n)
if c is not None:
self.joint.append(c)
self.proxy.subscribe_status(self.joint[-1])
self.proxy.publish_command(self.joint[-1])
self.proxy.publish_param(self.joint[-1])
#Enable motor power
pwr_rt=m3t.get_actuator_ec_pwr_component_name(actuator_ec_names[0])
self.pwr=m3f.create_component(pwr_rt)
if self.pwr is not None:
self.proxy.publish_command(self.pwr)
self.pwr.set_motor_power_on()
#Start them all up
self.proxy.make_operational_all()
#Force safe-op of robot, etc are present
types=['m3humanoid','m3hand','m3gripper']
for t in types:
cc=self.proxy.get_available_components(t)
for ccc in cc:
self.proxy.make_safe_operational(ccc)
#Force safe-op of chain so that gravity terms are computed
self.chain=None
if len(joint_names)>0:
for j in joint_names:
chain_name=m3t.get_joint_chain_name(j)
if chain_name!="":
self.proxy.make_safe_operational(chain_name)
#self.chain=m3f.create_component(chain_name)
#self.proxy.publish_param(self.chain) #allow to set payload
#Force safe-op of chain so that gravity terms are computed
dynamatics_name = m3t.get_chain_dynamatics_component_name(chain_name)
if dynamatics_name != "":
self.proxy.make_safe_operational(dynamatics_name)
self.dyn=m3f.create_component(dynamatics_name)
self.proxy.publish_param(self.dyn) #allow to set payload
#Force safe-op of robot so that gravity terms are computed
robot_name = m3t.get_robot_name()
if robot_name != "":
try:
self.proxy.make_safe_operational(robot_name)
self.robot=m3f.create_component(robot_name)
self.proxy.subscribe_status(self.robot)
self.proxy.publish_param(self.robot) #allow to set payload
except:
print 'Component',robot_name,'not available'
tmax=max([x.param.max_tq for x in self.actuator])
tmin=min([x.param.min_tq for x in self.actuator])
qmax=max([x.param.max_q for x in self.joint])
qmin=min([x.param.min_q for x in self.joint])
## Plots
self.scope_torque=[]
self.scope_theta=[]
self.scope_thetadot=[]
self.scope_thetadotdot=[]
self.scope_torquedot=[]
for i,name in zip(xrange(len(joint_names)),joint_names):
self.scope_torque.append( m3t.M3ScopeN(xwidth=100,yrange=None,title='Torque') )
self.scope_theta.append( m3t.M3ScopeN(xwidth=100,yrange=None,title='Theta') )
self.scope_thetadot.append( m3t.M3ScopeN(xwidth=100,yrange=None,title='ThetaDot') )
self.scope_thetadotdot.append( m3t.M3ScopeN(xwidth=100,yrange=None,title='ThetaDotDot'))
self.scope_torquedot.append( m3t.M3ScopeN(xwidth=100,yrange=None,title='TorqueDot') )
#Create gui
self.mode=[0]*len(self.joint)
self.tq_desire_a=[0]*len(self.joint)
self.tq_desire_b=[0]*len(self.joint)
self.pwm_desire=[0]*len(self.joint)
self.theta_desire_a=[0]*len(self.joint)
self.theta_desire_b=[0]*len(self.joint)
self.thetadot_desire=[0]*len(self.joint)
self.stiffness=[0]*len(self.joint)
self.slew=[0]*len(self.joint)
self.step_period=[2000.0]*len(self.joint)
self.cycle_theta=False
self.cycle_last_theta=False
self.cycle_thetadot=False
self.cycle_last_thetadot=False
self.cycle_torque=False
self.cycle_last_torque=False
self.save=False
self.do_scope_torque=False
self.do_scope_torquedot=False
self.do_scope_theta=False
self.do_scope_thetadot=False
self.do_scope_thetadotdot=False
self.brake=False
self.save_last=False
self.status_dict=self.proxy.get_status_dict()
self.param_dict=self.proxy.get_param_dict()
self.gui.add('M3GuiTree', 'Status', (self,'status_dict'),[],[],m3g.M3GuiRead,column=2)
self.gui.add('M3GuiTree', 'Param', (self,'param_dict'),[],[],m3g.M3GuiWrite,column=3)
self.gui.add('M3GuiModes', 'Mode', (self,'mode'),range(len(self.joint)),[['Off','Pwm','Torque','Theta','Torque_GC','Theta_GC','Theta_MJ', 'Theta_GC_MJ','Pose','Torque_GRAV_MODEL','ThetaDot_GC','ThetaDot'],1],m3g.M3GuiWrite)
self.gui.add('M3GuiSliders','TorqueA (mNm)', (self,'tq_desire_a'),range(len(self.joint)),[tmin,tmax],m3g.M3GuiWrite)
self.gui.add('M3GuiSliders','TorqueB (mNm)', (self,'tq_desire_b'),range(len(self.joint)),[tmin,tmax],m3g.M3GuiWrite)
self.gui.add('M3GuiSliders','Pwm', (self,'pwm_desire'),range(len(self.joint)),[-3200,3200],m3g.M3GuiWrite)
self.gui.add('M3GuiSliders','Theta A(Deg)', (self,'theta_desire_a'),range(len(self.joint)),[qmin,qmax],m3g.M3GuiWrite,column=2)
self.gui.add('M3GuiSliders','Theta B(Deg)', (self,'theta_desire_b'),range(len(self.joint)),[qmin,qmax],m3g.M3GuiWrite,column=2)
self.gui.add('M3GuiSliders','Thetadot (Deg)', (self,'thetadot_desire'),range(len(self.joint)),[-120.0,120.0],m3g.M3GuiWrite,column=2)
self.gui.add('M3GuiSliders','Stiffness ', (self,'stiffness'),range(len(self.joint)),[0,100],m3g.M3GuiWrite,column=3)
self.gui.add('M3GuiSliders','Slew ', (self,'slew'),range(len(self.joint)),[0,100],m3g.M3GuiWrite,column=3)
self.gui.add('M3GuiToggle', 'Save', (self,'save'),[],[['On','Off']],m3g.M3GuiWrite)
self.gui.add('M3GuiToggle', 'ScopeTorque', (self,'do_scope_torque'),[],[['On','Off']],m3g.M3GuiWrite)
self.gui.add('M3GuiToggle', 'ScopeTorqueDot', (self,'do_scope_torquedot'),[],[['On','Off']],m3g.M3GuiWrite)
self.gui.add('M3GuiToggle', 'ScopeTheta', (self,'do_scope_theta'),[],[['On','Off']],m3g.M3GuiWrite)
self.gui.add('M3GuiToggle', 'ScopeThetaDot', (self,'do_scope_thetadot'),[],[['On','Off']],m3g.M3GuiWrite)
self.gui.add('M3GuiToggle', 'ScopeThetaDotDot', (self,'do_scope_thetadotdot'),[],[['On','Off']],m3g.M3GuiWrite)
self.gui.add('M3GuiSliders','StepPeriod (ms) ', (self,'step_period'),range(len(self.joint)),[0,8000],m3g.M3GuiWrite)
self.gui.add('M3GuiToggle', 'CycleTheta', (self,'cycle_theta'),[],[['On','Off']],m3g.M3GuiWrite)
self.gui.add('M3GuiToggle', 'CycleThetaDot', (self,'cycle_thetadot'),[],[['On','Off']],m3g.M3GuiWrite)
self.gui.add('M3GuiToggle', 'CycleTorque', (self,'cycle_torque'),[],[['On','Off']],m3g.M3GuiWrite)
self.gui.add('M3GuiToggle', 'Brake', (self,'brake'),[],[['On','Off']],m3g.M3GuiWrite)
self.gui.start(self.step)
