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 = []