Skip to content

Commit

Permalink
Enable setting default position of the simulated robot using ros2_con…
Browse files Browse the repository at this point in the history 
…trol URDF tag. (#100)

Signed-off-by: Denis Štogl <denis@stogl.de>
  • Loading branch information
@destogl
destogl committed Jan 28, 2022
1 parent 07a1734 commit 26216b4
Show file tree
Hide file tree
Showing 3 changed files with 64 additions and 29 deletions.
4 changes: 3 additions & 1 deletion README.md
View file
Expand Up @@ -79,7 +79,9 @@ include
<param name="min">-1000</param>
<param name="max">1000</param>
</command_interface>
<state_interface name="position"/>
<state_interface name="position">
<param name="initial_value">1.0</param>
</state_interface>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
Expand Down
85 changes: 58 additions & 27 deletions gazebo_ros2_control/src/gazebo_system.cpp
View file
Expand Up @@ -12,6 +12,7 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#include <limits>
#include <map>
#include <memory>
#include <string>
Expand All @@ -23,6 +24,7 @@
#include "gazebo/sensors/ForceTorqueSensor.hh"
#include "gazebo/sensors/SensorManager.hh"

#include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"

class gazebo_ros2_control::GazeboSystemPrivate
Expand Down Expand Up @@ -154,60 +156,89 @@ void GazeboSystem::registerJoints(
// Accept this joint and continue configuration
RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Loading joint: " << joint_name);

RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\tCommand:");
RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\tState:");

// register the command handles
for (unsigned int i = 0; i < hardware_info.joints[j].command_interfaces.size(); i++) {
if (hardware_info.joints[j].command_interfaces[i].name == "position") {
auto get_initial_value = [this](const hardware_interface::InterfaceInfo & interface_info) {
if (!interface_info.initial_value.empty()) {
double value = std::stod(interface_info.initial_value);
RCLCPP_INFO(this->nh_->get_logger(), "\t\t\t found initial value: %f", value);
return value;
} else {
return 0.0;
}
};

double initial_position = std::numeric_limits<double>::quiet_NaN();
double initial_velocity = std::numeric_limits<double>::quiet_NaN();
double initial_effort = std::numeric_limits<double>::quiet_NaN();

// register the state handles
for (unsigned int i = 0; i < hardware_info.joints[j].state_interfaces.size(); i++) {
if (hardware_info.joints[j].state_interfaces[i].name == "position") {
RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t position");
this->dataPtr->joint_control_methods_[j] |= POSITION;
this->dataPtr->command_interfaces_.emplace_back(
this->dataPtr->state_interfaces_.emplace_back(
joint_name,
hardware_interface::HW_IF_POSITION,
&this->dataPtr->joint_position_cmd_[j]);
&this->dataPtr->joint_position_[j]);
initial_position = get_initial_value(hardware_info.joints[j].state_interfaces[i]);
this->dataPtr->joint_position_[j] = initial_position;
}
if (hardware_info.joints[j].command_interfaces[i].name == "velocity") {
if (hardware_info.joints[j].state_interfaces[i].name == "velocity") {
RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t velocity");
this->dataPtr->joint_control_methods_[j] |= VELOCITY;
this->dataPtr->command_interfaces_.emplace_back(
this->dataPtr->state_interfaces_.emplace_back(
joint_name,
hardware_interface::HW_IF_VELOCITY,
&this->dataPtr->joint_velocity_cmd_[j]);
&this->dataPtr->joint_velocity_[j]);
initial_velocity = get_initial_value(hardware_info.joints[j].state_interfaces[i]);
this->dataPtr->joint_velocity_[j] = initial_velocity;
}
if (hardware_info.joints[j].command_interfaces[i].name == "effort") {
this->dataPtr->joint_control_methods_[j] |= EFFORT;
if (hardware_info.joints[j].state_interfaces[i].name == "effort") {
RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t effort");
this->dataPtr->command_interfaces_.emplace_back(
this->dataPtr->state_interfaces_.emplace_back(
joint_name,
hardware_interface::HW_IF_EFFORT,
&this->dataPtr->joint_effort_cmd_[j]);
&this->dataPtr->joint_effort_[j]);
initial_effort = get_initial_value(hardware_info.joints[j].state_interfaces[i]);
this->dataPtr->joint_effort_[j] = initial_effort;
}
}

RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\tState:");
RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\tCommand:");

// register the state handles
for (unsigned int i = 0; i < hardware_info.joints[j].state_interfaces.size(); i++) {
if (hardware_info.joints[j].state_interfaces[i].name == "position") {
// register the command handles
for (unsigned int i = 0; i < hardware_info.joints[j].command_interfaces.size(); i++) {
if (hardware_info.joints[j].command_interfaces[i].name == "position") {
RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t position");
this->dataPtr->state_interfaces_.emplace_back(
this->dataPtr->joint_control_methods_[j] |= POSITION;
this->dataPtr->command_interfaces_.emplace_back(
joint_name,
hardware_interface::HW_IF_POSITION,
&this->dataPtr->joint_position_[j]);
&this->dataPtr->joint_position_cmd_[j]);
if (!std::isnan(initial_position)) {
this->dataPtr->joint_position_cmd_[j] = initial_position;
}
}
if (hardware_info.joints[j].state_interfaces[i].name == "velocity") {
if (hardware_info.joints[j].command_interfaces[i].name == "velocity") {
RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t velocity");
this->dataPtr->state_interfaces_.emplace_back(
this->dataPtr->joint_control_methods_[j] |= VELOCITY;
this->dataPtr->command_interfaces_.emplace_back(
joint_name,
hardware_interface::HW_IF_VELOCITY,
&this->dataPtr->joint_velocity_[j]);
&this->dataPtr->joint_velocity_cmd_[j]);
if (!std::isnan(initial_velocity)) {
this->dataPtr->joint_velocity_cmd_[j] = initial_velocity;
}
}
if (hardware_info.joints[j].state_interfaces[i].name == "effort") {
if (hardware_info.joints[j].command_interfaces[i].name == "effort") {
this->dataPtr->joint_control_methods_[j] |= EFFORT;
RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t effort");
this->dataPtr->state_interfaces_.emplace_back(
this->dataPtr->command_interfaces_.emplace_back(
joint_name,
hardware_interface::HW_IF_EFFORT,
&this->dataPtr->joint_effort_[j]);
&this->dataPtr->joint_effort_cmd_[j]);
if (!std::isnan(initial_effort)) {
this->dataPtr->joint_effort_cmd_[j] = initial_effort;
}
}
}
}
Expand Down
4 changes: 3 additions & 1 deletion gazebo_ros2_control_demos/urdf/test_cart_position.xacro.urdf
View file
Expand Up @@ -57,7 +57,9 @@
<param name="min">-15</param>
<param name="max">15</param>
</command_interface>
<state_interface name="position"/>
<state_interface name="position">
<param name="initial_value">1.0</param>
</state_interface>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
Expand Down

0 comments on commit 26216b4

Please sign in to comment.