def get_jacabian_from_joint(self, urdfname, jointq, flag):
#robot = URDF.from_xml_file("/data/ros/ur_ws/src/universal_robot/ur_description/urdf/ur5.urdf")
robot = URDF.from_xml_file(urdfname)
tree = kdl_tree_from_urdf_model(robot)
# print tree.getNrOfSegments()
chain = tree.getChain("base_link", "ee_link")
# print chain.getNrOfJoints()
# forwawrd kinematics
kdl_kin = KDLKinematics(robot, "base_link", "ee_link")
q = jointq
#q = [0, 0, 1, 0, 1, 0]
pose = kdl_kin.forward(
q) # forward kinematics (returns homogeneous 4x4 matrix)
# print pose
#print list(pose)
q0 = Kinematic()
if flag == 1:
q_ik = q0.best_sol_for_other_py([1.] * 6, 0, q0.Forward(q))
else:
q_ik = kdl_kin.inverse(pose) # inverse kinematics
# print "----------iverse-------------------\n", q_ik
if q_ik is not None:
pose_sol = kdl_kin.forward(q_ik) # should equal pose
print "------------------forward ------------------\n", pose_sol
J = kdl_kin.jacobian(q)
#print 'J:', J
return J, pose
def get_camera_to_base(self,q_now,ar_pose):
T = Kinematic()
Tbe = T.Forward(q_now)
Tbe = np.array(Tbe).reshape((4, 4))
print "Tbe", Tbe
print "posdict", ar_pose
Tce = q2t(ar_pose)
Tce = np.array(Tce).reshape((4, 4))
print "Tce", Tce
Tec = tr2transports_1(Tce)
print "TecdotTce",np.dot(Tec, Tce)
print "Tec", Tec
Tbc = np.dot(Tbe, Tec)
print "Tbc-----------\n", Tbc
return Tbc
def get_inverse_to_box(self, pub_now, x_length, y_length, depth):
q0 = Kinematic()
weights = [1.] * 6
T0 = q0.Forward(pub_now)
# print(T0)
temp = []
for i in range(len(T0)):
if i == 3:
temp.append(T0[3] + x_length)
elif i == 7:
temp.append(T0[7] + y_length)
elif i == 11:
temp.append(T0[11] + depth)
else:
temp.append(T0[i])
# print("temp",temp)
kk = q0.best_sol(weights, pub_now, temp)
return kk
def main():
urdfname="/data/ros/ur_ws_yue/src/ur5_planning/urdf/ur5.urdf"
filename="/data/ros/ur_ws_yue/src/ur5_planning/yaml/cam_500_industry.yaml"
# urdfname="/data/ros/ur_ws/src/universal_robot/ur_description/urdf/ur5.urdf"
# desiruv=[]
# desiruv=[[325,254]]
lambda1=3.666666
detat=0.05
z=0.25
ace=50
vel=0.1
urt=0
ratet=10
p0=VisonControl(filename,lambda1,urdfname)
# print p0.get_cam_data()
#2get uv from ar
ar_reader = arReader()
ar_sub = rospy.Subscriber("/ar_pose_marker", AlvarMarkers, ar_reader.callback)
ur_reader = Urposition()
ur_sub = rospy.Subscriber("/joint_states", JointState, ur_reader.callback)
u_error_pub = rospy.Publisher("/feature_u_error", Float64, queue_size=10)
v_error_pub = rospy.Publisher("/feature_v_error", Float64, queue_size=10)
z_depth_pub = rospy.Publisher("/camera_depth", Float64, queue_size=10)
now_uv_pub = rospy.Publisher("/nowuv_info", uv, queue_size=10)
#give q to ur3
ur_pub = rospy.Publisher("/ur_driver/URScript", String, queue_size=10)
#get uvlist for circle
uv_get=UVRead()
uv_sub=rospy.Subscriber("/camera_uv/uvlist", uv,uv_get.callback)
T = Kinematic()
rate = rospy.Rate(ratet)
while not rospy.is_shutdown():
# desiruv=[]
# uvlist=[]
# try:
# pos_dict = ar_reader.ave_pos_dict
# if len(pos_dict)==0:
# print "##############################################################"
# print "wait ar pose ok .....",time.sleep(3)
# print "##############################################################"
# print "##############################################################"
# print "##############################################################"
# print "pos_dict\n",pos_dict[0][:3]
# print p0.get_uv_from_ar(pos_dict[0][:3])
# zz=pos_dict[0][2]
# z_depth_pub.publish(zz)
# print "depth info z\n",zz
# except:
# continue
# uvlist.append(p0.get_uv_from_ar(pos_dict[0][:3])[:2])
# print "##############################################################"
# print "uv-list------\n",uvlist
# now_uv_pub.publish(uvlist[0])
# print "##############################################################"
# print "###########################################################"
# if len(uv_get.uvlist_buf)==0:
# print "wait desire data sub---\n"
# time.sleep(4)
# if len(uv_get.uvlist_buf)!=0:
# print "desire uv------\n", uv_get.uvlist_buf[-1]
# desiruv.append(uv_get.uvlist_buf[-1])
# #get error
# print "##############################################################"
# feature_error=p0.get_feature_error(desiruv,uvlist[0])
# print "feature error\n",feature_error
# print feature_error.tolist()
# u_error_pub.publish(feature_error.tolist()[0][0])
# v_error_pub.publish(feature_error.tolist()[0][1])
# print "##############################################################"
# #get visual jacbian
# #print "visual jacbian"
# #print p0.vis2jac([612,412],z)
# print "##############################################################"
# #get cam speed vdot
# # print "camera vdot\n",p0.get_cam_vdot_wz(uvlist,z,desiruv,uvlist[0])
# print "##############################################################"
# q_now=ur_reader.ave_ur_pose
# #get joint speed in ee frame
# print "##############################################################"
# print "q_now\n", q_now
# print "joint speed\n",p0.get_joint_speed(uvlist,z,desiruv,uvlist[0],q_now)
# print "##############################################################"
# print "deta joint angular---"
# detaangular=p0.get_deta_joint_angular(detat,uvlist, z, desiruv, uvlist[0], q_now)
# print detaangular
# print "##############################################################"
# print "joint angular----"
# q_pub_now=p0.get_joint_angular(q_now,detaangular)
# print q_pub_now
# print "##############################################################"
# print "move ur base the servo system----"
# print "q_now\n", q_now
# print "q_pub_now\n",q_pub_now
# ss = "movej([" + str(q_pub_now[0]) + "," + str(q_pub_now[1]) + "," + str(q_pub_now[2]) + "," + str(
# q_pub_now[3]) + "," + str(q_pub_now[4]) + "," + str(q_pub_now[5]) + "]," + "a=" + str(ace) + "," + "v=" + str(
# vel) + "," + "t=" + str(urt) + ")"
# print ss
# ur_pub.publish(ss)
# rate.sleep()
# else:
# continue
pos_dict = ar_reader.ave_pos_dict
if len(pos_dict)!=0:
q_now = ur_reader.ave_ur_pose
Tbe = T.Forward(q_now)
Tbe = np.array(Tbe).reshape((4, 4))
print "Tbe", Tbe
print "posdict", pos_dict[0]
Tce = q2t(pos_dict[0])
Tce = np.array(Tce).reshape((4, 4))
print "Tce", Tce
Tec = tr2transports(Tce)
print np.dot(Tec, Tce)
print "Tec", Tec
Tbc = np.dot(Tbe, Tec)
print "Tbc-----------\n", Tbc
rate.sleep()
