def start(self, ctype):
#Return true if success
#ctype. e.g.,: {'comp_ec':'m3actuator_ec','comp_rt':'m3actuator'}
self.proxy = m3p.M3RtProxy()
self.proxy.start()
print 'Select component'
ac = self.proxy.get_available_components(ctype['comp_rt'])
if len(ac) == 0:
print 'Required components not available'
return False
self.name_rt = m3t.user_select_components_interactive(ac,
single=True)[0]
self.name_ec = self.name_rt.replace(ctype['comp_rt'], ctype['comp_ec'])
self.comp_ec = m3f.create_component(self.name_ec)
self.comp_rt = m3f.create_component(self.name_rt)
if self.proxy.is_component_available(self.name_ec):
self.proxy.subscribe_status(self.comp_ec)
self.proxy.publish_command(self.comp_ec)
self.proxy.publish_param(self.comp_ec)
self.proxy.make_operational(self.name_ec)
if self.proxy.is_component_available(self.name_rt):
self.proxy.subscribe_status(self.comp_rt)
pwr_ec = self.proxy.get_available_components('m3pwr_ec')
pwr_rt = self.proxy.get_available_components('m3pwr')
if len(pwr_rt):
pr = m3f.create_component(pwr_rt[0])
self.proxy.publish_command(pr)
self.proxy.make_operational(pwr_rt[0])
pr.set_motor_power_on()
if len(pwr_ec):
self.proxy.make_operational(pwr_ec[0])
self.step()
return True
def __init__(self):
self.proxy = m3p.M3RtProxy()
self.gui = m3g.M3Gui(stride_ms=125) #125
self.slew_rate = 0.5
self.stiffness = 0.5
self.num_dof = 7
self.k = m3k.M3Kontrol()
def initialize_joints(self, right_arm_settings, left_arm_settings):
self.proxy = m3p.M3RtProxy()
self.proxy.start(True, True) # the second true enables ROS (needed for the skin patch)
for c in ['m3pwr_pwr003','m3loadx6_ma1_l0','m3arm_ma1','m3loadx6_ma2_l0','m3arm_ma2']:
if not self.proxy.is_component_available(c):
raise m3t.M3Exception('Component '+c+' is not available.')
self.joint_list_dict = {}
right_l = []
for c in ['m3joint_ma1_j0','m3joint_ma1_j1','m3joint_ma1_j2',
'm3joint_ma1_j3','m3joint_ma1_j4','m3joint_ma1_j5',
'm3joint_ma1_j6']:
if not self.proxy.is_component_available(c):
raise m3t.M3Exception('Component '+c+' is not available.')
right_l.append(m3f.create_component(c))
self.joint_list_dict['right_arm'] = right_l
left_l = []
for c in ['m3joint_ma2_j0','m3joint_ma2_j1','m3joint_ma2_j2',
'm3joint_ma2_j3','m3joint_ma2_j4','m3joint_ma2_j5',
'm3joint_ma2_j6']:
if not self.proxy.is_component_available(c):
raise m3t.M3Exception('Component '+c+' is not available.')
left_l.append(m3f.create_component(c))
self.joint_list_dict['left_arm'] = left_l
for arm,arm_settings in zip(['right_arm','left_arm'],[right_arm_settings,left_arm_settings]):
if arm_settings == None:
continue
for comp in self.joint_list_dict[arm]:
self.proxy.subscribe_status(comp)
self.proxy.publish_command(comp)
self.set_arm_settings(right_arm_settings,left_arm_settings)
right_fts=m3.loadx6.M3LoadX6('m3loadx6_ma1_l0')
self.proxy.subscribe_status(right_fts)
left_fts=m3.loadx6.M3LoadX6('m3loadx6_ma2_l0')
self.proxy.subscribe_status(left_fts)
self.fts = {'right_arm':right_fts,'left_arm':left_fts}
#self.pwr=m3w.M3Pwr('m3pwr_pwr003')
self.pwr=m3f.create_component('m3pwr_pwr003')
self.proxy.subscribe_status(self.pwr)
self.proxy.publish_command(self.pwr)
self.arms = {}
self.arms['right_arm']=m3.arm.M3Arm('m3arm_ma1')
self.proxy.subscribe_status(self.arms['right_arm'])
self.arms['left_arm']=m3.arm.M3Arm('m3arm_ma2')
self.proxy.subscribe_status(self.arms['left_arm'])
self.proxy.step()
self.proxy.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 __init__(self):
self.proxy = m3p.M3RtProxy()
self.gui = m3g.M3Gui(stride_ms=125) #125
'''self.max_lin_acc = 0.25
self.max_lin_vel = 0.5
self.max_rot_acc = 8
self.max_rot_vel = 17'''
self.max_lin_vel = 0.25 # m/s (0.6)
self.max_lin_acc = 0.4 # m/s^2 (0.2) --- 1.0 gives really good performance but saturates motors...
self.max_rot_vel = 25 # deg/s
self.max_rot_acc = 60 # 100 for better # deg/s^2
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, process_cb):
self.proxy = m3p.M3RtProxy()
self.proxy.start()
cnames = self.proxy.get_available_components('m3tactile_pps22_ec')
if len(cnames) == 0:
print 'No PPS22 sensor present'
return
if len(cnames) > 1:
name = m3t.user_select_components_interactive(cnames)[0]
else:
name = cnames[0]
self.pps = m3f.create_component(name)
self.proxy.subscribe_status(self.pps)
print 'Place sensor in unloaded state.Hit return when ready'
raw_input()
self.proxy.step()
self.zero = self.pps.get_taxels()
self.process_cb = process_cb
gtk.main()
def __init__ (self, make_all_op = False, make_all_op_shm = False, make_all_op_no_shm = True,data_svc=False):
Thread.__init__(self)
self.make_all_op = make_all_op
self.make_all_op_shm = make_all_op_shm
self.make_all_op_no_shm = make_all_op_no_shm
self.stop_event = Event()
self.proxy = m3p.M3RtProxy(rpc_port=port)
self.proxy.start(start_data_svc, False)
print "M3 INFO: M3 is now running ",
if self.make_all_op:
print "(with option -make operational all+shm)"
self.proxy.make_operational_all()
self.proxy.make_operational_all_shm()
if self.make_all_op_shm:
print "(with option -make operational shm only)"
self.proxy.make_operational_all_shm()
if self.make_all_op_no_shm:
print "(with option -make operational all (no shm))"
self.proxy.make_operational_all()
def __init__(self,
bot,
enable_zero_gravity,
record_now,
output_dir,
functions_to_call,
arguments,
proxy=None,
filename_out=time.strftime("%Y%m%d-%H%M%S"),
add_timestamp=True):
# Simple arguments checks
assert type(enable_zero_gravity) is bool
assert type(record_now) is bool
assert type(output_dir) is str
assert os.path.isdir(output_dir)
assert type(arguments) is list
assert type(functions_to_call) is list
assert len(functions_to_call) == len(arguments)
# The humanoid class
self.bot = bot
# Do we add timestamp ?
self.add_timestamp = add_timestamp
# Get/create the M3 Proxy to talk with the robot
if proxy:
assert isinstance(proxy, m3p.M3RtProxy)
self.proxy = proxy
else:
self.proxy = m3p.M3RtProxy()
# Start the proxy
self.proxy.start()
# Do we send command to the robot ?
self.enable_zero_gravity = enable_zero_gravity
# Bring up the robot
self.setup_robot(command=enable_zero_gravity)
# Functions to call as a list of str
self.functions_to_call = []
# Functions to call as a ptr to the function
self.functions = []
# Arguments to pass to the function as list of str
self.arguments = []
# Adding timestamp as first row
if self.add_timestamp:
# Adding the str (for printing)
self.functions_to_call.append('get_timestamp_S')
# Adding the pointer to the function (for calling)
self.functions.append(self.get_timestamp_S)
# Adding the argument (None here)
self.arguments.append('')
# Adding the functions passed as arguments
self.functions_to_call.extend(functions_to_call)
# Getting the function pointer from the str passed as argument
# Note: bot could be any class
for f in functions_to_call:
self.functions.append(getattr(self.bot, f))
# Adding the arguments passed as..argument
self.arguments.extend(arguments)
# Shall we start recording now ot wait for enter
self.record_now = record_now
# The time we started the recording
self.start_time = 0.0
# If no filename is provided, the name is the date
if not filename_out:
filename_out = time.strftime("%Y%m%d-%H%M%S")
# The generic recorder class
self.recorder = Recorder(self.functions,
self.arguments,
filename_out,
self.proxy,
folder_path=os.path.abspath(output_dir))
def main():
proxy = m3p.M3RtProxy()
proxy.start()
base_name = proxy.get_available_components('m3omnibase')
if len(base_name) != 1:
print 'Invalid number of base components available'
proxy.stop()
exit()
omni = m3o.M3OmniBase(base_name[0])
proxy.publish_param(omni) # we need this for calibration
proxy.subscribe_status(omni)
proxy.publish_command(omni)
pwr_name = [m3t.get_omnibase_pwr_component_name(base_name[0])]
#proxy.get_available_components('m3pwr')
#if len(pwr_name)>1:
#pwr_name=m3t.user_select_components_interactive(pwr_name,single=True)
pwr = m3f.create_component(pwr_name[0])
proxy.subscribe_status(pwr)
proxy.publish_command(pwr)
proxy.make_operational(pwr_name[0])
proxy.step()
omni.set_mode_off()
pwr.set_motor_power_on()
proxy.make_operational_all()
proxy.step()
time.sleep(0.5)
proxy.step()
omni.calibrate(proxy)
time.sleep(0.5)
print "Turn power on to robot and press any key."
raw_input()
omni.set_local_position(0, 0, 0, proxy)
omni.set_global_position(0, 0, 0, proxy)
omni.set_max_linear_accel(max_lin_acc)
omni.set_max_linear_velocity(max_lin_vel)
omni.set_max_rotation_velocity(max_rot_vel)
omni.set_max_rotation_accel(max_rot_acc)
proxy.step()
'''p = omni.get_global_position()
print 'Pos:', p'''
omni.set_mode_traj_goal()
omni.set_traj_goal(0, 0, 0)
proxy.step()
time.sleep(4)
print 'Bus voltage:', omni.get_bus_voltage()
print 'Press any key to begin pacing.'
raw_input()
try:
while True:
omni.set_traj_goal(2.0, 0, 180)
proxy.step()
time.sleep(0.1)
proxy.step()
while not omni.is_traj_goal_reached():
proxy.step()
p = omni.get_global_position()
print 'Pos:', p
time.sleep(0.1)
omni.set_traj_goal(0, 0, 0)
proxy.step()
time.sleep(0.1)
proxy.step()
while not omni.is_traj_goal_reached():
proxy.step()
p = omni.get_global_position()
print 'Pos:', p
time.sleep(0.1)
except KeyboardInterrupt:
pass
omni.set_mode_off()
pwr.set_motor_power_off()
proxy.step()
proxy.stop()
def __init__(self):
self.proxy = m3p.M3RtProxy()
self.gui = m3g.M3Gui()
#! /usr/bin/python
import pylab as pyl
import time
import m3.rt_proxy as m3p
import m3.toolbox as m3t
import m3.component_factory as mcf
import numpy.numarray as na
import math
proxy = m3p.M3RtProxy()
proxy.start()
cnames = proxy.get_available_components()
cnames = [q for q in cnames if q.find('actuator_ec') != -1]
cnames = [q for q in cnames if q.find('mh') == -1]
comps = []
for c in cnames:
comps.append(mcf.create_component(c))
proxy.subscribe_status(comps[-1])
last_period = []
last_rollover = []
offset_period = []
offset_rollover = []
for i in range(len(comps)):
last_period.append(0)
last_rollover.append(0)
offset_period.append(0)
offset_rollover.append(0)
try:
ts = time.time()
terr_q = 0
terr_tq = 0
def main():
proxy = m3p.M3RtProxy()
proxy.start()
base_name=proxy.get_available_components('m3omnibase')
omni=None
if len(base_name)!=1:
print 'Omnibase not available. Proceeding without it...'
else:
print 'Use OmniBase [y]?'
if m3t.get_yes_no('y'):
omni=m3o.M3OmniBase(base_name[0])
proxy.publish_param(omni) # we need this for calibration
proxy.subscribe_status(omni)
proxy.publish_command(omni)
pwr_name=[m3t.get_omnibase_pwr_component_name(base_name[0])]
#proxy.get_available_components('m3pwr')
#if len(pwr_name)>1:
#pwr_name=m3t.user_select_components_interactive(pwr_name,single=True)
pwr=m3f.create_component(pwr_name[0])
proxy.subscribe_status(pwr)
proxy.publish_command(pwr)
zlift_names=proxy.get_available_components('m3joint_zlift')
zl=None
if len(zlift_names)==1:
print 'Use Zlift [y]?'
if m3t.get_yes_no('y'):
zl=m3z.M3JointZLift(zlift_names[0])
proxy.subscribe_status(zl)
proxy.publish_command(zl)
proxy.publish_param(zl)
proxy.make_operational(pwr_name[0])
proxy.step()
if omni is not None:
omni.set_mode_off()
pwr.set_motor_power_on()
proxy.make_operational_all()
zlift_shm_names=proxy.get_available_components('m3joint_zlift_shm')
if len(zlift_shm_names) > 0:
proxy.make_safe_operational(zlift_shm_names[0])
omnibase_shm_names=proxy.get_available_components('m3omnibase_shm')
if len(omnibase_shm_names) > 0:
proxy.make_safe_operational(omnibase_shm_names[0])
humanoid_shm_names=proxy.get_available_components('m3humanoid_shm')
if len(humanoid_shm_names) > 0:
proxy.make_safe_operational(humanoid_shm_names[0])
m3ledx2xn_ec_shm_names=proxy.get_available_components('m3ledx2xn_ec_shm')
if len(m3ledx2xn_ec_shm_names) > 0:
proxy.make_safe_operational(m3ledx2xn_ec_shm_names[0])
m3led_matrix_ec_shm_names=proxy.get_available_components('m3led_matrix_ec_shm')
if len(m3led_matrix_ec_shm_names) > 0:
proxy.make_safe_operational(m3led_matrix_ec_shm_names[0])
proxy.step()
time.sleep(0.5)
if omni is not None:
proxy.step()
omni.calibrate(proxy)
time.sleep(0.5)
if zl is not None:
if not zl.calibrate(proxy):
zl=None
print 'ZLift failed to calibrate'
if omni is None and zl is None:
exit()
print "Turn motor power on to Omnibase and press any key."
raw_input()
joy=m3to.M3OmniBaseJoy()
joy.start(proxy,omni,zl)
k = 0
try:
while True:
joy.step()
#print 'Bus Current:', pwr.get_bus_torque()
p = omni.get_local_position()
#omni.set_op_space_forces(f.jx*200.0, f.jy*200.0, f.jyaw*50.0)
k += 1
if k == 100:
print '-----------Local Pos-------'
print 'X:', p[0]
print 'Y:', p[1]
print 'Yaw:', p[2]
#print '---------------------------'
k = 0
'''print '-------Joystick Pos-------'
print 'jx:',f.jx
print 'jy:', f.jy
print 'jyaw:', f.jyaw
print 'button:', f.jbutton
print '---------------------------'
#print 'Bus voltage',omni.get_bus_voltage()'''
'''tqs = omni.get_motor_torques()
print tqs[0], tqs[2], tqs[4], tqs[6]'''
#print omni.get_steer_torques()
proxy.step()
except KeyboardInterrupt:
pass
joy.stop()
if omni is not None:
omni.set_mode_off()
pwr.set_motor_power_off()
proxy.step()
proxy.stop()
def main():
proxy = m3p.M3RtProxy()
proxy.start()
base_name=proxy.get_available_components('m3omnibase')
if len(base_name)!=1:
print 'Invalid number of base components available'
proxy.stop()
exit()
omni=m3o.M3OmniBase(base_name[0])
proxy.publish_param(omni) # we need this for calibration
proxy.subscribe_status(omni)
proxy.publish_command(omni)
pwr_name=[m3t.get_omnibase_pwr_component_name(base_name[0])]
#proxy.get_available_components('m3pwr')
#if len(pwr_name)>1:
#pwr_name=m3t.user_select_components_interactive(pwr_name,single=True)
pwr=m3f.create_component(pwr_name[0])
proxy.subscribe_status(pwr)
proxy.publish_command(pwr)
proxy.make_operational(pwr_name[0])
proxy.step()
pwr.set_motor_power_on()
proxy.step()
proxy.make_operational_all()
proxy.step()
time.sleep(0.5)
proxy.step()
omni.calibrate(proxy)
time.sleep(0.5)
omni.set_local_position(0,0,0,proxy)
omni.set_global_position(0,0,0,proxy)
omni.set_mode_traj_via()
'''omni.add_via(0, 1.2, -180)
omni.add_via(1, 1.2, 0)
omni.add_via(1, 0, 180)
omni.add_via(0, 0, 0)'''
omni.add_via(0, 0.2, 0)
omni.add_via(0.2, 0.2, 0)
omni.add_via(0.2, 0, 0)
omni.add_via(0, 0, 0)
proxy.step()
time.sleep(0.2)
proxy.step()
'''while not omni.is_traj_goal_reached():
proxy.step()'''
try:
while True:
proxy.step()
p = omni.get_global_position()
d = omni.get_desired_position()
print '-----------Local Pos-------'
print 'X:', p[0]
print 'Y:', p[1]
print 'Yaw:', p[2]
print '---------------------------'
print '-----------Desired Pos-------'
print 'dX:', d[0]
print 'dY:', d[1]
print 'dYaw:', d[2]
print '---------------------------'
time.sleep(0.1)
except (KeyboardInterrupt):
pass
stop()
def __init__(self):
self.proxy = m3p.M3RtProxy()
self.gui = m3g.M3Gui(stride_ms=50)
def __init__(self):
self.proxy = m3p.M3RtProxy()
self.gui = m3g.M3Gui(stride_ms=125) #125
self.slew_rate = 0.5
def start(self):
print '--------------------------'
print 'm: Target M3 arm'
print 'v: Target RVIZ'
print 'b: Target Both M3 and RVIZ'
print 'q: quit'
print '--------------'
print
self.k = 'a'
while self.k != 'm' and self.k != 'v' and self.k != 'b' and self.k != 'q':
self.k = m3t.get_keystroke()
if self.k == 'q':
return
self.proxy = m3p.M3RtProxy()
if self.k == 'm' or self.k == 'b':
self.proxy.start()
bot_name = m3t.get_robot_name()
if bot_name == "":
print 'Error: no robot components found:', bot_names
return
self.bot = m3f.create_component(bot_name)
arm_names = ['right_arm', 'left_arm']
self.arm_name = m3t.user_select_components_interactive(arm_names,
single=True)[0]
if self.arm_name == 'right_arm':
self.center = [0.450, -0.25, -0.1745]
else:
self.center = [0.450, 0.25, -0.1745]
avail_chains = self.bot.get_available_chains()
for c in avail_chains:
if c == 'torso':
self.center[2] += 0.5079
if self.k == 'v' or self.k == 'b':
self.viz = m3v.M3Viz(self.proxy, self.bot)
self.viz_launched = True
self.viz.turn_sim_on()
if self.k == 'v':
self.theta_0[:] = self.bot.get_theta_sim_deg(self.arm_name)[:]
if self.k == 'm' or self.k == 'b':
self.proxy.subscribe_status(self.bot)
self.proxy.publish_command(self.bot)
self.proxy.make_operational_all()
self.proxy.step()
self.theta_0[:] = self.bot.get_theta_deg(self.arm_name)[:]
self.m3_launched = True
self.theta_soln_deg = [0.] * self.bot.get_num_dof(self.arm_name)
self.thetadot_0 = [0.] * self.bot.get_num_dof(self.arm_name)
self.bot.set_slew_rate_proportion(self.arm_name, [1.0] * 7)
while True:
print '--------------------------'
print 'g: generate vias'
print 'd: display vias'
print 'v: set avg velocity (Current ', self.vel_avg, ')'
print 's: set stiffness (Current', self.stiffness, ')'
if self.k == 'b' or self.k == 'm':
print 'e: execute vias'
if self.k == 'b' or self.k == 'v':
print 't: test vias in visualizer'
print 'q: quit'
print '--------------'
print
m = m3t.get_keystroke()
if m == 'q':
return
if m == 'v':
print 'Enter avg velocity (0-60 Deg/S) [', self.vel_avg, ']'
self.vel_avg = max(0, min(60, m3t.get_float(self.vel_avg)))
if m == 's':
print 'Enter stiffness (0-1.0) [', self.stiffness, ']'
self.stiffness = max(0, min(1.0,
m3t.get_float(self.stiffness)))
if m == 'g':
self.vias = []
print
print '(s)quare or (c)ircle?'
shape = None
while shape != 's' and shape != 'c':
shape = m3t.get_keystroke()
length_m = 0.0
if shape == 's':
print
print 'Length of square side in cm (10-25) [25]'
length_cm = nu.float(max(10, min(25, m3t.get_int(25))))
length_m = length_cm / 100.0
diameter_m = 0.0
if shape == 'c':
print
print 'Diameter of circle in cm (10-25) [25]'
diameter_cm = nu.float(max(10, min(25, m3t.get_int(25))))
diameter_m = diameter_cm / 100.0
print
print 'Enter shape resolution (1-20 vias/side) [20]'
resolution = max(1, min(20, m3t.get_int(20)))
if self.m3_launched:
self.proxy.step()
x = self.center[0]
if shape == 's':
y_left = self.center[1] + length_m / 2.0
y_right = self.center[1] - length_m / 2.0
z_top = self.center[2] + length_m / 2.0
z_bottom = self.center[2] - length_m / 2.0
dy = (y_left - y_right) / nu.float(resolution)
dz = (z_top - z_bottom) / nu.float(resolution)
if self.arm_name == 'right_arm':
# first add start point
self.ik_vias.append([
x, y_left, z_top, self.axis_demo[0],
self.axis_demo[1], self.axis_demo[2]
])
# add top line
for i in range(resolution):
self.ik_vias.append([
x, y_left - (i + 1) * dy, z_top,
self.axis_demo[0], self.axis_demo[1],
self.axis_demo[2]
])
# add right line
for i in range(resolution):
self.ik_vias.append([
x, y_right, z_top - (i + 1) * dz,
self.axis_demo[0], self.axis_demo[1],
self.axis_demo[2]
])
# add bottom line
for i in range(resolution):
self.ik_vias.append([
x, y_right + (i + 1) * dy, z_bottom,
self.axis_demo[0], self.axis_demo[1],
self.axis_demo[2]
])
# add left line
for i in range(resolution):
self.ik_vias.append([
x, y_left, z_bottom + (i + 1) * dz,
self.axis_demo[0], self.axis_demo[1],
self.axis_demo[2]
])
else:
# first add start point
self.ik_vias.append([
x, y_right, z_top, self.axis_demo[0],
self.axis_demo[1], self.axis_demo[2]
])
# add top line
for i in range(resolution):
self.ik_vias.append([
x, y_right + (i + 1) * dy, z_top,
self.axis_demo[0], self.axis_demo[1],
self.axis_demo[2]
])
# add right line
for i in range(resolution):
self.ik_vias.append([
x, y_left, z_top - (i + 1) * dz,
self.axis_demo[0], self.axis_demo[1],
self.axis_demo[2]
])
# add bottom line
for i in range(resolution):
self.ik_vias.append([
x, y_left - (i + 1) * dy, z_bottom,
self.axis_demo[0], self.axis_demo[1],
self.axis_demo[2]
])
# add left line
for i in range(resolution):
self.ik_vias.append([
x, y_right, z_bottom + (i + 1) * dz,
self.axis_demo[0], self.axis_demo[1],
self.axis_demo[2]
])
if shape == 'c':
for i in range(resolution * 4 + 1):
dt = 2 * nu.pi / (nu.float(resolution) * 4)
t = (nu.pi / 2) + i * dt
if t > nu.pi:
t -= 2 * nu.pi
y = self.center[1] + (diameter_m / 2.0) * nu.cos(t)
z = self.center[2] + (diameter_m / 2.0) * nu.sin(t)
self.ik_vias.append([
x, y, z, self.axis_demo[0], self.axis_demo[1],
self.axis_demo[2]
])
self.vias.append(self.theta_0[:])
# use zero position as reference for IK solver
ref = [0] * self.bot.get_num_dof(self.arm_name)
# use holdup position as reference
ref = [30, 0, 0, 40, 0, 0, 0]
self.bot.set_theta_sim_deg(self.arm_name, ref)
for ikv in self.ik_vias:
theta_soln = []
print 'solving for ik via:', ikv
if self.bot.get_tool_axis_2_theta_deg_sim(
self.arm_name, ikv[:3], ikv[3:], theta_soln):
self.vias.append(theta_soln)
self.bot.set_theta_sim_deg(self.arm_name, theta_soln)
else:
print 'WARNING: no IK solution found for via ', ikv
self.bot.set_theta_sim_deg(self.arm_name, ref)
if self.viz_launched:
self.viz.step()
self.vias.append(self.theta_0[:])
if m == 'd':
print
print '--------- IK Vias (', len(self.ik_vias), ')--------'
print '---------------[end_xyz[3], end_axis[3]]-----------'
for ikv in self.ik_vias:
print ikv
print
print '--------- Joint Vias (', len(self.vias), ')--------'
for v in self.vias:
print v
if m == 'e' or m == 't':
if len(self.vias) != 0:
for v in self.vias:
#print 'Adding via',v
self.jt.add_via_deg(v, [self.vel_avg] * self.ndof)
self.jt.start(self.theta_0[:], self.thetadot_0[:])
print
print '--------- Splines (', len(
self.jt.splines), ')--------'
print '------------q_0, q_f, qdot_0, qdot_f, tf--------------'
for s in self.jt.splines:
print s.q_0, s.q_f, s.qdot_0, s.qdot_f, s.tf
print
print 'Hit any key to start or (q) to quit execution'
p = m3t.get_keystroke()
if p != 'q':
if self.m3_launched and m == 'e':
self.bot.set_motor_power_on()
self.bot.set_mode_theta_gc(self.arm_name)
self.bot.set_stiffness(
self.arm_name, [self.stiffness] *
self.bot.get_num_dof(self.arm_name))
while not self.jt.is_splined_traj_complete():
q = self.jt.step()
if self.viz_launched and m == 't':
self.bot.set_theta_sim_deg(self.arm_name, q)
self.viz.step()
time.sleep(0.1)
elif self.m3_launched and m == 'e':
self.bot.set_theta_deg(self.arm_name, q)
self.proxy.step()
self.ik_vias = []
def __init__(self):
M3Tuning.__init__(self)
self.proxy = m3p.M3RtProxy()
self.gui = m3g.M3Gui(stride_ms=125)
self.cnt = 0
self.bias = []
def __init__(self):
self.proxy = m3p.M3RtProxy()
self.components = []
