From a7c4a01a1464800250c785ecc46deb4e1d1a5eb9 Mon Sep 17 00:00:00 2001 From: Joseph Carpinelli Date: Tue, 26 Mar 2024 13:26:54 +0000 Subject: [PATCH] Demo for #52 --- .../joint_position_example_controller.hpp | 1 + .../src/joint_position_example_controller.cpp | 16 +++++----------- 2 files changed, 6 insertions(+), 11 deletions(-) diff --git a/franka_example_controllers/include/franka_example_controllers/joint_position_example_controller.hpp b/franka_example_controllers/include/franka_example_controllers/joint_position_example_controller.hpp index 37d942d8..0a813707 100644 --- a/franka_example_controllers/include/franka_example_controllers/joint_position_example_controller.hpp +++ b/franka_example_controllers/include/franka_example_controllers/joint_position_example_controller.hpp @@ -44,6 +44,7 @@ class JointPositionExampleController : public controller_interface::ControllerIn std::string arm_id_; const int num_joints = 7; std::array initial_q_{0, 0, 0, 0, 0, 0, 0}; + std::array q_{0, 0, 0, 0, 0, 0, 0}; const double trajectory_period{0.001}; double elapsed_time_ = 0.0; std::string arm_id{"panda"}; diff --git a/franka_example_controllers/src/joint_position_example_controller.cpp b/franka_example_controllers/src/joint_position_example_controller.cpp index 98e72af6..f1be2253 100644 --- a/franka_example_controllers/src/joint_position_example_controller.cpp +++ b/franka_example_controllers/src/joint_position_example_controller.cpp @@ -23,22 +23,13 @@ namespace franka_example_controllers { -controller_interface::InterfaceConfiguration -JointPositionExampleController::command_interface_configuration() const { - controller_interface::InterfaceConfiguration config; - config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - for (int i = 1; i <= num_joints; ++i) { - config.names.push_back(arm_id_ + "_joint" + std::to_string(i) + "/position"); - } - return config; -} - controller_interface::InterfaceConfiguration JointPositionExampleController::state_interface_configuration() const { controller_interface::InterfaceConfiguration config; config.type = controller_interface::interface_configuration_type::INDIVIDUAL; for (int i = 1; i <= num_joints; ++i) { config.names.push_back(arm_id_ + "_joint" + std::to_string(i) + "/" + k_HW_IF_INITIAL_POSITION); + config.names.push_back(arm_id_ + "_joint" + std::to_string(i) + "/position"); } return config; } @@ -48,10 +39,13 @@ controller_interface::return_type JointPositionExampleController::update( const rclcpp::Duration& /*period*/) { if (initialization_flag_) { for (int i = 0; i < num_joints; ++i) { - initial_q_.at(i) = state_interfaces_[i].get_value(); + initial_q_.at(i) = state_interfaces_[2 * i].get_value(); } initialization_flag_ = false; } + for(int i =0; i< num_joints; ++i){ + q_.at(i) = state_interfaces_[2*i + 1].get_value(); + } elapsed_time_ = elapsed_time_ + trajectory_period; double delta_angle = M_PI / 16 * (1 - std::cos(M_PI / 5.0 * elapsed_time_)) * 0.2;