From 101fc6d4546b01cc6b69aa53073a81e02a0ec924 Mon Sep 17 00:00:00 2001 From: Tim Schneider Date: Mon, 27 May 2024 21:52:43 +0200 Subject: [PATCH] Implemented non-zero velocity joint waypoints --- franky/__init__.py | 1 + include/franky.hpp | 1 + include/franky/joint_state.hpp | 40 +++++++++++++++++++ include/franky/motion/joint_motion.hpp | 4 +- .../franky/motion/joint_waypoint_motion.hpp | 13 +++--- include/franky/motion/stop_motion.hpp | 4 +- include/franky/motion/waypoint_motion.hpp | 1 + python/python.cpp | 29 +++++++++++++- src/motion/joint_motion.cpp | 2 +- src/motion/joint_waypoint_motion.cpp | 18 ++++----- 10 files changed, 88 insertions(+), 25 deletions(-) create mode 100644 include/franky/joint_state.hpp diff --git a/franky/__init__.py b/franky/__init__.py index fc27dff4..57651212 100644 --- a/franky/__init__.py +++ b/franky/__init__.py @@ -39,6 +39,7 @@ RobotPose, RobotVelocity, CartesianState, + JointState, CartesianWaypoint, Affine, NullSpaceHandling, diff --git a/include/franky.hpp b/include/franky.hpp index 3daac10f..d73ed31d 100644 --- a/include/franky.hpp +++ b/include/franky.hpp @@ -26,6 +26,7 @@ #include "franky/cartesian_state.hpp" #include "franky/control_signal_type.hpp" #include "franky/gripper.hpp" +#include "franky/joint_state.hpp" #include "franky/kinematics.hpp" #include "franky/robot.hpp" #include "franky/robot_pose.hpp" diff --git a/include/franky/joint_state.hpp b/include/franky/joint_state.hpp new file mode 100644 index 00000000..1de3ca68 --- /dev/null +++ b/include/franky/joint_state.hpp @@ -0,0 +1,40 @@ +#pragma once + +#include +#include +#include + +#include "franky/robot_pose.hpp" +#include "franky/robot_velocity.hpp" + +namespace franky { + +class JointState { + public: + // Suppress implicit conversion warning +#pragma clang diagnostic push +#pragma clang diagnostic ignored "-Wimplicit-conversion" + JointState(Vector7d position) : position_(std::move(position)), velocity_(Vector7d::Zero()) {} +#pragma clang diagnostic pop + + JointState(Vector7d pose, Vector7d velocity) + : position_(std::move(pose)), velocity_(std::move(velocity)) {} + + JointState(const JointState &) = default; + + JointState() = default; + + [[nodiscard]] inline Vector7d position() const { + return position_; + } + + [[nodiscard]] inline Vector7d velocity() const { + return velocity_; + } + + private: + Vector7d position_; + Vector7d velocity_; +}; + +} // namespace franky diff --git a/include/franky/motion/joint_motion.hpp b/include/franky/motion/joint_motion.hpp index 9459d3cb..28639449 100644 --- a/include/franky/motion/joint_motion.hpp +++ b/include/franky/motion/joint_motion.hpp @@ -4,7 +4,7 @@ #include #include -#include "franky/robot_pose.hpp" +#include "franky/joint_state.hpp" #include "franky/motion/joint_waypoint_motion.hpp" #include "franky/motion/reference_type.hpp" @@ -13,7 +13,7 @@ namespace franky { class JointMotion : public JointWaypointMotion { public: explicit JointMotion( - const Vector7d &target, + const JointState &target, ReferenceType reference_type, RelativeDynamicsFactor relative_dynamics_factor, bool return_when_finished); diff --git a/include/franky/motion/joint_waypoint_motion.hpp b/include/franky/motion/joint_waypoint_motion.hpp index b6fff0a8..3900d7d4 100644 --- a/include/franky/motion/joint_waypoint_motion.hpp +++ b/include/franky/motion/joint_waypoint_motion.hpp @@ -6,19 +6,16 @@ #include -#include "franky/robot_pose.hpp" -#include "franky/robot.hpp" -#include "franky/motion/reference_type.hpp" -#include "franky/util.hpp" +#include "franky/joint_state.hpp" #include "franky/motion/waypoint_motion.hpp" namespace franky { -class JointWaypointMotion : public WaypointMotion { +class JointWaypointMotion : public WaypointMotion { public: - explicit JointWaypointMotion(const std::vector> &waypoints); + explicit JointWaypointMotion(const std::vector> &waypoints); - explicit JointWaypointMotion(const std::vector> &waypoints, Params params); + explicit JointWaypointMotion(const std::vector> &waypoints, Params params); protected: @@ -30,7 +27,7 @@ class JointWaypointMotion : public WaypointMotion &previous_command, - const Waypoint &new_waypoint, + const Waypoint &new_waypoint, ruckig::InputParameter<7> &input_parameter) override; [[nodiscard]] std::tuple getAbsoluteInputLimits() const override; diff --git a/include/franky/motion/stop_motion.hpp b/include/franky/motion/stop_motion.hpp index ea828045..d85169eb 100644 --- a/include/franky/motion/stop_motion.hpp +++ b/include/franky/motion/stop_motion.hpp @@ -13,8 +13,8 @@ template<> class StopMotion : public JointWaypointMotion { public: explicit StopMotion() : JointWaypointMotion( - {{ - .target=Vector7d::Zero(), + {Waypoint{ + .target=JointState(Vector7d::Zero()), .reference_type= ReferenceType::Relative, .relative_dynamics_factor = RelativeDynamicsFactor::MAX_DYNAMICS() }}) {} diff --git a/include/franky/motion/waypoint_motion.hpp b/include/franky/motion/waypoint_motion.hpp index 4eeeb2b4..6155f900 100644 --- a/include/franky/motion/waypoint_motion.hpp +++ b/include/franky/motion/waypoint_motion.hpp @@ -7,6 +7,7 @@ #include "franky/util.hpp" #include "franky/motion/motion.hpp" #include "franky/relative_dynamics_factor.hpp" +#include "franky/motion/reference_type.hpp" namespace franky { diff --git a/python/python.cpp b/python/python.cpp index aed7ae09..485acb00 100644 --- a/python/python.cpp +++ b/python/python.cpp @@ -584,6 +584,31 @@ PYBIND11_MODULE(_franky, m) { )); py::implicitly_convertible(); + py::class_(m, "JointState") + .def(py::init(), "position"_a) + .def(py::init(), "position"_a,"velocity"_a) + .def(py::init()) // Copy constructor + .def_property_readonly("position", &JointState::position) + .def_property_readonly("velocity", &JointState::velocity) + .def("__repr__", [](const JointState &joint_state) { + std::stringstream ss; + ss << "(pose=" << vecToStr(joint_state.position()) + << ", velocity=" << vecToStr(joint_state.velocity()) << ")"; + return ss.str(); + }) + .def(py::pickle( + [](const JointState &joint_state) { // __getstate__ + return py::make_tuple(joint_state.position(), joint_state.velocity()); + }, + [](const py::tuple &t) { // __setstate__ + if (t.size() != 2) + throw std::runtime_error("Invalid state!"); + return JointState(t[0].cast(), t[1].cast()); + } + )); + py::implicitly_convertible(); + + py::class_(m, "NullSpaceHandling") .def(py::init(), "joint_index"_a, "value"_a) .def_readwrite("joint_index", &Kinematics::NullSpaceHandling::joint_index) @@ -745,7 +770,7 @@ PYBIND11_MODULE(_franky, m) { py::class_, std::shared_ptr>( m, "JointWaypointMotion") .def(py::init<>([]( - const std::vector> &waypoints, + const std::vector> &waypoints, RelativeDynamicsFactor relative_dynamics_factor, bool return_when_finished) { return std::make_shared( @@ -757,7 +782,7 @@ PYBIND11_MODULE(_franky, m) { "return_when_finished"_a = true); py::class_>(m, "JointMotion") - .def(py::init(), + .def(py::init(), "target"_a, py::arg_v("reference_type", ReferenceType::Absolute, "_franky.ReferenceType.Absolute"), "relative_dynamics_factor"_a = 1.0, diff --git a/src/motion/joint_motion.cpp b/src/motion/joint_motion.cpp index 4c2bd533..723218e6 100644 --- a/src/motion/joint_motion.cpp +++ b/src/motion/joint_motion.cpp @@ -5,7 +5,7 @@ namespace franky { JointMotion::JointMotion( - const Vector7d &target, + const JointState &target, ReferenceType reference_type, RelativeDynamicsFactor relative_dynamics_factor, bool return_when_finished) diff --git a/src/motion/joint_waypoint_motion.cpp b/src/motion/joint_waypoint_motion.cpp index 79469edb..170eb6d4 100644 --- a/src/motion/joint_waypoint_motion.cpp +++ b/src/motion/joint_waypoint_motion.cpp @@ -2,17 +2,15 @@ #include -#include "franky/robot_pose.hpp" -#include "franky/robot.hpp" -#include "franky/util.hpp" +#include "franky/motion/reference_type.hpp" namespace franky { -JointWaypointMotion::JointWaypointMotion(const std::vector> &waypoints) +JointWaypointMotion::JointWaypointMotion(const std::vector> &waypoints) : JointWaypointMotion(waypoints, Params()) {} -JointWaypointMotion::JointWaypointMotion(const std::vector> &waypoints, Params params) - : WaypointMotion(waypoints, params) {} +JointWaypointMotion::JointWaypointMotion(const std::vector> &waypoints, Params params) + : WaypointMotion(waypoints, params) {} void JointWaypointMotion::initWaypointMotion( const franka::RobotState &robot_state, @@ -34,13 +32,13 @@ franka::JointPositions JointWaypointMotion::getControlSignal( void JointWaypointMotion::setNewWaypoint( const franka::RobotState &robot_state, const std::optional &previous_command, - const Waypoint &new_waypoint, + const Waypoint &new_waypoint, ruckig::InputParameter<7> &input_parameter) { auto new_target = new_waypoint.target; if (new_waypoint.reference_type == ReferenceType::Relative) - new_target += toEigen(input_parameter.current_position); - input_parameter.target_position = toStd<7>(new_target); - input_parameter.target_velocity = toStd<7>(Vector7d::Zero()); + new_target.position() += toEigen(input_parameter.current_position); + input_parameter.target_position = toStd<7>(new_target.position()); + input_parameter.target_velocity = toStd<7>(new_target.velocity()); input_parameter.target_acceleration = toStd<7>(Vector7d::Zero()); }