def __init__(self, verbose=True, ra=1.0, rm=0.1):
Thread.__init__(self)
self.detect = None
self.detect_time = None
self.verbose = verbose
self.slew_mag = m3t.M3Slew()
self.slew_angle = m3t.M3Slew()
self.rate_mag = rm
self.rate_ang = ra
def __init__(self):
self.val = 0.0
def __init__(self, name):
M3Component.__init__(self, name, type='m3ledx2xn_ec')
self.status = mec.M3LedX2XNEcStatus()
self.command = mec.M3LedX2XNEcCommand()
self.param = mec.M3LedX2XNEcParam()
self.read_config()
self.slew = {'branch_a': [], 'branch_b': []}
self.nled = {
'branch_a': self.config['param']['n_branch_a'],
'branch_b': self.config['param']['n_branch_b']
}
self.phase = {'branch_a': [], 'branch_b': []}
for i in range(self.nled['branch_a']):
self.command.branch_a.r.append(0)
self.command.branch_a.g.append(0)
self.command.branch_a.b.append(0)
self.slew['branch_a'].append(
[m3t.M3Slew(), m3t.M3Slew(),
m3t.M3Slew()])
self.phase['branch_a'].append(math.pi * i / (math.pi * 2))
for i in range(self.nled['branch_b']):
self.command.branch_b.r.append(0)
self.command.branch_b.g.append(0)
self.command.branch_b.b.append(0)
self.slew['branch_b'].append(
[m3t.M3Slew(), m3t.M3Slew(),
m3t.M3Slew()])
self.phase['branch_b'].append(math.pi * i / (math.pi * 2))
self.circ_slew = {'branch_a': [], 'branch_b': []}
for i in range(6):
for b in ['branch_a', 'branch_b']:
self.circ_slew[b].append(m3t.M3Slew())
self.command_branch = {
'branch_a': self.command.branch_a,
'branch_b': self.command.branch_b
}
self.mode = {
'branch_a': mode_ledx2xn_off,
'branch_b': mode_ledx2xn_off
}
self.rgb = {
'branch_a': [[0, 0, 0]] * self.nled['branch_a'],
'branch_b': [[0, 0, 0]] * self.nled['branch_b']
}
self.slew_rate = {'branch_a': 0, 'branch_b': 0}
self.circ_rate = {'branch_a': 0, 'branch_b': 0}
self.circ_slew_rate = {'branch_a': 0, 'branch_b': 0}
self.circ_rgb_1 = {'branch_a': None, 'branch_b': None}
self.circ_rgb_2 = {'branch_a': None, 'branch_b': None}
self.step = {'branch_a': 0, 'branch_b': 0}
self.disable_leds()
def __init__(self, name):
M3Component.__init__(self, name, type='m3ledx2_ec')
self.status = mec.M3LedX2EcStatus()
self.command = mec.M3LedX2EcCommand()
self.param = mec.M3LedX2EcParam()
self.read_config()
self.slew_aa_r = m3t.M3Slew()
self.slew_aa_g = m3t.M3Slew()
self.slew_aa_b = m3t.M3Slew()
self.slew_ab_r = m3t.M3Slew()
self.slew_ab_g = m3t.M3Slew()
self.slew_ab_b = m3t.M3Slew()
self.slew_ba_r = m3t.M3Slew()
self.slew_ba_g = m3t.M3Slew()
self.slew_ba_b = m3t.M3Slew()
self.slew_bb_r = m3t.M3Slew()
self.slew_bb_g = m3t.M3Slew()
self.slew_bb_b = m3t.M3Slew()
qdes = {}
fn = m3t.get_m3_animation_path() + m3t.get_robot_name() + '_poser_config.yml'
f = file(fn, 'r')
config = yaml.safe_load(f.read())
f.close()
stiffness = config['stiffness']
delta_thresh = config['delta_thresh']
slew_rate = config['slew_rate']
proxy.step()
dbg = ''
for c in chains:
ndof = bot.get_num_dof(c)
slew[c] = []
qdes[c] = []
for i in range(ndof):
slew[c].append(m3t.M3Slew())
qdes[c] = nu.array(bot.get_theta_deg(c), nu.Float)
bot.set_mode_theta_gc(c)
bot.set_theta_deg(c, qdes[c])
bot.set_stiffness(c, stiffness[c])
bot.set_slew_rate_proportion(c, [1.0] * bot.get_num_dof(c))
try:
while not menu.done:
proxy.step()
for c in chains:
q = bot.get_theta_deg(c)
ndof = bot.get_num_dof(c)
delta = q - qdes[c]
s = [0.0] * ndof
for i in range(ndof):
if abs(delta[i]) > delta_thresh[c][i]: #deg
if u > u_max or u < u_min:
return 0.0
if u >= aa:
return 1.0
if u <= bb:
return -1.0
return cc
def step_input(u):
for k in range(K):
y = g_op(u, b[k], a[k], g[k])
g[k] = y
slew = m3t.M3Slew()
def ramp_to_torque(des, scope):
act.set_mode_torque()
if slew.val == 0:
slew.val = act.get_torque_mNm()
rate = 500.0 #100mNm/step, ~30steps/s, 3Nm/s, ~7s full range
x = slew.step(des, rate)
ut = []
wt = []
gt = []
while True:
act.set_torque_mNm(x)
proxy.step()
time.sleep(0.03)