Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Dynamixel effort state processing #291

Open
wants to merge 20 commits into
base: main
Choose a base branch
from
Open
Changes from 1 commit
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
joint position controller overloaded check
  • Loading branch information
filiparag committed Feb 23, 2024
commit 481ce37ddd3ff2519a3aaa5078355b26411baf47
17 changes: 9 additions & 8 deletions mep3_controllers/src/joint_position_controller.cpp
Original file line number Diff line number Diff line change
@@ -23,15 +23,15 @@ namespace mep3_controllers
if (goal->max_velocity != 0)
max_velocity = goal->max_velocity;

double tolerance = 99999;
double tolerance = std::numeric_limits<double>::max();
if (goal->tolerance != 0)
tolerance = goal->tolerance;

double timeout = 99999;
double timeout = std::numeric_limits<double>::max();
if (goal->timeout != 0)
timeout = goal->timeout;

double max_effort = 99999;
double max_effort = std::numeric_limits<double>::max();
if (goal->max_effort != 0)
max_effort = goal->max_effort;

@@ -157,7 +157,7 @@ namespace mep3_controllers
joint->active = false;
RCLCPP_ERROR(get_node()->get_logger(), "Joint %s timeout", joint->name.c_str());
}
else if (joint->effort_handle->get().get_value() > joint->max_effort)
else if (std::isinf(joint->effort_handle->get().get_value()) || joint->effort_handle->get().get_value() > joint->max_effort)
{
result->set__result(mep3_msgs::action::JointPositionCommand::Goal::RESULT_OVERLOAD);
joint->action_server->terminate_current(result);
@@ -193,7 +193,7 @@ namespace mep3_controllers
});
if (position_command_handle == command_interfaces_.end())
{
RCLCPP_ERROR(get_node()->get_logger(), "Unable to obtain joint command handle for %s", joint->name.c_str());
RCLCPP_ERROR(get_node()->get_logger(), "Unable to obtain joint position command handle for %s", joint->name.c_str());
return controller_interface::CallbackReturn::FAILURE;
}
joint->position_command_handle = std::ref(*position_command_handle);
@@ -208,7 +208,7 @@ namespace mep3_controllers
});
if (velocity_command_handle == command_interfaces_.end())
{
RCLCPP_ERROR(get_node()->get_logger(), "Unable to obtain joint command handle for %s", joint->name.c_str());
RCLCPP_ERROR(get_node()->get_logger(), "Unable to obtain joint velocity command handle for %s", joint->name.c_str());
return controller_interface::CallbackReturn::FAILURE;
}
joint->velocity_command_handle = std::ref(*velocity_command_handle);
@@ -223,7 +223,7 @@ namespace mep3_controllers
});
if (position_handle == state_interfaces_.end())
{
RCLCPP_ERROR(get_node()->get_logger(), "Unable to obtain joint state handle for %s", joint->name.c_str());
RCLCPP_ERROR(get_node()->get_logger(), "Unable to obtain joint position state handle for %s", joint->name.c_str());
return controller_interface::CallbackReturn::FAILURE;
}
joint->position_handle = std::ref(*position_handle);
@@ -238,10 +238,11 @@ namespace mep3_controllers
});
if (effort_handle == state_interfaces_.end())
{
RCLCPP_ERROR(get_node()->get_logger(), "Unable to obtain joint state handle for %s", joint->name.c_str());
RCLCPP_ERROR(get_node()->get_logger(), "Unable to obtain joint effort state handle for %s", joint->name.c_str());
return controller_interface::CallbackReturn::FAILURE;
}
joint->effort_handle = std::ref(*effort_handle);

}
return controller_interface::CallbackReturn::SUCCESS;
}
4 changes: 2 additions & 2 deletions mep3_msgs/action/JointPositionCommand.action
Original file line number Diff line number Diff line change
@@ -8,11 +8,11 @@ float64 max_velocity # rad/s or m/s
float64 max_acceleration # rad/s^2 or m/s^2
float64 tolerance # rad or m
float64 timeout # s
float64 max_effort
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Zasto mA? Mozes staviti da se parametar zove max_current i onda jedinicu A. effort moze biti N ili Nm

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Ima smisla, slažem se

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Nisam video da može biti Nm, a samo N mi nema smisla da bude jer je rotaciono kretanje?

float64 max_effort # mA
---
float64 last_position
float64 last_effort
uint8 result
---
float64 position # rad or m
float64 effort
float64 effort # mA