From: Walter Fetter Lages Date: Tue, 18 Jul 2023 10:40:36 +0000 (-0300) Subject: Fix controller loading for hardware execution. X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=f698557d2ef9a438f9070e0da786b8fd1e81de16;p=q2d.git Fix controller loading for hardware execution. --- diff --git a/q2d_bringup/config/group_bypass.yaml b/q2d_bringup/config/group_bypass.yaml index 61eef34..0413e10 100644 --- a/q2d_bringup/config/group_bypass.yaml +++ b/q2d_bringup/config/group_bypass.yaml @@ -1,4 +1,4 @@ -group_controller: +group_bypass: ros__parameters: joints: - shoulder_active_joint diff --git a/q2d_bringup/launch/hardware.launch.xml b/q2d_bringup/launch/hardware.launch.xml index 8a47983..38ef83c 100644 --- a/q2d_bringup/launch/hardware.launch.xml +++ b/q2d_bringup/launch/hardware.launch.xml @@ -26,11 +26,18 @@ - + + + + + diff --git a/q2d_hardware/CMakeLists.txt b/q2d_hardware/CMakeLists.txt index 9331a8a..0aa76be 100644 --- a/q2d_hardware/CMakeLists.txt +++ b/q2d_hardware/CMakeLists.txt @@ -45,7 +45,7 @@ install( RUNTIME DESTINATION bin ) -install(DIRECTORY launch +install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME} ) diff --git a/q2d_hardware/config/controller_manager.yaml b/q2d_hardware/config/controller_manager.yaml new file mode 100644 index 0000000..17610f3 --- /dev/null +++ b/q2d_hardware/config/controller_manager.yaml @@ -0,0 +1,11 @@ +controller_manager: + ros__parameters: + update_rate: 500 + use_sim_time: false + + group_bypass: + type: effort_controllers/JointGroupEffortController + + joints: + - shoulder_active_joint + - elbow_active_joint diff --git a/q2d_hardware/launch/controller_manager.launch.xml b/q2d_hardware/launch/controller_manager.launch.xml index 7e1e66c..e81ef36 100644 --- a/q2d_hardware/launch/controller_manager.launch.xml +++ b/q2d_hardware/launch/controller_manager.launch.xml @@ -23,6 +23,7 @@ - + + diff --git a/q2d_hardware/src/q2d_system.cpp b/q2d_hardware/src/q2d_system.cpp index fddb645..96f7ca6 100644 --- a/q2d_hardware/src/q2d_system.cpp +++ b/q2d_hardware/src/q2d_system.cpp @@ -53,6 +53,8 @@ namespace q2d_hardware Ra_[0]=11.5; Ra_[1]=2.32; N_.resize(info_.joints.size()); + N_[0]=0.01; + N_[1]=0.02; for(const hardware_interface::ComponentInfo & joint : info_.joints) { @@ -110,6 +112,7 @@ namespace q2d_hardware return CallbackReturn::ERROR; } } + RCLCPP_INFO_STREAM(rclcpp::get_logger("Q2dSystemHardware"),"on_init"); return CallbackReturn::SUCCESS; } @@ -121,10 +124,11 @@ namespace q2d_hardware for(auto i=0u;i < board_.size();i++) { - param.aic_serial_port="/dev/tty/USB"+ ('0'+i); + param.aic_serial_port="/dev/ttyUSB"+std::string({char('0'+i),'\0'}); if((board_[i]=std::make_unique(param)) == NULL) - RCLCPP_ERROR_STREAM(rclcpp::get_logger("Q2dSystemHardware"), + RCLCPP_ERROR_STREAM(rclcpp::get_logger("Q2dSystemHardware"), "Can not configure " << param.aic_serial_port << " for interfacing with joint " << i); + RCLCPP_WARN_STREAM(rclcpp::get_logger("Q2dSystemHardware"),"on_configure: " << param.aic_serial_port); } return CallbackReturn::SUCCESS; } @@ -139,8 +143,10 @@ namespace q2d_hardware state_interfaces.emplace_back(hardware_interface::StateInterface(info_.joints[i].name, hardware_interface::HW_IF_VELOCITY,&velocity_[i])); state_interfaces.emplace_back(hardware_interface::StateInterface(info_.joints[i].name, - hardware_interface::HW_IF_EFFORT,&command_[i])); + hardware_interface::HW_IF_EFFORT,&effort_[i])); } + + RCLCPP_INFO_STREAM(rclcpp::get_logger("Q2dSystemHardware"),"export_state_interface"); return state_interfaces; } @@ -152,6 +158,8 @@ namespace q2d_hardware command_interfaces.emplace_back(hardware_interface::CommandInterface(info_.joints[i].name, hardware_interface::HW_IF_EFFORT,&command_[i])); } + + RCLCPP_INFO_STREAM(rclcpp::get_logger("Q2dSystemHardware"),"export_command_interface"); return command_interfaces; } @@ -166,39 +174,44 @@ namespace q2d_hardware } /* activate hardware */ - for(auto i=0u;i < board_.size();i++) board_[i]->set_motor_voltage(0); - + for(auto i=0u;i < board_.size()-1;i++) board_[i]->set_motor_voltage(0); + + RCLCPP_INFO_STREAM(rclcpp::get_logger("Q2dSystemHardware"),"on_activate"); return CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn Q2dSystemHardware::on_deactivate(const rclcpp_lifecycle::State &/*previous_state*/) { /* deactivate hardware */ - for(auto i=0u;i < board_.size();i++) board_[i]->~aic(); + for(auto i=0u;i < board_.size()-1;i++) board_[i]->~aic(); + RCLCPP_INFO_STREAM(rclcpp::get_logger("Q2dSystemHardware"),"on_deactivate"); return CallbackReturn::SUCCESS;; } hardware_interface::return_type Q2dSystemHardware::read(const rclcpp::Time &/*time*/,const rclcpp::Duration &period) { /* read hardware */ - for(auto i=0u;i < board_.size();i++) + for(auto i=0u;i < board_.size()-1;i++) { auto dp=board_[i]->read_displacement_sensors(); - velocity_[i]=(dp.joint_displacement-position_[i])/period.seconds(); - position_[i]=dp.joint_displacement; + velocity_[i]=dp.joint_displacement/period.seconds()*N_[i]; + position_[i]+=dp.joint_displacement*N_[i]; effort_[i]=command_[i]; } - + +// RCLCPP_INFO_STREAM(rclcpp::get_logger("Q2dSystemHardware"),"read"); return hardware_interface::return_type::OK; } hardware_interface::return_type Q2dSystemHardware::write(const rclcpp::Time &/*time*/,const rclcpp::Duration &/*period*/) { /* write hardware */ - for(auto i=0u;i < board_.size();i++) - board_[i]->set_motor_voltage(N_[i]*Ra_[i]/Kt_[i]*command_[i]+Ka_[i]/N_[i]*velocity_[i]); + for(auto i=0u;i < board_.size()-1;i++) + board_[i]->set_motor_voltage(command_[i]); +// board_[i]->set_motor_voltage(N_[i]*Ra_[i]/Kt_[i]*command_[i]+Ka_[i]/N_[i]*velocity_[i]); +// RCLCPP_INFO_STREAM(rclcpp::get_logger("Q2dSystemHardware"),"write"); return hardware_interface::return_type::OK; }