def record_good_configs(use_left_arm): import m3.toolbox as m3t settings_arm = ar.MekaArmSettings(stiffness_list=[0.,0.,0.,0.,0.],control_mode='torque_gc') if use_left_arm: firenze = ar.M3HrlRobot(connect=True,left_arm_settings=settings_arm) arm = 'left_arm' else: firenze = ar.M3HrlRobot(connect=True,right_arm_settings=settings_arm) arm = 'right_arm' print 'hit ENTER to start the recording process' k=m3t.get_keystroke() firenze.power_on() good_configs_list = [] while k == '\r': print 'hit ENTER to record configuration, something else to exit' k=m3t.get_keystroke() firenze.proxy.step() q = firenze.get_joint_angles(arm) good_configs_list.append(np.matrix(q).A1.tolist()) firenze.stop() ut.save_pickle(good_configs_list,ut.formatted_time()+'_good_configs_list.pkl')
def pose_arm(self,hook_angle): print 'press ENTER to pose the robot.' k=m3t.get_keystroke() if k!='\r': print 'You did not press ENTER.' return # settings_r_orig = copy.copy(self.firenze.arm_settings['right_arm']) settings_torque_gc = hr.MekaArmSettings(stiffness_list=[0.,0.,0.,0.,0.],control_mode='torque_gc') self.firenze.set_arm_settings(settings_torque_gc,None) self.firenze.step() print 'hit ENTER to end posing, something else to exit' k=m3t.get_keystroke() p = self.firenze.end_effector_pos('right_arm') q = self.firenze.get_joint_angles('right_arm') # self.firenze.set_arm_settings(settings_r_orig,None) self.firenze.set_arm_settings(self.settings_stiff,None) self.firenze.set_joint_angles('right_arm',q) self.firenze.step() self.firenze.set_joint_angles('right_arm',q) self.firenze.step() rot_mat = tr.Rz(hook_angle-hook_3dprint_angle)*tr.Ry(math.radians(-90)) self.firenze.go_cartesian('right_arm',p,rot_mat,speed=0.1) print 'hit ENTER after making finer adjustment, something else to exit' k=m3t.get_keystroke() p = self.firenze.end_effector_pos('right_arm') q = self.firenze.get_joint_angles('right_arm') self.firenze.set_joint_angles('right_arm',q) self.firenze.step()
def record_good_configs(use_left_arm): import m3.toolbox as m3t settings_arm = ar.MekaArmSettings(stiffness_list=[0., 0., 0., 0., 0.], control_mode='torque_gc') if use_left_arm: firenze = ar.M3HrlRobot(connect=True, left_arm_settings=settings_arm) arm = 'left_arm' else: firenze = ar.M3HrlRobot(connect=True, right_arm_settings=settings_arm) arm = 'right_arm' print 'hit ENTER to start the recording process' k = m3t.get_keystroke() firenze.power_on() good_configs_list = [] while k == '\r': print 'hit ENTER to record configuration, something else to exit' k = m3t.get_keystroke() firenze.proxy.step() q = firenze.get_joint_angles(arm) good_configs_list.append(np.matrix(q).A1.tolist()) firenze.stop() ut.save_pickle(good_configs_list, ut.formatted_time() + '_good_configs_list.pkl')
def record_joint_displacements(): print 'hit ENTER to start the recording process' k=m3t.get_keystroke() pos_list = [] force_list = [] while k == '\r': print 'hit ENTER to record configuration, something else to exit' k=m3t.get_keystroke() firenze.proxy.step() pos_list.append(firenze.end_effector_pos('right_arm')) force_list.append(firenze.get_wrist_force('right_arm', base_frame=True)) ut.save_pickle({'pos_list':pos_list,'force_list':force_list},'stiffness_'+ut.formatted_time()+'.pkl') firenze.stop()
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): print '---------M3 RECORDER--------' print 'To be recorded : ' for f,a in zip(self.functions_to_call,self.arguments): print f+'('+a+')' if self.enable_zero_gravity: self.set_to_zero_gravity_mode() if not self.record_now: print '-------------------------------' print 'Press any key to start' print 'q to quit' c = m3t.get_keystroke() if c=='q': return None self.recorder.start() print "" print "Press any key to STOP " m3t.get_keystroke()
def start(self): print '---------M3 RECORDER--------' print 'To be recorded : ' for f, a in zip(self.functions_to_call, self.arguments): print f + '(' + a + ')' if a is not '' and self.enable_zero_gravity: self.set_to_zero_gravity_mode(chain=a) if not self.record_now: print '-------------------------------' print 'Press any key to start' print 'q to quit' c = m3t.get_keystroke() if c == 'q': return None self.recorder.start() print "" print "Press any key to STOP " m3t.get_keystroke()
def test_IK(arm, rot_mat): ''' try out the IK at a number of different cartesian points in the workspace, with the given rotation matrix for the end effector. ''' print 'press ENTER to start.' k=m3t.get_keystroke() while k=='\r': p = firenze.end_effector_pos(arm) firenze.go_cartesian(arm,p,rot_mat,speed=0.1) firenze.step() print 'press ENTER to save joint angles.' k=m3t.get_keystroke() if k == '\r': firenze.step() q = firenze.get_joint_angles(arm) ut.save_pickle(q,'arm_config_'+ut.formatted_time()+'.pkl') print 'press ENTER for next IK test. something else to exit.' k=m3t.get_keystroke()
def do_test(self, ct): if ct == 'ds': while True: print 'Hit q to quit, any other key to display' if m3t.get_keystroke() == 'q': break ts = time.time() while (time.time() - ts < 5.0): self.display_sensors() self.step(0.1) return False
def do_test(self,ct): if ct=='ds': while True: print 'Hit q to quit, any other key to display' if m3t.get_keystroke()=='q': break ts=time.time() while(time.time()-ts<5.0): self.display_sensors() self.step(0.1) return False
def record_joint_displacements(): print 'hit ENTER to start the recording process' k = m3t.get_keystroke() pos_list = [] force_list = [] while k == '\r': print 'hit ENTER to record configuration, something else to exit' k = m3t.get_keystroke() firenze.proxy.step() pos_list.append(firenze.end_effector_pos('right_arm')) force_list.append(firenze.get_wrist_force('right_arm', base_frame=True)) ut.save_pickle({ 'pos_list': pos_list, 'force_list': force_list }, 'stiffness_' + ut.formatted_time() + '.pkl') firenze.stop()
def record_good_configs(use_left_arm): import m3.toolbox as m3t import cody_arm_client as cac if use_left_arm: arm = 'l' else: arm = 'r' firenze = cac.CodyArmClient(arm) print 'hit ENTER to start the recording process' k=m3t.get_keystroke() good_configs_list = [] while k == '\r': print 'hit ENTER to record configuration, something else to exit' k=m3t.get_keystroke() q = firenze.get_joint_angles(arm) good_configs_list.append(np.matrix(q).A1.tolist()) ut.save_pickle(good_configs_list,ut.formatted_time()+'_good_configs_list.pkl')
def pose_arm(self, hook_angle): print 'press ENTER to pose the robot.' k = m3t.get_keystroke() if k != '\r': print 'You did not press ENTER.' return # settings_r_orig = copy.copy(self.firenze.arm_settings['right_arm']) settings_torque_gc = hr.MekaArmSettings( stiffness_list=[0., 0., 0., 0., 0.], control_mode='torque_gc') self.firenze.set_arm_settings(settings_torque_gc, None) self.firenze.step() print 'hit ENTER to end posing, something else to exit' k = m3t.get_keystroke() p = self.firenze.end_effector_pos('right_arm') q = self.firenze.get_joint_angles('right_arm') # self.firenze.set_arm_settings(settings_r_orig,None) self.firenze.set_arm_settings(self.settings_stiff, None) self.firenze.set_joint_angles('right_arm', q) self.firenze.step() self.firenze.set_joint_angles('right_arm', q) self.firenze.step() rot_mat = tr.Rz(hook_angle - hook_3dprint_angle) * tr.Ry( math.radians(-90)) self.firenze.go_cartesian('right_arm', p, rot_mat, speed=0.1) print 'hit ENTER after making finer adjustment, something else to exit' k = m3t.get_keystroke() p = self.firenze.end_effector_pos('right_arm') q = self.firenze.get_joint_angles('right_arm') self.firenze.set_joint_angles('right_arm', q) self.firenze.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 do_task(self,ct): #ct: command type #if ct=='gf': #for k in self.calib_default.keys(): #self.reset_sensor(k) #self.comp_rt.load_attr_from_config(self.param_default,self.comp_rt.param) #self.write_config() #return True if ct=='ds': while True: print 'Hit q to quit, any other key to display' if m3t.get_keystroke()=='q': break ts=time.time() while(time.time()-ts<5.0): self.display_sensors() self.step(0.1) return False
def run_ik(proxy, bot, step_delta, arm_name, pub, viz): pose = get_pose(proxy, bot, arm_name, viz) bot.set_mode_theta_gc(arm_name) bot.set_stiffness(arm_name, [stiffness] * bot.get_num_dof(arm_name)) bot.set_theta_deg(arm_name, pose) #bot.set_theta_deg([0,0,0],[4,5,6]) #No roll/pitch/yaw of wrist proxy.step() t = ik_thread(proxy, bot, step_delta, arm_name, pub, viz) t.start() d = [0, 0, 0] while 1: print '-----------------------' print 'Delta: ', t.delta print '-----------------------' print 'q: quit' print '1: x+' print '2: x-' print '3: y+' print '4: y-' print '5: z+' print '6: z-' print 'space: step' k = m3t.get_keystroke() if k == 'q': t.delta_done = True return if k == '1': d[0] = d[0] + step_delta if k == '2': d[0] = d[0] - step_delta if k == '3': d[1] = d[1] + step_delta if k == '4': d[1] = d[1] - step_delta if k == '5': d[2] = d[2] + step_delta if k == '6': d[2] = d[2] - step_delta t.set_delta(d) print print 'Error: ', t.aerror print 'Target: ', t.target_pos
def run_ik(proxy,bot, step_delta, arm_name,pub, viz): pose=get_pose(proxy,bot, arm_name,viz) bot.set_mode_theta_gc(arm_name) bot.set_stiffness(arm_name, [stiffness]*bot.get_num_dof(arm_name)) bot.set_theta_deg(arm_name, pose) #bot.set_theta_deg([0,0,0],[4,5,6]) #No roll/pitch/yaw of wrist proxy.step() t=ik_thread(proxy,bot,step_delta, arm_name,pub, viz) t.start() d=[0,0,0] while 1: print '-----------------------' print 'Delta: ',t.delta print '-----------------------' print 'q: quit' print '1: x+' print '2: x-' print '3: y+' print '4: y-' print '5: z+' print '6: z-' print 'space: step' k=m3t.get_keystroke() if k=='q': t.delta_done=True return if k=='1': d[0]=d[0]+step_delta if k=='2': d[0]=d[0]-step_delta if k=='3': d[1]=d[1]+step_delta if k=='4': d[1]=d[1]-step_delta if k=='5': d[2]=d[2]+step_delta if k=='6': d[2]=d[2]-step_delta t.set_delta(d) print print 'Error: ',t.aerror print 'Target: ',t.target_pos
print 'Error: ',t.aerror print 'Target: ',t.target_pos # ###################################################### proxy = m3p.M3RtProxy() 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:
def pull(self, hook_angle, force_threshold, use_utm=False, use_camera=False, strategy='line_neg_x', pull_loc=None, info_string=''): ''' force_threshold - max force at which to stop pulling. hook_angle - radians(0, -90, 90 etc.) 0 - horizontal, -pi/2 hook points up, +pi/2 hook points down use_utm - to take 3D scans or not. use_camera - to take pictures from the camera or not. strategy - 'line_neg_x': move eq point along -x axis. 'piecewise_linear': try and estimate circle and move along it. 'control_radial_force': try and keep the radial force constant 'control_radial_dist' pull_loc - 3x1 np matrix of location for pulling. If None then arm will go into gravity comp and user can show the location. info_string - string saved with key 'info' in the pkl. ''' if use_utm: self.firenze.step() armconfig1 = self.firenze.get_joint_angles('right_arm') plist1, slist1 = self.scan_3d() if use_camera: cam_plist1, cam_imlist1 = self.image_region() else: cam_plist1, cam_imlist1 = None, None rot_mat = tr.Rz(hook_angle - hook_3dprint_angle) * tr.Ry( math.radians(-90)) if pull_loc == None: self.pose_arm(hook_angle) pull_loc = self.firenze.end_effector_pos('right_arm') ut.save_pickle( pull_loc, 'pull_loc_' + info_string + '_' + ut.formatted_time() + '.pkl') else: pt1 = copy.copy(pull_loc) pt1[0, 0] = pt1[0, 0] - 0.1 print 'pt1:', pt1.A1 print 'pull_loc:', pull_loc.A1 self.firenze.go_cartesian('right_arm', pt1, rot_mat, speed=0.2) self.firenze.go_cartesian('right_arm', pull_loc, rot_mat, speed=0.07) print 'press ENTER to pull' k = m3t.get_keystroke() if k != '\r': return time_dict = {} time_dict['before_hook'] = time.time() print 'first getting a good hook' self.get_firm_hook(hook_angle) time.sleep(0.5) time_dict['before_pull'] = time.time() print 'pull begins' stiffness_scale = self.settings_r.stiffness_scale vec = tr.rotX(-hook_angle) * np.matrix( [0., 0.05 / stiffness_scale, 0.]).T self.keep_hook_vec = vec self.hook_maintain_dist_plane = np.dot(vec.A1, np.array([0., 1., 0.])) self.eq_pt_cartesian = self.firenze.end_effector_pos('right_arm') + vec q_eq = self.firenze.IK('right_arm', self.eq_pt_cartesian, rot_mat) self.firenze.go_jointangles('right_arm', q_eq, speed=math.radians(30)) self.q_guess = q_eq # self.q_guess = self.firenze.get_joint_angles('right_arm') self.pull_trajectory = at.JointTrajectory() self.jt_torque_trajectory = at.JointTrajectory() self.eq_pt_trajectory = at.JointTrajectory() self.force_trajectory = at.ForceTrajectory() self.firenze.step() start_config = self.firenze.get_joint_angles('right_arm') self.eq_IK_rot_mat = rot_mat # for equi pt generators. self.eq_force_threshold = force_threshold self.hooked_location_moved = False # flag to indicate when the hooking location started moving. self.prev_force_mag = np.linalg.norm( self.firenze.get_wrist_force('right_arm')) self.eq_motion_vec = np.matrix([-1., 0., 0.]).T self.slip_count = 0 if strategy == 'line_neg_x': result = self.compliant_motion(self.equi_pt_generator_line, 0.025) elif strategy == 'control_radial_force': self.cartesian_pts_list = [] self.piecewise_force_threshold = force_threshold self.rad_guess = 1.0 self.cx = 0.6 self.cy = -self.rad_guess self.start() # start the circle estimation thread result = self.compliant_motion( self.equi_pt_generator_control_radial_force, 0.025) else: raise RuntimeError('unknown pull strategy: ', strategy) if result == 'slip: force step decrease' or result == 'danger of self collision': self.firenze.motors_off() print 'powered off the motors.' print 'Compliant motion result:', result print 'Original force threshold:', force_threshold print 'Adapted force threshold:', self.eq_force_threshold time_dict['after_pull'] = time.time() d = { 'actual': self.pull_trajectory, 'eq_pt': self.eq_pt_trajectory, 'force': self.force_trajectory, 'torque': self.jt_torque_trajectory, 'stiffness': self.firenze.arm_settings['right_arm'], 'info': info_string, 'force_threshold': force_threshold, 'eq_force_threshold': self.eq_force_threshold, 'hook_angle': hook_angle, 'result': result, 'strategy': strategy, 'time_dict': time_dict } self.firenze.step() armconfig2 = self.firenze.get_joint_angles('right_arm') if use_utm: plist2, slist2 = self.scan_3d() d['start_config'] = start_config d['armconfig1'] = armconfig1 d['armconfig2'] = armconfig2 d['l1'], d['l2'] = 0., -0.055 d['scanlist1'], d['poslist1'] = slist1, plist1 d['scanlist2'], d['poslist2'] = slist2, plist2 d['cam_plist1'] = cam_plist1 d['cam_imlist1'] = cam_imlist1 ut.save_pickle( d, 'pull_trajectories_' + d['info'] + '_' + ut.formatted_time() + '.pkl')
sys.exit() if opt.la: use_left_arm = True use_right_arm = False arm = 'left_arm' else: use_left_arm = False use_right_arm = True arm = 'right_arm' cmg = CompliantMotionGenerator(move_segway_flag, use_right_arm, use_left_arm) print 'hit a key to power up the arms.' k=m3t.get_keystroke() cmg.firenze.power_on() if reposition_flag: rot_mat = tr.Rz(hook_angle)*tr.Ry(math.radians(-90)) cmg.firenze.pose_robot(arm,rot_mat) hook_location = cmg.firenze.end_effector_pos(arm) g = cmg.reposition_robot(hook_location) cmg.firenze.go_cartesian(arm,g,rot_mat,speed=0.1) if search_hook_flag: hook_angle = math.radians(ha) surface_angle = math.radians(0.) p = np.matrix([0.45,-0.2,-0.23]).T if arm == 'left_arm': p[1,0] = p[1,0] * -1
def run_ik(step_delta, arm_name): pose = get_pose(arm_name) cmd = M3JointCmd() cmd.chain = [0] * 7 cmd.control_mode = [0] * 7 cmd.smoothing_mode = [0] * 7 for i in range(7): if arm_name == 'right_arm': cmd.chain[i] = (int(M3Chain.RIGHT_ARM)) elif arm_name == 'left_arm': cmd.chain[i] = (int(M3Chain.LEFT_ARM)) cmd.stiffness.append(stiffness) cmd.position.append(pose[i]) cmd.velocity.append(10.0) cmd.effort.append(0.0) cmd.control_mode[i] = (int(mab.JOINT_MODE_ROS_THETA_GC)) cmd.smoothing_mode[i] = (int(mas.SMOOTHING_MODE_SLEW)) cmd.chain_idx.append(i) cmd.header = Header(0, rospy.Time.now(), '0') humanoid_pub.publish(cmd) print 'Press any key to start IK demo.' k = m3t.get_keystroke() t = ik_thread(step_delta, arm_name) t.start() d = [0, 0, 0] while 1: print '-----------------------' print 'Delta: ', t.delta print '-----------------------' print 'q: quit' print '1: x+' print '2: x-' print '3: y+' print '4: y-' print '5: z+' print '6: z-' print 'space: step' k = m3t.get_keystroke() if k == 'q': t.delta_done = True return if k == '1': d[0] = d[0] + step_delta if k == '2': d[0] = d[0] - step_delta if k == '3': d[1] = d[1] + step_delta if k == '4': d[1] = d[1] - step_delta if k == '5': d[2] = d[2] + step_delta if k == '6': d[2] = d[2] - step_delta t.set_delta(d) print print 'Error: ', t.aerror print 'Target: ', t.target_pos
import time proxy = m3p.M3RtProxy() proxy.start() name_ec = m3t.user_select_components_interactive( proxy.get_available_components('m3actuator_ec'), single=True) if name_ec is None: exit() comp = m3f.create_component(name_ec) proxy.subscribe_status(comp) proxy.make_operational(name_ec) q_log = [] tq_log = [] print 'Ready to generate motion? Hit any key to start' m3t.get_keystroke() ts = time.time() try: while time.time() - ts > 5.0: proxy.step() q = comp.status.qei_on tq = comp.status.adc_torque time.sleep(0.25) print 'DT', time.time() - ts, 'Q', q, 'TQ', tq q_log.append(q) tq_log.append(tq) except (KeyboardInterrupt, EOFError): proxy.stop() poly, inv_poly = m3tc.get_polyfit_to_data(q_log, tq_log, n=1) print 'Poly', poly
proxy = m3p.M3RtProxy() proxy.start() name_ec=m3t.user_select_components_interactive(proxy.get_available_components('m3actuator_ec'),single=True) if name_ec is None: exit() comp=m3f.create_component(name_ec) proxy.subscribe_status(comp) proxy.make_operational(name_ec) q_log=[] tq_log=[] print 'Ready to generate motion? Hit any key to start' m3t.get_keystroke() ts=time.time() try: while time.time()-ts>5.0: proxy.step() q=comp.status.qei_on tq=comp.status.adc_torque time.sleep(0.25) print 'DT',time.time()-ts, 'Q',q,'TQ',tq q_log.append(q) tq_log.append(tq) except (KeyboardInterrupt,EOFError): proxy.stop() poly,inv_poly=m3tc.get_polyfit_to_data(q_log,tq_log,n=1) print 'Poly',poly
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 pull(self,hook_angle,force_threshold,use_utm=False,use_camera=False,strategy='line_neg_x', pull_loc=None, info_string=''): ''' force_threshold - max force at which to stop pulling. hook_angle - radians(0, -90, 90 etc.) 0 - horizontal, -pi/2 hook points up, +pi/2 hook points down use_utm - to take 3D scans or not. use_camera - to take pictures from the camera or not. strategy - 'line_neg_x': move eq point along -x axis. 'piecewise_linear': try and estimate circle and move along it. 'control_radial_force': try and keep the radial force constant 'control_radial_dist' pull_loc - 3x1 np matrix of location for pulling. If None then arm will go into gravity comp and user can show the location. info_string - string saved with key 'info' in the pkl. ''' if use_utm: self.firenze.step() armconfig1 = self.firenze.get_joint_angles('right_arm') plist1,slist1 = self.scan_3d() if use_camera: cam_plist1, cam_imlist1 = self.image_region() else: cam_plist1,cam_imlist1 = None,None rot_mat = tr.Rz(hook_angle-hook_3dprint_angle)*tr.Ry(math.radians(-90)) if pull_loc == None: self.pose_arm(hook_angle) pull_loc = self.firenze.end_effector_pos('right_arm') ut.save_pickle(pull_loc,'pull_loc_'+info_string+'_'+ut.formatted_time()+'.pkl') else: pt1 = copy.copy(pull_loc) pt1[0,0] = pt1[0,0]-0.1 print 'pt1:', pt1.A1 print 'pull_loc:', pull_loc.A1 self.firenze.go_cartesian('right_arm',pt1,rot_mat,speed=0.2) self.firenze.go_cartesian('right_arm',pull_loc,rot_mat,speed=0.07) print 'press ENTER to pull' k=m3t.get_keystroke() if k != '\r': return time_dict = {} time_dict['before_hook'] = time.time() print 'first getting a good hook' self.get_firm_hook(hook_angle) time.sleep(0.5) time_dict['before_pull'] = time.time() print 'pull begins' stiffness_scale = self.settings_r.stiffness_scale vec = tr.rotX(-hook_angle) * np.matrix([0.,0.05/stiffness_scale,0.]).T self.keep_hook_vec = vec self.hook_maintain_dist_plane = np.dot(vec.A1,np.array([0.,1.,0.])) self.eq_pt_cartesian = self.firenze.end_effector_pos('right_arm') + vec q_eq = self.firenze.IK('right_arm',self.eq_pt_cartesian,rot_mat) self.firenze.go_jointangles('right_arm',q_eq,speed=math.radians(30)) self.q_guess = q_eq # self.q_guess = self.firenze.get_joint_angles('right_arm') self.pull_trajectory = at.JointTrajectory() self.jt_torque_trajectory = at.JointTrajectory() self.eq_pt_trajectory = at.JointTrajectory() self.force_trajectory = at.ForceTrajectory() self.firenze.step() start_config = self.firenze.get_joint_angles('right_arm') self.eq_IK_rot_mat = rot_mat # for equi pt generators. self.eq_force_threshold = force_threshold self.hooked_location_moved = False # flag to indicate when the hooking location started moving. self.prev_force_mag = np.linalg.norm(self.firenze.get_wrist_force('right_arm')) self.eq_motion_vec = np.matrix([-1.,0.,0.]).T self.slip_count = 0 if strategy == 'line_neg_x': result = self.compliant_motion(self.equi_pt_generator_line,0.025) elif strategy == 'control_radial_force': self.cartesian_pts_list = [] self.piecewise_force_threshold = force_threshold self.rad_guess = 1.0 self.cx = 0.6 self.cy = -self.rad_guess self.start() # start the circle estimation thread result = self.compliant_motion(self.equi_pt_generator_control_radial_force,0.025) else: raise RuntimeError('unknown pull strategy: ', strategy) if result == 'slip: force step decrease' or result == 'danger of self collision': self.firenze.motors_off() print 'powered off the motors.' print 'Compliant motion result:', result print 'Original force threshold:', force_threshold print 'Adapted force threshold:', self.eq_force_threshold time_dict['after_pull'] = time.time() d = {'actual': self.pull_trajectory, 'eq_pt': self.eq_pt_trajectory, 'force': self.force_trajectory, 'torque': self.jt_torque_trajectory, 'stiffness': self.firenze.arm_settings['right_arm'], 'info': info_string, 'force_threshold': force_threshold, 'eq_force_threshold': self.eq_force_threshold, 'hook_angle':hook_angle, 'result':result,'strategy':strategy,'time_dict':time_dict} self.firenze.step() armconfig2 = self.firenze.get_joint_angles('right_arm') if use_utm: plist2,slist2 = self.scan_3d() d['start_config']=start_config d['armconfig1']=armconfig1 d['armconfig2']=armconfig2 d['l1'],d['l2']=0.,-0.055 d['scanlist1'],d['poslist1']=slist1,plist1 d['scanlist2'],d['poslist2']=slist2,plist2 d['cam_plist1']=cam_plist1 d['cam_imlist1']=cam_imlist1 ut.save_pickle(d,'pull_trajectories_'+d['info']+'_'+ut.formatted_time()+'.pkl')
f = file(fn, 'r') vias = yaml.safe_load(f.read()) print '-------------' print len(vias), " vias loaded." print '-------------' k = 0 bot.set_slew_rate('right_arm',[5]*7) bot.set_mode_theta_gc('right_arm') bot.set_motor_power_on() proxy.step() while k < len(vias): print "Hit <SPACE> to move to via ", k+1, " with positions: ", vias[k], " or <Q> to quit." key = m3t.get_keystroke() if key == ' ': t = vias[k] bot.set_theta_deg('right_arm', t) proxy.step() k += 1 if key == 'q': print 'Exiting..' break bot.set_mode_off('right_arm') bot.set_motor_power_off() proxy.step() proxy.stop()
def run_ik(step_delta, arm_name): pose=get_pose(arm_name) cmd = M3JointCmd() cmd.chain = [0]*7 cmd.control_mode = [0]*7 cmd.smoothing_mode = [0]*7 for i in range(7): if arm_name == 'right_arm': cmd.chain[i] = (int(M3Chain.RIGHT_ARM)) elif arm_name == 'left_arm': cmd.chain[i] = (int(M3Chain.LEFT_ARM)) cmd.stiffness.append(stiffness) cmd.position.append(pose[i]) cmd.velocity.append(10.0) cmd.effort.append(0.0) cmd.control_mode[i] = (int(mab.JOINT_MODE_ROS_THETA_GC)) cmd.smoothing_mode[i] = (int(mas.SMOOTHING_MODE_SLEW)) cmd.chain_idx.append(i) cmd.header = Header(0,rospy.Time.now(),'0') humanoid_pub.publish(cmd) print 'Press any key to start IK demo.' k=m3t.get_keystroke() t=ik_thread(step_delta, arm_name) t.start() d=[0,0,0] while 1: print '-----------------------' print 'Delta: ',t.delta print '-----------------------' print 'q: quit' print '1: x+' print '2: x-' print '3: y+' print '4: y-' print '5: z+' print '6: z-' print 'space: step' k=m3t.get_keystroke() if k=='q': t.delta_done=True return if k=='1': d[0]=d[0]+step_delta if k=='2': d[0]=d[0]-step_delta if k=='3': d[1]=d[1]+step_delta if k=='4': d[1]=d[1]-step_delta if k=='5': d[2]=d[2]+step_delta if k=='6': d[2]=d[2]-step_delta t.set_delta(d) print print 'Error: ',t.aerror print 'Target: ',t.target_pos
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=[]