Fixed RobotVelocity transformation

Author: TimSchneider42

include/franky/robot_velocity.hpp

@@ -29,13 +29,17 @@ class RobotVelocity {
 
   [[nodiscard]] franka::CartesianVelocities as_franka_velocity() const;
 
-  [[nodiscard]] inline RobotVelocity transform(const Affine &transform) const {
-    return RobotVelocity(
-        transform * end_effector_linear_velocity_, transform * end_effector_angular_velocity_, elbow_velocity_);
+  [[nodiscard]] inline RobotVelocity transform(const Affine &affine) const {
+    return transform(affine.rotation());
+  }
+
+  template<typename RotationMatrixType>
+  [[nodiscard]] inline RobotVelocity transform(const RotationMatrixType &rotation) const {
+    return {rotation * end_effector_linear_velocity_, rotation * end_effector_angular_velocity_, elbow_velocity_};
   }
 
   [[nodiscard]] inline RobotVelocity with_elbow_velocity(const std::optional<double> elbow_velocity) const {
-    return RobotVelocity(end_effector_linear_velocity_, end_effector_angular_velocity_, elbow_velocity);
+    return {end_effector_linear_velocity_, end_effector_angular_velocity_, elbow_velocity};
   }
 
   [[nodiscard]] inline Eigen::Vector3d end_effector_linear_velocity() const {
@@ -56,8 +60,13 @@ class RobotVelocity {
   std::optional<double> elbow_velocity_;
 };
 
-inline RobotVelocity operator*(const Affine &transform, const RobotVelocity &robot_velocity) {
-  return robot_velocity.transform(transform);
+inline RobotVelocity operator*(const Affine &affine, const RobotVelocity &robot_velocity) {
+  return robot_velocity.transform(affine);
+}
+
+template<typename RotationMatrixType>
+inline RobotVelocity operator*(const RotationMatrixType &rotation, const RobotVelocity &robot_velocity) {
+  return robot_velocity.transform(rotation);
 }
 
 }  // namespace franky

python/python.cpp

@@ -476,7 +476,9 @@ PYBIND11_MODULE(_franky, m) {
       .def("with_elbow_position", &RobotPose::with_elbow_position, "elbow_position"_a)
       .def_property_readonly("end_effector_pose", &RobotPose::end_effector_pose)
       .def_property_readonly("elbow_position", &RobotPose::elbow_position)
-      .def("__mul__", py::overload_cast<const RobotPose &, const Affine &>(&operator*), py::is_operator())
+      .def("__mul__",
+           [](const RobotPose &robot_pose, const Affine &affine) { return robot_pose * affine; },
+           py::is_operator())
       .def("__rmul__",
            [](const RobotPose &robot_pose, const Affine &affine) { return affine * robot_pose; },
            py::is_operator())
@@ -506,6 +508,9 @@ PYBIND11_MODULE(_franky, m) {
       .def("__rmul__",
            [](const RobotVelocity &robot_velocity, const Affine &affine) { return affine * robot_velocity; },
            py::is_operator())
+      .def("__rmul__",
+           [](const RobotVelocity &robot_velocity, const Eigen::Quaterniond &quaternion) { return quaternion * robot_velocity; },
+           py::is_operator())
       .def("__repr__", robotVelocityToStr)
       .def(py::pickle(
           [](const RobotVelocity &robot_velocity) {  // __getstate__