Beispiel #1
0
    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
Beispiel #2
0
    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()