# -*- python -*-

load(
    "@drake//tools/skylark:drake_cc.bzl",
    "drake_cc_googletest",
    "drake_cc_library",
    "drake_cc_package_library",
)
load("//tools/lint:lint.bzl", "add_lint_tests")

package(
    default_visibility = ["//visibility:public"],
)

drake_cc_package_library(
    name = "planner",
    visibility = ["//visibility:public"],
    deps = [
        ":constraint_relaxing_ik",
        ":differential_inverse_kinematics",
        ":differential_inverse_kinematics_integrator",
        ":robot_plan_interpolator",
    ],
)

drake_cc_library(
    name = "constraint_relaxing_ik",
    srcs = ["constraint_relaxing_ik.cc"],
    hdrs = ["constraint_relaxing_ik.h"],
    deps = [
        "//common/trajectories:piecewise_polynomial",
        "//multibody/inverse_kinematics",
        "//multibody/parsing",
        "//multibody/plant",
        "//solvers:solve",
    ],
)

drake_cc_library(
    name = "differential_inverse_kinematics",
    srcs = ["differential_inverse_kinematics.cc"],
    hdrs = ["differential_inverse_kinematics.h"],
    deps = [
        "//multibody/plant",
        "//solvers:mathematical_program",
        "//solvers:osqp_solver",
    ],
)

drake_cc_library(
    name = "differential_inverse_kinematics_integrator",
    srcs = ["differential_inverse_kinematics_integrator.cc"],
    hdrs = ["differential_inverse_kinematics_integrator.h"],
    deps = [
        ":differential_inverse_kinematics",
        "//systems/framework:leaf_system",
    ],
)

drake_cc_library(
    name = "robot_plan_interpolator",
    srcs = ["robot_plan_interpolator.cc"],
    hdrs = ["robot_plan_interpolator.h"],
    deps = [
        "//common/trajectories:piecewise_polynomial",
        "//lcmtypes:robot_plan",
        "//multibody/parsing",
        "//systems/framework:leaf_system",
    ],
)

# === test/ ===

drake_cc_googletest(
    name = "constraint_relaxing_ik_test",
    srcs = ["test/constraint_relaxing_ik_test.cc"],
    data = [
        "//manipulation/models/iiwa_description:models",
    ],
    tags = ["snopt"],
    deps = [
        ":constraint_relaxing_ik",
        "//common:find_resource",
    ],
)

drake_cc_googletest(
    name = "differential_inverse_kinematics_test",
    data = [
        "//manipulation/models/iiwa_description:models",
    ],
    deps = [
        ":differential_inverse_kinematics",
        "//common/test_utilities:eigen_matrix_compare",
        "//manipulation/kuka_iiwa:iiwa_constants",
        "//multibody/parsing",
    ],
)

drake_cc_googletest(
    name = "differential_inverse_kinematics_integrator_test",
    data = [
        "//manipulation/models/iiwa_description:models",
    ],
    deps = [
        ":differential_inverse_kinematics_integrator",
        "//common/test_utilities:eigen_matrix_compare",
        "//manipulation/kuka_iiwa:iiwa_constants",
        "//multibody/parsing",
    ],
)

drake_cc_googletest(
    name = "robot_plan_interpolator_test",
    data = [
        "//examples/kuka_iiwa_arm:models",
        "//manipulation/models/iiwa_description:models",
    ],
    deps = [
        ":robot_plan_interpolator",
        "//common:find_resource",
        "//systems/framework",
    ],
)

add_lint_tests()
