add forward kinematics to robot state

moveit · Sep 3, 2022 · b2ed79c · b2ed79c
1 parent 79cdab9
commit b2ed79c
Show file tree

Hide file tree

Showing 3 changed files with 24 additions and 0 deletions.
diff --git a/python/moveit_py/src/moveit_py/core.cpp b/python/moveit_py/src/moveit_py/core.cpp
@@ -392,6 +392,19 @@ PYBIND11_MODULE(core, m)
                :py:class:`numpy.ndarray`: The accelerations of the joints in the joint model group.
            )mydelimiter")
 
+      // Forward kinematics
+      .def("get_global_link_transform", &moveit_py::bind_robot_state::get_global_link_transform,
+	   py::arg("link_name"),
+	   R"mydelimiter(
+	   Returns the transform of the specified link in the global frame.
+
+	   Args:
+	       link_name (str): The name of the link to get the transform for.
+
+	   Returns:
+	       :py:class:`numpy.ndarray`: The transform of the specified link in the global frame.
+	   )mydelimiter")
+
       // Setting state from inverse kinematics
       .def("set_from_ik", &moveit_py::bind_robot_state::set_from_ik, py::arg("joint_model_group_name"),
            py::arg("geometry_pose"), py::arg("tip_name"), py::arg("timeout") = 0.0,

diff --git a/python/moveit_py/src/moveit_py/moveit_core/robot_state/robot_state.cpp b/python/moveit_py/src/moveit_py/moveit_core/robot_state/robot_state.cpp
@@ -50,6 +50,15 @@ Eigen::MatrixXd get_frame_transform(std::shared_ptr<moveit::core::RobotState>& r
   return transformation.matrix();
 }
 
+std::vector<Eigen::MatrixXd> get_global_link_transform(std::shared_ptr<moveit::core::RobotState>& robot_state, std::string& link_name)
+{
+  auto transformation = robot_state->getGlobalLinkTransform(link_name);
+  std::vector<Eigen::MatrixXd> transforms;
+  transforms.push_back(transformation.rotation());
+  transforms.push_back(transformation.translation());
+  return transforms;
+}
+
 py::object get_pose(std::shared_ptr<moveit::core::RobotState>& robot_state, std::string link_name)
 {
   Eigen::Isometry3d pose = robot_state->getGlobalLinkTransform(link_name);

diff --git a/python/moveit_py/src/moveit_py/moveit_core/robot_state/robot_state.h b/python/moveit_py/src/moveit_py/moveit_core/robot_state/robot_state.h
@@ -48,6 +48,8 @@ namespace bind_robot_state
 
 Eigen::MatrixXd get_frame_transform(std::shared_ptr<moveit::core::RobotState>& robot_state, std::string& frame_id);
 
+std::vector<Eigen::MatrixXd> get_global_link_transform(std::shared_ptr<moveit::core::RobotState>& robot_state, std::string& link_name);
+
 py::object get_pose(std::shared_ptr<moveit::core::RobotState>& robot_state, std::string link_name);
 
 Eigen::VectorXd copy_joint_group_positions(std::shared_ptr<moveit::core::RobotState>& robot_state,