Demo for frankaemika#52

jcarpinelli-bdai · Mar 26, 2024 · a7c4a01 · a7c4a01
1 parent 40cf08b
commit a7c4a01
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 11 deletions.
diff --git a/...mple_controllers/include/franka_example_controllers/joint_position_example_controller.hpp b/...mple_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<double, 7> initial_q_{0, 0, 0, 0, 0, 0, 0};
+  std::array<double, 7> 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
@@ -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;