From 17c450f008fb98fa000d1966d188514eab66d602 Mon Sep 17 00:00:00 2001 From: Max Polzin <max.polzin@epfl.ch> Date: Wed, 23 Feb 2022 10:09:37 +0000 Subject: [PATCH 1/9] Rearrange hardware_interface. --- electra_hardware_interface/CMakeLists.txt | 6 +- .../config/controllers.yaml | 60 +++++++++---------- .../electra_hardware_interface.h | 8 +-- .../launch/electra_hardware_interface.launch | 26 ++++---- ...ode.cpp => electra_hardware_interface.cpp} | 35 ++++------- .../src/electra_hardware_interface_node.cpp | 12 ++++ ...ra.urdf.xacro => electra_model.urdf.xacro} | 0 electra_launch/launch/electra.launch | 2 + 8 files changed, 78 insertions(+), 71 deletions(-) rename electra_hardware_interface/src/{hardware_interface_node.cpp => electra_hardware_interface.cpp} (61%) create mode 100644 electra_hardware_interface/src/electra_hardware_interface_node.cpp rename electra_hardware_interface/urdf/{electra.urdf.xacro => electra_model.urdf.xacro} (100%) diff --git a/electra_hardware_interface/CMakeLists.txt b/electra_hardware_interface/CMakeLists.txt index cdf5aa0..5364682 100644 --- a/electra_hardware_interface/CMakeLists.txt +++ b/electra_hardware_interface/CMakeLists.txt @@ -20,6 +20,6 @@ include_directories( ${catkin_INCLUDE_DIRS} ) -add_executable(electra_hardware_interface src/hardware_interface_node.cpp) -add_dependencies(electra_hardware_interface ${catkin_EXPORTED_TARGETS}) -target_link_libraries(electra_hardware_interface ${catkin_LIBRARIES}) +add_executable(electra_hardware_interface_node src/electra_hardware_interface_node.cpp src/electra_hardware_interface.cpp) +add_dependencies(electra_hardware_interface_node ${catkin_EXPORTED_TARGETS}) +target_link_libraries(electra_hardware_interface_node ${catkin_LIBRARIES}) diff --git a/electra_hardware_interface/config/controllers.yaml b/electra_hardware_interface/config/controllers.yaml index 30a7154..61fb490 100644 --- a/electra_hardware_interface/config/controllers.yaml +++ b/electra_hardware_interface/config/controllers.yaml @@ -1,35 +1,35 @@ electra: - # Publish all joint states ----------------------------------- - joints_update: - type: joint_state_controller/JointStateController - publish_rate: 50 + # Publish all joint states ----------------------------------- + joints_update: + type: joint_state_controller/JointStateController + publish_rate: 50 + + # # Position Controllers --------------------------------------- + # reel_position_controller: + # type: effort_controllers/JointPositionController + # joint: reel + # pid: {p: 1300.0, i: 0.0, d: 5, i_clamp_min: -130.0, i_clamp_max: 130, antiwindup: True} - # # Position Controllers --------------------------------------- - # reel_position_controller: - # type: effort_controllers/JointPositionController - # joint: reel - # pid: {p: 1300.0, i: 0.0, d: 5, i_clamp_min: -130.0, i_clamp_max: 130, antiwindup: True} - - # Velocity Controllers --------------------------------------- - velocity_controller: - type: effort_controllers/JointVelocityController - joint: electra - # pid: {p: 7.0, i: 15.0, d: 0.5, i_clamp_min: -10000.0, i_clamp_max: 88.0, antiwindup: True} - # pid: {p: 20.0, i: 0.0, d: 0.0, i_clamp_min: 255.0, i_clamp_max: 254.0, antiwindup: True} - # pid: {p: 0.0, i: 5.0, d: 0.0, i_clamp_min: -10000.0, i_clamp_max: 90.0, antiwindup: True} - pid: {p: 7.0, i: 15.0, d: 0.5, i_clamp_min: -100.0, i_clamp_max: 100.0, antiwindup: True} + # Velocity Controllers --------------------------------------- + velocity_controller: + type: effort_controllers/JointVelocityController + joint: electra + # pid: {p: 7.0, i: 15.0, d: 0.5, i_clamp_min: -10000.0, i_clamp_max: 88.0, antiwindup: True} + # pid: {p: 20.0, i: 0.0, d: 0.0, i_clamp_min: 255.0, i_clamp_max: 254.0, antiwindup: True} + # pid: {p: 0.0, i: 5.0, d: 0.0, i_clamp_min: -10000.0, i_clamp_max: 90.0, antiwindup: True} + pid: {p: 7.0, i: 15.0, d: 0.5, i_clamp_min: -100.0, i_clamp_max: 100.0, antiwindup: True} - force_controller: - type: electra_controllers/ForceController - joint: electra - pid: {p: 50.0, i: 0.0, d: 0.0, i_clamp_min: 0.0, i_clamp_max: 0.0, antiwindup: True} + # force_controller: + # type: electra_controllers/ForceController + # joint: electra + # pid: {p: 50.0, i: 0.0, d: 0.0, i_clamp_min: 0.0, i_clamp_max: 0.0, antiwindup: True} - reel_tether_controller: - type: electra_controllers/TetherController - joint: electra + # reel_tether_controller: + # type: electra_controllers/TetherController + # joint: electra - # # Command forward controller - # reel_effort_controller: - # type: effort_controllers/JointEffortController - # joint: reel - # pid: {p: 1.0, i: 0.0, d: 0.0, i_clamp_min: -10000.0, i_clamp_max: 10000.0, antiwindup: True} + # Command forward controller + effort_controller: + type: effort_controllers/JointEffortController + joint: electra + pid: {p: 1.0, i: 0.0, d: 0.0, i_clamp_min: -10000.0, i_clamp_max: 10000.0, antiwindup: True} diff --git a/electra_hardware_interface/include/electra_hardware_interface/electra_hardware_interface.h b/electra_hardware_interface/include/electra_hardware_interface/electra_hardware_interface.h index 001852c..643dd0a 100644 --- a/electra_hardware_interface/include/electra_hardware_interface/electra_hardware_interface.h +++ b/electra_hardware_interface/include/electra_hardware_interface/electra_hardware_interface.h @@ -8,16 +8,16 @@ #include <controller_manager/controller_manager.h> #include <boost/scoped_ptr.hpp> #include <ros/ros.h> -#include <std_msgs/Int16.h> +#include <std_msgs/Int32.h> #include <std_msgs/Float32.h> #include <electra_msgs/State.h> #include <angles/angles.h> -class ROBOTHardwareInterface : public hardware_interface::RobotHW +class ElectraHardwareInterface : public hardware_interface::RobotHW { public: - ROBOTHardwareInterface(ros::NodeHandle& nh); - ~ROBOTHardwareInterface(); + ElectraHardwareInterface(ros::NodeHandle& nh); + ~ElectraHardwareInterface(){}; void init(); void update(const ros::TimerEvent& e); void write(ros::Duration elapsed_time); diff --git a/electra_hardware_interface/launch/electra_hardware_interface.launch b/electra_hardware_interface/launch/electra_hardware_interface.launch index c1bdab5..94c7d3c 100644 --- a/electra_hardware_interface/launch/electra_hardware_interface.launch +++ b/electra_hardware_interface/launch/electra_hardware_interface.launch @@ -1,24 +1,30 @@ <launch> + <!-- <group ns="electra"> --> <rosparam file="$(find electra_hardware_interface)/config/controllers.yaml" command="load" /> <rosparam file="$(find electra_hardware_interface)/config/joint_limits.yaml" command="load" /> - <arg name="model" default="$(find electra_hardware_interface)/urdf/electra.urdf.xacro" /> + <arg name="model" default="$(find electra_hardware_interface)/urdf/electra_model.urdf.xacro" /> <param name="robot_description" command="$(find xacro)/xacro $(arg model)" /> - <node name="electra_hardware_interface" pkg="electra_hardware_interface" type="electra_hardware_interface" output="screen" /> + <node name="hardware_interface_node" pkg="electra_hardware_interface" type="electra_hardware_interface_node" output="screen" /> <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"></node> - <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args=" - /electra/joints_update - /electra/velocity_controller - " /> + <!-- <remap from="controller_manager/load_controller" to="electra/controller_manager/load_controller"/> + <remap from="controller_manager/switch_controller" to="electra/controller_manager/switch_controller"/> + <remap from="controller_manager/unload_controller" to="electra/controller_manager/unload_controller"/> --> - <!-- <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args=" - /electra/joints_update - /electra/force_controller - " /> --> + <!-- </group> --> + + + <!-- <arg name="controller" value=" + /electra/joints_update + /electra/effort_controller" /> --> + + <arg name="controller" value="/electra/velocity_controller /electra/joints_update" /> + + <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="$(arg controller)" /> </launch> \ No newline at end of file diff --git a/electra_hardware_interface/src/hardware_interface_node.cpp b/electra_hardware_interface/src/electra_hardware_interface.cpp similarity index 61% rename from electra_hardware_interface/src/hardware_interface_node.cpp rename to electra_hardware_interface/src/electra_hardware_interface.cpp index 2093e83..4425d39 100644 --- a/electra_hardware_interface/src/hardware_interface_node.cpp +++ b/electra_hardware_interface/src/electra_hardware_interface.cpp @@ -1,31 +1,28 @@ #include <electra_hardware_interface/electra_hardware_interface.h> -ROBOTHardwareInterface::ROBOTHardwareInterface(ros::NodeHandle& nh) : nh_(nh) { +ElectraHardwareInterface::ElectraHardwareInterface(ros::NodeHandle& nh) : nh_(nh) { init(); controller_manager_.reset(new controller_manager::ControllerManager(this, nh_)); loop_hz_= 15; ros::Duration update_freq = ros::Duration(1.0/loop_hz_); - command_publisher = nh_.advertise<std_msgs::Int16>("/electra/command/raw",10); - encoder_subscriber = nh_.subscribe("/electra/encoder/raw", 1, &ROBOTHardwareInterface::encoder_callback, this); - loadcell_subscriber = nh_.subscribe("/electra/loadcell/raw", 1, &ROBOTHardwareInterface::loadcell_callback, this); + command_publisher = nh_.advertise<std_msgs::Int32>("/electra/motor/command",10); + encoder_subscriber = nh_.subscribe("/electra/encoder/raw", 1, &ElectraHardwareInterface::encoder_callback, this); + loadcell_subscriber = nh_.subscribe("/electra/loadcell/raw", 1, &ElectraHardwareInterface::loadcell_callback, this); - non_realtime_loop_ = nh_.createTimer(update_freq, &ROBOTHardwareInterface::update, this); + non_realtime_loop_ = nh_.createTimer(update_freq, &ElectraHardwareInterface::update, this); } -ROBOTHardwareInterface::~ROBOTHardwareInterface() { -} - -void ROBOTHardwareInterface::encoder_callback(const electra_msgs::StateConstPtr& msg){ +void ElectraHardwareInterface::encoder_callback(const electra_msgs::StateConstPtr& msg){ joint_position_ = msg->position; joint_velocity_ = msg->velocity; } -void ROBOTHardwareInterface::loadcell_callback(const std_msgs::Float32ConstPtr& msg){ +void ElectraHardwareInterface::loadcell_callback(const std_msgs::Float32ConstPtr& msg){ joint_effort_ = msg->data; } -void ROBOTHardwareInterface::init() { +void ElectraHardwareInterface::init() { joint_name_="electra"; // Create joint state interface @@ -49,27 +46,17 @@ void ROBOTHardwareInterface::init() { registerInterface(&effortJointSaturationInterface); } -void ROBOTHardwareInterface::update(const ros::TimerEvent& e) { +void ElectraHardwareInterface::update(const ros::TimerEvent& e) { elapsed_time_ = ros::Duration(e.current_real - e.last_real); controller_manager_->update(ros::Time::now(), elapsed_time_); write(elapsed_time_); } -void ROBOTHardwareInterface::write(ros::Duration elapsed_time) { +void ElectraHardwareInterface::write(ros::Duration elapsed_time) { effortJointSaturationInterface.enforceLimits(elapsed_time); - std_msgs::Int16 motorCommand; + std_msgs::Int32 motorCommand; motorCommand.data = (int) joint_effort_command_; ROS_INFO_THROTTLE(1,"PWM Cmd: %d",motorCommand.data); command_publisher.publish(motorCommand); } - - -int main(int argc, char** argv) { - ros::init(argc, argv, "electra_hardware_interface"); - ros::NodeHandle nh; - ros::MultiThreadedSpinner spinner(2); // Multiple threads for controller service callback and for the Service client callback used to get the feedback from ardiuno - ROBOTHardwareInterface ROBOT(nh); - spinner.spin(); - return 0; -} diff --git a/electra_hardware_interface/src/electra_hardware_interface_node.cpp b/electra_hardware_interface/src/electra_hardware_interface_node.cpp new file mode 100644 index 0000000..e641598 --- /dev/null +++ b/electra_hardware_interface/src/electra_hardware_interface_node.cpp @@ -0,0 +1,12 @@ +#include <electra_hardware_interface/electra_hardware_interface.h> + +int main(int argc, char** argv) { + ros::init(argc, argv, "electra_hardware_interface"); + ros::NodeHandle nh; + // ros::MultiThreadedSpinner spinner(2); + ElectraHardwareInterface electra(nh); + // spinner.spin(); + ros::spin(); + + return 0; +} diff --git a/electra_hardware_interface/urdf/electra.urdf.xacro b/electra_hardware_interface/urdf/electra_model.urdf.xacro similarity index 100% rename from electra_hardware_interface/urdf/electra.urdf.xacro rename to electra_hardware_interface/urdf/electra_model.urdf.xacro diff --git a/electra_launch/launch/electra.launch b/electra_launch/launch/electra.launch index f106fd2..22a6d98 100644 --- a/electra_launch/launch/electra.launch +++ b/electra_launch/launch/electra.launch @@ -7,5 +7,7 @@ <include file="$(find electra_motor)/launch/electra_motor.launch" /> <include file="$(find electra_encoder)/launch/electra_encoder.launch" /> + <include file="$(find electra_hardware_interface)/launch/electra_hardware_interface.launch" /> + </launch> \ No newline at end of file -- GitLab From 939f3deee3c36250c556e598d51886004e8b0f1d Mon Sep 17 00:00:00 2001 From: Max Polzin <max.polzin@epfl.ch> Date: Wed, 23 Feb 2022 10:27:04 +0000 Subject: [PATCH 2/9] Like this? --- .../launch/electra_hardware_interface.launch | 26 +++++++++---------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/electra_hardware_interface/launch/electra_hardware_interface.launch b/electra_hardware_interface/launch/electra_hardware_interface.launch index 94c7d3c..a22e44c 100644 --- a/electra_hardware_interface/launch/electra_hardware_interface.launch +++ b/electra_hardware_interface/launch/electra_hardware_interface.launch @@ -1,30 +1,30 @@ <launch> - <!-- <group ns="electra"> --> + <group ns="electra"> - <rosparam file="$(find electra_hardware_interface)/config/controllers.yaml" command="load" /> - <rosparam file="$(find electra_hardware_interface)/config/joint_limits.yaml" command="load" /> + <rosparam file="$(find electra_hardware_interface)/config/controllers.yaml" command="load" /> + <rosparam file="$(find electra_hardware_interface)/config/joint_limits.yaml" command="load" /> - <arg name="model" default="$(find electra_hardware_interface)/urdf/electra_model.urdf.xacro" /> - <param name="robot_description" command="$(find xacro)/xacro $(arg model)" /> + <arg name="model" default="$(find electra_hardware_interface)/urdf/electra_model.urdf.xacro" /> + <param name="robot_description" command="$(find xacro)/xacro $(arg model)" /> - <node name="hardware_interface_node" pkg="electra_hardware_interface" type="electra_hardware_interface_node" output="screen" /> - <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"></node> + <node name="hardware_interface_node" pkg="electra_hardware_interface" type="electra_hardware_interface_node" output="screen" /> + <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"></node> - <!-- <remap from="controller_manager/load_controller" to="electra/controller_manager/load_controller"/> + <!-- <remap from="controller_manager/load_controller" to="electra/controller_manager/load_controller"/> <remap from="controller_manager/switch_controller" to="electra/controller_manager/switch_controller"/> <remap from="controller_manager/unload_controller" to="electra/controller_manager/unload_controller"/> --> - <!-- </group> --> + <arg name="controller" value="electra/velocity_controller electra/joints_update" /> + + <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="$(arg controller)" /> + + </group> <!-- <arg name="controller" value=" /electra/joints_update /electra/effort_controller" /> --> - <arg name="controller" value="/electra/velocity_controller /electra/joints_update" /> - - <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="$(arg controller)" /> - </launch> \ No newline at end of file -- GitLab From 6649a56d9338032d41f8418df46529c42acf00b3 Mon Sep 17 00:00:00 2001 From: Max Polzin <max.polzin@epfl.ch> Date: Wed, 23 Feb 2022 10:46:46 +0000 Subject: [PATCH 3/9] Add ns. --- .../launch/electra_hardware_interface.launch | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/electra_hardware_interface/launch/electra_hardware_interface.launch b/electra_hardware_interface/launch/electra_hardware_interface.launch index a22e44c..40c8689 100644 --- a/electra_hardware_interface/launch/electra_hardware_interface.launch +++ b/electra_hardware_interface/launch/electra_hardware_interface.launch @@ -15,9 +15,11 @@ <remap from="controller_manager/switch_controller" to="electra/controller_manager/switch_controller"/> <remap from="controller_manager/unload_controller" to="electra/controller_manager/unload_controller"/> --> - <arg name="controller" value="electra/velocity_controller electra/joints_update" /> + <arg name="controller" value=" + electra/velocity_controller + electra/joints_update" /> - <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="$(arg controller)" /> + <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="--namespace=/electra $(arg controller)" /> </group> -- GitLab From 1bf27f7959ed6d5c826fdf3252b23e7ec2aabf9b Mon Sep 17 00:00:00 2001 From: Max Polzin <max.polzin@epfl.ch> Date: Wed, 23 Feb 2022 11:02:54 +0000 Subject: [PATCH 4/9] Works namespaced now. --- .../config/controllers.yaml | 62 +++++++++---------- .../launch/electra_hardware_interface.launch | 28 ++++----- 2 files changed, 42 insertions(+), 48 deletions(-) diff --git a/electra_hardware_interface/config/controllers.yaml b/electra_hardware_interface/config/controllers.yaml index 61fb490..ebf5e01 100644 --- a/electra_hardware_interface/config/controllers.yaml +++ b/electra_hardware_interface/config/controllers.yaml @@ -1,35 +1,35 @@ -electra: - # Publish all joint states ----------------------------------- - joints_update: - type: joint_state_controller/JointStateController - publish_rate: 50 - - # # Position Controllers --------------------------------------- - # reel_position_controller: - # type: effort_controllers/JointPositionController - # joint: reel - # pid: {p: 1300.0, i: 0.0, d: 5, i_clamp_min: -130.0, i_clamp_max: 130, antiwindup: True} +# electra_ns: + # Publish all joint states ----------------------------------- + joints_update: + type: joint_state_controller/JointStateController + publish_rate: 50 - # Velocity Controllers --------------------------------------- - velocity_controller: - type: effort_controllers/JointVelocityController - joint: electra - # pid: {p: 7.0, i: 15.0, d: 0.5, i_clamp_min: -10000.0, i_clamp_max: 88.0, antiwindup: True} - # pid: {p: 20.0, i: 0.0, d: 0.0, i_clamp_min: 255.0, i_clamp_max: 254.0, antiwindup: True} - # pid: {p: 0.0, i: 5.0, d: 0.0, i_clamp_min: -10000.0, i_clamp_max: 90.0, antiwindup: True} - pid: {p: 7.0, i: 15.0, d: 0.5, i_clamp_min: -100.0, i_clamp_max: 100.0, antiwindup: True} + # # Position Controllers --------------------------------------- + # reel_position_controller: + # type: effort_controllers/JointPositionController + # joint: reel + # pid: {p: 1300.0, i: 0.0, d: 5, i_clamp_min: -130.0, i_clamp_max: 130, antiwindup: True} + + # Velocity Controllers --------------------------------------- + velocity_controller: + type: effort_controllers/JointVelocityController + joint: electra + # pid: {p: 7.0, i: 15.0, d: 0.5, i_clamp_min: -10000.0, i_clamp_max: 88.0, antiwindup: True} + # pid: {p: 20.0, i: 0.0, d: 0.0, i_clamp_min: 255.0, i_clamp_max: 254.0, antiwindup: True} + # pid: {p: 0.0, i: 5.0, d: 0.0, i_clamp_min: -10000.0, i_clamp_max: 90.0, antiwindup: True} + pid: {p: 7.0, i: 15.0, d: 0.5, i_clamp_min: -100.0, i_clamp_max: 100.0, antiwindup: True} - # force_controller: - # type: electra_controllers/ForceController - # joint: electra - # pid: {p: 50.0, i: 0.0, d: 0.0, i_clamp_min: 0.0, i_clamp_max: 0.0, antiwindup: True} + force_controller: + type: electra_controllers/ForceController + joint: electra + pid: {p: 50.0, i: 0.0, d: 0.0, i_clamp_min: 0.0, i_clamp_max: 0.0, antiwindup: True} - # reel_tether_controller: - # type: electra_controllers/TetherController - # joint: electra + reel_tether_controller: + type: electra_controllers/TetherController + joint: electra - # Command forward controller - effort_controller: - type: effort_controllers/JointEffortController - joint: electra - pid: {p: 1.0, i: 0.0, d: 0.0, i_clamp_min: -10000.0, i_clamp_max: 10000.0, antiwindup: True} + # # Command forward controller + # reel_effort_controller: + # type: effort_controllers/JointEffortController + # joint: reel + # pid: {p: 1.0, i: 0.0, d: 0.0, i_clamp_min: -10000.0, i_clamp_max: 10000.0, antiwindup: True} diff --git a/electra_hardware_interface/launch/electra_hardware_interface.launch b/electra_hardware_interface/launch/electra_hardware_interface.launch index 40c8689..ff0b704 100644 --- a/electra_hardware_interface/launch/electra_hardware_interface.launch +++ b/electra_hardware_interface/launch/electra_hardware_interface.launch @@ -1,6 +1,6 @@ <launch> - <group ns="electra"> + <group ns="/electra"> <rosparam file="$(find electra_hardware_interface)/config/controllers.yaml" command="load" /> <rosparam file="$(find electra_hardware_interface)/config/joint_limits.yaml" command="load" /> @@ -8,25 +8,19 @@ <arg name="model" default="$(find electra_hardware_interface)/urdf/electra_model.urdf.xacro" /> <param name="robot_description" command="$(find xacro)/xacro $(arg model)" /> - <node name="hardware_interface_node" pkg="electra_hardware_interface" type="electra_hardware_interface_node" output="screen" /> + <node name="electra_hardware_interface" pkg="electra_hardware_interface" type="electra_hardware_interface" output="screen" /> <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"></node> - <!-- <remap from="controller_manager/load_controller" to="electra/controller_manager/load_controller"/> - <remap from="controller_manager/switch_controller" to="electra/controller_manager/switch_controller"/> - <remap from="controller_manager/unload_controller" to="electra/controller_manager/unload_controller"/> --> - - <arg name="controller" value=" - electra/velocity_controller - electra/joints_update" /> - - <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="--namespace=/electra $(arg controller)" /> + <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args=" + --namespace=/electra + /electra/joints_update + /electra/velocity_controller + " /> + <!-- <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args=" + /electra/joints_update + /electra/force_controller + " /> --> </group> - - <!-- <arg name="controller" value=" - /electra/joints_update - /electra/effort_controller" /> --> - - </launch> \ No newline at end of file -- GitLab From f5ba9073a91111bc020df09312f0910fcfa06800 Mon Sep 17 00:00:00 2001 From: Max Polzin <max.polzin@epfl.ch> Date: Wed, 23 Feb 2022 11:13:02 +0000 Subject: [PATCH 5/9] Reformat file. --- .../config/controllers.yaml | 63 +++++++++---------- 1 file changed, 31 insertions(+), 32 deletions(-) diff --git a/electra_hardware_interface/config/controllers.yaml b/electra_hardware_interface/config/controllers.yaml index ebf5e01..cc1f8f6 100644 --- a/electra_hardware_interface/config/controllers.yaml +++ b/electra_hardware_interface/config/controllers.yaml @@ -1,35 +1,34 @@ -# electra_ns: - # Publish all joint states ----------------------------------- - joints_update: - type: joint_state_controller/JointStateController - publish_rate: 50 - - # # Position Controllers --------------------------------------- - # reel_position_controller: - # type: effort_controllers/JointPositionController - # joint: reel - # pid: {p: 1300.0, i: 0.0, d: 5, i_clamp_min: -130.0, i_clamp_max: 130, antiwindup: True} - - # Velocity Controllers --------------------------------------- - velocity_controller: - type: effort_controllers/JointVelocityController - joint: electra - # pid: {p: 7.0, i: 15.0, d: 0.5, i_clamp_min: -10000.0, i_clamp_max: 88.0, antiwindup: True} - # pid: {p: 20.0, i: 0.0, d: 0.0, i_clamp_min: 255.0, i_clamp_max: 254.0, antiwindup: True} - # pid: {p: 0.0, i: 5.0, d: 0.0, i_clamp_min: -10000.0, i_clamp_max: 90.0, antiwindup: True} - pid: {p: 7.0, i: 15.0, d: 0.5, i_clamp_min: -100.0, i_clamp_max: 100.0, antiwindup: True} +# Publish all joint states ----------------------------------- +joints_update: + type: joint_state_controller/JointStateController + publish_rate: 50 - force_controller: - type: electra_controllers/ForceController - joint: electra - pid: {p: 50.0, i: 0.0, d: 0.0, i_clamp_min: 0.0, i_clamp_max: 0.0, antiwindup: True} +# # Position Controllers --------------------------------------- +# reel_position_controller: +# type: effort_controllers/JointPositionController +# joint: reel +# pid: {p: 1300.0, i: 0.0, d: 5, i_clamp_min: -130.0, i_clamp_max: 130, antiwindup: True} + +# Velocity Controllers --------------------------------------- +velocity_controller: + type: effort_controllers/JointVelocityController + joint: electra + # pid: {p: 7.0, i: 15.0, d: 0.5, i_clamp_min: -10000.0, i_clamp_max: 88.0, antiwindup: True} + # pid: {p: 20.0, i: 0.0, d: 0.0, i_clamp_min: 255.0, i_clamp_max: 254.0, antiwindup: True} + # pid: {p: 0.0, i: 5.0, d: 0.0, i_clamp_min: -10000.0, i_clamp_max: 90.0, antiwindup: True} + pid: {p: 7.0, i: 15.0, d: 0.5, i_clamp_min: -100.0, i_clamp_max: 100.0, antiwindup: True} - reel_tether_controller: - type: electra_controllers/TetherController - joint: electra +force_controller: + type: electra_controllers/ForceController + joint: electra + pid: {p: 50.0, i: 0.0, d: 0.0, i_clamp_min: 0.0, i_clamp_max: 0.0, antiwindup: True} - # # Command forward controller - # reel_effort_controller: - # type: effort_controllers/JointEffortController - # joint: reel - # pid: {p: 1.0, i: 0.0, d: 0.0, i_clamp_min: -10000.0, i_clamp_max: 10000.0, antiwindup: True} +reel_tether_controller: + type: electra_controllers/TetherController + joint: electra + +# # Command forward controller +# reel_effort_controller: +# type: effort_controllers/JointEffortController +# joint: reel +# pid: {p: 1.0, i: 0.0, d: 0.0, i_clamp_min: -10000.0, i_clamp_max: 10000.0, antiwindup: True} -- GitLab From a4aa31dfa49c8429b8eda51da55c4fa918fd9386 Mon Sep 17 00:00:00 2001 From: Max Polzin <max.polzin@epfl.ch> Date: Wed, 23 Feb 2022 11:13:14 +0000 Subject: [PATCH 6/9] Multispinner to prevent blocking. --- .../src/electra_hardware_interface_node.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/electra_hardware_interface/src/electra_hardware_interface_node.cpp b/electra_hardware_interface/src/electra_hardware_interface_node.cpp index e641598..6881e80 100644 --- a/electra_hardware_interface/src/electra_hardware_interface_node.cpp +++ b/electra_hardware_interface/src/electra_hardware_interface_node.cpp @@ -3,10 +3,8 @@ int main(int argc, char** argv) { ros::init(argc, argv, "electra_hardware_interface"); ros::NodeHandle nh; - // ros::MultiThreadedSpinner spinner(2); + ros::MultiThreadedSpinner spinner(2); ElectraHardwareInterface electra(nh); - // spinner.spin(); - ros::spin(); - + spinner.spin(); return 0; } -- GitLab From 80c07eef3ce0f3341453da929f4b0562467d60a8 Mon Sep 17 00:00:00 2001 From: Max Polzin <max.polzin@epfl.ch> Date: Wed, 23 Feb 2022 11:13:21 +0000 Subject: [PATCH 7/9] Launch _node. --- .../launch/electra_hardware_interface.launch | 21 +++++++++++-------- 1 file changed, 12 insertions(+), 9 deletions(-) diff --git a/electra_hardware_interface/launch/electra_hardware_interface.launch b/electra_hardware_interface/launch/electra_hardware_interface.launch index ff0b704..e8dddcd 100644 --- a/electra_hardware_interface/launch/electra_hardware_interface.launch +++ b/electra_hardware_interface/launch/electra_hardware_interface.launch @@ -1,5 +1,4 @@ <launch> - <group ns="/electra"> <rosparam file="$(find electra_hardware_interface)/config/controllers.yaml" command="load" /> @@ -8,19 +7,23 @@ <arg name="model" default="$(find electra_hardware_interface)/urdf/electra_model.urdf.xacro" /> <param name="robot_description" command="$(find xacro)/xacro $(arg model)" /> - <node name="electra_hardware_interface" pkg="electra_hardware_interface" type="electra_hardware_interface" output="screen" /> + <node name="electra_hardware_interface" pkg="electra_hardware_interface" type="electra_hardware_interface_node" output="screen" /> <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"></node> + <arg name="controller" value=" + /electra/joints_update + /electra/velocity_controller + " /> + + <!-- <arg name="controller" value=" + /electra/joints_update + /electra/force_controller + " /> --> + <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args=" --namespace=/electra - /electra/joints_update - /electra/velocity_controller + $(arg controller) " /> - <!-- <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args=" - /electra/joints_update - /electra/force_controller - " /> --> </group> - </launch> \ No newline at end of file -- GitLab From 29986a20ec25ae718368a828c646d58c6d167d16 Mon Sep 17 00:00:00 2001 From: Max Polzin <max.polzin@epfl.ch> Date: Wed, 23 Feb 2022 15:26:34 +0000 Subject: [PATCH 8/9] Update controllers. --- .../config/controllers.yaml | 5 +--- .../config/joint_limits.yaml | 2 +- .../src/electra_hardware_interface.cpp | 2 +- .../firmware/ElectraMotor/ElectraMotor.ino | 27 +++++++++---------- 4 files changed, 16 insertions(+), 20 deletions(-) diff --git a/electra_hardware_interface/config/controllers.yaml b/electra_hardware_interface/config/controllers.yaml index cc1f8f6..849a100 100644 --- a/electra_hardware_interface/config/controllers.yaml +++ b/electra_hardware_interface/config/controllers.yaml @@ -13,10 +13,7 @@ joints_update: velocity_controller: type: effort_controllers/JointVelocityController joint: electra - # pid: {p: 7.0, i: 15.0, d: 0.5, i_clamp_min: -10000.0, i_clamp_max: 88.0, antiwindup: True} - # pid: {p: 20.0, i: 0.0, d: 0.0, i_clamp_min: 255.0, i_clamp_max: 254.0, antiwindup: True} - # pid: {p: 0.0, i: 5.0, d: 0.0, i_clamp_min: -10000.0, i_clamp_max: 90.0, antiwindup: True} - pid: {p: 7.0, i: 15.0, d: 0.5, i_clamp_min: -100.0, i_clamp_max: 100.0, antiwindup: True} + pid: {p: 20.0, i: 30.0, d: 1.0, i_clamp_min: -1000.0, i_clamp_max: 1000.0, antiwindup: True} force_controller: type: electra_controllers/ForceController diff --git a/electra_hardware_interface/config/joint_limits.yaml b/electra_hardware_interface/config/joint_limits.yaml index c61c761..7fe2c01 100644 --- a/electra_hardware_interface/config/joint_limits.yaml +++ b/electra_hardware_interface/config/joint_limits.yaml @@ -10,6 +10,6 @@ joint_limits: has_jerk_limits: false max_jerk: 0 has_effort_limits: true - max_effort: 91 + max_effort: 255 min_effort: -255 diff --git a/electra_hardware_interface/src/electra_hardware_interface.cpp b/electra_hardware_interface/src/electra_hardware_interface.cpp index 4425d39..3359ff3 100644 --- a/electra_hardware_interface/src/electra_hardware_interface.cpp +++ b/electra_hardware_interface/src/electra_hardware_interface.cpp @@ -3,7 +3,7 @@ ElectraHardwareInterface::ElectraHardwareInterface(ros::NodeHandle& nh) : nh_(nh) { init(); controller_manager_.reset(new controller_manager::ControllerManager(this, nh_)); - loop_hz_= 15; + loop_hz_= 15.0; ros::Duration update_freq = ros::Duration(1.0/loop_hz_); command_publisher = nh_.advertise<std_msgs::Int32>("/electra/motor/command",10); diff --git a/electra_motor/firmware/ElectraMotor/ElectraMotor.ino b/electra_motor/firmware/ElectraMotor/ElectraMotor.ino index 96250e1..8b5f780 100644 --- a/electra_motor/firmware/ElectraMotor/ElectraMotor.ino +++ b/electra_motor/firmware/ElectraMotor/ElectraMotor.ino @@ -11,6 +11,8 @@ const uint8_t BRAKEGND = 3; const int inAPin = 7; const int inBPin = 8; const int pwmPin = 5; + +const int PWM_DEADBAND = 20; const int PWM_MAX = 255; ros::NodeHandle nh; @@ -37,21 +39,18 @@ void rosSetup(){ } void rosCommandCallback( const std_msgs::Int32& command){ - if (command.data < -PWM_MAX){ - nh.logerror("Invalid command: <-255"); - } - else if (-PWM_MAX <= command.data && command.data < 0){ - motorGo(CW, command.data); - } - else if (command.data == 0 ){ - motorOff(); - } - else if (0 < command.data && command.data <= PWM_MAX){ - motorGo(CCW, command.data); - } - else if (PWM_MAX < command.data){ - nh.logerror("Invalid command: >255"); + uint8_t command_direction = command.data > 0 ? CW : CCW; + uint8_t command_magnitude = abs(command.data); + + if (PWM_DEADBAND <= command_magnitude && command_magnitude <= PWM_MAX){ + motorGo(command_direction, command_magnitude); } + else if (command_magnitude <= PWM_DEADBAND){ + motorGo(command_direction, 0); + } + else if (PWM_MAX <= command_magnitude){ + nh.logerror("Invalid command: >|255|"); + } else{ nh.logerror("Invalid command"); } -- GitLab From 3db1505fa237307f474492c4a9bf17fdec1bd308 Mon Sep 17 00:00:00 2001 From: Max Polzin <max.polzin@epfl.ch> Date: Wed, 23 Feb 2022 15:31:06 +0000 Subject: [PATCH 9/9] Update sketch. --- electra_motor/firmware/ElectraMotor/ElectraMotor.ino | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/electra_motor/firmware/ElectraMotor/ElectraMotor.ino b/electra_motor/firmware/ElectraMotor/ElectraMotor.ino index 8b5f780..d7f8822 100644 --- a/electra_motor/firmware/ElectraMotor/ElectraMotor.ino +++ b/electra_motor/firmware/ElectraMotor/ElectraMotor.ino @@ -49,7 +49,7 @@ void rosCommandCallback( const std_msgs::Int32& command){ motorGo(command_direction, 0); } else if (PWM_MAX <= command_magnitude){ - nh.logerror("Invalid command: >|255|"); + nh.logerror("Invalid command: |255|<|command|"); } else{ nh.logerror("Invalid command"); -- GitLab