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 animator():
k=postures.keys()
print 'Run time [30.0]'
rt=m3t.get_float(30.0)
te=time.time()+rt
while time.time()<te:
bot.set_theta_deg('head',[0.0]*bot.get_num_dof('head'))
bliker()
random_posture()
ear_lighting()
proxy.step()
time.sleep(0.1)
def test_torque_banger(self, auto=False):
d = 30.0
if not auto:
print '------------- Test Conditions --------------'
print '1. Actuator on test-plate'
print '2. Rocker arm installed'
print '3. PID tuning complete for actuator_ec'
print '4. Rocker arm unloaded'
print 'Continue? [y]'
if not m3t.get_yes_no('y'):
return
print 'Enter run duration (s) [30.0]'
d = m3t.get_float(30.0)
print 'Enable power. Hit enter to continue'
raw_input()
self.step()
amplitude = self.test_default[self.jid]['test_banger_amp']
adc_zero = float(
self.get_sensor_list_avg(['adc_torque'],
1.0,
auto=True,
verbose=False)['adc_torque'])
print 'Zero of ', adc_zero
self.comp_ec.command.mode = aec.ACTUATOR_EC_MODE_TORQUE
self.comp_ec.command.t_desire = int(adc_zero)
self.step(1.0)
nc = 2
period = 5.0
start = time.time()
dt = 0
self.comp_ec.command.mode = aec.ACTUATOR_EC_MODE_TORQUE
while dt < d:
dt = time.time() - start
a = amplitude[self.jid] * math.sin(2 * math.pi * dt / period)
self.comp_ec.command.t_desire = int(adc_zero + a)
self.step(time_sleep=0.1)
print '----------'
print 'DT', dt
print 'Amplitude', a
print 'Current (mA)', self.comp_rt.get_current_mA()
self.comp_ec.command.mode = aec.ACTUATOR_EC_MODE_OFF
self.step()
def test_torque_banger(self,auto=False):
d=30.0
if not auto:
print '------------- Test Conditions --------------'
print '1. Actuator on test-plate'
print '2. Rocker arm installed'
print '3. PID tuning complete for actuator_ec'
print '4. Rocker arm unloaded'
print 'Continue? [y]'
if not m3t.get_yes_no('y'):
return
print 'Enter run duration (s) [30.0]'
d=m3t.get_float(30.0)
print 'Enable power. Hit enter to continue'
raw_input()
self.step()
amplitude=self.test_default[self.jid]['test_banger_amp']
adc_zero=float(self.get_sensor_list_avg(['adc_torque'],1.0,auto=True,verbose=False)['adc_torque'])
print 'Zero of ',adc_zero
self.comp_ec.command.mode=aec.ACTUATOR_EC_MODE_TORQUE
self.comp_ec.command.t_desire=int(adc_zero)
self.step(1.0)
nc=2
period=5.0
start=time.time()
dt=0
self.comp_ec.command.mode=aec.ACTUATOR_EC_MODE_TORQUE
while dt<d:
dt=time.time()-start
a=amplitude[self.jid]*math.sin(2*math.pi*dt/period)
self.comp_ec.command.t_desire=int(adc_zero+a)
self.step(time_sleep=0.1)
print '----------'
print 'DT',dt
print 'Amplitude',a
print 'Current (mA)',self.comp_rt.get_current_mA()
self.comp_ec.command.mode=aec.ACTUATOR_EC_MODE_OFF
self.step()
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])
#Calibrate ZLift
if zlift is not None:
if zlift.calibrate(proxy):
#Position ZLift
zlift.set_mode_theta_gc()
print 'Set resting Z-Lift position (0-700) [300]'
des = max(0, min(700, m3t.get_float(300)))
zlift.set_slew_rate_proportion(1.0)
zlift.set_stiffness(1.0)
zlift.set_pos_mm(des)
proxy.step()
print 'Setting position to', des, '. Hit return when done'
raw_input()
zlift.set_mode_off()
proxy.step()
#Calibrate Base
if omni is not None:
time.sleep(0.5)
proxy.step()
omni.calibrate(proxy)
time.sleep(0.5)
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 = []
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])
#Calibrate ZLift
if zlift is not None:
if zlift.calibrate(proxy):
#Position ZLift
zlift.set_mode_theta_gc()
print 'Set resting Z-Lift position (0-700) [300]'
des=max(0,min(700,m3t.get_float(300)))
zlift.set_slew_rate_proportion(1.0)
zlift.set_stiffness(1.0)
zlift.set_pos_mm(des)
proxy.step()
print 'Setting position to',des,'. Hit return when done'
raw_input()
zlift.set_mode_off()
proxy.step()
#Calibrate Base
if omni is not None:
time.sleep(0.5)
proxy.step()
omni.calibrate(proxy)
time.sleep(0.5)
print "-------- Mode ---------"
if use_theta_gc:
print "Currently in mode: THETA_GC"
else:
print "Currently in mode: THETA"
print "------ Scaling --------"
print "Stiffness: ", stiffness
print "Vel avg: ", scale_vel_avg
print "--------- Vias --------"
print vias
if k == "v":
print "Select chain"
c = m3t.user_select_components_interactive(chains, single=True)[0]
print "Current scale for", c, ": ", scale_vel_avg[c]
print "New scale: (0-2.0)"
scale_vel_avg[c] = max(0, min(2.0, m3t.get_float()))
if k == "s":
print "Current stiffness"
print "--------------------"
for c in chains:
print c, " : ", stiffness[c]
print
print "Select chain"
c = m3t.user_select_components_interactive(chains, single=True)[0]
print "Enter stiffness: "
s = max(0, min(1.0, m3t.get_float()))
stiffness[c] = [s] * len(stiffness[c])
print "New stiffness: ", c, " : ", stiffness[c]
if k == "e":
use_chain = {}
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=[]
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=False) for c in chains: ndof=bot.get_num_dof(c) bot.set_mode_torque(c) print 'Enter duration (s) [30.0]' d=m3t.get_float(30.0) ts=time.time() amp=nu.array([5000.0,5000.0,2500.0,2500.0,0.0,0.0,0.0]) period=nu.array([3,3.1,2.8,3.2,3.0,3.0,3.0]) nc=0 print 'Power up system. Hit enter to start' raw_input() try: while time.time()-ts<d: nc=nc+1 t=time.time()-ts if math.fmod(nc,10)==0: print '---------------------------------------------' print 'Time: ',t,'/',d proxy.step() for c in chains:
print 'Select arm:'
arm_names = ['right_arm', 'left_arm']
arm_name = m3t.user_select_components_interactive(arm_names, single=True)[0]
while True:
proxy.step()
print '--------------'
print 's: set stiffness (Current', stiffness, ')'
print 'd: set step delta (Current', step_delta, '(m))'
print 'e: execute ijkt controller'
print 'q: quit'
print '--------------'
print
k = m3t.get_keystroke()
if k == 'q':
break
if k == 's':
print 'Enter stiffness (0-1.0) [', stiffness, ']'
stiffness = max(0, min(1.0, m3t.get_float(stiffness)))
if k == 'd':
print 'Enter step delta (m) [', step_delta, ']'
step_delta = max(0, min(.25, m3t.get_float(step_delta)))
if k == 'e':
run_ik(proxy, bot, step_delta, arm_name, pub, viz)
proxy.stop()
if rviz == True:
viz.stop()
# ######################################################
# ###########################
fields=[['adc_ext_temp'],['adc_amp_temp'],['adc_current_a','adc_current_b']]
print 'Select sensor to zero'
for i in range(len(fields)):
print i,' : ',fields[i]
sidx=m3t.get_int()
sensor=fields[sidx]
print 'This assumes ISS version config files'
print 'All boards should be at room temp'
print 'All motor powers should be off'
print 'Continue [y]?'
if not m3t.get_yes_no('y'):
exit()
if sensor[0]=='adc_ext_temp' or sensor[0]=='adc_amp_temp':
print 'Enter room temp (C)'
ambient=m3t.get_float()
# ###########################
proxy = m3p.M3RtProxy()
proxy.start()
cnames=proxy.get_available_components()
comp_ec=[]
comp_rt=[]
cnames=[q for q in cnames if q.find('actuator_ec')!=-1]
for c in cnames:
comp_ec.append(mcf.create_component(c))
comp_rt.append(mcf.create_component(c.replace('_ec','')))
if len(comp_ec)==0:
print 'No actuator_ec components found'
exit()
print '-------- Mode ---------'
if use_theta_gc:
print 'Currently in mode: THETA_GC'
else:
print 'Currently in mode: THETA'
print '------ Scaling --------'
print 'Stiffness: ', stiffness
print 'Vel avg: ', scale_vel_avg
print '--------- Vias --------'
print vias
if k == 'v':
print 'Select chain'
c = m3t.user_select_components_interactive(chains, single=True)[0]
print 'Current scale for', c, ': ', scale_vel_avg[c]
print 'New scale: (0-2.0)'
scale_vel_avg[c] = max(0, min(2.0, m3t.get_float()))
if k == 's':
print 'Current stiffness'
print '--------------------'
for c in chains:
print c, ' : ', stiffness[c]
print
print 'Select chain'
c = m3t.user_select_components_interactive(chains, single=True)[0]
print 'Enter stiffness: '
s = max(0, min(1.0, m3t.get_float()))
stiffness[c] = [s] * len(stiffness[c])
print 'New stiffness: ', c, ' : ', stiffness[c]
if k == 'e':
use_chain = {}
def experiment_a(self):
print 'Prepare experiment. Hit enter when ready'
raw_input()
print 'Enable power. Hit enter when ready'
raw_input()
self.step() #send current commands out to dsp and read sensors
adc_zero=self.comp_ec.status.adc_torque
print 'Enter amplitude (ticks)[100]'
amp=m3t.get_float(100)
ns=100.0
#Sawtooth torque profile
fwd=nu.arange(0,1.0,1/ns)*amp
rev=nu.arange(0,-1.0,-1/ns)*amp
zero=nu.zeros(int(ns))
fwd=fwd.tolist()+zero.tolist()
rev=rev.tolist()+zero.tolist()
#Place actuator in torque mode
self.comp_ec.command.mode=aec.ACTUATOR_EC_MODE_TORQUE
log_adc_torque=[]
log_load_mNm=[]
self.comp_ec.command.t_desire=0 #Initial desired torque
#Send out intial configuration (zero torque) and wait 1 second
self.step(time_sleep=1.0)
for f in fwd:
self.comp_ec.command.t_desire=int(f+adc_zero) #send out command torque
self.step(time_sleep=0.05) #send new desired out and wait
lmNm=self.dpm3.get_load_mNm()#read load-cell
tq=self.comp_ec.status.adc_torque #read raw torque sensor (ticks)
print '------------------------'
print 'Fwd',f
print 'Adc SEA',tq
print 'mNm Load Cell',lmNm
log_adc_torque.append(tq) #log data
log_load_mNm.append(lmNm)
for r in rev:
self.comp_ec.command.t_desire=int(r+adc_zero) #send out command torque
self.step(time_sleep=0.05)
lmNm=self.dpm3.get_load_mNm()#-load_zero
tq=self.comp_ec.status.adc_torque
print '------------------------'
print 'Rev',r
print 'Adc SEA',tq
print 'mNm Load Cell',lmNm
log_adc_torque.append(tq)
log_load_mNm.append(lmNm)
self.comp_ec.command.mode=aec.ACTUATOR_EC_MODE_OFF #Turn off actuator
self.step()#send out command
#Least squares fit to get nominal calibration
poly,inv_poly=self.get_polyfit_to_data(x=log_adc_torque,y=log_load_mNm,n=1,draw=False)
#Compute calibrated view of raw torque data
s=m3tc.PolyEval(poly,log_adc_torque)
#Draw the comparison between loadcell and torque sensor
m3t.mplot2(range(len(log_adc_torque)),log_load_mNm,s,xlabel='Samples',ylabel='Torque (mNm)',
y1name='loadcell',y2name='actuator')
#Save raw data
self.write_raw_calibration({'log_adc_torque':log_adc_torque,\
'log_load_mNm':log_load_mNm})
proxy.subscribe_status(bot)
proxy.publish_command(bot)
proxy.make_operational_all()
bot.set_motor_power_on()
humanoid_shm_names=proxy.get_available_components('m3humanoid_shm')
if len(humanoid_shm_names) > 0:
proxy.make_safe_operational(humanoid_shm_names[0])
chains=bot.get_available_chains()
print 'Select chain'
chains=m3t.user_select_components_interactive(chains,single=True)
stiffness=0.5
print 'Enter stiffness (0-1.0) [',stiffness,']'
stiffness=max(0,min(1.0,m3t.get_float(stiffness)))
for c in chains:
ndof=bot.get_num_dof(c)
bot.set_mode_pose(c)
bot.set_stiffness(c,[stiffness]*ndof)
bot.set_slew_rate_proportion(c,[1.0]*ndof)
try:
while True:
proxy.step()
for c in chains:
print '---------------------------------------------'
print 'Chain: ',c
print 'Tool Position: (m)',bot.get_tool_position(c)
print 'Theta (Deg): ',bot.get_theta_deg(c)
print 'Tool Velocity (m/S)',bot.get_tool_velocity(c)
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=False)
for c in chains:
ndof = bot.get_num_dof(c)
bot.set_mode_torque(c)
print 'Enter duration (s) [30.0]'
d = m3t.get_float(30.0)
ts = time.time()
amp = nu.array([5000.0, 5000.0, 2500.0, 2500.0, 0.0, 0.0, 0.0])
period = nu.array([3, 3.1, 2.8, 3.2, 3.0, 3.0, 3.0])
nc = 0
print 'Power up system. Hit enter to start'
raw_input()
try:
while time.time() - ts < d:
nc = nc + 1
t = time.time() - ts
if math.fmod(nc, 10) == 0:
print '---------------------------------------------'
print 'Time: ', t, '/', d
proxy.step()
for c in chains:
# ###########################
fields = [["adc_ext_temp"], ["adc_amp_temp"], ["adc_current_a", "adc_current_b"]]
print "Select sensor to zero"
for i in range(len(fields)):
print i, " : ", fields[i]
sidx = m3t.get_int()
sensor = fields[sidx]
print "This assumes ISS version config files"
print "All boards should be at room temp"
print "All motor powers should be off"
print "Continue [y]?"
if not m3t.get_yes_no("y"):
exit()
if sensor[0] == "adc_ext_temp" or sensor[0] == "adc_amp_temp":
print "Enter room temp (C)"
ambient = m3t.get_float()
# ###########################
proxy = m3p.M3RtProxy()
proxy.start()
cnames = proxy.get_available_components()
comp_ec = []
comp_rt = []
cnames = [q for q in cnames if q.find("actuator_ec") != -1]
for c in cnames:
comp_ec.append(mcf.create_component(c))
comp_rt.append(mcf.create_component(c.replace("_ec", "")))
if len(comp_ec) == 0:
print "No actuator_ec components found"
exit()
while True:
proxy.step()
print '--------------'
print 's: set stiffness (Current',stiffness,')'
print 'd: set step delta (Current',step_delta,'(m))'
print 'e: execute ijkt controller'
print 'q: quit'
print '--------------'
print
k=m3t.get_keystroke()
if k=='q':
break
if k=='s':
print 'Enter stiffness (0-1.0) [',stiffness,']'
stiffness=max(0,min(1.0,m3t.get_float(stiffness)))
if k=='d':
print 'Enter step delta (m) [',step_delta,']'
step_delta=max(0,min(.25,m3t.get_float(step_delta)))
if k=='e':
run_ik(proxy,bot, step_delta,arm_name,pub,viz)
proxy.stop()
if rviz==True:
viz.stop()
# ######################################################
proxy.subscribe_status(bot)
proxy.publish_command(bot)
proxy.make_operational_all()
bot.set_motor_power_on()
humanoid_shm_names = proxy.get_available_components('m3humanoid_shm')
if len(humanoid_shm_names) > 0:
proxy.make_safe_operational(humanoid_shm_names[0])
chains = bot.get_available_chains()
print 'Select chain'
chains = m3t.user_select_components_interactive(chains, single=True)
stiffness = 0.5
print 'Enter stiffness (0-1.0) [', stiffness, ']'
stiffness = max(0, min(1.0, m3t.get_float(stiffness)))
for c in chains:
ndof = bot.get_num_dof(c)
bot.set_mode_pose(c)
bot.set_stiffness(c, [stiffness] * ndof)
bot.set_slew_rate_proportion(c, [1.0] * ndof)
try:
while True:
proxy.step()
for c in chains:
print '---------------------------------------------'
print 'Chain: ', c
print 'Tool Position: (m)', bot.get_tool_position(c)
print 'Theta (Deg): ', bot.get_theta_deg(c)
print 'Tool Velocity (m/S)', bot.get_tool_velocity(c)
