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, 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 start(self):
self.proxy.start()
self.proxy.make_operational_all()
self.bot = m3h.M3Humanoid()
self.bot.initialize(self.proxy)
self.chains = [c for c in self.bot.get_available_chains() if 'hand' in c]
self.hands = []
i=1
for chain in self.chains:
self.hands.append(HandDemo(self.bot,chain,self.gui,i)) ; i=i+2
self.proxy.step()
self.status_dict=self.proxy.get_status_dict()
self.gui.add('M3GuiTree', 'Status', (self,'status_dict'),[],[],m3g.M3GuiRead,2)
self.gui.start(self.step)
def main(argv):
parser = argparse.ArgumentParser(
formatter_class=argparse.RawDescriptionHelpFormatter,
description=textwrap.dedent("""
\t\tWelcome to the M3 Recorder.
----------------------------------------------------------------------------
| This script helps you record stuff on the robot. |
| Example: m3recorder --right_arm get_theta_rad get_thetadot_rad -r |
| Author : Antoine Hoarau |
----------------------------------------------------------------------------
"""),
epilog='Maintainer: Antoine Hoarau <hoarau.robotics AT gmail DOT com>')
parser.add_argument('--version', action='version', version='%(prog)s 2.0')
parser.add_argument(
'-e',
'--enable_zero_gravity',
action='store_true',
help=
'If True, commands will be sent to the robot to enable zero gravity mode, so be carefull not to send commands to the robot in an other script.'
)
parser.add_argument(
'-r',
'--record_now',
action='store_true',
help=
'Starts recording directly, does not wait for enter to be pressed by user'
)
working_dir = os.getcwd()
parser.add_argument(
'-o',
'--output_dir',
type=str,
help=
'The output dir for your recordings (default is where you launched the script from: '
+ working_dir + ')',
default=working_dir)
parser.add_argument('--filename',
type=str,
help='Optional filename output')
bot = m3h.M3Humanoid()
chains = bot.get_available_chains()
import inspect
no_args_getters = [
g[0]
for g in inspect.getmembers(m3h.M3Humanoid, predicate=inspect.ismethod)
if 'get_' in g[0] and (
len(inspect.getargspec(getattr(bot, g[0])).args) == 1)
and not '_M3Humanoid__' in g[0]
]
parser.add_argument('-t',
type=str,
nargs='*',
help='Some extra functions',
choices=no_args_getters)
parser.add_argument('-n',
'--no_time_from_start',
help='Remove time from start in s (first row)',
action='store_false')
chain_getters = [
g[0]
for g in inspect.getmembers(m3h.M3Humanoid, predicate=inspect.ismethod)
if 'get_' in g[0] and set(inspect.getargspec(getattr(bot, g[0])).args)
== set(['self', 'chain']) and not '_M3Humanoid__' in g[0]
]
for c in chains:
parser.add_argument('--' + c,
type=str,
nargs='+',
help='A list of functions to call for ' + c,
choices=chain_getters)
args = parser.parse_args()
arguments = []
functions = []
try:
for f in args.t:
functions.append(f)
arguments.append('')
except:
pass
for c in chains:
if getattr(args, c):
for f in getattr(args, c):
functions.append(f)
argspec = inspect.getargspec(getattr(bot, f))
arguments.append(c)
filename_out = ''
try:
filename_out = args.filename
except:
pass
if not len(functions):
print 'No functions to record, please see --help more info.'
exit()
m3recorder = M3Recorder(bot,
args.enable_zero_gravity,
args.record_now,
args.output_dir,
functions,
arguments,
filename_out=filename_out,
add_timestamp=args.no_time_from_start)
try:
m3recorder.start()
except Exception, e:
print e
Created on Thu Oct 30 20:55:08 2014
@author: Antoine Hoarau
"""
"""
This is just a simple demo of the new velocity_gc mode, setting vel to zero"
"""
import m3.rt_proxy as m3p
import m3.humanoid as m3h
proxy = m3p.M3RtProxy()
proxy.start()
bot = m3h.M3Humanoid()
bot.initialize(proxy)
proxy.step()
import m3.toolbox as m3t
arm = m3t.user_select_components_interactive(['right_arm','left_arm'],single=True)[0]
bot.set_mode_thetadot_gc(arm)
bot.set_slew_rate_proportion(arm,[1.0]*bot.get_num_dof(arm))
bot.set_stiffness(arm,[1.0]*bot.get_num_dof(arm))
bot.set_thetadot_deg(arm,[0.0]*bot.get_num_dof(arm))
print("This will set the velocity to 0 (tries to stay still)")
raw_input("Press Enter to start")
proxy.step()
raw_input("Press Enter to stop")
proxy.stop()
import matplotlib
if __name__ == '__main__':
th_plot = m3t.M3ScopeN(n=7, title='Theta')
thdot_plot = m3t.M3ScopeN(n=7, title='ThetaDot')
thdotdot_plot = m3t.M3ScopeN(n=7, title='ThetaDotDot')
pos_plot = m3t.M3ScopeN(n=3, title='pos xyz')
vel_plot = m3t.M3ScopeN(n=3, title='vel cart')
acc_plot = m3t.M3ScopeN(n=3, title='acc cart')
tq_plot = m3t.M3ScopeN(n=7, title='torque')
tqgrav_plot = m3t.M3ScopeN(n=7, title='torque gravity')
f_plot = m3t.M3ScopeN(n=3)
proxy = m3p.M3RtProxy()
proxy.start()
bot_name = m3t.get_robot_name()
bot = m3h.M3Humanoid(bot_name)
proxy.subscribe_status(bot)
#scope.plot(np.concatenate((bot.get_tool_velocity('right_arm'),xdot_d[:3])))
tstart = time.time()
n = 0
print 'tstart : ', tstart
end = False
chain = 'right_arm'
while not end:
try:
#ts = time.time()
proxy.step()
th = bot.get_theta_deg(chain)
th_plot.plot(th)
thdot = bot.get_thetadot_deg(chain)
#!/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