From cf23150743f49b142c5b4a4c2df3d8a3ef0142bf Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Wed, 21 Dec 2022 11:12:34 +0100
Subject: [PATCH] Factor out JointModelGroup from RobotModel API

---
 moveit_core/robot_model/src/pyrobot_model.cpp | 51 +++++++++++++++----
 1 file changed, 42 insertions(+), 9 deletions(-)

diff --git a/moveit_core/robot_model/src/pyrobot_model.cpp b/moveit_core/robot_model/src/pyrobot_model.cpp
index 5e59b406b9..8fd64d7922 100644
--- a/moveit_core/robot_model/src/pyrobot_model.cpp
+++ b/moveit_core/robot_model/src/pyrobot_model.cpp
@@ -46,18 +46,51 @@ using namespace robot_model;
 void def_robot_model_bindings(py::module& m)
 {
   m.doc() = "Definition of a kinematic model. Not thread safe, however multiple instances can be created.";
+  py::class_<JointModelGroup, JointModelGroupPtr>(m, "JointModelGroup")
+      .def("get_link_model_names", &JointModelGroup::getLinkModelNames)
+      .def("get_joint_model_names", &JointModelGroup::getJointModelNames)
+      .def("get_active_joint_model_names", &JointModelGroup::getActiveJointModelNames)
+      .def("get_default_state_names", &JointModelGroup::getDefaultStateNames)
+      .def("get_end_effector_tips",
+           [](const JointModelGroup& self) {
+             std::vector<std::string> tips;
+             self.getEndEffectorTips(tips);
+             return tips;
+           })
+      // keep semicolon on next line
+      ;
+
   py::class_<RobotModel, RobotModelPtr>(m, "RobotModel")
       .def(py::init<const urdf::ModelInterfaceSharedPtr&, const srdf::ModelConstSharedPtr&>(), py::arg("urdf_model"),
            py::arg("srdf_model"))
-      .def("getName", &RobotModel::getName)
-      .def("getLinkModelNames", &RobotModel::getLinkModelNames)
-      .def("getJointModelNames", &RobotModel::getJointModelNames)
-      //
-      ;
+      .def("get_name", &RobotModel::getName)
+      .def("get_model_frame", &RobotModel::getModelFrame)
+      .def("get_joint_model_group_names", &RobotModel::getJointModelGroupNames)
+      .def("get_joint_model_group", &RobotModel::getJointModelGroup, py::return_value_policy::reference_internal,
+           py::arg("group"))
+      .def("get_link_model_names", &RobotModel::getLinkModelNames)
+      .def("get_joint_model_names", &RobotModel::getJointModelNames)
+      .def("get_active_joint_model_names", &RobotModel::getActiveJointModelNames)
 
-  py::class_<JointModelGroup, JointModelGroupPtr>(m, "JointModelGroup")
-      .def("getLinkModelNames", &JointModelGroup::getLinkModelNames)
-      .def("getJointModelNames", &JointModelGroup::getJointModelNames)
-      //
+      .def("get_link_pose", &RobotInterfacePython::getLinkPose, py::arg("name"))
+      .def("get_planning_frame", &RobotInterfacePython::getPlanningFrame)
+      .def("get_current_state", &RobotInterfacePython::getCurrentState)
+      .def("get_current_variable_values", &RobotInterfacePython::getCurrentVariableValues)
+      .def("get_current_joint_values", &RobotInterfacePython::getCurrentJointValues, py::arg("name"))
+      .def("get_joint_values", &RobotInterfacePython::getJointValues, py::arg("group"), py::arg("named_state"))
+      .def("get_robot_root_link", &RobotInterfacePython::getRobotRootLink)
+      .def("has_group", &RobotInterfacePython::hasGroup, py::arg("group"))
+      .def("get_robot_name", &RobotInterfacePython::getRobotName)
+      .def("get_robot_markers", &RobotInterfacePython::getRobotMarkers)
+      .def("get_robot_markers", &RobotInterfacePython::getRobotMarkersPythonList, py::arg("links"))
+      .def("get_robot_markers", &RobotInterfacePython::getRobotMarkersFromMsg, py::arg("robot_state_msg"))
+      .def("get_robot_markers", &RobotInterfacePython::getRobotMarkersPythonDictList, py::arg("joint_values"),
+           py::arg("links"))
+      .def("get_robot_markers", &RobotInterfacePython::getRobotMarkersPythonDict, py::arg("joint_values"))
+      .def("get_group_markers", &RobotInterfacePython::getRobotMarkersGroup, py::arg("group"))
+      .def("get_group_markers", &RobotInterfacePython::getRobotMarkersGroupPythonDict, py::arg("group"),
+           py::arg("joint_values"))
+      .def("get_parent_group", &RobotInterfacePython::getEndEffectorParentGroup, py::arg("group"))
+      // keep semicolon on next line
       ;
 }