diff --git a/franka_example_controllers/include/franka_example_controllers/cartesian_impedance_example_controller.h b/franka_example_controllers/include/franka_example_controllers/cartesian_impedance_example_controller.h index e4ea62ca8e6bde9ba988540b6289eb794717ab55..4b496cfe340a75245913d6064ba789fb93c107af 100644 --- a/franka_example_controllers/include/franka_example_controllers/cartesian_impedance_example_controller.h +++ b/franka_example_controllers/include/franka_example_controllers/cartesian_impedance_example_controller.h @@ -26,10 +26,9 @@ class CartesianImpedanceExampleController : public controller_interface::MultiIn hardware_interface::EffortJointInterface, franka_hw::FrankaStateInterface> { public: - CartesianImpedanceExampleController(); - bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& node_handle); - void starting(const ros::Time&); - void update(const ros::Time&, const ros::Duration& period); + bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& node_handle) override; + void starting(const ros::Time&) override; + void update(const ros::Time&, const ros::Duration& period) override; private: std::string arm_id_; diff --git a/franka_example_controllers/include/franka_example_controllers/cartesian_pose_example_controller.h b/franka_example_controllers/include/franka_example_controllers/cartesian_pose_example_controller.h index 59d5aa01aba1855003b5079f664e3ba920436303..6a747dfb933b91dafe21ca1b93d3d0f3bf8e79ed 100644 --- a/franka_example_controllers/include/franka_example_controllers/cartesian_pose_example_controller.h +++ b/franka_example_controllers/include/franka_example_controllers/cartesian_pose_example_controller.h @@ -7,6 +7,7 @@ #include <string> #include <controller_interface/multi_interface_controller.h> +#include <franka_hw/franka_state_interface.h> #include <hardware_interface/robot_hw.h> #include <ros/node_handle.h> #include <ros/time.h> @@ -15,24 +16,22 @@ namespace franka_example_controllers { -class CartesianPoseExampleController : public controller_interface::MultiInterfaceController< - franka_hw::FrankaPoseCartesianInterface> { +class CartesianPoseExampleController + : public controller_interface::MultiInterfaceController<franka_hw::FrankaPoseCartesianInterface, + franka_hw::FrankaStateInterface> { public: - CartesianPoseExampleController(); bool init(hardware_interface::RobotHW* robot_hardware, ros::NodeHandle& root_node_handle, - ros::NodeHandle&); - void starting(const ros::Time&); - - void update(const ros::Time&, const ros::Duration& period); + ros::NodeHandle& /* controller_node_handle */) override; + void starting(const ros::Time&) override; + void update(const ros::Time&, const ros::Duration& period) override; private: std::string arm_id_; franka_hw::FrankaPoseCartesianInterface* cartesian_pose_interface_; - std::array<double, 16> initial_pose_ = { - {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; - ros::Duration elapsed_time_; std::unique_ptr<franka_hw::FrankaCartesianPoseHandle> cartesian_pose_handle_; + ros::Duration elapsed_time_; + std::array<double, 16> initial_pose_{}; }; } // namespace franka_example_controllers diff --git a/franka_example_controllers/include/franka_example_controllers/cartesian_velocity_example_controller.h b/franka_example_controllers/include/franka_example_controllers/cartesian_velocity_example_controller.h index ccc8ff33304dd1525f641bfc667b2c53fa307891..bbeba97e6e1be0e5e797b7c413bccb41f956de7a 100644 --- a/franka_example_controllers/include/franka_example_controllers/cartesian_velocity_example_controller.h +++ b/franka_example_controllers/include/franka_example_controllers/cartesian_velocity_example_controller.h @@ -7,6 +7,7 @@ #include <controller_interface/multi_interface_controller.h> #include <franka_hw/franka_cartesian_command_interface.h> +#include <franka_hw/franka_state_interface.h> #include <hardware_interface/robot_hw.h> #include <ros/node_handle.h> #include <ros/time.h> @@ -14,14 +15,15 @@ namespace franka_example_controllers { class CartesianVelocityExampleController : public controller_interface::MultiInterfaceController< - franka_hw::FrankaVelocityCartesianInterface> { + franka_hw::FrankaVelocityCartesianInterface, + franka_hw::FrankaStateInterface> { public: - CartesianVelocityExampleController(); bool init(hardware_interface::RobotHW* robot_hardware, ros::NodeHandle& root_node_handle, - ros::NodeHandle&); - void update(const ros::Time&, const ros::Duration& period); - void stopping(const ros::Time&); + ros::NodeHandle& /* controller_node_handle */) override; + void update(const ros::Time&, const ros::Duration& period) override; + void starting(const ros::Time&) override; + void stopping(const ros::Time&) override; private: std::string arm_id_; diff --git a/franka_example_controllers/include/franka_example_controllers/force_example_controller.h b/franka_example_controllers/include/franka_example_controllers/force_example_controller.h index 1100ff92745e86ed55d53f4cbd89335c9168fdac..642738691ca78fe90154446eeb91720120a35934 100644 --- a/franka_example_controllers/include/franka_example_controllers/force_example_controller.h +++ b/franka_example_controllers/include/franka_example_controllers/force_example_controller.h @@ -25,10 +25,9 @@ class ForceExampleController : public controller_interface::MultiInterfaceContro hardware_interface::EffortJointInterface, franka_hw::FrankaStateInterface> { public: - ForceExampleController(); - bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& node_handle); - void starting(const ros::Time&); - void update(const ros::Time&, const ros::Duration& period); + bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& node_handle) override; + void starting(const ros::Time&) override; + void update(const ros::Time&, const ros::Duration& period) override; private: std::unique_ptr<franka_hw::FrankaModelHandle> model_handle_; diff --git a/franka_example_controllers/include/franka_example_controllers/joint_impedance_example_controller.h b/franka_example_controllers/include/franka_example_controllers/joint_impedance_example_controller.h index 240fbddcd6f20bb2f0f869f97a068f24e37fcbc9..c10a7dafdb834c40d21636c4cebe89dee2596836 100644 --- a/franka_example_controllers/include/franka_example_controllers/joint_impedance_example_controller.h +++ b/franka_example_controllers/include/franka_example_controllers/joint_impedance_example_controller.h @@ -25,17 +25,16 @@ class JointImpedanceExampleController : public controller_interface::MultiInterf hardware_interface::EffortJointInterface, franka_hw::FrankaPoseCartesianInterface> { public: - JointImpedanceExampleController(); - bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& node_handle); - void starting(const ros::Time&); - void update(const ros::Time&, const ros::Duration& period); + bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& node_handle) override; + void starting(const ros::Time&) override; + void update(const ros::Time&, const ros::Duration& period) override; private: std::unique_ptr<franka_hw::FrankaCartesianPoseHandle> cartesian_pose_handle_; std::unique_ptr<franka_hw::FrankaModelHandle> model_handle_; std::vector<hardware_interface::JointHandle> joint_handles_; std::array<double, 7> last_tau_d_ = {{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; - franka_hw::TriggerRate rate_trigger_; + franka_hw::TriggerRate rate_trigger_{1.0}; double radius_{0.1}; double acceleration_time_{2.0}; double vel_max_{0.05}; diff --git a/franka_example_controllers/include/franka_example_controllers/joint_position_example_controller.h b/franka_example_controllers/include/franka_example_controllers/joint_position_example_controller.h index 59938c68ca1ecb27a6815dbebf82c28b2aecb165..2527cb33502a3c4df6654a7501df9b4ace44e294 100644 --- a/franka_example_controllers/include/franka_example_controllers/joint_position_example_controller.h +++ b/franka_example_controllers/include/franka_example_controllers/joint_position_example_controller.h @@ -20,16 +20,16 @@ class JointPositionExampleController : public controller_interface::MultiInterfa JointPositionExampleController(); bool init(hardware_interface::RobotHW* robot_hardware, ros::NodeHandle& root_node_handle, - ros::NodeHandle& /*controller_node_handle*/); - void starting(const ros::Time&); - void update(const ros::Time&, const ros::Duration& period); + ros::NodeHandle& /* controller_node_handle */) override; + void starting(const ros::Time&) override; + void update(const ros::Time&, const ros::Duration& period) override; private: std::vector<std::string> joint_names_; hardware_interface::PositionJointInterface* position_joint_interface_; std::vector<hardware_interface::JointHandle> position_joint_handles_; - std::array<double, 7> initial_pose_ = {{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; ros::Duration elapsed_time_; + std::array<double, 7> initial_pose_{}; }; } // namespace franka_example_controllers diff --git a/franka_example_controllers/include/franka_example_controllers/joint_velocity_example_controller.h b/franka_example_controllers/include/franka_example_controllers/joint_velocity_example_controller.h index 34bec83ef29579f14c4c7e24a94cc96ffb460147..d236007beb338a77507d0ae0dbd78267e439900b 100644 --- a/franka_example_controllers/include/franka_example_controllers/joint_velocity_example_controller.h +++ b/franka_example_controllers/include/franka_example_controllers/joint_velocity_example_controller.h @@ -6,6 +6,7 @@ #include <vector> #include <controller_interface/multi_interface_controller.h> +#include <franka_hw/franka_state_interface.h> #include <hardware_interface/joint_command_interface.h> #include <hardware_interface/robot_hw.h> #include <ros/node_handle.h> @@ -14,14 +15,15 @@ namespace franka_example_controllers { class JointVelocityExampleController : public controller_interface::MultiInterfaceController< - hardware_interface::VelocityJointInterface> { + hardware_interface::VelocityJointInterface, + franka_hw::FrankaStateInterface> { public: - JointVelocityExampleController(); bool init(hardware_interface::RobotHW* robot_hardware, ros::NodeHandle& root_node_handle, - ros::NodeHandle&); - void update(const ros::Time&, const ros::Duration& period); - void stopping(const ros::Time&); + ros::NodeHandle& /* controller_node_handle */) override; + void update(const ros::Time&, const ros::Duration& period) override; + void starting(const ros::Time&) override; + void stopping(const ros::Time&) override; private: std::vector<std::string> joint_names_; diff --git a/franka_example_controllers/include/franka_example_controllers/model_example_controller.h b/franka_example_controllers/include/franka_example_controllers/model_example_controller.h index fd1139a03ebbd9f7596f9596bc0c6c444beef959..c80ad74761784a53ef2f875b3c8058f31bc9f1e6 100644 --- a/franka_example_controllers/include/franka_example_controllers/model_example_controller.h +++ b/franka_example_controllers/include/franka_example_controllers/model_example_controller.h @@ -19,17 +19,16 @@ namespace franka_example_controllers { class ModelExampleController : public controller_interface::MultiInterfaceController<franka_hw::FrankaModelInterface> { public: - ModelExampleController(); - bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& node_handle); - void update(const ros::Time&, const ros::Duration&); + bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& node_handle) override; + void update(const ros::Time&, const ros::Duration&) override; private: franka_hw::FrankaStateInterface* franka_state_interface_; std::unique_ptr<franka_hw::FrankaStateHandle> franka_state_handle_; franka_hw::FrankaModelInterface* model_interface_; std::unique_ptr<franka_hw::FrankaModelHandle> model_handle_; - franka_hw::TriggerRate rate_trigger_; std::string arm_id_; + franka_hw::TriggerRate rate_trigger_{1.0}; }; } // namespace franka_example_controllers diff --git a/franka_example_controllers/package.xml b/franka_example_controllers/package.xml index 9c37a4f4e1c6d6e967afb7a9e95516639d8384af..0ccc65def7b4d2ebceb7c25544cb3d1fd1931c3a 100644 --- a/franka_example_controllers/package.xml +++ b/franka_example_controllers/package.xml @@ -2,7 +2,7 @@ <package format="2"> <name>franka_example_controllers</name> <version>0.2.0</version> - <description>The franka_example_controllers package</description> + <description>franka_example_controllers provides example code for controlling Franka Emika research robots with ros_control</description> <maintainer email="support@franka.de">Franka Emika GmbH</maintainer> <license>Apache 2.0</license> diff --git a/franka_example_controllers/src/cartesian_impedance_example_controller.cpp b/franka_example_controllers/src/cartesian_impedance_example_controller.cpp index 5e96ecbe35d0933076edbdcc262df3d4b82fb364..fec8a77193e717c59c0ef6868d0f08b51816c1f2 100644 --- a/franka_example_controllers/src/cartesian_impedance_example_controller.cpp +++ b/franka_example_controllers/src/cartesian_impedance_example_controller.cpp @@ -13,8 +13,6 @@ namespace franka_example_controllers { -CartesianImpedanceExampleController::CartesianImpedanceExampleController() = default; - bool CartesianImpedanceExampleController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& node_handle) { std::vector<double> cartesian_stiffness_vector; diff --git a/franka_example_controllers/src/cartesian_pose_example_controller.cpp b/franka_example_controllers/src/cartesian_pose_example_controller.cpp index 441119d4703d41786c9217ae220b0f7d348917db..f3e96ed13ab87ace19f166846cc48b66e20f6629 100644 --- a/franka_example_controllers/src/cartesian_pose_example_controller.cpp +++ b/franka_example_controllers/src/cartesian_pose_example_controller.cpp @@ -15,12 +15,9 @@ namespace franka_example_controllers { -CartesianPoseExampleController::CartesianPoseExampleController() - : cartesian_pose_interface_(nullptr), elapsed_time_(0.0), cartesian_pose_handle_(nullptr) {} - bool CartesianPoseExampleController::init(hardware_interface::RobotHW* robot_hardware, ros::NodeHandle& root_node_handle, - ros::NodeHandle& /*controller_node_handle*/) { + ros::NodeHandle& /* controller_node_handle */) { cartesian_pose_interface_ = robot_hardware->get<franka_hw::FrankaPoseCartesianInterface>(); if (cartesian_pose_interface_ == nullptr) { ROS_ERROR( @@ -39,18 +36,44 @@ bool CartesianPoseExampleController::init(hardware_interface::RobotHW* robot_har cartesian_pose_interface_->getHandle(arm_id_ + "_robot"))); } catch (const hardware_interface::HardwareInterfaceException& e) { ROS_ERROR_STREAM( - "CartesianPoseExampleController: Exception getting cartesian handle: " << e.what()); + "CartesianPoseExampleController: Exception getting Cartesian handle: " << e.what()); return false; } - elapsed_time_ = ros::Duration(0.0); + + auto state_interface = robot_hardware->get<franka_hw::FrankaStateInterface>(); + if (state_interface == nullptr) { + ROS_ERROR("CartesianPoseExampleController: Could not get state interface from hardware"); + return false; + } + + try { + auto state_handle = state_interface->getHandle(arm_id_ + "_robot"); + + std::array<double, 7> q_start{{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}}; + for (size_t i = 0; i < q_start.size(); i++) { + if (std::abs(state_handle.getRobotState().q_d[i] - q_start[i]) > 0.1) { + ROS_ERROR_STREAM( + "CartesianPoseExampleController: Robot is not in the expected starting position for " + "running this example. Run `roslaunch panda_moveit_config move_to_start.launch " + "robot_ip:=<robot-ip> load_gripper:=<has-attached-gripper>` first."); + return false; + } + } + } catch (const hardware_interface::HardwareInterfaceException& e) { + ROS_ERROR_STREAM( + "CartesianPoseExampleController: Exception getting state handle: " << e.what()); + return false; + } + return true; } -void CartesianPoseExampleController::starting(const ros::Time& /*time*/) { +void CartesianPoseExampleController::starting(const ros::Time& /* time */) { initial_pose_ = cartesian_pose_handle_->getRobotState().O_T_EE_d; + elapsed_time_ = ros::Duration(0.0); } -void CartesianPoseExampleController::update(const ros::Time& /*time*/, +void CartesianPoseExampleController::update(const ros::Time& /* time */, const ros::Duration& period) { elapsed_time_ += period; diff --git a/franka_example_controllers/src/cartesian_velocity_example_controller.cpp b/franka_example_controllers/src/cartesian_velocity_example_controller.cpp index 6a12e1b6716324c5f8b68a93be3fd47da8f54fd0..4593601e306845186716fbf75bdd5aaed66236e3 100644 --- a/franka_example_controllers/src/cartesian_velocity_example_controller.cpp +++ b/franka_example_controllers/src/cartesian_velocity_example_controller.cpp @@ -15,14 +15,9 @@ namespace franka_example_controllers { -CartesianVelocityExampleController::CartesianVelocityExampleController() - : velocity_cartesian_interface_(nullptr), - velocity_cartesian_handle_(nullptr), - elapsed_time_(0.0) {} - bool CartesianVelocityExampleController::init(hardware_interface::RobotHW* robot_hardware, ros::NodeHandle& root_node_handle, - ros::NodeHandle& /*controller_node_handle*/) { + ros::NodeHandle& /* controller_node_handle */) { if (!root_node_handle.getParam("arm_id", arm_id_)) { ROS_ERROR("CartesianVelocityExampleController: Could not get parameter arm_id"); return false; @@ -41,15 +36,44 @@ bool CartesianVelocityExampleController::init(hardware_interface::RobotHW* robot velocity_cartesian_interface_->getHandle(arm_id_ + "_robot"))); } catch (const hardware_interface::HardwareInterfaceException& e) { ROS_ERROR_STREAM( - "CartesianVelocityExampleController: Exception getting cartesian handle: " << e.what()); + "CartesianVelocityExampleController: Exception getting Cartesian handle: " << e.what()); + return false; + } + + auto state_interface = robot_hardware->get<franka_hw::FrankaStateInterface>(); + if (state_interface == nullptr) { + ROS_ERROR("CartesianVelocityExampleController: Could not get state interface from hardware"); + return false; + } + + try { + auto state_handle = state_interface->getHandle(arm_id_ + "_robot"); + + std::array<double, 7> q_start = {{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}}; + for (size_t i = 0; i < q_start.size(); i++) { + if (std::abs(state_handle.getRobotState().q_d[i] - q_start[i]) > 0.1) { + ROS_ERROR_STREAM( + "CartesianVelocityExampleController: Robot is not in the expected starting position " + "for " + "running this example. Run `roslaunch panda_moveit_config move_to_start.launch " + "robot_ip:=<robot-ip> load_gripper:=<has-attached-gripper>` first."); + return false; + } + } + } catch (const hardware_interface::HardwareInterfaceException& e) { + ROS_ERROR_STREAM( + "CartesianVelocityExampleController: Exception getting state handle: " << e.what()); return false; } - elapsed_time_ = ros::Duration(0.0); return true; } -void CartesianVelocityExampleController::update(const ros::Time& /*time*/, +void CartesianVelocityExampleController::starting(const ros::Time& /* time */) { + elapsed_time_ = ros::Duration(0.0); +} + +void CartesianVelocityExampleController::update(const ros::Time& /* time */, const ros::Duration& period) { elapsed_time_ += period; diff --git a/franka_example_controllers/src/force_example_controller.cpp b/franka_example_controllers/src/force_example_controller.cpp index a44c55f2d3bf18c662b20e2a0cb5ee8b3ba14a3a..a06f22d4b2f53cefa4adbb2515f0f0852175d5fc 100644 --- a/franka_example_controllers/src/force_example_controller.cpp +++ b/franka_example_controllers/src/force_example_controller.cpp @@ -12,8 +12,6 @@ namespace franka_example_controllers { -ForceExampleController::ForceExampleController() = default; - bool ForceExampleController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& node_handle) { std::vector<std::string> joint_names; diff --git a/franka_example_controllers/src/joint_impedance_example_controller.cpp b/franka_example_controllers/src/joint_impedance_example_controller.cpp index 991713c9cd5057ad9939fed35ce7a2160d8c585c..28025282161902b55286acaf7c9e45b7e9238335 100644 --- a/franka_example_controllers/src/joint_impedance_example_controller.cpp +++ b/franka_example_controllers/src/joint_impedance_example_controller.cpp @@ -23,8 +23,6 @@ std::ostream& operator<<(std::ostream& ostream, const std::array<T, N>& array) { namespace franka_example_controllers { -JointImpedanceExampleController::JointImpedanceExampleController() : rate_trigger_(1.0) {} - bool JointImpedanceExampleController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& node_handle) { std::string arm_id; diff --git a/franka_example_controllers/src/joint_position_example_controller.cpp b/franka_example_controllers/src/joint_position_example_controller.cpp index 8417ce4f2067486ad39fe5476f09f9c5ca93d328..3d680d18ff660b03a5709f87fc01b68ca5023251 100644 --- a/franka_example_controllers/src/joint_position_example_controller.cpp +++ b/franka_example_controllers/src/joint_position_example_controller.cpp @@ -13,12 +13,9 @@ namespace franka_example_controllers { -JointPositionExampleController::JointPositionExampleController() - : position_joint_interface_(nullptr), elapsed_time_(0.0) {} - bool JointPositionExampleController::init(hardware_interface::RobotHW* robot_hardware, ros::NodeHandle& root_node_handle, - ros::NodeHandle& /*controller_node_handle*/) { + ros::NodeHandle& /* controller_node_handle */) { position_joint_interface_ = robot_hardware->get<hardware_interface::PositionJointInterface>(); if (position_joint_interface_ == nullptr) { ROS_ERROR( @@ -46,14 +43,26 @@ bool JointPositionExampleController::init(hardware_interface::RobotHW* robot_har return false; } } - elapsed_time_ = ros::Duration(0.0); + + std::array<double, 7> q_start{{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}}; + for (size_t i = 0; i < q_start.size(); i++) { + if (std::abs(position_joint_handles_[i].getPosition() - q_start[i]) > 0.1) { + ROS_ERROR_STREAM( + "JointPositionExampleController: Robot is not in the expected starting position for " + "running this example. Run `roslaunch panda_moveit_config move_to_start.launch " + "robot_ip:=<robot-ip> load_gripper:=<has-attached-gripper>` first."); + return false; + } + } + return true; } -void JointPositionExampleController::starting(const ros::Time& /*time*/) { +void JointPositionExampleController::starting(const ros::Time& /* time */) { for (size_t i = 0; i < 7; ++i) { initial_pose_[i] = position_joint_handles_[i].getPosition(); } + elapsed_time_ = ros::Duration(0.0); } void JointPositionExampleController::update(const ros::Time& /*time*/, diff --git a/franka_example_controllers/src/joint_velocity_example_controller.cpp b/franka_example_controllers/src/joint_velocity_example_controller.cpp index 5db177cf6fd68cfbf896c1ace4cd0caa201cd103..5b701c32f9c5321fb17d01f5d14af85fccac8517 100644 --- a/franka_example_controllers/src/joint_velocity_example_controller.cpp +++ b/franka_example_controllers/src/joint_velocity_example_controller.cpp @@ -13,12 +13,9 @@ namespace franka_example_controllers { -JointVelocityExampleController::JointVelocityExampleController() - : velocity_joint_interface_(nullptr) {} - bool JointVelocityExampleController::init(hardware_interface::RobotHW* robot_hardware, ros::NodeHandle& root_node_handle, - ros::NodeHandle& /*controller_node_handle*/) { + ros::NodeHandle& /* controller_node_handle */) { velocity_joint_interface_ = robot_hardware->get<hardware_interface::VelocityJointInterface>(); if (velocity_joint_interface_ == nullptr) { ROS_ERROR( @@ -46,11 +43,46 @@ bool JointVelocityExampleController::init(hardware_interface::RobotHW* robot_har return false; } } - elapsed_time_ = ros::Duration(0.0); + + std::string arm_id; + if (!root_node_handle.getParam("arm_id", arm_id)) { + ROS_ERROR("JointVelocityExampleController: Could not get parameter arm_id"); + return false; + } + + auto state_interface = robot_hardware->get<franka_hw::FrankaStateInterface>(); + if (state_interface == nullptr) { + ROS_ERROR("JointVelocityExampleController: Could not get state interface from hardware"); + return false; + } + + try { + auto state_handle = state_interface->getHandle(arm_id + "_robot"); + + std::array<double, 7> q_start{{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}}; + for (size_t i = 0; i < q_start.size(); i++) { + if (std::abs(state_handle.getRobotState().q_d[i] - q_start[i]) > 0.1) { + ROS_ERROR_STREAM( + "JointVelocityExampleController: Robot is not in the expected starting position for " + "running this example. Run `roslaunch panda_moveit_config move_to_start.launch " + "robot_ip:=<robot-ip> load_gripper:=<has-attached-gripper>` first."); + return false; + } + } + } catch (const hardware_interface::HardwareInterfaceException& e) { + ROS_ERROR_STREAM( + "JointVelocityExampleController: Exception getting state handle: " << e.what()); + return false; + } + return true; } -void JointVelocityExampleController::update(const ros::Time& /*time*/, +void JointVelocityExampleController::starting(const ros::Time& /* time */) { + elapsed_time_ = ros::Duration(0.0); +} + +void JointVelocityExampleController::update(const ros::Time& /* time */, const ros::Duration& period) { elapsed_time_ += period; diff --git a/franka_example_controllers/src/model_example_controller.cpp b/franka_example_controllers/src/model_example_controller.cpp index 0bc0a876fa773f6d7b42c681465f3434291cbd74..1742520b717144ade02ab60330c0847c1f48df2c 100644 --- a/franka_example_controllers/src/model_example_controller.cpp +++ b/franka_example_controllers/src/model_example_controller.cpp @@ -29,13 +29,6 @@ std::ostream& operator<<(std::ostream& ostream, const std::array<T, N>& array) { namespace franka_example_controllers { -ModelExampleController::ModelExampleController() - : franka_state_interface_(nullptr), - franka_state_handle_(nullptr), - model_interface_(nullptr), - model_handle_(nullptr), - rate_trigger_(1.0) {} - bool ModelExampleController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& node_handle) { franka_state_interface_ = robot_hw->get<franka_hw::FrankaStateInterface>(); diff --git a/panda_moveit_config/config/panda_arm_hand.srdf b/panda_moveit_config/config/panda_arm_hand.srdf index d7697ec845fa2584f724ecba21de7fafaa6889e8..56981c799cc7cd62acc75184f627599bcb5cf70e 100644 --- a/panda_moveit_config/config/panda_arm_hand.srdf +++ b/panda_moveit_config/config/panda_arm_hand.srdf @@ -66,16 +66,16 @@ <joint name="panda_joint4" value="0" /> <joint name="panda_joint5" value="0" /> <joint name="panda_joint6" value="0" /> - <joint name="panda_joint7" value="0" /> + <joint name="panda_joint7" value="0.785" /> </group_state> <group_state name="ready" group="panda_arm"> <joint name="panda_joint1" value="0" /> - <joint name="panda_joint2" value="-0.2128" /> + <joint name="panda_joint2" value="-0.785" /> <joint name="panda_joint3" value="0" /> - <joint name="panda_joint4" value="-1.928" /> + <joint name="panda_joint4" value="-2.356" /> <joint name="panda_joint5" value="0" /> - <joint name="panda_joint6" value="1.7741" /> - <joint name="panda_joint7" value="0" /> + <joint name="panda_joint6" value="1.571" /> + <joint name="panda_joint7" value="0.785" /> </group_state> <group_state name="home" group="panda_arm_hand"> <joint name="panda_joint1" value="0" /> @@ -84,16 +84,16 @@ <joint name="panda_joint4" value="0" /> <joint name="panda_joint5" value="0" /> <joint name="panda_joint6" value="0" /> - <joint name="panda_joint7" value="0.7918" /> + <joint name="panda_joint7" value="0.785" /> </group_state> <group_state name="ready" group="panda_arm_hand"> <joint name="panda_joint1" value="0" /> - <joint name="panda_joint2" value="-0.3912" /> + <joint name="panda_joint2" value="-0.785" /> <joint name="panda_joint3" value="0" /> - <joint name="panda_joint4" value="-1.9161" /> + <joint name="panda_joint4" value="-2.356" /> <joint name="panda_joint5" value="0" /> - <joint name="panda_joint6" value="1.5029" /> - <joint name="panda_joint7" value="0.7918" /> + <joint name="panda_joint6" value="1.571" /> + <joint name="panda_joint7" value="0.785" /> </group_state> <!--END EFFECTOR: Purpose: Represent information about an end effector.--> <end_effector name="hand" parent_link="panda_hand" group="hand" parent_group="panda_arm_hand" /> diff --git a/panda_moveit_config/launch/move_to_start.launch b/panda_moveit_config/launch/move_to_start.launch index a6a966e950beec65182db255fbd4fd406756ab9a..1082569da60be871fc09c098b96f03410f57f44d 100644 --- a/panda_moveit_config/launch/move_to_start.launch +++ b/panda_moveit_config/launch/move_to_start.launch @@ -2,10 +2,12 @@ <launch> <arg name="robot_ip" default="robot.franka.de" /> <arg name="arm_id" default="panda" /> + <arg name="load_gripper" default="true" /> <include file="$(find franka_control)/launch/franka_control.launch"> <arg name="robot_ip" value="$(arg robot_ip)" /> <arg name="arm_id" value="$(arg arm_id)" /> + <arg name="load_gripper" value="$(arg load_gripper)" /> </include> <include file="$(find panda_moveit_config)/launch/panda_moveit.launch"> <arg name="arm_id" value="$(arg arm_id)" />