def main():
try:
print "============ 按’Enter’连接到规划组....."
raw_input()
armc_moveit = amp.MoveGroupPythonIntefaceArmc()
##运动到初始位置
print "============ 按’Enter’运动到初始位置....."
raw_input()
q_init = np.array([0, -30, 0, 90, 0, 30, 0]) * (np.pi / 180)
armc_moveit.go_to_joint_state(q_init)
#规划圆轨迹，将位置点存如数组中
print "============ 按’Enter’将末端位置点存入....."
raw_input()
rc = 100
[qr_init, X0_e, T] = enp.circlePoint_armc(rc)
##直线轨迹测试
#l = 100
#[qr_init, X0_e, T] = enp.multipointLine_armc(l)
#运动到一个点
#armc_moveit.go_to_pose_goal()
#将点加入轨迹列表中
wpose = Pose()
waypoints = []
for i in range(len(X0_e[0, :])):
quaternion = tr.quaternion_from_euler(X0_e[3, i], X0_e[4, i],
X0_e[5, i])
wpose.position.x = X0_e[0, i]
wpose.position.y = X0_e[1, i]
wpose.position.z = X0_e[2, i]
wpose.orientation.x = quaternion[0]
wpose.orientation.y = quaternion[1]
wpose.orientation.z = quaternion[2]
wpose.orientation.w = quaternion[3]
waypoints.append(copy.deepcopy(wpose))
#计算规划轨迹
print "============ 按’Enter’计算圆轨迹点位姿....."
raw_input()
[plan, fraction_] = armc_moveit.compute_cartesian_path(waypoints)
#显示轨迹
print "============ 按’Enter’显示末端轨迹点....."
raw_input()
armc_moveit.display_trajectory(plan)
#执行轨迹
print "============ 按’Enter’执行规划数据....."
raw_input()
armc_moveit.execute_plan(plan)
except rospy.ROSInterruptException:
return
except KeyboardInterrupt:
return
def parse(self,filename):
inputfile = open(filename, 'r')
firstline = inputfile.readline()
param = firstline[:-1].split(' ')
num_videos, num_endpoints, num_descriptions, num_caches, capacity = int(param[0]), int(param[1]), int(param[2]), int(param[3]), int(param[4])
videoline = inputfile.readline()
videos = videoline[:-1].split(' ')
for size in videos:
vid = Video(size)
self.videoList.append(vid)
for i in range(num_caches):
cache = Cache(capacity)
self.cacheList.append(cache)
for endpoint in num_endpoints:
ep_line = inputfile.readline()[:-1]
ep_line = ep_line.split(' ')
datacenter_latency = ep_line[0]
cache_count = ep_line[1]
ep = EndPoint(datacenter_latency)
for cache in cache_count:
cache_line = inputfile.readline()[:-1]
cache_line = cache_line.split(' ')
cache_id = cache_line[0]
cache_latency = cache_line[1]
ep.addCache(cache_id, cache_latency)
self.endPointList.append(ep)
for i in range(num_descriptions):
request_line = inputfile.readline()[:-1]
request_line = request_line.split(' ')
id_vid, id_ep, num= int(request_line[0]), int(request_line[1]), int(request_line[2])
req = Request(id_vid,id_ep,num)
self.requestList.append(req)
def end_point_plan_test():
##圆轨迹测试
rc = 0.1
[qr_init, X0_e, T] = enp.circlePoint_armc(rc)
print len(X0_e[:, 1])
##直线轨迹测试
#l = 100
#[qr_init,X0_e,T] = enp.multipointLine_armc(l)
#时间变量
k = len(X0_e[:, 0])
t = np.linspace(0, T * (k - 1), k)
#单变量随时间变化绘图
for i in range(6):
i_string = "Xe " + str(i + 1) + "plot"
MyPlot.plot2d(t, X0_e[:, i], i_string)
#末端轨迹三维图
MyPlot.plot3d(X0_e[:, 0], X0_e[:, 1], X0_e[:, 2], "3D plot")
def multipoint_plan_test():
##圆轨迹测试
rc = 0.1
[qr_init, X0_e, T] = enp.circlePoint_armc(rc)
##直线轨迹测试
#l = 0.3
#[qr_init,X0_e,T] = enp.multipointLine_armc(l)
n = len(qr_init)
#速度级求逆函数测试测试(规定每行为一个数据点）
[qq, qv, qa] = pap.multipoint_plan_position(qr_init, X0_e, T)
#时间变量
k = len(qq[:, 0])
t = np.linspace(0, T * (k - 1) / 10.0, k)
#插值部分测试
#[s,vel,acc]=pap.spline1(tq,q[:,0],0.01,0,0)
#i_string = "s " + "plot"
#MyPlot.plot2d(t,s, i_string)
#关节空间单变量随时间绘图
for i in range(n):
i_string = "qq " + str(i + 1) + "plot"
MyPlot.plot2d(t, qq[:, i], i_string)
#正运动学求解末端位位置
Xe = np.zeros([k, 6])
for i in range(k):
T0_e = kin.fkine(theta0 + qq[i, :], alpha, a, d)
Xe[i, 0:3] = T0_e[0:3, 3]
Xe[i, 3:6] = T0_e[0:3, 2]
#笛卡尔单变量随时间绘图
for i in range(6):
i_string = "Xe " + str(i + 1) + "plot"
MyPlot.plot2d(t, Xe[:, i], i_string)
#末端轨迹三维图
xx = np.around(Xe[:, 0], decimals=6)
yy = np.around(Xe[:, 1], decimals=6)
zz = np.around(Xe[:, 2], decimals=6)
MyPlot.plot3d(xx, yy, zz, "3D plot")
def line_plan_test():
##圆轨迹测试
#rc = 0.1
#[qr_init,X0_e,T] = enp.circlePoint_armc(rc)
##直线轨迹测试
l = 0.3
[qr_init, X0_e, T] = enp.multipointLine_armc(l)
n = len(qr_init)
#从家点运动到初始位置
[qq1, qv1, qa1] = jp.home_to_init(qr_init)
k1 = len(qq1[0, :])
#停顿点
qq2 = jp.keep_pos(400, qq1[:, -1])
k2 = len(qq2[0, :])
#位置级求逆函数测试测试
[qq3, qv3, qa3] = pap.multipoint_plan_position(qr_init, X0_e, T)
k3 = len(qq3[0, :])
#停顿点
qq4 = jp.keep_pos(400, qq3[:, -1])
k4 = len(qq4[0, :])
#规划完毕，原路返回
k = 2 * (k1 + k2 + k3) + k4
qq = np.zeros([n, k])
#合成完整路径
kk1 = k1
kk2 = k1 + k2
kk3 = k1 + k2 + k3
kk4 = k1 + k2 + k3 + k4
kk5 = k1 + k2 + 2 * k3 + k4
kk6 = k1 + 2 * k2 + 2 * k3 + k4
kk7 = 2 * (k1 + k2 + k3) + k4
qq[:, 0:kk1] = qq1
qq[:, kk1:kk2] = qq2
qq[:, kk2:kk3] = qq3
qq[:, kk3:kk4] = qq4
qq[:, kk4:kk5] = qq3[:, ::-1]
qq[:, kk5:kk6] = qq2[:, ::-1]
qq[:, kk6:kk7] = qq1[:, ::-1]
#关节点写入文档
parent_path = os.path.abspath('..')
file_name = "data/position.txt"
path = os.path.join(parent_path, file_name)
FileOpen.write(qq.T, path)
#时间变量
k = len(qq[0, :])
n = len(qq[:, 0])
t = np.linspace(0, T * (k - 1), k)
#关节空间单变量随时间绘图
for i in range(n):
i_string = "qq " + str(i + 1) + "plot"
MyPlot.plot2d(t, qq[i, :], i_string)
#正运动学求解末端位位置
Xe = np.zeros([6, k])
for i in range(k):
T0_e = kin.fkine(theta0 + qq[:, i], alpha, a, d)
Xe[0:3, i] = T0_e[0:3, 3]
Xe[3:6, i] = T0_e[0:3, 1]
#笛卡尔单变量随时间绘图
for i in range(6):
i_string = "Xe " + str(i + 1) + "plot"
MyPlot.plot2d(t, Xe[i, :], i_string)
#末端轨迹三维图
xx = np.around(Xe[0, :], decimals=6)
yy = np.around(Xe[1, :], decimals=6)
zz = np.around(Xe[2, :], decimals=6)
MyPlot.plot3d(xx, yy, zz, "3D plot")
def circle_plan_demo():
##圆轨迹测试
rc = 0.1
[qr_init, X0_e, T] = enp.circlePoint_armc(rc)
n = len(qr_init)
#位置级求逆获取圆的轨迹,3801个点38秒
[qq3, qv3, qa3] = pap.multipoint_plan_position(qr_init, X0_e, T)
#[qq3,qv3,qa3] = pap.multipoint_plan_position_aa(qr_init,X0_e,T)
k3 = len(qq3[:, 0])
#从家点运动到初始位置,此处10秒1001个点
[qq1, qv1, qa1] = jp.home_to_init(qq3[0, :])
k1 = len(qq1[:, 0])
#停顿点1，停顿时间4秒，398个点
qq2 = jp.keep_pos(398, qq1[-1, :])
k2 = len(qq2[:, 0])
#停顿点2,停顿4秒，398个点
qq4 = jp.keep_pos(398, qq3[-1, :])
k4 = len(qq4[:, 0])
#规划完毕，返回家点
[qq5, qv5, qa5] = jp.go_to_home(qq4[-1, :])
k5 = len(qq5[:, 0])
#合成完整路径
kk1 = k1
kk2 = k1 + k2
kk3 = kk2 + k3
kk4 = kk3 + k4
kk5 = kk4 + k5
k = kk5
qq = np.zeros([k, n])
qq[0:kk1, :] = qq1
qq[kk1:kk2, :] = qq2
qq[kk2:kk3, :] = qq3
qq[kk3:kk4, :] = qq4
qq[kk4:kk5, :] = qq5
#关节点写入文档
parent_path = os.path.abspath('..')
#file_name = "data/circular_position.txt"
file_name = "data/circular_position.txt"
path = os.path.join(parent_path, file_name)
FileOpen.write(qq, path)
#时间变量
k = len(qq[:, 0])
n = len(qq[0, :])
t = np.linspace(0, T * (k - 1), k)
#关节空间单变量随时间绘图
for i in range(n):
i_string = "qq " + str(i + 1) + "plot"
MyPlot.plot2d(t, qq[:, i], i_string)
#正运动学求解末端位位置
Xe = np.zeros([k, 6])
for i in range(k):
T0_e = kin.fkine(theta0 + qq[i, :], alpha, a, d)
Xe[i, 0:3] = T0_e[0:3, 3]
Xe[i, 3:6] = T0_e[0:3, 2]
#笛卡尔单变量随时间绘图
for i in range(6):
i_string = "Xe " + str(i + 1) + "plot"
MyPlot.plot2d(t, Xe[:, i], i_string)
#末端轨迹三维图
xx = np.around(Xe[:, 0], decimals=6)
yy = np.around(Xe[:, 1], decimals=6)
zz = np.around(Xe[:, 2], decimals=6)
MyPlot.plot3d(xx, yy, zz, "3D plot")
#ros相关模块
import rospy
from std_msgs.msg import String
from sensor_msgs.msg import JointState
#获得DH参数
theta0 = DH_0[:, 0]
alpha = DH_0[:, 1]
a = DH_0[:, 2]
d = DH_0[:, 3]
#=================规划轨迹测试模块=================#
##圆轨迹测试
rc = 100
[qr_init, X0_e, T] = enp.circlePoint_armc(rc)
##直线轨迹测试
#l = 100
#[qr_init,X0_e,T] = enp.multipointLine_armc(l)
#位置级求逆函数测试测试
[qq, qv, qa] = pap.multipoint_plan_position(qr_init, X0_e, T)
def talker(Q):
'''
param Q:输入关节角度
return:关节角度
'''
pub = rospy.Publisher('joint_states', JointState, queue_size=10)
"""
A client's connection to the server.
This module contains two components: a singleton called 'connection' and a class called 'ConnectionListener'.
'connection' is a singleton instantiation of an EndPoint which will be connected to the server at the other end. It's a singleton because each client should only need one of these in most multiplayer scenarios. (If a client needs more than one connection to the server, a more complex architecture can be built out of instantiated EndPoint()s.) The connection is based on Python's asyncore and so it should have it's polling loop run periodically, probably once per gameloop. This just means putting "from Connection import connection; connection.Pump()" somewhere in your top level gameloop.
Subclass ConnectionListener in order to have an object that will receive network events. For example, you might have a GUI element which is a label saying how many players there are online. You would declare it like 'class NumPlayersLabel(ConnectionListener, ...):' Later you'd instantitate it 'n = NumPlayersLabel()' and then somewhere in your loop you'd have 'n.Pump()' which asks the connection singleton if there are any new messages from the network, and calls the 'Network_' callbacks for each bit of new data from the server. So you'd implement a method like "def Network_players(self, data):" which would be called whenever a message from the server arrived which looked like {"action": "players", "number": 5}.
"""
from EndPoint import *
connection = EndPoint()
class ConnectionListener:
"""
Looks at incoming data and calls "Network_" methods in self, based on what messages come in.
Subclass this to have your own classes monitor incoming network messages.
For example, a method called "Network_players(self, data)" will be called when a message arrives like:
{"action": "players", "number": 5, ....}
"""
def Connect(self, *args, **kwargs):
connection.DoConnect(*args, **kwargs)
# check for connection errors:
self.Pump()
def Pump(self):
for data in connection.GetQueue():
[
getattr(self, n)(data)
for n in ("Network_" + data['action'], "Network")