Implemented CartesianPose.changeEndEffectorFrame

TimSchneider42 · May 27, 2024 · 4f86361 · 4f86361
1 parent 215de7d
commit 4f86361
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 0 deletions.
diff --git a/include/franky/cartesian_state.hpp b/include/franky/cartesian_state.hpp
@@ -27,6 +27,11 @@ class CartesianState {
     return {transform * pose_, transform * velocity_};
   }
 
+  [[nodiscard]] CartesianState changeEndEffectorFrame(const Affine &transform) const {
+    auto offset_world_frame = pose_.end_effector_pose() * transform.translation();
+    return {pose_.changeEndEffectorFrame(transform), velocity_.changeEndEffectorFrame(offset_world_frame)};
+  }
+
   [[nodiscard]] inline RobotPose pose() const {
     return pose_;
   }

diff --git a/python/python.cpp b/python/python.cpp
@@ -560,6 +560,7 @@ PYBIND11_MODULE(_franky, m) {
       .def(py::init<const RobotPose &, const RobotVelocity &>(), "pose"_a,"velocity"_a)
       .def(py::init<const CartesianState &>()) // Copy constructor
       .def("transform_with", &CartesianState::transformWith, "transform"_a)
+      .def("change_end_effector_frame", &CartesianState::changeEndEffectorFrame, "transform"_a)
       .def_property_readonly("pose", &CartesianState::pose)
       .def_property_readonly("velocity", &CartesianState::velocity)
       .def("__rmul__",