def get_children_components(self,comp_name,comp_type): if self.args.verbose: print "component name is " + str(comp_name) if self.args.verbose: print "component type is " + comp_type if self.args.verbose: print "component prefix is " + self.comps[comp_type]['name'] self.joint_suffix = comp_name.replace(self.comps[comp_type]['name'],"") # more generic than comp_name[-2:] if self.args.verbose: print "joint suffix is " + self.joint_suffix for k in ['act','act_ec','ctrl']: self.comps[k]['name'] = self.comps[k]['name'] + self.joint_suffix pwr_name = m3t.get_actuator_ec_pwr_component_name(self.comps['act_ec']['name']) self.comps['pwr']['name'] = pwr_name
def get_children_components(self, comp_name, comp_type):
if self.args.verbose: print "component name is " + str(comp_name)
if self.args.verbose: print "component type is " + comp_type
if self.args.verbose:
print "component prefix is " + self.comps[comp_type]['name']
self.joint_suffix = comp_name.replace(
self.comps[comp_type]['name'],
"") # more generic than comp_name[-2:]
if self.args.verbose: print "joint suffix is " + self.joint_suffix
for k in ['act', 'act_ec', 'ctrl']:
self.comps[k]['name'] = self.comps[k]['name'] + self.joint_suffix
pwr_name = m3t.get_actuator_ec_pwr_component_name(
self.comps['act_ec']['name'])
self.comps['pwr']['name'] = pwr_name
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)
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):
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 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()
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):
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)
