def Loop(self): sign = lambda x: 1 if x > 0 else -1 if x < 0 else 0 rate = TRate(self.ctrl_rate) #Virtual offset: self.trg_offset = 0.0 while self.is_running: pos, vel, pwm = self.observer() if self.trg_offset != 0 and sign(self.trg_pos - pos) != sign( self.trg_offset): self.trg_pos = self.trg_pos + self.trg_offset self.trg_offset = 0.0 elif abs(vel) >= self.th_v: self.trg_pos = self.trg_pos + self.trg_offset self.trg_offset = 0.0 elif abs(self.trg_pos - pos) > self.th_p and abs( vel) < self.th_v and abs(pwm) < self.max_pwm: self.trg_offset = self.trg_offset + self.ostep * sign( self.trg_pos - pos) self.controller(int(self.trg_pos + self.trg_offset)) rate.sleep()
def TrajectoryController(self, joint_names, q_traj, t_traj, current, callback): assert (len(t_traj) > 0) assert (len(q_traj) == len(t_traj)) assert (len(joint_names) == len(q_traj[0])) dof = len(joint_names) #Revising trajectory (add an initial point). if t_traj[0] > 1.0e-3: q_traj = [self.Position(joint_names)] + q_traj t_traj = [0.0] + t_traj #Modeling the trajectory with spline. splines = [TCubicHermiteSpline() for d in xrange(dof)] for d in xrange(len(splines)): data_d = [[t, q[d]] for q, t in zip(q_traj, t_traj)] splines[d].Initialize(data_d, tan_method=splines[d].CARDINAL, c=0.0, m=0.0) rate = TRate(self.hz_traj_ctrl) t0 = time.time() while all(((time.time() - t0) < t_traj[-1], self.threads['TrajectoryController'][0])): t = time.time() - t0 #q= [splines[d].Evaluate(t) for d in xrange(dof)] q_dq = [splines[d].Evaluate(t, with_tan=True) for d in range(dof)] q = [q for q, _ in q_dq] dq = [dq for _, dq in q_dq] if callback is not None: if callback('loop_begin', t, q, dq) == False: break #print t, q if current is None: with self.port_locker: self.MoveTo( {jname: qj for jname, qj in zip(joint_names, q)}, blocking=False) else: with self.port_locker: self.MoveToC( { jname: (qj, ej) for jname, qj, ej in zip(joint_names, q, current) }, blocking=False) if callback is not None: callback('loop_end', None, None, None) rate.sleep() self.threads['TrajectoryController'][0] = False if callback is not None: callback('final', None, None, None)
def StateObserver(self, callback): rate = TRate(self.hz_state_obs) while self.threads['StateObserver'][0]: with self.port_locker: state = { 'stamp': time.time(), 'name': self.JointNames(), 'position': self.Position(self.JointNames()), 'velocity': self.Velocity(self.JointNames()), 'effort': self.PWM(self.JointNames()), #FIXME: PWM vs. Current } with self.state_locker: self.state = state if callback is not None: if callback(state) == False: break #print state['position'] rate.sleep() self.threads['StateObserver'][0] = False
print '-----' print 'Shutdown configuration:' dxl.PrintShutdown() print '-----' sys.stdout = stdout with open('/tmp/test_motion.dat', 'w') as fp: sys.stdout = fp #Move to initial position p_start = 100 dxl.MoveTo(p_start) time.sleep(0.5) print 0.0, p_start, dxl.Position(), dxl.Velocity() t_start = time.time() rate = TRate(100) #Move to a target position for t in np.mgrid[0:4 * math.pi:0.01]: p_trg = p_start + 500 * (0.5 - 0.5 * math.cos(t)) dxl.MoveTo(p_trg, blocking=False) rate.sleep() print time.time() - t_start, p_trg, dxl.Position(), dxl.Velocity() sys.stdout = stdout dxl.DisableTorque() dxl.Quit() print '''Test completed. Data files are saved into: /tmp/test_status.dat /tmp/test_motion.dat
def Loop(self): sign= lambda x: 1 if x>0 else -1 if x<0 else 0 rate= TRate(self.ctrl_rate) #Virtual offset: self.trg_offset= 0.0 while self.is_running: start = time.time() #モータの情報取得 # pos = [dxl[id].Position() for id in range(len(dxl))] pos, vel, pwm = self.observer() #取得した情報パブリッシュ dy_data = dynamixel_msg() dy_data.header.stamp = rospy.Time.now() dy_data.Pos = pos dy_data.Vel = vel pub.publish(dy_data) # #スティックX軸はモータ2(左指近位関節)を動かす # #曲げる方向を正とした # if self.DIRECTIONS[0] == 1: # self.trg_pos[1] = dxl[1].Position()-self.p # elif self.DIRECTIONS[0] == -1: # self.trg_pos[1] = dxl[1].Position()+self.p # #スティックY軸はモータ1(左指遠位関節)を動かす # if self.DIRECTIONS[1] == 1: # self.trg_pos[0] = dxl[0].Position()-self.p # elif self.DIRECTIONS[1] == -1: # self.trg_pos[0] = dxl[0].Position()+self.p #スティックX軸はスリ動作 if self.DIRECTIONS[0] == 1: rubbing.surface_pos = rubbing.surface_pos + self.v*0.1 elif self.DIRECTIONS[0] == -1: rubbing.surface_pos = rubbing.surface_pos - self.v*0.1 #スティックY軸は指缶距離の調整 if self.DIRECTIONS[1] == 1: rubbing.interval = rubbing.interval + 1 elif self.DIRECTIONS[1] == -1: rubbing.interval = rubbing.interval - 1 # #ボタン左右はモータ4(右指近位関節)を動かす # if self.DIRECTIONS[3] == 1: # self.trg_vel[3] = self.v # elif self.DIRECTIONS[3] == -1: # self.trg_vel[3] = -self.v # else: # self.trg_vel[3] = 0 # #ボタン上下はモータ3(右指遠位関節)を動かす # if self.DIRECTIONS[2] == 1: # self.trg_vel[2] = self.v # elif self.DIRECTIONS[2] == -1: # self.trg_vel[2] = -self.v # else: # self.trg_vel[2] = 0 # if self.DIRECTIONS[4] == 1: # self.trg_vel[0] = self.v # self.trg_vel[2] = self.v # elif self.DIRECTIONS[4] == -1: # self.trg_vel[0] = -self.v # self.trg_vel[2] = -self.v #目標位置の計算 if rubbing.running: a, b = rubbing.calculation_degree() pos_r, pos_l = rubbing.deg2pos(a, b) self.trg_pos[2], self.trg_pos[0] = pos_r, pos_l #キャリブレーションした位置をコンフィグファイルから取得 if self.offset_f: # rubbing.init_pos_r = pos[2] # rubbing.init_pos_l = pos[0] with open(file_name) as file: obj = yaml.safe_load(file) rubbing.init_pos_r = obj['init_pos_r'] rubbing.init_pos_l = obj['init_pos_l'] rubbing.running = 1 # self.controller(self.trg_pos) # for id in range(len(dxl)): # # if (self.trg_vel[id] > 0 and dxl[id].Position() > dxl[id].MAX_POSITION) or (self.trg_vel[id] < 0 and dxl[id].Position() < dxl[id].MIN_POSITION): # # self.trg_vel[id] = 0 # self.controller(int(self.trg_pos[id]), id) # print '' #目標位置送信 self.controller(self.trg_pos) print '' print rubbing.surface_pos, rubbing.interval update_fps = 1/(time.time() - start) print "fps: ", update_fps # for id in range(len(dxl)): # print pos[id], # print '' print '\033[3A',
def Loop(self): sign = lambda x: 1 if x > 0 else -1 if x < 0 else 0 rate = TRate(self.ctrl_rate) #Virtual offset: self.trg_offset = 0.0 while self.is_running: start = time.time() # pos = [dxl[id].Position() for id in range(len(dxl))] # pos = [dxl[0].Position(), dxl[1].Position(), dxl[2].Position(), dxl[3].Position()] pos, vel, pwm = self.observer() # all_data = dxl[0].ReadAllSync() # print(all_data) # #スティックX軸はモータ2(左指近位関節)を動かす # #曲げる方向を正とした # if self.DIRECTIONS[0] == 1: # self.trg_pos[1] = dxl[1].Position()-self.p # elif self.DIRECTIONS[0] == -1: # self.trg_pos[1] = dxl[1].Position()+self.p # #スティックY軸はモータ1(左指遠位関節)を動かす # if self.DIRECTIONS[1] == 1: # self.trg_pos[0] = dxl[0].Position()-self.p # elif self.DIRECTIONS[1] == -1: # self.trg_pos[0] = dxl[0].Position()+self.p if self.DIRECTIONS[0] == 1: rubbing.surface_pos = rubbing.surface_pos + self.v * 0.1 elif self.DIRECTIONS[0] == -1: rubbing.surface_pos = rubbing.surface_pos - self.v * 0.1 #スティックY軸はモータ1(左指遠位関節)を動かす if self.DIRECTIONS[1] == 1: rubbing.interval = rubbing.interval + 1 elif self.DIRECTIONS[1] == -1: rubbing.interval = rubbing.interval - 1 # #ボタン左右はモータ4(右指近位関節)を動かす # if self.DIRECTIONS[3] == 1: # self.trg_vel[3] = self.v # elif self.DIRECTIONS[3] == -1: # self.trg_vel[3] = -self.v # else: # self.trg_vel[3] = 0 # #ボタン上下はモータ3(右指遠位関節)を動かす # if self.DIRECTIONS[2] == 1: # self.trg_vel[2] = self.v # elif self.DIRECTIONS[2] == -1: # self.trg_vel[2] = -self.v # else: # self.trg_vel[2] = 0 # if self.DIRECTIONS[4] == 1: # self.trg_vel[0] = self.v # self.trg_vel[2] = self.v # elif self.DIRECTIONS[4] == -1: # self.trg_vel[0] = -self.v # self.trg_vel[2] = -self.v if rubbing.running: a, b = rubbing.calculation_degree() pos_r, pos_l = rubbing.deg2pos(a, b) self.trg_pos[2], self.trg_pos[0] = int(pos_r), int(pos_l) if self.offset_f: # rubbing.init_pos_r = pos[2] # rubbing.init_pos_l = pos[0] with open(file_name) as file: obj = yaml.safe_load(file) rubbing.init_pos_r = obj['init_pos_r'] rubbing.init_pos_l = obj['init_pos_l'] rubbing.running = 1 # self.controller(self.trg_pos) # for id in range(len(dxl)): # # if (self.trg_vel[id] > 0 and dxl[id].Position() > dxl[id].MAX_POSITION) or (self.trg_vel[id] < 0 and dxl[id].Position() < dxl[id].MIN_POSITION): # # self.trg_vel[id] = 0 # self.controller(int(self.trg_pos[id]), id) # print '' dummy_byte = [b'\x00\x00\x00\x00' for i in range(4)] # print(dummy_byte) # print(type(self.trg_pos[0])) self.controller(self.trg_pos) # for id in [0, 1, 2, 3]: # # if (self.trg_vel[id] > 0 and dxl[id].Position() > dxl[id].MAX_POSITION) or (self.trg_vel[id] < 0 and dxl[id].Position() < dxl[id].MIN_POSITION): # # self.trg_vel[id] = 0 # self.controller(int(self.trg_pos[id]), id) print '' print "fps: ", 1 / (time.time() - start) print rubbing.surface_pos, rubbing.interval print pos, vel, pwm # for id in range(len(dxl)): # print pos[id], # print '' print '\033[4A',
robot_hostname= 'ur3a' robot_state= GetRobotState(robot_hostname) port= 30003 #socketobj= socket.create_connection((robot_hostname, port)) socketobj= socket.socket(socket.AF_INET, socket.SOCK_STREAM) socketobj.connect((robot_hostname, port)) #robot_state= TRobotState() #buf= socketobj.recv(512) ##print buf #robot_state.Unpack(buf) #print robot_state.version.major_version, robot_state.version.minor_version rate= TRate(125) #UR receives velocities at 125 Hz. try: while True: vel= [0.0]*6 acc= 10.0 ver= robot_state.GetVersion() if ver>=3.3: cmd= "speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f, 0.008)\n" % ( vel[0],vel[1],vel[2],vel[3],vel[4],vel[5],acc) elif ver>=3.1: cmd= "speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f)\n" % ( vel[0],vel[1],vel[2],vel[3],vel[4],vel[5],acc) else: cmd= "speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f, 0.02)\n" % ( vel[0],vel[1],vel[2],vel[3],vel[4],vel[5],acc)
if __name__ == '__main__': import math, time, sys from rate_adjust import TRate #robot_hostname,is_e= 'ur3a',False robot_hostname, is_e = 'ur3ea', True print 'robot & version:', robot_hostname, GetURState( robot_hostname).GetVersion() res = raw_input('continue? > ') if len(res) > 0 and res[0] not in ('y', 'Y', ' '): sys.exit(0) velctrl = TURVelCtrl(robot_hostname) velctrl.Init() t0 = time.time() if not is_e: rate = TRate(125) #UR receives velocities at 125 Hz. else: rate = TRate(500) #UR receives velocities at 500 Hz. try: while True: #dq= [0.0]*6 t = time.time() - t0 dq = [0.08 * math.sin(math.pi * t)] * 6 acc = 10.0 velctrl.Step(dq, acc) rate.sleep() except KeyboardInterrupt: print 'Interrupted' finally:
def Loop(self): sign = lambda x: 1 if x > 0 else -1 if x < 0 else 0 #擦り動作用アレイ thick_array = self.Gen_thick_array() rate = TRate(self.ctrl_rate) #Virtual offset: self.trg_offset = 0.0 while self.is_running: start = time.time() #モータの情報取得 # pos = [dxl[id].Position() for id in range(len(dxl))] pos, vel, pwm, cur = self.observer() for i in range(4): self.trg_pos[i] = pos[i] #取得した情報をパブリッシュ dy_data = dynamixel_msg() dy_data.header.stamp = rospy.Time.now() dy_data.Pos = pos dy_data.Vel = vel dy_data.Pwm = pwm dy_data.Cur = cur data_pub.publish(dy_data) # #スティックX軸はモータ2(左指近位関節)を動かす # #曲げる方向を正とした # if self.DIRECTIONS[0] == 1: # self.trg_pos[1] = dxl[1].Position()-self.p # elif self.DIRECTIONS[0] == -1: # self.trg_pos[1] = dxl[1].Position()+self.p # #スティックY軸はモータ1(左指遠位関節)を動かす # if self.DIRECTIONS[1] == 1: # self.trg_pos[0] = dxl[0].Position()-self.p # elif self.DIRECTIONS[1] == -1: # self.trg_pos[0] = dxl[0].Position()+self.p #スティックX軸は擦り動作 if self.DIRECTIONS[0] == 1: rubbing.surface_pos = rubbing.surface_pos + self.v * 0.1 elif self.DIRECTIONS[0] == -1: rubbing.surface_pos = rubbing.surface_pos - self.v * 0.1 #スティックY軸は指缶距離調整 if self.DIRECTIONS[1] == 1: rubbing.interval = rubbing.interval - self.p * 0.1 elif self.DIRECTIONS[1] == -1: rubbing.interval = rubbing.interval + self.p * 0.1 #ボタン左右は両指先を同方向へ動かす if self.DIRECTIONS[3] == 1: rubbing.offset_r_dist += self.p rubbing.offset_l_dist += self.p elif self.DIRECTIONS[3] == -1: rubbing.offset_r_dist -= self.p rubbing.offset_l_dist -= self.p #ボタン上下は両指先を逆方向へ動かす if self.DIRECTIONS[2] == 1: rubbing.offset_r_dist -= self.p rubbing.offset_l_dist += self.p elif self.DIRECTIONS[2] == -1: rubbing.offset_r_dist += self.p rubbing.offset_l_dist -= self.p # if self.DIRECTIONS[4] == 1: # self.trg_vel[0] = self.v # self.trg_vel[2] = self.v # elif self.DIRECTIONS[4] == -1: # self.trg_vel[0] = -self.v # self.trg_vel[2] = -self.v #自動擦り動作用 if self.thick_f: if self.thick_i < len(thick_array): auto_surface_pos = thick_array[self.thick_i] self.thick_i += 1 rubbing.surface_pos = auto_surface_pos else: self.thick_i = 0 self.thick_f = 0 #目標位置の計算 if rubbing.running: rubbing.range_check() a, b = rubbing.calculation_degree() pos_r, pos_l = rubbing.deg2pos(a, b) pos_r_dist, pos_l_dist = rubbing.calculation_pos_dist() self.trg_pos[2], self.trg_pos[0] = int(pos_r), int(pos_l) self.trg_pos[3], self.trg_pos[1] = int(pos_r_dist), int( pos_l_dist) # 指先を平行に保つの開始 if self.offset_f: rubbing.surface_pos = 0 rubbing.running = 1 if self.calibration_f: # self.Calibration_initial_position() self.Manual_Calibration_initial_position() self.calibration_f = 0 # print(self.trg_pos) self.controller(self.trg_pos) # print '' # print rubbing.surface_pos, rubbing.interval update_fps = 1 / (time.time() - start) # print "fps: ", update_fps # # for id in range(len(dxl)): # # print pos[id], # # print '' # print '\033[3A', #各種パラメータをパブリッシュ dy_param = dynamixel_param_msg() dy_param.surface_pos = rubbing.surface_pos dy_param.interval = rubbing.interval dy_param.fps = update_fps dy_param.trg_pos = self.trg_pos param_pub.publish(dy_param)