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軸はモータ２（左指近位関節）を動かす
# #曲げる方向を正とした
# 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軸はモータ１（左指遠位関節）を動かす
# 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軸はモータ２（左指近位関節）を動かす
# #曲げる方向を正とした
# 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軸はモータ１（左指遠位関節）を動かす
# 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軸はモータ１（左指遠位関節）を動かす
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軸はモータ２（左指近位関節）を動かす
# #曲げる方向を正とした
# 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軸はモータ１（左指遠位関節）を動かす
# 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)