Python URDF.load_xml_file方法代码示例

# 需要导入模块: from urdf_parser_py.urdf import URDF [as 别名]
# 或者: from urdf_parser_py.urdf.URDF import load_xml_file [as 别名]
def main():
    import sys
    def usage():
        print("Tests for kdl_parser:\n")
        print("kdl_parser <urdf file>")
        print("\tLoad the URDF from file.")
        print("kdl_parser")
        print("\tLoad the URDF from the parameter server.")
        sys.exit(1)

    if len(sys.argv) > 2:
        usage()
    if len(sys.argv) == 2 and (sys.argv[1] == "-h" or sys.argv[1] == "--help"):
        usage()
    if (len(sys.argv) == 1):
        robot = URDF.load_from_parameter_server(verbose=False)
    else:
        robot = URDF.load_xml_file(sys.argv[1], verbose=False)
    tree = kdl_tree_from_urdf_model(robot)
    num_non_fixed_joints = 0
    for j in robot.joints:
        if robot.joints[j].joint_type != 'fixed':
            num_non_fixed_joints += 1
    print "URDF non-fixed joints: %d;" % num_non_fixed_joints,
    print "KDL joints: %d" % tree.getNrOfJoints()
    print "URDF joints: %d; KDL segments: %d" %(len(robot.joints),
                                                tree.getNrofSegments())
    import random
    base_link = robot.get_root()
    end_link = robot.links.keys()[random.randint(0, len(robot.links)-1)]
    chain = tree.getChain(base_link, end_link)
    print "Root link: %s; Random end link: %s" % (base_link, end_link)
    for i in range(chain.getNrOfSegments()):
        print chain.getSegment(i).getName()

# 需要导入模块: from urdf_parser_py.urdf import URDF [as 别名]
# 或者: from urdf_parser_py.urdf.URDF import load_xml_file [as 别名]
def main():
    rospy.init_node("joint_kinematics")
    import sys
    def usage():
        print("Tests for kdl_parser:\n")
        print("kdl_parser <urdf file>")
        print("\tLoad the URDF from file.")
        print("kdl_parser")
        print("\tLoad the URDF from the parameter server.")
        sys.exit(1)

    if len(sys.argv) > 2:
        usage()
    if len(sys.argv) == 2 and (sys.argv[1] == "-h" or sys.argv[1] == "--help"):
        usage()
    if (len(sys.argv) == 1):
        robot = URDF.load_from_parameter_server(verbose=False)
    else:
        robot = URDF.load_xml_file(sys.argv[1], verbose=False)

    if True:
        import random
        base_link = robot.get_root()
        end_link = robot.links.keys()[random.randint(0, len(robot.links)-1)]
        print "Root link: %s; Random end link: %s" % (base_link, end_link)
        js_kin = JointKinematics(robot, base_link, end_link)
        print "Joint angles:", js_kin.get_joint_angles()
        print "Joint angles (wrapped):", js_kin.get_joint_angles(True)
        print "Joint velocities:", js_kin.get_joint_velocities()
        print "Joint efforts:", js_kin.get_joint_efforts()
        print "Jacobian:", js_kin.jacobian()
        kdl_pose = js_kin.forward()
        print "FK:", kdl_pose
        print "End effector force:", js_kin.end_effector_force()

        if True:
            import tf 
            from hrl_geom.pose_converter import PoseConv
            tf_list = tf.TransformListener()
            rospy.sleep(1)
            t = tf_list.getLatestCommonTime(base_link, end_link)
            tf_pose = PoseConv.to_homo_mat(tf_list.lookupTransform(base_link, end_link, t))
            print "Error with TF:", np.linalg.norm(kdl_pose - tf_pose)

# 需要导入模块: from urdf_parser_py.urdf import URDF [as 别名]
# 或者: from urdf_parser_py.urdf.URDF import load_xml_file [as 别名]
def create_ep_arm(arm_side, arm_type=EPArmBase, urdf_filename=None, **args):
    if urdf_filename is None:
        robot = URDF.load_from_parameter_server(verbose=False)
    else:
        robot = URDF.load_xml_file(urdf_filename, verbose=False)
    return arm_type(arm_side, robot, **args)

# 需要导入模块: from urdf_parser_py.urdf import URDF [as 别名]
# 或者: from urdf_parser_py.urdf.URDF import load_xml_file [as 别名]
def create_joint_kin(base_link, end_link, urdf_filename=None, timeout=1.):
    if urdf_filename is None:
        robot = URDF.load_from_parameter_server(verbose=False)
    else:
        robot = URDF.load_xml_file(urdf_filename, verbose=False)
    return JointKinematics(robot, base_link, end_link, timeout=timeout)

# 需要导入模块: from urdf_parser_py.urdf import URDF [as 别名]
# 或者: from urdf_parser_py.urdf.URDF import load_xml_file [as 别名]
def create_kdl_kin(base_link, end_link, urdf_filename=None):
    if urdf_filename is None:
        robot = URDF.load_from_parameter_server(verbose=False)
    else:
        robot = URDF.load_xml_file(urdf_filename, verbose=False)
    return KDLKinematics(robot, base_link, end_link)

# 需要导入模块: from urdf_parser_py.urdf import URDF [as 别名]
# 或者: from urdf_parser_py.urdf.URDF import load_xml_file [as 别名]
def main():
    import sys

    def usage():
        print("Tests for kdl_parser:\n")
        print("kdl_parser <urdf file>")
        print("\tLoad the URDF from file.")
        print("kdl_parser")
        print("\tLoad the URDF from the parameter server.")
        sys.exit(1)

    if len(sys.argv) > 2:
        usage()
    if len(sys.argv) == 2 and (sys.argv[1] == "-h" or sys.argv[1] == "--help"):
        usage()
    if (len(sys.argv) == 1):
        robot = URDF.load_from_parameter_server(verbose=False)
    else:
        robot = URDF.load_xml_file(sys.argv[1], verbose=False)

    if True:
        import random
        base_link = robot.get_root()
        end_link = robot.links.keys()[random.randint(0, len(robot.links)-1)]
        print "Root link: %s; Random end link: %s" % (base_link, end_link)
        kdl_kin = KDLKinematics(robot, base_link, end_link)
        q = kdl_kin.random_joint_angles()
        print "Random angles:", q
        pose = kdl_kin.forward(q)
        print "FK:", pose
        q_new = kdl_kin.inverse(pose)
        print "IK (not necessarily the same):", q_new
        if q_new is not None:
            pose_new = kdl_kin.forward(q_new)
            print "FK on IK:", pose_new
            print "Error:", np.linalg.norm(pose_new * pose**-1 - np.mat(np.eye(4)))
        else:
            print "IK failure"
        J = kdl_kin.jacobian(q)
        print "Jacobian:", J
        M = kdl_kin.inertia(q)
        print "Inertia matrix:", M
        if False:
            M_cart = kdl_kin.cart_inertia(q)
            print "Cartesian inertia matrix:", M_cart

    if True:
        rospy.init_node("kdl_kinematics")
        num_times = 20
        while not rospy.is_shutdown() and num_times > 0:
            base_link = robot.get_root()
            end_link = robot.links.keys()[random.randint(0, len(robot.links)-1)]
            print "Root link: %s; Random end link: %s" % (base_link, end_link)
            kdl_kin = KDLKinematics(robot, base_link, end_link)
            q = kdl_kin.random_joint_angles()
            pose = kdl_kin.forward(q)
            q_guess = kdl_kin.random_joint_angles()
            q_new = kdl_kin.inverse(pose, q_guess)
            if q_new is None:
                print "Bad IK, trying search..."
                q_search = kdl_kin.inverse_search(pose)
                pose_search = kdl_kin.forward(q_search)
                print "Result error:", np.linalg.norm(pose_search * pose**-1 - np.mat(np.eye(4)))
            num_times -= 1