diff --git a/include/franky/robot.hpp b/include/franky/robot.hpp index 4377b3cd..5739def1 100644 --- a/include/franky/robot.hpp +++ b/include/franky/robot.hpp @@ -133,7 +133,7 @@ class Robot : public franka::Robot { [[nodiscard]] inline CartesianState currentCartesianState() { auto s = state(); return {{Affine(Eigen::Matrix4d::Map(s.O_T_EE.data())), s.elbow[0]}, - RobotVelocity(Vector6d::Map(s.O_dP_EE_c.data()), std::optional(s.delbow_c[0]))}; + RobotVelocity(franka::CartesianVelocities(s.O_dP_EE_c, s.delbow_c))}; } [[nodiscard]] Vector7d currentJointPositions(); diff --git a/include/franky/robot_velocity.hpp b/include/franky/robot_velocity.hpp index f42cafb0..ed27b5d4 100644 --- a/include/franky/robot_velocity.hpp +++ b/include/franky/robot_velocity.hpp @@ -11,13 +11,19 @@ namespace franky { class RobotVelocity { public: + RobotVelocity(); + RobotVelocity(const RobotVelocity &robot_velocity); - RobotVelocity(Twist end_effector_twist, std::optional elbow_velocity = std::nullopt); + RobotVelocity(RobotVelocity &&robot_velocity) noexcept; - explicit RobotVelocity(const Vector7d &vector_repr, bool ignore_elbow = false); + // Suppress implicit conversion warning +#pragma clang diagnostic push +#pragma clang diagnostic ignored "-Wimplicit-conversion" + RobotVelocity(Twist end_effector_twist, double elbow_velocity = 0.0); +#pragma clang diagnostic pop - explicit RobotVelocity(const Vector6d &vector_repr, std::optional elbow_velocity = std::nullopt); + explicit RobotVelocity(const Vector7d &vector_repr); explicit RobotVelocity(franka::CartesianVelocities franka_velocity); @@ -42,10 +48,6 @@ class RobotVelocity { return {end_effector_twist_.propagateThroughLink(offset_world_frame), elbow_velocity_}; } - [[nodiscard]] inline RobotVelocity with_elbow_velocity(const std::optional elbow_velocity) const { - return {end_effector_twist_, elbow_velocity}; - } - [[nodiscard]] inline Twist end_effector_twist() const { return end_effector_twist_; } @@ -56,7 +58,7 @@ class RobotVelocity { private: Twist end_effector_twist_; - std::optional elbow_velocity_; + double elbow_velocity_ = 0.0; }; inline RobotVelocity operator*(const Affine &affine, const RobotVelocity &robot_velocity) { diff --git a/python/python.cpp b/python/python.cpp index 51eb50f7..5464522f 100644 --- a/python/python.cpp +++ b/python/python.cpp @@ -532,10 +532,9 @@ PYBIND11_MODULE(_franky, m) { py::implicitly_convertible(); py::class_(m, "RobotVelocity") - .def(py::init>(), "end_effector_twist"_a, "elbow_velocity"_a = std::nullopt) + .def(py::init(), "end_effector_twist"_a, "elbow_velocity"_a = 0.0) .def(py::init()) // Copy constructor .def("change_end_effector_frame", &RobotVelocity::changeEndEffectorFrame, "offset_world_frame"_a) - .def("with_elbow_velocity", &RobotVelocity::with_elbow_velocity, "elbow_velocity"_a) .def_property_readonly("end_effector_twist", &RobotVelocity::end_effector_twist) .def_property_readonly("elbow_velocity", &RobotVelocity::elbow_velocity) .def("__rmul__", @@ -550,9 +549,9 @@ PYBIND11_MODULE(_franky, m) { return py::make_tuple(robot_velocity.end_effector_twist(), robot_velocity.elbow_velocity()); }, [](const py::tuple &t) { // __setstate__ - if (t.size() != 3) + if (t.size() != 2) throw std::runtime_error("Invalid state!"); - return RobotVelocity(t[0].cast(), t[1].cast>()); + return RobotVelocity(t[0].cast(), t[1].cast()); } )); diff --git a/src/robot_velocity.cpp b/src/robot_velocity.cpp index 4430f048..a625501b 100644 --- a/src/robot_velocity.cpp +++ b/src/robot_velocity.cpp @@ -9,26 +9,29 @@ namespace franky { +RobotVelocity::RobotVelocity() = default; + RobotVelocity::RobotVelocity(const RobotVelocity &) = default; -RobotVelocity::RobotVelocity(Twist end_effector_twist, std::optional elbow_velocity) +RobotVelocity::RobotVelocity(RobotVelocity &&) noexcept = default; + +RobotVelocity::RobotVelocity(Twist end_effector_twist, double elbow_velocity) : end_effector_twist_(std::move(end_effector_twist)), elbow_velocity_(elbow_velocity) {} -RobotVelocity::RobotVelocity(const Vector7d &vector_repr, bool ignore_elbow) - : RobotVelocity( - vector_repr.head<6>(), - ignore_elbow ? std::optional(std::nullopt) : vector_repr[6]) {} - -RobotVelocity::RobotVelocity(const Vector6d &vector_repr, std::optional elbow_velocity) - : end_effector_twist_(vector_repr), elbow_velocity_(elbow_velocity) {} +RobotVelocity::RobotVelocity(const Vector7d &vector_repr) + : RobotVelocity(Twist{vector_repr.head<6>()}, vector_repr[6]) {} RobotVelocity::RobotVelocity(const franka::CartesianVelocities franka_velocity) - : RobotVelocity(Vector6d::Map(franka_velocity.O_dP_EE.data()), std::optional(franka_velocity.elbow[0])) {} + : RobotVelocity( + { + Vector6d::Map(franka_velocity.O_dP_EE.data()).tail<3>(), + Vector6d::Map(franka_velocity.O_dP_EE.data()).head<3>() + }, franka_velocity.elbow[0]) {} Vector7d RobotVelocity::vector_repr() const { Vector7d result; - result << end_effector_twist_.vector_repr(), elbow_velocity_.value_or(0.0); + result << end_effector_twist_.vector_repr(), elbow_velocity_; return result; } @@ -36,10 +39,7 @@ franka::CartesianVelocities RobotVelocity::as_franka_velocity() const { std::array array; auto vec = vector_repr().head<6>(); std::copy(vec.data(), vec.data() + array.size(), array.begin()); - if (elbow_velocity_.has_value()) { - return franka::CartesianVelocities(array, {elbow_velocity_.value(), -1}); - } - return {array}; + return franka::CartesianVelocities(array, {elbow_velocity_, -1}); } } // namespace franky