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 = 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]
# ####### 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)
self.theta_curr = [0.0] * self.ndof
zlift_shm_names = self.proxy.get_available_components(
'm3joint_zlift_shm')
if len(zlift_shm_names) > 0:
self.proxy.make_safe_operational(zlift_shm_names[0])
omnibase_shm_names = self.proxy.get_available_components(
'm3omnibase_shm')
if len(omnibase_shm_names) > 0:
self.proxy.make_safe_operational(omnibase_shm_names[0])
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])
print 'WARNING: Before raising E-Stop move a slider to initialize positions.'
#Create gui
self.mode = [0]
self.theta_desire_a = [0] * self.num_dof
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','Current','Torque','Torque_GC','Theta','Theta_IMP','Jnt_Theta','No Brake'],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.start(self.step)
def meka_fk_server():
rospy.init_node('meka_fk_node')
bot_name=m3t.get_robot_name()
global bot
bot = m3.humanoid.M3Humanoid(bot_name)
s = rospy.Service('meka_fk', MekaFK, meka_fk_srv)
print "Ready to service FK request."
rospy.spin()
def meka_ik_server():
rospy.init_node("meka_ik_node")
bot_name = m3t.get_robot_name()
global bot
bot = m3.humanoid.M3Humanoid(bot_name)
s = rospy.Service("meka_ik", MekaIK, meka_ik_srv)
print "Ready to service IK request."
rospy.spin()
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 run(self):
vias = {}
for c in self.chains:
vias[c] = []
while not self.done:
print '--------------'
print 'r: record via'
print 's: save via file'
print 'p: print current pose'
print 'q: quit'
print '--------------'
print
k = m3t.get_keystroke()
print 'Dbg', dbg
if k == 'r':
print '-------------'
for c in self.chains:
vias[c].append(m3t.float_list(self.bot.get_theta_deg(c)))
print 'Record of: ', vias[c][-1], 'for', c
if k == 'p':
print '----------------'
for c in self.chains:
print 'Chain:', c, ' : ', self.bot.get_theta_deg(c)
print '----------------'
if k == 's':
bot_name = m3t.get_robot_name()
fn = bot_name + '_' + m3t.time_string()
print 'Enter via file name [', fn, ']'
fn = m3t.get_string(fn)
fn = m3t.get_m3_animation_path() + fn + '.via'
print 'Writing file: ', fn
f = file(fn, 'w')
d = {}
for c in self.chains:
ndof = self.bot.get_num_dof(c)
if c == 'torso':
param = {
'slew': [1.0] * ndof,
'stiffness': [0.8] * ndof,
'velocity': [25.0, 15.0]
}
else:
param = {
'slew': [1.0] * ndof,
'stiffness': [0.4] * ndof,
'velocity': [25.0] * ndof
}
d[c] = {
'postures': vias[c],
'param': param
} #safe defaults
f.write(yaml.safe_dump(d))
vias = {}
for c in self.chains:
vias[c] = []
if k == 'q':
self.done = True
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 = m3h.M3Humanoid(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.x = [5]
self.y = [0]
self.z = [0]
self.theta_j2 = [0]
self.ts_rand = 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',
'x', (self, 'x'), [0], [-10, 10],
m3g.M3GuiWrite,
column=1)
self.gui.add('M3GuiSliders',
'y', (self, 'y'), [0], [-10, 10],
m3g.M3GuiWrite,
column=1)
self.gui.add('M3GuiSliders',
'z', (self, 'z'), [0], [-10, 10],
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 __init__ (self,stride_ms=100):
Thread.__init__(self)
self.stride_ms = stride_ms
self.proxy = m3p.M3RtProxy()
self.proxy.start(True,True)
bot_name = m3t.get_robot_name()
bot = m3.humanoid.M3Humanoid(bot_name)
self.proxy.subscribe_status(bot)
self.viz = m3v.M3Viz(self.proxy,bot)
self.done = False
def get_bot(proxy):
assert isinstance(proxy,m3p.M3RtProxy)
## Returns the bot object, None if none or more is found
bot = None
try:
bot_name = m3t.get_robot_name()
bot = m3h.M3Humanoid(bot_name)
proxy.subscribe_status(bot)
except Exception,e:
print 'Something went wrong when trying to get the bot : ',e
bot = None
def __init__(self, name):
self._action_name = name
self._as = actionlib.SimpleActionServer(self._action_name,
meka_trajectory.msg.TrajAction,
execute_cb=self.execute_cb)
self.proxy = m3p.M3RtProxy()
self.proxy.start()
bot_name = m3t.get_robot_name()
self.bot = m3h.M3Humanoid(bot_name)
self.chains = self.bot.get_available_chains()
self.proxy.subscribe_status(self.bot)
self.proxy.publish_command(self.bot)
def __init__(self, name):
self._action_name = name
self._as = actionlib.SimpleActionServer(
self._action_name, meka_trajectory.msg.TrajAction, execute_cb=self.execute_cb
)
self.proxy = m3p.M3RtProxy()
self.proxy.start()
bot_name = m3t.get_robot_name()
self.bot = m3h.M3Humanoid(bot_name)
self.chains = self.bot.get_available_chains()
self.proxy.subscribe_status(self.bot)
self.proxy.publish_command(self.bot)
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 = 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]
# ####### 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)
self.theta_curr = [0.0] * self.ndof
zlift_shm_names = self.proxy.get_available_components("m3joint_zlift_shm")
if len(zlift_shm_names) > 0:
self.proxy.make_safe_operational(zlift_shm_names[0])
omnibase_shm_names = self.proxy.get_available_components("m3omnibase_shm")
if len(omnibase_shm_names) > 0:
self.proxy.make_safe_operational(omnibase_shm_names[0])
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])
print "WARNING: Before raising E-Stop move a slider to initialize positions."
# Create gui
self.mode = [0]
self.theta_desire_a = [0] * self.num_dof
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','Current','Torque','Torque_GC','Theta','Theta_IMP','Jnt_Theta','No Brake'],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.start(self.step)
def run(self):
vias = {}
for c in self.chains:
vias[c] = []
while not self.done:
print "--------------"
print "r: record via"
print "s: save via file"
print "p: print current pose"
print "q: quit"
print "--------------"
print
k = m3t.get_keystroke()
print "Dbg", dbg
if k == "r":
print "-------------"
for c in self.chains:
vias[c].append(m3t.float_list(self.bot.get_theta_deg(c)))
print "Record of: ", vias[c][-1], "for", c
if k == "p":
print "----------------"
for c in self.chains:
print "Chain:", c, " : ", self.bot.get_theta_deg(c)
print "----------------"
if k == "s":
bot_name = m3t.get_robot_name()
fn = bot_name + "_" + m3t.time_string()
print "Enter via file name [", fn, "]"
fn = m3t.get_string(fn)
fn = m3t.get_m3_animation_path() + fn + ".via"
print "Writing file: ", fn
f = file(fn, "w")
d = {}
for c in self.chains:
ndof = self.bot.get_num_dof(c)
if c == "torso":
param = {"slew": [1.0] * ndof, "stiffness": [0.8] * ndof, "velocity": [25.0, 15.0]}
else:
param = {"slew": [1.0] * ndof, "stiffness": [0.4] * ndof, "velocity": [25.0] * ndof}
d[c] = {"postures": vias[c], "param": param} # safe defaults
f.write(yaml.safe_dump(d))
vias = {}
for c in self.chains:
vias[c] = []
if k == "q":
self.done = True
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=m3h.M3Humanoid(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.x=[5]
self.y=[0]
self.z=[0]
self.theta_j2=[0]
self.ts_rand=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','x', (self,'x'),[0],[-10,10],m3g.M3GuiWrite,column=1)
self.gui.add('M3GuiSliders','y', (self,'y'),[0],[-10,10],m3g.M3GuiWrite,column=1)
self.gui.add('M3GuiSliders','z', (self,'z'),[0],[-10,10],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()
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):
# ######## 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()
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):
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 = []
else:
param = {"slew": [1.0] * ndof, "stiffness": [0.4] * ndof, "velocity": [25.0] * ndof}
d[c] = {"postures": vias[c], "param": param} # safe defaults
f.write(yaml.safe_dump(d))
vias = {}
for c in self.chains:
vias[c] = []
if k == "q":
self.done = True
# ######################################################
proxy = m3p.M3RtProxy()
proxy.start()
bot_name = m3t.get_robot_name()
if bot_name == "":
print "Error: no robot components found:", bot_name
exit()
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()
proxy.step()
chains = bot.get_available_chains()
print "Select chains to pose"
chains = m3t.user_select_components_interactive(chains)
menu = menu_thread(bot, chains)
#CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
#LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
#ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
#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)
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]
# ####### Setup Hand #############
hand_names = self.proxy.get_available_components('m3hand')
hand_name = ''
if len(hand_names) > 1:
hand_name = m3t.user_select_components_interactive(chain_names,
single=True)
if len(hand_names) == 1:
hand_name = hand_names[0]
if len(hand_name):
self.hand = m3.hand.M3Hand(hand_name)
self.proxy.publish_command(self.hand)
self.proxy.subscribe_status(self.hand)
else:
self.hand = None
# ####### 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)
self.via_traj = {}
self.via_traj_first = True
self.theta_curr = [0.0] * self.ndof
# ######## Square/Circle stuff #########################
if self.arm_name == 'right_arm':
self.center = [0.450, -0.28, -0.1745]
else:
self.center = [0.450, 0.28, -0.1745]
avail_chains = self.bot.get_available_chains()
for c in avail_chains:
if c == 'torso':
self.center[2] += 0.5079
self.jt = m3jt.JointTrajectory(7)
self.axis_demo = [0, 0, 1]
# ##### Generate square vias ############################
ik_vias = []
length_m = 25 / 100.0
resolution = 20
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)
x = self.center[0]
if self.arm_name == 'right_arm':
# first add start point
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):
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):
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):
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):
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
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):
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):
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):
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):
ik_vias.append([
x, y_right, z_bottom + (i + 1) * dz, self.axis_demo[0],
self.axis_demo[1], self.axis_demo[2]
])
self.via_traj['Square'] = []
# use zero position as reference for IK solver
self.bot.set_theta_sim_deg(self.arm_name,
[0] * self.bot.get_num_dof(self.arm_name))
for ikv in ik_vias:
theta_soln = []
#print 'solving for square ik via:', ikv
if self.bot.get_tool_axis_2_theta_deg_sim(self.arm_name, ikv[:3],
ikv[3:], theta_soln):
self.via_traj['Square'].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,
[0] * self.bot.get_num_dof(self.arm_name))
# ##### Generate circle vias ############################
ik_vias = []
diameter_m = 25.0 / 100.0
resolution = 20
x = self.center[0]
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)
ik_vias.append([
x, y, z, self.axis_demo[0], self.axis_demo[1],
self.axis_demo[2]
])
self.via_traj['Circle'] = []
# use zero position as reference for IK solver
self.bot.set_theta_sim_deg(self.arm_name,
[0] * self.bot.get_num_dof(self.arm_name))
for ikv in ik_vias:
theta_soln = []
if self.bot.get_tool_axis_2_theta_deg_sim(self.arm_name, ikv[:3],
ikv[3:], theta_soln):
self.via_traj['Circle'].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,
[0] * self.bot.get_num_dof(self.arm_name))
# ##### Load Via Trajectories ############################
self.via_files = {'TrajA': 'kha1.via'}
pt = m3t.get_m3_animation_path()
for k in self.via_files.keys():
fn = pt + self.via_files[k]
try:
f = file(fn, 'r')
d = yaml.safe_load(f.read())
self.via_traj[k] = d[self.arm_name]
except IOError:
print 'Via file', k, 'not present. Skipping...'
# ##### Hand Trajectories ############################
if self.hand is not None:
pf = m3t.get_m3_animation_path() + hand_name + '_postures.yml'
f = file(pf, 'r')
self.hand_data = yaml.safe_load(f.read())
f.close()
self.hand_traj_first = True
# ######## Demo and GUI #########################
self.off = False
self.grasp = False
self.arm_mode_names = [
'Off', 'Zero', 'Current', 'HoldUp', 'Square', 'Circle', 'TrajA'
]
self.hand_mode_names = ['Off', 'Open', 'Grasp', 'Animation']
self.arm_mode_methods = [
self.step_off, self.step_zero, self.step_current,
self.step_hold_up, self.step_via_traj, self.step_via_traj,
self.step_via_traj
]
self.hand_mode_methods = [
self.step_hand_off, self.step_hand_open, self.step_hand_grasp,
self.step_hand_animation
]
self.arm_mode = [0]
self.hand_mode = [0]
self.poses = {
'zero': {
'right_arm': [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
'left_arm': [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
},
'holdup': {
'right_arm': [56.0, 26.0, -8.0, 84.0, 119.0, -36.0, 2.0],
'left_arm': [56.0, -26.0, 8.0, 84.0, -119.0, -36.0, -2.0]
}
}
self.stiffness = [50]
self.velocity = [25]
self.gui.add('M3GuiModes',
'Arm Mode', (self, 'arm_mode'),
range(1), [self.arm_mode_names, 1],
m3g.M3GuiWrite,
column=1)
self.gui.add('M3GuiToggle',
'ESTOP', (self, 'off'), [],
[['Arm Enabled', 'Arm Disabled']],
m3g.M3GuiWrite,
column=1)
if self.hand is not None:
self.gui.add('M3GuiToggle',
'Grasp', (self, 'grasp'), [],
[['GraspOpen', 'GraspClosed']],
m3g.M3GuiWrite,
column=1)
self.gui.add('M3GuiModes',
'Hand Mode', (self, 'hand_mode'),
range(1), [self.hand_mode_names, 1],
m3g.M3GuiWrite,
column=1)
self.gui.add('M3GuiSliders',
'Stiffness ', (self, 'stiffness'), [0], [0, 100],
m3g.M3GuiWrite,
column=1)
self.gui.add('M3GuiSliders',
'Velocity ', (self, 'velocity'), [0], [0, 40],
m3g.M3GuiWrite,
column=1)
self.gui.start(self.step)
def start(self):
self.proxy.start()
self.omni = m3o.MekaOmnibaseControl('meka_omnibase_control_mb2', 'meka_omnibase_control')
self.proxy.subscribe_status(self.omni)
self.proxy.publish_param(self.omni)
self.proxy.publish_command(self.omni)
joint_names=self.proxy.get_joint_components()
if len(joint_names)==0:
print 'No joint components available'
self.proxy.stop()
exit()
joint_names= ['m3joint_mb2_j0',
'm3joint_mb2_j1',
'm3joint_mb2_j2',
'm3joint_mb2_j3',
'm3joint_mb2_j4',
'm3joint_mb2_j5',
'm3joint_mb2_j6',
'm3joint_mb2_j7']
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_names = self.proxy.get_available_components('m3pwr')
self.pwr = [0]*len(pwr_names)
for i in range(0,len(pwr_names)):
print i, pwr_names[i]
pwr_rt = pwr_names[i]
self.pwr[i]=m3f.create_component(pwr_rt)
if self.pwr[i] is not None:
self.proxy.publish_command(self.pwr[i])
self.pwr[i].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'
## 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
# Velocity limits (in rad/s)
vmin = -6.0
vmax = 6.0
try:
self.mode = [0]
self.betad_ = [0.0]*4
self.phid_ = [0.0]*4
self.xd_ = [0.0]
self.yd_ = [0.0]
self.td_ = [0.0]
self.tqr_ = [0.0]*4
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.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'),[0],[['Off','CC', 'Normal'],1],m3g.M3GuiWrite)
self.gui.add('M3GuiSliders','Beta dot',
(self,'betad_'),range(0,4),[vmin,vmax]*4,m3g.M3GuiWrite)
self.gui.add('M3GuiSliders','Phi dot',
(self,'phid_'),range(0,4),[vmin,vmax]*4,m3g.M3GuiWrite)
self.gui.add('M3GuiSliders','Caster Ratio',
(self,'tqr_'),range(0,4),[0.0,1.0]*4,m3g.M3GuiWrite)
self.gui.add('M3GuiSliders','xdot',
(self,'xd_'),[0],[-0.3,0.3],m3g.M3GuiWrite)
self.gui.add('M3GuiSliders','ydot',
(self,'yd_'),[0],[-0.3,0.3],m3g.M3GuiWrite)
self.gui.add('M3GuiSliders','tdot',
(self,'td_'),[0],[-1.0,1.0],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.start(self.step)
except Exception,e:
print "ERROR: Could not properly create GUI:", e
return
#!/usr/bin/python ''' Created on Apr 28, 2014 @author: Antoine Hoarau ([email protected]) ''' import time import m3.rt_proxy as m3p import m3.humanoid as m3h import m3.toolbox as m3t if __name__ == '__main__': proxy = m3p.M3RtProxy() proxy.start() bot = m3h.M3Humanoid(m3t.get_robot_name()) bot.initialize(proxy) cnt = 0 start = time.time() while 1: try: cnt = cnt + 1 for chain in bot.get_available_chains(): bot.set_mode_theta_gc(chain) bot.set_slew_rate_proportion(chain, [1.0] * bot.get_num_dof(chain)) bot.set_stiffness(chain, [1.0] * bot.get_num_dof(chain)) bot.set_theta_deg(chain, [0.0] * bot.get_num_dof(chain)) if (time.time() - start >= 1): for chain in bot.get_available_chains(): print chain, bot.get_theta_deg(chain) print 'freq : ', cnt cnt = 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()
print '--------------------------'
print 'Enable RVIZ? (y/n)'
print '--------------------------'
print
self.rviz = False
if m3t.get_yes_no():
self.rviz = True
sea_joint_names=self.proxy.get_joint_components()
sea_joint_names=m3t.user_select_components_interactive(sea_joint_names)
self.sea_joint=[]
for n in sea_joint_names:
self.sea_joint.append(m3f.create_component(n))
self.proxy.subscribe_status(self.sea_joint[-1])
chain_names=self.proxy.get_chain_components()
self.chain=[]
if len(chain_names)>0:
print 'Select kinematic chain'
chain_names=m3t.user_select_components_interactive(chain_names)
for n in chain_names:
self.chain.append(m3f.create_component(n))
self.proxy.subscribe_status(self.chain[-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.theta_desire_right_arm=[0]*self.bot.get_num_dof('right_arm')
self.theta_desire_left_arm=[0]*self.bot.get_num_dof('left_arm')
self.theta_desire_torso=[0]*self.bot.get_num_dof('torso')
self.theta_desire_head=[0]*self.bot.get_num_dof('head')
self.stiffness_right_arm=[0]*self.bot.get_num_dof('right_arm')
self.stiffness_left_arm=[0]*self.bot.get_num_dof('left_arm')
self.stiffness_torso=[0]*self.bot.get_num_dof('torso')
self.stiffness_head=[0]*self.bot.get_num_dof('head')
self.stiffness=[0]
#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('M3GuiTree', 'Status', (self,'status_dict'),[],[],m3g.M3GuiRead,column=3)
self.gui.add('M3GuiTree', 'Param', (self,'param_dict'),[],[],m3g.M3GuiWrite,column=3)
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('M3GuiSliders','Torque (mNm)', (self,'tq_desire'),range(len(self.bot.right_arm_ndof)),[-8000,8000],m3g.M3GuiWrite)
#self.gui.add('M3GuiSliders','Pwm', (self,'pwm_desire'),range(len(self.sea_joint)),[-3200,3200],m3g.M3GuiWrite)
#self.gui.add('M3GuiSliders','Stiffness ', (self,'stiffness'),range(1),[0,100],m3g.M3GuiWrite,column=1)
#self.gui.add('M3GuiSliders','Slew ', (self,'slew'),range(0),[0,100],m3g.M3GuiWrite,column=3)
self.gui.add('M3GuiSliders','Theta RA (Deg)', (self,'theta_desire_right_arm'),range(len(self.theta_desire_right_arm)),[-45,140],m3g.M3GuiWrite,column=1)
self.gui.add('M3GuiSliders','Theta LA (Deg)', (self,'theta_desire_left_arm'),range(len(self.theta_desire_left_arm)),[-45,140],m3g.M3GuiWrite,column=1)
self.gui.add('M3GuiSliders','Theta T (Deg)', (self,'theta_desire_torso'),range(len(self.theta_desire_torso)),[-45,140],m3g.M3GuiWrite,column=1)
self.gui.add('M3GuiSliders','Theta H (Deg)', (self,'theta_desire_head'),range(len(self.theta_desire_head)),[-45,140],m3g.M3GuiWrite,column=1)
self.gui.add('M3GuiSliders','Stiffness RA ', (self,'stiffness_right_arm'),range(len(self.stiffness_right_arm)),[0,100],m3g.M3GuiWrite,column=2)
self.gui.add('M3GuiSliders','Stiffness LA )', (self,'stiffness_left_arm'),range(len(self.stiffness_left_arm)),[0,100],m3g.M3GuiWrite,column=2)
self.gui.add('M3GuiSliders','Stiffness T ', (self,'stiffness_torso'),range(len(self.stiffness_torso)),[0,100],m3g.M3GuiWrite,column=2)
#self.gui.add('M3GuiSliders','Stiffness ', (self,'stiffness'),range(len(self.sea_joint)),[0,100],m3g.M3GuiWrite,column=3)
#self.gui.add('M3GuiToggle', 'Save', (self,'save'),[],[['On','Off']],m3g.M3GuiWrite)
self.gui.start(self.step)
#!/usr/bin/python ''' Created on Apr 28, 2014 @author: Antoine Hoarau ([email protected]) ''' import time import m3.rt_proxy as m3p import m3.humanoid as m3h import m3.toolbox as m3t if __name__ == '__main__': proxy = m3p.M3RtProxy() proxy.start() bot = m3h.M3Humanoid(m3t.get_robot_name()) bot.initialize(proxy) cnt=0 start = time.time() while 1: try: cnt=cnt+1 for chain in bot.get_available_chains(): bot.set_mode_theta_gc(chain) bot.set_slew_rate_proportion(chain,[1.0]*bot.get_num_dof(chain)) bot.set_stiffness(chain, [1.0]*bot.get_num_dof(chain)) bot.set_theta_deg(chain, [0.0]*bot.get_num_dof(chain)) if(time.time()-start >= 1): for chain in bot.get_available_chains(): print chain,bot.get_theta_deg(chain) print 'freq : ',cnt cnt=0 start = time.time()
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()
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]
# ####### Setup Hand #############
hand_names = self.bot.get_available_chains()
hand_names = [x for x in hand_names if x.find('hand')!=-1]
if len(hand_names)==0:
print 'No hands found'
return
if len(arm_names)==1:
self.hand_name=hand_names[0]
else:
self.hand_name = m3t.user_select_components_interactive(hand_names,single=True)[0]
# ####### 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.via_traj={}
self.via_traj_first=True
self.theta_curr = [0.0]*self.ndof
# ######## Square/Circle stuff #########################
if self.arm_name == 'right_arm':
self.center = [0.450, -0.28, -0.1745]
else:
self.center = [0.450, 0.28, -0.1745]
avail_chains = self.bot.get_available_chains()
for c in avail_chains:
if c == 'torso':
self.center[2] += 0.5079
self.jt = m3jt.JointTrajectory(7)
self.axis_demo = [0, 0, 1]
# ##### Generate square vias ############################
'''ik_vias=[]
length_m = 25/ 100.0
resolution = 20
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)
x = self.center[0]
if self.arm_name=='right_arm':
# first add start point
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):
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):
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):
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):
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
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):
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):
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):
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):
ik_vias.append([x, y_right, z_bottom + (i+1)*dz, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
self.via_traj['Square']=[]
# use zero position as reference for IK solver
self.bot.set_theta_sim_deg(self.arm_name,[0]*self.bot.get_num_dof(self.arm_name))
for ikv in ik_vias:
theta_soln = []
#print 'solving for square ik via:', ikv
if self.bot.get_tool_axis_2_theta_deg_sim(self.arm_name, ikv[:3], ikv[3:], theta_soln):
self.via_traj['Square'].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,[0]*self.bot.get_num_dof(self.arm_name))
'''
# ##### Generate circle vias ############################
ik_vias=[]
diameter_m = 25.0 / 100.0
resolution = 40
x = self.center[0]
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)
ik_vias.append([x, y, z, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
self.via_traj['Circle']=[]
# use zero position as reference for IK solver
self.bot.set_theta_sim_deg(self.arm_name,[0]*self.bot.get_num_dof(self.arm_name))
for ikv in ik_vias:
theta_soln = []
if self.bot.get_tool_axis_2_theta_deg_sim(self.arm_name, ikv[:3], ikv[3:], theta_soln):
self.via_traj['Circle'].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,[0]*self.bot.get_num_dof(self.arm_name))
# ##### Load Via Trajectories ############################
self.via_files={'TrajA':'kha1.via'}
for k in self.via_files.keys():
fn=m3t.get_animation_file(self.via_files[k])
print "Loading:",fn
try:
with open(fn,'r') as f:
d=yaml.safe_load(f.read())
self.via_traj[k]=d[self.arm_name]
except IOError:
print 'Via file',k,'not present. Skipping...'
# ##### Hand Trajectories ############################
if self.hand_name is not None:
try:
with open(m3t.get_animation_file(self.hand_name+'_postures.yml'),'r') as f:
self.hand_data= yaml.safe_load(f.read())
except Exception,e:
print e
def start(self):
# ######## Setup Proxy and Components #########################
self.proxy.start()
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]
# ####### Setup Hand #############
hand_names = self.proxy.get_available_components("m3hand")
hand_name = ""
if len(hand_names) > 1:
hand_name = m3t.user_select_components_interactive(chain_names, single=True)
if len(hand_names) == 1:
hand_name = hand_names[0]
if len(hand_name):
self.hand = m3.hand.M3Hand(hand_name)
self.proxy.publish_command(self.hand)
self.proxy.subscribe_status(self.hand)
else:
self.hand = None
# ####### 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)
self.via_traj = {}
self.via_traj_first = True
# ######## Demo and GUI #########################
self.off = False
self.grasp = False
self.hammer_up = False
self.switch_start = True
self.task_mode_names = ["Off", "Zero", "HammerGrasp", "Hammer"]
self.arm_mode_methods = [self.step_off, self.step_zero, self.step_hammer_grasp, self.step_hammer]
self.hand_mode_methods = [self.step_hand_off, self.step_hand_off, self.step_hand_grasp, self.step_hand_grasp]
self.task_mode = [0]
self.poses = {
"zero": {"right_arm": [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]},
"hammer_grasp": {"right_arm": [33, 0, 0, 120.18858337402344, 0, -60, 0]},
"hammer_up": {"right_arm": [33, 0, 0, 120.18858337402344, 0, -60, 0]},
"hammer_down": {"right_arm": [33, 0, 0, 71.249755859375, 0, -60, 0]},
}
self.stiffness = [100]
self.velocity = [25]
self.cycle_time = [3000]
self.gui.add(
"M3GuiModes", "TaskMode", (self, "task_mode"), range(1), [self.task_mode_names, 1], m3g.M3GuiWrite, column=1
)
if self.hand is not None:
self.gui.add(
"M3GuiToggle", "Grasp", (self, "grasp"), [], [["GraspOpen", "GraspClosed"]], m3g.M3GuiWrite, column=1
)
self.gui.add("M3GuiSliders", "Stiffness ", (self, "stiffness"), [0], [0, 100], m3g.M3GuiWrite, column=1)
self.gui.add("M3GuiSliders", "Velocity ", (self, "velocity"), [0], [0, 40], m3g.M3GuiWrite, column=1)
self.gui.add("M3GuiSliders", "CycleTimeMs", (self, "cycle_time"), [0], [0, 4000], m3g.M3GuiWrite, column=1)
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]
# ####### Setup Hand #############
hand_names=self.proxy.get_available_components('m3hand')
hand_name=''
if len(hand_names)>1:
hand_name=m3t.user_select_components_interactive(chain_names,single=True)
if len(hand_names)==1:
hand_name=hand_names[0]
if len(hand_name):
self.hand=m3.hand.M3Hand(hand_name)
self.proxy.publish_command(self.hand)
self.proxy.subscribe_status(self.hand)
else:
self.hand=None
# ####### 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)
self.via_traj={}
self.via_traj_first=True
self.theta_curr = [0.0]*self.ndof
# ######## Square/Circle stuff #########################
if self.arm_name == 'right_arm':
self.center = [0.450, -0.28, -0.1745]
else:
self.center = [0.450, 0.28, -0.1745]
avail_chains = self.bot.get_available_chains()
for c in avail_chains:
if c == 'torso':
self.center[2] += 0.5079
self.jt = m3jt.JointTrajectory(7)
self.axis_demo = [0, 0, 1]
# ##### Generate square vias ############################
ik_vias=[]
length_m = 25/ 100.0
resolution = 20
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)
x = self.center[0]
if self.arm_name=='right_arm':
# first add start point
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):
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):
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):
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):
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
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):
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):
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):
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):
ik_vias.append([x, y_right, z_bottom + (i+1)*dz, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
self.via_traj['Square']=[]
# use zero position as reference for IK solver
self.bot.set_theta_sim_deg(self.arm_name,[0]*self.bot.get_num_dof(self.arm_name))
for ikv in ik_vias:
theta_soln = []
#print 'solving for square ik via:', ikv
if self.bot.get_tool_axis_2_theta_deg_sim(self.arm_name, ikv[:3], ikv[3:], theta_soln):
self.via_traj['Square'].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,[0]*self.bot.get_num_dof(self.arm_name))
# ##### Generate circle vias ############################
ik_vias=[]
diameter_m = 25.0 / 100.0
resolution = 20
x = self.center[0]
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)
ik_vias.append([x, y, z, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
self.via_traj['Circle']=[]
# use zero position as reference for IK solver
self.bot.set_theta_sim_deg(self.arm_name,[0]*self.bot.get_num_dof(self.arm_name))
for ikv in ik_vias:
theta_soln = []
if self.bot.get_tool_axis_2_theta_deg_sim(self.arm_name, ikv[:3], ikv[3:], theta_soln):
self.via_traj['Circle'].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,[0]*self.bot.get_num_dof(self.arm_name))
# ##### Load Via Trajectories ############################
self.via_files={'TrajA':'kha1.via'}
pt=m3t.get_m3_animation_path()
for k in self.via_files.keys():
fn=pt+self.via_files[k]
try:
f=file(fn,'r')
d=yaml.safe_load(f.read())
self.via_traj[k]=d[self.arm_name]
except IOError:
print 'Via file',k,'not present. Skipping...'
# ##### Hand Trajectories ############################
if self.hand is not None:
pf=m3t.get_m3_animation_path()+hand_name+'_postures.yml'
f=file(pf,'r')
self.hand_data= yaml.safe_load(f.read())
f.close()
self.hand_traj_first=True
# ######## Demo and GUI #########################
self.off=False
self.grasp=False
self.arm_mode_names=['Off','Zero','Current','HoldUp','Square','Circle','TrajA']
self.hand_mode_names=['Off','Open','Grasp','Animation']
self.arm_mode_methods=[self.step_off,self.step_zero,self.step_current,self.step_hold_up,self.step_via_traj,
self.step_via_traj,self.step_via_traj]
self.hand_mode_methods=[self.step_hand_off,self.step_hand_open,self.step_hand_grasp,self.step_hand_animation]
self.arm_mode=[0]
self.hand_mode=[0]
self.poses={'zero': {'right_arm':[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],'left_arm':[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]},
'holdup':{'right_arm':[56.0, 26.0, -8.0, 84.0, 119.0, -36.0, 2.0],'left_arm':[56.0, -26.0, 8.0, 84.0, -119.0, -36.0, -2.0]}}
self.stiffness=[50]
self.velocity=[25]
self.gui.add('M3GuiModes', 'Arm Mode', (self,'arm_mode'),range(1),[self.arm_mode_names,1],m3g.M3GuiWrite,column=1)
self.gui.add('M3GuiToggle', 'ESTOP', (self,'off'),[],[['Arm Enabled','Arm Disabled']],m3g.M3GuiWrite,column=1)
if self.hand is not None:
self.gui.add('M3GuiToggle', 'Grasp', (self,'grasp'),[],[['GraspOpen','GraspClosed']],m3g.M3GuiWrite,column=1)
self.gui.add('M3GuiModes', 'Hand Mode', (self,'hand_mode'),range(1),[self.hand_mode_names,1],m3g.M3GuiWrite,column=1)
self.gui.add('M3GuiSliders','Stiffness ', (self,'stiffness'),[0],[0,100],m3g.M3GuiWrite,column=1)
self.gui.add('M3GuiSliders','Velocity ', (self,'velocity'),[0],[0,40],m3g.M3GuiWrite,column=1)
self.gui.start(self.step)
proxy.start()
proxy.make_operational_all()
print '--------------------------'
print 'Note: RVIZ support is only intended for debugging in the demo.'
print ' Motor power should be turned off if using RVIZ.'
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()
'postures': vias[c],
'param': param
} #safe defaults
f.write(yaml.safe_dump(d))
vias = {}
for c in self.chains:
vias[c] = []
if k == 'q':
self.done = True
# ######################################################
proxy = m3p.M3RtProxy()
proxy.start()
bot_name = m3t.get_robot_name()
if bot_name == "":
print 'Error: no robot components found:', bot_name
exit()
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()
proxy.step()
chains = bot.get_available_chains()
print 'Select chains to pose'
chains = m3t.user_select_components_interactive(chains)
menu = menu_thread(bot, chains)
