diff --git a/.github/workflows/focal_noetic.yml b/.github/workflows/focal_noetic.yml index 61cf21e4..20d75fa7 100644 --- a/.github/workflows/focal_noetic.yml +++ b/.github/workflows/focal_noetic.yml @@ -17,6 +17,7 @@ env: REGISTRY: ghcr.io IMAGE_NAME: ${{ github.repository }} ROS_DISTRO: noetic + PREFIX: ${{ github.repository }}_ jobs: ci: @@ -56,9 +57,11 @@ jobs: UPSTREAM_WORKSPACE: dependencies.repos PREFIX: ${{ github.repository }}_ CMAKE_ARGS: '-DENABLE_TESTING=ON -DENABLE_RUN_TESTING=OFF' - BEFORE_RUN_TARGET_TEST_EMBED: 'ici_with_unset_variables source $BASEDIR/${PREFIX}target_ws/install/setup.bash' DOCKER_IMAGE: 'ros:${{ env.ROS_DISTRO }}' DOCKER_COMMIT: ${{ steps.meta.outputs.tags }} + AFTER_INSTALL_TARGET_DEPENDENCIES: 'python3 -m pip install -r reach/requirements.txt -qq' + BEFORE_RUN_TARGET_TEST_EMBED: 'ici_with_unset_variables source $BASEDIR/${PREFIX}target_ws/install/setup.bash' + AFTER_RUN_TARGET_TEST: 'rosenv python3 -m pytest -v' - name: Push post-build Docker if: ${{ github.ref == 'refs/heads/master' || github.event_name == 'release' }} diff --git a/reach/CMakeLists.txt b/reach/CMakeLists.txt index d27ea010..c96e7eee 100644 --- a/reach/CMakeLists.txt +++ b/reach/CMakeLists.txt @@ -5,6 +5,15 @@ find_package(ros_industrial_cmake_boilerplate REQUIRED) extract_package_metadata(pkg) project(${pkg_extracted_name} VERSION ${pkg_extracted_version} LANGUAGES CXX) +option(BUILD_PYTHON "Build Python bindings" ON) + +# Python dependencies need to be found first +if(BUILD_PYTHON) + find_package(Python REQUIRED COMPONENTS Interpreter Development) + find_package(PythonLibs 3 REQUIRED) + find_package(Boost REQUIRED COMPONENTS python numpy) +endif() + find_package(Boost REQUIRED COMPONENTS serialization program_options) find_package(Eigen3 REQUIRED) find_package(PCL REQUIRED COMPONENTS io search) @@ -23,6 +32,8 @@ if(OPENMP_FOUND) endif() endif() +set(TARGETS "") + # Interface library add_library(${PROJECT_NAME}_interface INTERFACE) target_include_directories(${PROJECT_NAME}_interface INTERFACE "$" @@ -35,8 +46,10 @@ target_compile_definitions( EVALUATOR_SECTION="eval" IK_SOLVER_SECTION="ik" LOGGER_SECTION="logger" - TARGET_POSE_GEN_SECTION="pose") + TARGET_POSE_GEN_SECTION="pose" + BUILD_PYTHON=${BUILD_PYTHON}) target_cxx_version(${PROJECT_NAME}_interface INTERFACE VERSION 14) +list(APPEND TARGETS ${PROJECT_NAME}_interface) # Reach Study Library add_library( @@ -56,6 +69,7 @@ target_link_libraries( target_compile_definitions(${PROJECT_NAME} PUBLIC SEARCH_LIBRARIES_ENV="REACH_PLUGINS" PLUGIN_LIBRARIES="${PROJECT_NAME}_plugins:reach_ros_plugins") target_cxx_version(${PROJECT_NAME} PUBLIC VERSION 14) +list(APPEND TARGETS ${PROJECT_NAME}) # Plugins Library add_library( @@ -72,16 +86,25 @@ target_link_libraries( ${PCL_LIBRARIES} boost_plugin_loader::boost_plugin_loader) target_cxx_version(${PROJECT_NAME}_plugins PUBLIC VERSION 14) +list(APPEND TARGETS ${PROJECT_NAME}_plugins) # Reach Study App add_executable(${PROJECT_NAME}_app src/app/reach_study.cpp) target_link_libraries(${PROJECT_NAME}_app PRIVATE ${PROJECT_NAME} Boost::program_options) target_cxx_version(${PROJECT_NAME}_app PUBLIC VERSION 14) +list(APPEND TARGETS ${PROJECT_NAME}_app) # Data Loader App add_executable(${PROJECT_NAME}_data_loader src/app/data_loader.cpp) target_link_libraries(${PROJECT_NAME}_data_loader PRIVATE ${PROJECT_NAME} Boost::program_options) target_cxx_version(${PROJECT_NAME}_data_loader PUBLIC VERSION 14) +list(APPEND TARGETS ${PROJECT_NAME}_data_loader) + +if(BUILD_PYTHON) + message("Building Python bindings") + add_subdirectory(src/python) + install(DIRECTORY scripts/ DESTINATION bin) +endif() # ###################################################################################################################### # TEST ## @@ -109,8 +132,4 @@ configure_package( yaml-cpp boost_plugin_loader OpenMP - TARGETS ${PROJECT_NAME}_interface - ${PROJECT_NAME} - ${PROJECT_NAME}_plugins - ${PROJECT_NAME}_app - ${PROJECT_NAME}_data_loader) + TARGETS ${TARGETS}) diff --git a/reach/include/reach/interfaces/display.h b/reach/include/reach/interfaces/display.h index fb7c851c..af1b413f 100644 --- a/reach/include/reach/interfaces/display.h +++ b/reach/include/reach/interfaces/display.h @@ -25,6 +25,16 @@ namespace YAML class Node; } +#ifdef BUILD_PYTHON +namespace boost +{ +namespace python +{ +class dict; +} // namespace python +} // namespace boost +#endif + namespace reach { /** @@ -52,6 +62,10 @@ struct Display /** @brief Visualizes the results of a reach study */ virtual void showResults(const ReachResult& db) const = 0; + +#ifdef BUILD_PYTHON + void updateRobotPose(const boost::python::dict&) const; +#endif }; /** @@ -71,6 +85,10 @@ struct DisplayFactory { return DISPLAY_SECTION; } + +#ifdef BUILD_PYTHON + Display::ConstPtr create(const boost::python::dict& pyyaml_config) const; +#endif }; } // namespace reach diff --git a/reach/include/reach/interfaces/evaluator.h b/reach/include/reach/interfaces/evaluator.h index 48b8ee59..cd201313 100644 --- a/reach/include/reach/interfaces/evaluator.h +++ b/reach/include/reach/interfaces/evaluator.h @@ -25,6 +25,16 @@ namespace YAML class Node; } +#ifdef BUILD_PYTHON +namespace boost +{ +namespace python +{ +class dict; +} +} // namespace boost +#endif + namespace reach { /** @@ -43,6 +53,10 @@ struct Evaluator * @details The better the reachability of the pose, the higher the score should be. */ virtual double calculateScore(const std::map& pose) const = 0; + +#ifdef BUILD_PYTHON + double calculateScore(const boost::python::dict& pose) const; +#endif }; /** @@ -62,6 +76,10 @@ struct EvaluatorFactory { return EVALUATOR_SECTION; } + +#ifdef BUILD_PYTHON + Evaluator::ConstPtr create(const boost::python::dict& pyyaml_config) const; +#endif }; } // namespace reach diff --git a/reach/include/reach/interfaces/ik_solver.h b/reach/include/reach/interfaces/ik_solver.h index afc7ae8c..1b06d5b7 100644 --- a/reach/include/reach/interfaces/ik_solver.h +++ b/reach/include/reach/interfaces/ik_solver.h @@ -26,6 +26,21 @@ namespace YAML class Node; } +#ifdef BUILD_PYTHON +namespace boost +{ +namespace python +{ +namespace numpy +{ +class ndarray; +} +class list; +class dict; +} // namespace python +} // namespace boost +#endif + namespace reach { /** @@ -46,6 +61,10 @@ struct IKSolver /** @brief Solves IK for a given target pose and seed state */ virtual std::vector> solveIK(const Eigen::Isometry3d& target, const std::map& seed) const = 0; + +#ifdef BUILD_PYTHON + boost::python::list solveIK(const boost::python::numpy::ndarray& target, const boost::python::dict& seed) const; +#endif }; /** @brief Plugin interface for generating IK solver interfaces */ @@ -63,6 +82,10 @@ struct IKSolverFactory { return IK_SOLVER_SECTION; } + +#ifdef BUILD_PYTHON + IKSolver::ConstPtr create(const boost::python::dict& pyyaml_config) const; +#endif }; } // namespace reach diff --git a/reach/include/reach/interfaces/logger.h b/reach/include/reach/interfaces/logger.h index cd78ec74..febdd0b7 100644 --- a/reach/include/reach/interfaces/logger.h +++ b/reach/include/reach/interfaces/logger.h @@ -10,6 +10,16 @@ namespace YAML class Node; } +#ifdef BUILD_PYTHON +namespace boost +{ +namespace python +{ +class dict; +} // namespace python +} // namespace boost +#endif + namespace reach { class ReachResultSummary; @@ -40,6 +50,10 @@ struct LoggerFactory { return LOGGER_SECTION; } + +#ifdef BUILD_PYTHON + Logger::Ptr create(const boost::python::dict& pyyaml_config) const; +#endif }; } // namespace reach diff --git a/reach/include/reach/interfaces/target_pose_generator.h b/reach/include/reach/interfaces/target_pose_generator.h index 0ae9bdda..0b72575f 100644 --- a/reach/include/reach/interfaces/target_pose_generator.h +++ b/reach/include/reach/interfaces/target_pose_generator.h @@ -12,6 +12,16 @@ namespace YAML class Node; } +#ifdef BUILD_PYTHON +namespace boost +{ +namespace python +{ +class dict; +} // namespace python +} // namespace boost +#endif + namespace reach { /** @brief Interface for generating Cartesian target poses for the reach study */ @@ -43,6 +53,10 @@ struct TargetPoseGeneratorFactory { return TARGET_POSE_GEN_SECTION; } + +#ifdef BUILD_PYTHON + TargetPoseGenerator::ConstPtr create(const boost::python::dict& pyyaml_config) const; +#endif }; } // namespace reach diff --git a/reach/include/reach/reach_study.h b/reach/include/reach/reach_study.h index cd828a69..d02ae5ff 100644 --- a/reach/include/reach/reach_study.h +++ b/reach/include/reach/reach_study.h @@ -83,7 +83,7 @@ class ReachStudy */ std::tuple getAverageNeighborsCount() const; -private: +protected: Parameters params_; ReachDatabase db_; diff --git a/reach/requirements.txt b/reach/requirements.txt new file mode 100644 index 00000000..440b084c --- /dev/null +++ b/reach/requirements.txt @@ -0,0 +1,6 @@ +numpy +scipy +open3d +tqdm +matplotlib +pytest \ No newline at end of file diff --git a/reach/scripts/heat_map_generator.py b/reach/scripts/heat_map_generator.py new file mode 100755 index 00000000..ff44cd28 --- /dev/null +++ b/reach/scripts/heat_map_generator.py @@ -0,0 +1,60 @@ +import argparse +import numpy as np +import open3d as o3d +import os.path +from reach import ReachDatabase, load, computeHeatMapColors, normalizeScores +from scipy.interpolate import RBFInterpolator + + +def main(): + parser = argparse.ArgumentParser(description="Generate a reachability heatmap from a preexisting reach database.") + parser.add_argument(type=str, dest="db_file", help="Filepath of the reach database") + parser.add_argument(type=str, dest="mesh_file", help='Filepath of the part mesh') + parser.add_argument("-k", "--kernel", type=str, default="thin_plate_spline", help="Kernel for RBF interpolation") + parser.add_argument("-e", "--epsilon", type=float, default=None, help="Shape parameter for RBF interpolation") + parser.add_argument("-s", "--smoothing", type=float, default=0.0, help="Smoothing parameter for RBF interpolation") + parser.add_argument("-o", "--output-mesh", type=str, default=None, help="Filepath for output heatmap") + parser.add_argument("-n", "--number-subdivisions", type=int, default=2, + help="Order of subdivision. Each triangle is divided once for n iterations") + parser.add_argument("-fcr", "--full-color-range", action='store_true', default=False, + help="Display scores using the full color range rather than only scaling scores by the max") + args = parser.parse_args() + + # Load database + if not os.path.exists(args.db_file): + raise FileExistsError(f'File \'{args.db_file}\' does not exist') + db = load(args.db_file) + + # Use the last set of results in the database + res = db.results[-1] + + # Loop over records in database to extract point position and scores into Numpy array + positions = np.array([r.goal()[0:3, 3] for r in res]) + scores = normalizeScores(res, args.full_color_range) + + # Calculate the RBF + rbf = RBFInterpolator(y=positions, d=scores, kernel=args.kernel, epsilon=args.epsilon, + smoothing=args.smoothing) + + # Load the mesh and subdivide it + mesh = o3d.io.read_triangle_mesh(args.mesh_file).subdivide_midpoint(args.number_subdivisions) + + # Extract the vertices of the sub-sampled mesh as a numpy array and calculate the interpolated score for each + dims = rbf.y.shape[1] + vert_scores = rbf(np.asarray(mesh.vertices)[:, :dims]) + + # Clip the scores on [0, 1] + vert_scores = np.clip(vert_scores, a_min=0.0, a_max=1.0) + + # Colorize the mesh vertices + mesh.vertex_colors = o3d.utility.Vector3dVector(computeHeatMapColors(vert_scores.tolist())) + + # Visualize the output + o3d.visualization.draw_geometries([mesh], mesh_show_wireframe=False) + + if args.output_mesh is not None: + o3d.io.write_triangle_mesh(args.output_mesh, mesh) + + +if __name__ == '__main__': + main() diff --git a/reach/scripts/rbf_cross_validation.py b/reach/scripts/rbf_cross_validation.py new file mode 100755 index 00000000..806c3f73 --- /dev/null +++ b/reach/scripts/rbf_cross_validation.py @@ -0,0 +1,69 @@ +import argparse +import numpy as np +import matplotlib.pyplot as plt +from pprint import pprint +from reach import ReachDatabase, load +from scipy.interpolate import Rbf + + +def evaluate(positions, scores, func, eps=None): + rbf = Rbf(positions[:, 0], positions[:, 1], positions[:, 2], scores, function=func, epsilon=eps) + Ainv = np.linalg.inv(rbf.A) + e = rbf.nodes / np.diag(Ainv) + return np.linalg.norm(e, ord=1)/len(e) + + +def main(): + parser = argparse.ArgumentParser( + description="Calculate and display the errors generated by varying parametrizations of RBF interpolation") + parser.add_argument(type=str, dest="db_file", help="Filepath of the reach database") + parser.add_argument('--max_eps', type=float, default=1.0, help="Maximum value of epsilon to sample") + parser.add_argument('--n_eps_samples', type=int, default=50, help="Number of epsilon values to sample on [5e-3, max_eps]") + args = parser.parse_args() + + # Load database from ROS message + db = load(args.db_file) + result = db.results[-1] + + positions = np.array([r.goal()[0:3, 3] for r in result]) + scores = np.array([r.score for r in result]) + + # Evaluate the functions that use the epsilon parameter over a range of values + epsilon = np.linspace(0.0005, args.max_eps, args.n_eps_samples) + functions_eps = ['multiquadric', 'inverse', 'gaussian'] + error_eps = np.empty((len(functions_eps), len(epsilon))) + for i, func in enumerate(functions_eps): + for j, eps in enumerate(epsilon): + error_eps[i, j] = evaluate(positions, scores, func, eps) + + # Evaluate the functions that do not use epsilon + functions_no_eps = ['thin_plate', 'linear', 'cubic', 'quintic'] + error_no_eps = np.empty((len(functions_no_eps), len(epsilon))) + for i, func in enumerate(functions_no_eps): + error_no_eps[i] = np.repeat(evaluate(positions, scores, func), len(epsilon)) + + # Combine the results + functions = functions_eps + functions_no_eps + error = np.vstack([error_eps, error_no_eps]) + + # Print the error for each method and its corresponding epsilon value + print('Method - Error - Epsilon') + pprint(list(zip(functions, zip(np.min(error, axis=1), epsilon[np.argmin(error, axis=1)])))) + + # Plot the results + _,ax = plt.subplots() + lines = ax.plot(epsilon, error.T) + for i, line in enumerate(lines): + line.set_label(functions[i]) + ax.scatter(epsilon[np.argmin(error, axis=1)], np.min(error, axis=1)) + ax.set( + xlabel='Epsilon', + ylabel='Error' + ) + ax.grid() + ax.legend() + plt.show() + + +if __name__ == '__main__': + main() diff --git a/reach/scripts/test_reach.py b/reach/scripts/test_reach.py new file mode 100755 index 00000000..1bafd614 --- /dev/null +++ b/reach/scripts/test_reach.py @@ -0,0 +1,170 @@ +import numpy as np +import os +import reach +from tqdm import tqdm +import unittest +import yaml + + +class PyIKSolver(reach.IKSolver): + def getJointNames(self): + return ['j1', 'j2'] + + def solveIK(self, _pose: np.ndarray, _seed: dict): + return [[1.0, 1.0]] + + +class PyEvaluator(reach.Evaluator): + def calculateScore(self, _state: dict): + return 1.0 + + +class PyTargetPoseGenerator(reach.TargetPoseGenerator): + def generate(self): + pts = np.linspace(np.zeros((3,), dtype=np.float64), np.ones((3,), dtype=np.float64), 100, endpoint=True) + poses = [] + for pt in pts: + pose = np.eye(4, dtype=np.float64) + pose[0:3, 3] = pt + poses.append(pose) + + return poses + + +class PyDisplay(reach.Display): + def showEnvironment(self): + return + + def updateRobotPose(self, _pose: dict): + return + + def showReachNeighborhood(self, _neighborhood): + return + + def showResults(self, _results: reach.ReachResult): + return + + +class PyLogger(reach.Logger): + def __init__(self): + super().__init__() + self.pbar = tqdm(total=0, position=0, leave=True) + self.progress = 0 + + def setMaxProgress(self, max_progress: int): + self.progress = 0 + self.pbar = tqdm(total=max_progress, position=0, leave=True) + + def printProgress(self, progress: int): + self.pbar.update(progress - self.progress) + self.progress = progress + + def printResults(self, results: reach.ReachResultSummary): + print(results) + + def print(self, message: str): + print(message) + + +class ReachStudyFixture(unittest.TestCase): + def test_run_reach_study(self): + config_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), '../test/reach_study.yaml') + with open(config_file, 'r') as file: + config = yaml.safe_load(file) + + # Set the ROS package path to two directories up from this file + os.environ['ROS_PACKAGE_PATH'] = os.path.join(os.path.dirname(os.path.realpath(__file__)), '..', '..') + reach.runReachStudy(config, '', '', False) + + def test_run_reach_study_pure_python(self): + ik_solver = PyIKSolver() + evaluator = PyEvaluator() + target_pose_generator = PyTargetPoseGenerator() + display = PyDisplay() + logger = PyLogger() + + params = reach.Parameters() + params.radius = 0.4 + params.max_steps = 10 + params.step_improvement_threshold = 0.01 + + study = reach.ReachStudy(ik_solver, evaluator, target_pose_generator, display, logger, params) + study.run() + study.optimize() + + # Show the results + db = study.getDatabase() + + # Expect to run for one nominal iteration and one optimization iteration + self.assertEqual(len(db.results), 2) + + results = db.calculateResults() + logger.printResults(results) + self.assertAlmostEqual(results.total_pose_score, 100.0) + self.assertAlmostEqual(results.reach_percentage, 100.0) + self.assertAlmostEqual(results.norm_total_pose_score, 100.0) + + display.showEnvironment() + + self.assertEqual(len(db.results[-1]), 100) + display.showResults(db.results[-1]) + + def test_run_reach_study_mixed(self): + # Load the file + config_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), '../test/reach_study.yaml') + with open(config_file, 'r') as file: + config = yaml.safe_load(file) + + params = reach.Parameters() + opt_config = config["optimization"] + params.radius = opt_config["radius"] + params.max_steps = opt_config["max_steps"] + params.step_improvement_threshold = opt_config["step_improvement_threshold"] + + os.environ['REACH_PLUGINS'] = 'reach_plugins' + loader = reach.PluginLoader() + loader.search_libraries_env = 'REACH_PLUGINS' + + # Load the IK solver from a c++ plugin + ik_config = config["ik_solver"] + ik_solver_factory = loader.createIKSolverFactoryInstance(ik_config["name"]) + ik_solver = ik_solver_factory.create(ik_config) + + # Load the evaluator from a c++ plugin + eval_config = config["evaluator"] + evaluator_factory = loader.createEvaluatorFactoryInstance(eval_config["name"]) + evaluator = evaluator_factory.create(eval_config) + + # Load the logger from a c++ plugin + display_config = config["display"] + display_factory = loader.createDisplayFactoryInstance(display_config["name"]) + display = display_factory.create(display_config) + + # Use Python implementations for the target pose generator and logger + target_pose_generator = PyTargetPoseGenerator() + logger = PyLogger() + + study = reach.ReachStudy(ik_solver, evaluator, target_pose_generator, display, logger, params) + study.run() + study.optimize() + + # Show the results + db = study.getDatabase() + + # Should run for one nominal iteration and one optimization iteration + self.assertEqual(len(db.results), 2) + + results = db.calculateResults() + logger.printResults(results) + self.assertAlmostEqual(results.total_pose_score, 0.0) + self.assertAlmostEqual(results.reach_percentage, 100.0) + self.assertAlmostEqual(results.norm_total_pose_score, 0.0) + + display.showEnvironment() + + self.assertEqual(len(db.results[-1]), 100) + display.showResults(db.results[-1]) + + +if __name__ == "__main__": + unittest.main() diff --git a/reach/src/python/CMakeLists.txt b/reach/src/python/CMakeLists.txt new file mode 100644 index 00000000..f32b1163 --- /dev/null +++ b/reach/src/python/CMakeLists.txt @@ -0,0 +1,19 @@ +# Add Python libraries +python_add_library(${PROJECT_NAME}_python MODULE python_bindings.cpp) +target_include_directories(${PROJECT_NAME}_python PRIVATE ${PYTHON_INCLUDE_DIRS}) +target_link_libraries( + ${PROJECT_NAME}_python + PRIVATE ${PROJECT_NAME} + Boost::python + Boost::numpy + ${PYTHON_LIBRARIES}) +target_compile_definitions(${PROJECT_NAME}_python PRIVATE MODULE_NAME=${PROJECT_NAME}) +set_target_properties( + ${PROJECT_NAME}_python PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${CMAKE_INSTALL_PREFIX}/lib/python3/dist-packages + OUTPUT_NAME ${PROJECT_NAME}) + +list( + APPEND + TARGETS + ${PROJECT_NAME}_python + PARENT_SCOPE) diff --git a/reach/src/python/display.hpp b/reach/src/python/display.hpp new file mode 100644 index 00000000..552a6cae --- /dev/null +++ b/reach/src/python/display.hpp @@ -0,0 +1,64 @@ +#pragma once + +#include +#include "utils.hpp" + +#include +#include + +namespace bp = boost::python; + +namespace reach +{ +void Display::updateRobotPose(const bp::dict& dict_joint_positions) const +{ + return updateRobotPose(toMap(dict_joint_positions)); +} + +Display::ConstPtr DisplayFactory::create(const bp::dict& pyyaml_config) const +{ + return create(toYAML(pyyaml_config)); +} + +struct DisplayPython : Display, boost::python::wrapper +{ + void showEnvironment() const override + { + return call_and_handle([this]() { this->get_override("showEnvironment")(); }); + } + + void updateRobotPose(const std::map& map) const override + { + auto fn = [this, &map]() { + bp::dict dictionary; + for (auto pair : map) + { + dictionary[pair.first] = pair.second; + } + + this->get_override("updateRobotPose")(dictionary); + }; + + return call_and_handle(fn); + } + + void showReachNeighborhood(const std::map& neighborhood) const override + { + return call_and_handle([this, &neighborhood]() { this->get_override("showReachNeighborhood")(neighborhood); }); + } + + void showResults(const ReachResult& results) const override + { + return call_and_handle([this, &results]() { this->get_override("showResults")(results); }); + } +}; + +struct DisplayFactoryPython : DisplayFactory, boost::python::wrapper +{ + Display::ConstPtr create(const YAML::Node& config) const override + { + return call_and_handle([this, &config]() -> Display::ConstPtr { return this->get_override("create")(config); }); + } +}; + +} // namespace reach diff --git a/reach/src/python/evaluator.hpp b/reach/src/python/evaluator.hpp new file mode 100644 index 00000000..adeb83a3 --- /dev/null +++ b/reach/src/python/evaluator.hpp @@ -0,0 +1,51 @@ +#pragma once + +#include +#include "utils.hpp" + +#include +#include + +namespace bp = boost::python; + +namespace reach +{ +double Evaluator::calculateScore(const bp::dict& pose) const +{ + return calculateScore(toMap(pose)); +} + +Evaluator::ConstPtr EvaluatorFactory::create(const bp::dict& pyyaml_config) const +{ + return create(toYAML(pyyaml_config)); +} + +struct EvaluatorPython : Evaluator, boost::python::wrapper +{ + double calculateScore(const std::map& map) const override + { + auto fn = [this, &map]() -> double { + bp::dict dictionary; + for (auto pair : map) + { + dictionary[pair.first] = pair.second; + } + + double score = this->get_override("calculateScore")(dictionary); + + return score; + }; + + return call_and_handle(fn); + } +}; + +struct EvaluatorFactoryPython : EvaluatorFactory, boost::python::wrapper +{ + Evaluator::ConstPtr create(const YAML::Node& config) const override + { + return call_and_handle([this, &config]() -> Evaluator::ConstPtr { return this->get_override("create")(config); }); + } +}; + +} // namespace reach diff --git a/reach/src/python/ik_solver.hpp b/reach/src/python/ik_solver.hpp new file mode 100644 index 00000000..fb277464 --- /dev/null +++ b/reach/src/python/ik_solver.hpp @@ -0,0 +1,97 @@ +#pragma once + +#include +#include "utils.hpp" + +#include +#include +#include + +namespace bp = boost::python; + +namespace reach +{ +boost::python::list IKSolver::solveIK(const boost::python::numpy::ndarray& target, + const boost::python::dict& seed) const +{ + Eigen::Isometry3d cpp_target = toEigen(target); + std::map cpp_seed = toMap(seed); + + std::vector> cpp_solution = solveIK(cpp_target, cpp_seed); + + boost::python::list solution; + for (const std::vector& inner : cpp_solution) + { + solution.append(inner); + } + + return solution; +} + +IKSolver::ConstPtr IKSolverFactory::create(const boost::python::dict& pyyaml_config) const +{ + return create(toYAML(pyyaml_config)); +} + +struct IKSolverPython : IKSolver, boost::python::wrapper +{ + std::vector getJointNames() const override + { + auto fn = [this]() -> std::vector { + std::vector names; + + bp::list name_list = this->get_override("getJointNames")(); + + for (int i = 0; i < bp::len(name_list); ++i) + { + std::string name = bp::extract{ name_list[i] }(); + names.push_back(name); + } + + return names; + }; + return call_and_handle(fn); + } + + std::vector> solveIK(const Eigen::Isometry3d& target, + const std::map& seed) const override + { + auto fn = [this, &target, &seed]() -> std::vector> { + // Convert the seed to a Python dictionary + bp::dict dictionary; + for (const auto& pair : seed) + { + dictionary[pair.first] = pair.second; + } + + bp::list list = this->get_override("solveIK")(fromEigen(target), dictionary); + + // Convert the output + std::vector> output; + for (int i = 0; i < bp::len(list); ++i) + { + std::vector sub_vec; + bp::list sub_list = bp::extract{ list[i] }(); + for (int j = 0; j < bp::len(sub_list); ++j) + { + sub_vec.push_back(bp::extract{ sub_list[j] }()); + } + output.push_back(sub_vec); + } + + return output; + }; + + return call_and_handle(fn); + } +}; + +struct IKSolverFactoryPython : IKSolverFactory, boost::python::wrapper +{ + IKSolver::ConstPtr create(const YAML::Node& config) const override + { + return call_and_handle([this, &config]() -> IKSolver::ConstPtr { return this->get_override("create")(config); }); + } +}; + +} // namespace reach diff --git a/reach/src/python/logger.hpp b/reach/src/python/logger.hpp new file mode 100644 index 00000000..67b8c52f --- /dev/null +++ b/reach/src/python/logger.hpp @@ -0,0 +1,44 @@ +#pragma once + +#include +#include "utils.hpp" + +namespace reach +{ +Logger::Ptr LoggerFactory::create(const boost::python::dict& pyyaml_config) const +{ + return create(toYAML(pyyaml_config)); +} + +struct LoggerPython : Logger, boost::python::wrapper +{ + void setMaxProgress(unsigned long max_progress) override + { + return call_and_handle([this, &max_progress]() { this->get_override("setMaxProgress")(max_progress); }); + } + + void printProgress(unsigned long progress) const override + { + return call_and_handle([this, &progress]() { this->get_override("printProgress")(progress); }); + } + + void printResults(const ReachResultSummary& results) const override + { + return call_and_handle([this, &results]() { this->get_override("printResults")(results); }); + } + + void print(const std::string& msg) const override + { + return call_and_handle([this, &msg]() { this->get_override("print")(msg); }); + } +}; + +struct LoggerFactoryPython : LoggerFactory, boost::python::wrapper +{ + Logger::Ptr create(const YAML::Node& config) const override + { + return call_and_handle([this, &config]() -> Logger::Ptr { return this->get_override("create")(config); }); + } +}; + +} // namespace reach diff --git a/reach/src/python/python_bindings.cpp b/reach/src/python/python_bindings.cpp new file mode 100644 index 00000000..b2b2f96b --- /dev/null +++ b/reach/src/python/python_bindings.cpp @@ -0,0 +1,318 @@ +/* + * Copyright 2019 Southwest Research Institute + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#include +#include "display.hpp" +#include "evaluator.hpp" +#include "ik_solver.hpp" +#include "logger.hpp" +#include "target_pose_generator.hpp" +#include "utils.hpp" + +#include +#include +#include +#include +#include +#include +#include + +namespace bp = boost::python; + +namespace reach +{ +class ReachStudyPython : public ReachStudy +{ +public: + ReachStudyPython(const IKSolver* ik_solver, const Evaluator* evaluator, const TargetPoseGenerator* pose_generator, + const Display* display, Logger* logger, ReachStudy::Parameters params) + : ReachStudy(IKSolver::ConstPtr(ik_solver, [](const IKSolver*) {}), + Evaluator::ConstPtr(evaluator, [](const Evaluator*) {}), + TargetPoseGenerator::ConstPtr(pose_generator, [](const TargetPoseGenerator*) {}), + Display::ConstPtr(display, [](const Display*) {}), Logger::Ptr(logger, [](Logger*) {}), + std::move(params)) + { + std::vector python_interface_names; + + auto python_ik = dynamic_cast(ik_solver); + if (python_ik) + { + python_interface_names.push_back(boost::core::demangle(typeid(*python_ik).name())); + } + + auto python_evaluator = dynamic_cast(evaluator); + if (python_evaluator) + { + python_interface_names.push_back(boost::core::demangle(typeid(*python_evaluator).name())); + } + + auto python_pose_generator = dynamic_cast(pose_generator); + if (python_pose_generator) + { + python_interface_names.push_back(boost::core::demangle(typeid(*python_pose_generator).name())); + } + + auto python_display = dynamic_cast(display); + if (python_display) + { + python_interface_names.push_back(boost::core::demangle(typeid(*python_display).name())); + } + + auto python_logger = dynamic_cast(logger); + if (python_logger) + { + python_interface_names.push_back(boost::core::demangle(typeid(*python_logger).name())); + } + + if (!python_interface_names.empty()) + { + params_.max_threads = 1; + + logger->print("Detected Python interfaces of the following abstract types:"); + for (const std::string& name : python_interface_names) + { + logger->print(name); + } + logger->print("Setting max threads to 1"); + } + else + { + logger->print("Did not detect any Python interfaces"); + } + } + + ReachStudyPython(const ReachStudyPython& rhs) : ReachStudy(rhs) + { + } +}; + +void runReachStudyPython1(const bp::dict& config) +{ + runReachStudy(toYAML(config)); +} + +void runReachStudyPython2(const bp::dict& config, const std::string& config_name) +{ + runReachStudy(toYAML(config), config_name); +} + +void runReachStudyPython3(const bp::dict& config, const std::string& config_name, const std::string& results_dir) +{ + runReachStudy(toYAML(config), config_name, results_dir); +} + +void runReachStudyPython4(const bp::dict& config, const std::string& config_name, const std::string& results_dir, + bool wait_after_completion) +{ + runReachStudy(toYAML(config), config_name, results_dir, wait_after_completion); +} + +bp::list normalizeScoresPython(const ReachResult& result, bool use_full_range) +{ + auto norm_scores = normalizeScores(result, use_full_range); + bp::list out; + for (const auto& v : norm_scores) + out.append(v); + return out; +} + +np::ndarray computeHeatMapColorsPython1(const ReachResult& result, bool use_full_color_range) +{ + return fromEigen(computeHeatMapColors(result, use_full_color_range)); +} + +np::ndarray computeHeatMapColorsPython2(const bp::list& scores) +{ + std::vector scores_v; + scores_v.reserve(bp::len(scores)); + for (bp::ssize_t i = 0; i < bp::len(scores); ++i) + scores_v.push_back(bp::extract{ scores[i] }()); + + return fromEigen(computeHeatMapColors(scores_v)); +} + +BOOST_PYTHON_MODULE(MODULE_NAME) +{ + Py_Initialize(); + PyEval_InitThreads(); + bp::numpy::initialize(); + + // Wrap boost_plugin_loader::PluginLoader + { + bp::class_("PluginLoader") + .def_readwrite("search_libraries_env", &boost_plugin_loader::PluginLoader::search_libraries_env) + .def("createIKSolverFactoryInstance", &boost_plugin_loader::PluginLoader::createInstance) + .def("createTargetPoseGeneratorFactoryInstance", + &boost_plugin_loader::PluginLoader::createInstance) + .def("createEvaluatorFactoryInstance", &boost_plugin_loader::PluginLoader::createInstance) + .def("createDisplayFactoryInstance", &boost_plugin_loader::PluginLoader::createInstance) + .def("createLoggerFactoryInstance", &boost_plugin_loader::PluginLoader::createInstance); + } + + // Wrap the IKSolver + { + std::vector> (IKSolver::*solveIKCpp)( + const Eigen::Isometry3d&, const std::map&) const = &IKSolver::solveIK; + bp::list (IKSolver::*solveIKPython)(const bp::numpy::ndarray&, const bp::dict&) const = &IKSolver::solveIK; + bp::class_("IKSolver") + .def("getJointNames", bp::pure_virtual(&IKSolver::getJointNames)) + .def("solveIK", bp::pure_virtual(solveIKCpp)) + .def("solveIK", solveIKPython); + + IKSolver::ConstPtr (IKSolverFactory::*createCpp)(const YAML::Node&) const = &IKSolverFactory::create; + IKSolver::ConstPtr (IKSolverFactory::*createPython)(const bp::dict&) const = &IKSolverFactory::create; + bp::class_("IKSolverFactory") + .def("create", bp::pure_virtual(createCpp)) + .def("create", createPython); + } + + // Wrap the Evaluator + { + double (Evaluator::*calculateScoreCpp)(const std::map&) const = &Evaluator::calculateScore; + double (Evaluator::*calculateScorePython)(const bp::dict&) const; + bp::class_("Evaluator") + .def("calculateScore", bp::pure_virtual(calculateScoreCpp)) + .def("calculateScore", calculateScorePython); + + Evaluator::ConstPtr (EvaluatorFactory::*createCpp)(const YAML::Node&) const = &EvaluatorFactory::create; + Evaluator::ConstPtr (EvaluatorFactory::*createPython)(const bp::dict&) const = &EvaluatorFactory::create; + bp::class_("EvaluatorFactory") + .def("create", bp::pure_virtual(createCpp)) + .def("create", createPython); + } + + // Wrap the TargetPoseGenerator + { + bp::class_("TargetPoseGenerator") + .def("generate", bp::pure_virtual(&TargetPoseGenerator::generate)); + + TargetPoseGenerator::ConstPtr (TargetPoseGeneratorFactory::*createFromDict)(const bp::dict&) const = + &TargetPoseGeneratorFactory::create; + TargetPoseGenerator::ConstPtr (TargetPoseGeneratorFactory::*createFromNode)(const YAML::Node&) const = + &TargetPoseGeneratorFactory::create; + bp::class_("TargetPoseGeneratorFactory") + .def("create", bp::pure_virtual(createFromNode)) + .def("create", createFromDict); + } + + // Wrap the Display + { + void (Display::*updateRobotPoseMap)(const std::map&) const = &Display::updateRobotPose; + void (Display::*updateRobotPoseDict)(const bp::dict&) const = &Display::updateRobotPose; + bp::class_("Display") + .def("showEnvironment", bp::pure_virtual(&Display::showEnvironment)) + .def("updateRobotPose", bp::pure_virtual(updateRobotPoseMap)) + .def("updateRobotPose", updateRobotPoseDict) + .def("showReachNeighborhood", bp::pure_virtual(&Display::showReachNeighborhood)) + .def("showResults", bp::pure_virtual(&Display::showResults)); + + Display::ConstPtr (DisplayFactory::*createFromDict)(const bp::dict&) const = &DisplayFactory::create; + Display::ConstPtr (DisplayFactory::*createFromNode)(const YAML::Node&) const = &DisplayFactory::create; + bp::class_("DisplayFactory") + .def("create", bp::pure_virtual(createFromNode)) + .def("create", createFromDict); + } + + // Wrap the Logger + { + bp::class_("Logger") + .def("setMaxProgress", bp::pure_virtual(&Logger::setMaxProgress)) + .def("printProgress", bp::pure_virtual(&Logger::printProgress)) + .def("printResults", bp::pure_virtual(&Logger::printResults)) + .def("print", bp::pure_virtual(&Logger::print)); + + Logger::Ptr (LoggerFactory::*createFromDict)(const bp::dict&) const = &LoggerFactory::create; + Logger::Ptr (LoggerFactory::*createFromNode)(const YAML::Node&) const = &LoggerFactory::create; + bp::class_("LoggerFactory") + .def("create", bp::pure_virtual(createFromNode)) + .def("create", createFromDict); + } + + // Wrap the datatypes + { + bp::class_("Parameters") + .def_readwrite("max_steps", &ReachStudy::Parameters::max_steps) + .def_readwrite("step_improvement_threshold", &ReachStudy::Parameters::step_improvement_threshold) + .def_readwrite("radius", &ReachStudy::Parameters::radius); + + bp::class_("ReachRecord") + .def( + "goal", +[](const ReachRecord& r) -> bp::numpy::ndarray { return fromEigen(r.goal); }) + .def_readwrite("score", &ReachRecord::score) + .def_readwrite("goal_state", &ReachRecord::goal_state) + .def_readwrite("seed_state", &ReachRecord::seed_state) + .def_readwrite("reached", &ReachRecord::reached); + + bp::class_("ReachResult").def(bp::vector_indexing_suite()); + bp::class_("VectorReachResult").def(bp::vector_indexing_suite()); + + bp::class_("ReachDatabase") + .def_readwrite("results", &ReachDatabase::results) + .def("calculateResults", &ReachDatabase::calculateResults) + .def("computeHeatMapColors", &ReachDatabase::computeHeatMapColors); + + bp::class_("ReachResultSummary") + .def_readonly("total_pose_score", &ReachResultSummary::total_pose_score) + .def_readonly("norm_total_pose_score", &ReachResultSummary::norm_total_pose_score) + .def_readonly("reach_percentage", &ReachResultSummary::reach_percentage) + .def("__str__", &ReachResultSummary::print); + } + + // Wrap ReachStudy + { + bp::class_("ReachStudy", bp::init()) + .def("load", &ReachStudyPython::load) + .def("save", &ReachStudyPython::save) + .def("getDatabase", &ReachStudyPython::getDatabase, bp::return_internal_reference()) + .def("run", &ReachStudyPython::run) + .def("optimize", &ReachStudyPython::optimize) + .def("getAverageNeighborsCounts", &ReachStudyPython::getAverageNeighborsCount); + } + + // Wrap the free functions + { + bp::def("runReachStudy", &runReachStudyPython1); + bp::def("runReachStudy", &runReachStudyPython2); + bp::def("runReachStudy", &runReachStudyPython3); + bp::def("runReachStudy", &runReachStudyPython4); + bp::def("save", &save); + bp::def("load", &load); + bp::def("calculateResults", &calculateResults); + bp::def("normalizeScores", &normalizeScoresPython); + bp::def("computeHeatMapColors", &computeHeatMapColorsPython1); + bp::def("computeHeatMapColors", &computeHeatMapColorsPython2); + } + + // Register shared_ptrs + { + bp::register_ptr_to_python(); + bp::register_ptr_to_python(); + bp::register_ptr_to_python(); + bp::register_ptr_to_python(); + bp::register_ptr_to_python(); + bp::register_ptr_to_python(); + bp::register_ptr_to_python(); + bp::register_ptr_to_python(); + bp::register_ptr_to_python(); + bp::register_ptr_to_python(); + bp::register_ptr_to_python(); + bp::register_ptr_to_python(); + bp::register_ptr_to_python(); + bp::register_ptr_to_python(); + } +} + +} // namespace reach diff --git a/reach/src/python/target_pose_generator.hpp b/reach/src/python/target_pose_generator.hpp new file mode 100644 index 00000000..3a833e32 --- /dev/null +++ b/reach/src/python/target_pose_generator.hpp @@ -0,0 +1,50 @@ +#pragma once + +#include +#include "utils.hpp" + +#include +#include +#include + +namespace bp = boost::python; +namespace np = boost::python::numpy; + +namespace reach +{ +TargetPoseGenerator::ConstPtr TargetPoseGeneratorFactory::create(const bp::dict& pyyaml_config) const +{ + return create(toYAML(pyyaml_config)); +} + +struct TargetPoseGeneratorPython : TargetPoseGenerator, boost::python::wrapper +{ + VectorIsometry3d generate() const override + { + auto fn = [this]() -> VectorIsometry3d { + bp::list np_list = this->get_override("generate")(); + + // Convert the list of 4x4 numpy arrays to VectorIsometry3d + VectorIsometry3d eigen_list; + for (int i = 0; i < bp::len(np_list); ++i) + { + eigen_list.push_back(toEigen(bp::numpy::from_object(np_list[i]))); + } + + return eigen_list; + }; + + return call_and_handle(fn); + } +}; + +struct TargetPoseGeneratorFactoryPython : TargetPoseGeneratorFactory, boost::python::wrapper +{ + TargetPoseGenerator::ConstPtr create(const YAML::Node& config) const override + { + return call_and_handle( + [this, &config]() -> TargetPoseGenerator::ConstPtr { return this->get_override("create")(config); }); + } +}; + +} // namespace reach diff --git a/reach/src/python/utils.hpp b/reach/src/python/utils.hpp new file mode 100644 index 00000000..7d89a952 --- /dev/null +++ b/reach/src/python/utils.hpp @@ -0,0 +1,157 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace reach +{ +inline void print_py_error(std::stringstream& ss) +{ + try + { + PyErr_Print(); + boost::python::object sys(boost::python::handle<>(PyImport_ImportModule("sys"))); + boost::python::object err = sys.attr("stderr"); + std::string err_text = boost::python::extract{ err.attr("getvalue") }(); + ss << err_text << std::endl; + } + catch (...) + { + ss << "Failed to parse Python error" << std::endl; + } + PyErr_Clear(); +} + +template +auto call_and_handle(Function func) +{ + try + { + return func(); + } + catch (const boost::python::error_already_set& e) + { + std::stringstream ss; + print_py_error(ss); + throw std::runtime_error(ss.str()); + } +} + +template +std::map toMap(const boost::python::dict& dict) +{ + std::map map; + + boost::python::list keys = dict.keys(); + for (int i = 0; i < boost::python::len(keys); ++i) + { + const KeyT& key = boost::python::extract{ keys[i] }(); + const ValueT& value = boost::python::extract{ dict[key] }(); + map.insert({ key, value }); + } + + return map; +} + +inline YAML::Node toYAML(const boost::python::object& obj) +{ + namespace bp = boost::python; + + auto dict_extractor = bp::extract(obj); + auto list_extractor = bp::extract(obj); + auto str_extractor = bp::extract(obj); + auto int_extractor = bp::extract(obj); + auto float_extractor = bp::extract(obj); + + if (dict_extractor.check()) + { + YAML::Node node(YAML::NodeType::Map); + bp::dict dict = dict_extractor(); + bp::list keys = dict.keys(); + for (int i = 0; i < bp::len(keys); ++i) + { + const std::string key = bp::extract{ keys[i] }(); + node[key] = toYAML(dict[key]); + } + + return node; + } + else if (list_extractor.check()) + { + YAML::Node node(YAML::NodeType::Sequence); + bp::list list = list_extractor(); + for (bp::ssize_t i = 0; i < bp::len(list); ++i) + { + node[i] = toYAML(list[i]); + } + + return node; + } + else if (str_extractor.check()) + return YAML::Node(str_extractor()); + else if (int_extractor.check()) + return YAML::Node(int_extractor()); + else if (float_extractor.check()) + return YAML::Node(float_extractor()); + + throw std::runtime_error("Unsupported Python value type '" + + bp::extract{ obj.attr("__class__").attr("__name__") }() + "'"); +} + +template +Eigen::Matrix toEigen(const boost::python::numpy::ndarray& arr) +{ + int n_dims = arr.get_nd(); + if (n_dims != 2) + throw std::runtime_error("Numpy array has more than 2 dimensions (" + std::to_string(n_dims) + ")"); + + if (arr.get_dtype() != boost::python::numpy::dtype::get_builtin()) + throw std::runtime_error("Numpy array dtype must be " + boost::core::demangle(typeid(T).name())); + + return Eigen::Map>((T*)arr.get_data()); +} + +inline Eigen::Isometry3d toEigen(const boost::python::numpy::ndarray& arr) +{ + const Py_intptr_t* dims = arr.get_shape(); + Py_intptr_t rows = dims[0]; + Py_intptr_t cols = dims[1]; + if (rows != 4) + throw std::runtime_error("Numpy array has " + std::to_string(rows) + " rather than 4 rows"); + if (cols != 4) + throw std::runtime_error("Numpy array has " + std::to_string(cols) + " rather than 4 columns"); + + return Eigen::Isometry3d(toEigen(arr)); +} + +template +boost::python::numpy::ndarray fromEigen(const Eigen::Matrix& mat) +{ + namespace bp = boost::python; + namespace np = boost::python::numpy; + + const Eigen::Index rows = mat.rows(); + const Eigen::Index cols = mat.cols(); + + bp::tuple shape = bp::make_tuple(rows * cols); + bp::numpy::dtype dtype = np::dtype::get_builtin(); + bp::tuple stride = bp::make_tuple(sizeof(T)); + + // Construct an ndarray "map" to the data in the array + auto arr = + np::from_data(mat.data(), dtype, shape, stride, bp::object()).reshape(bp::make_tuple(cols, rows)).transpose(); + + // Return a copy of the array in case the input object goes out of scope later + return arr.copy(); +} + +inline boost::python::numpy::ndarray fromEigen(const Eigen::Isometry3d& pose) +{ + return fromEigen(pose.matrix()); +} + +} // namespace reach