Forward Controller working
Now 2 states where published: gripper_gap - which is the actuall gap between the two finger plates finger_joint - which is the old angle
This commit is contained in:
parent
5b86af00da
commit
dc53a7326f
@ -1,8 +1,9 @@
|
||||
robotiq_2f_gripper:
|
||||
# Physical limits
|
||||
min_position: 0.0 # radiant (fully open)
|
||||
max_position: 0.695 # radiant (fully open)
|
||||
min_position: 0.0 # Meters (fully closed)
|
||||
max_position: 0.14 # Meters (fully open)
|
||||
max_angle: 0.695 # radiants (closed)
|
||||
min_speed: 0.02 # Meters per second
|
||||
max_speed: 0.15 # Meters per second
|
||||
max_speed: 0.5 # Meters per second
|
||||
min_force: 20.0 # Newtons
|
||||
max_force: 235.0 # Newtons
|
||||
@ -3,42 +3,14 @@ controller_manager:
|
||||
update_rate: 500 # Hz
|
||||
joint_state_broadcaster:
|
||||
type: joint_state_broadcaster/JointStateBroadcaster
|
||||
robotiq_gripper_controller:
|
||||
type: position_controllers/GripperActionController
|
||||
fake_gripper_controller:
|
||||
type: joint_trajectory_controller/JointTrajectoryController
|
||||
forward_gripper_position_controller:
|
||||
type: robotiq_2f_controllers/RobotiqForwardController
|
||||
robotiq_activation_controller:
|
||||
type: robotiq_controllers/RobotiqActivationController
|
||||
|
||||
robotiq_gripper_controller:
|
||||
ros__parameters:
|
||||
default: true
|
||||
joint: finger_joint
|
||||
use_effort_interface: true
|
||||
use_speed_interface: true
|
||||
|
||||
forward_gripper_position_controller:
|
||||
ros__parameters:
|
||||
joint: finger_joint
|
||||
|
||||
fake_gripper_controller:
|
||||
ros__parameters:
|
||||
joints:
|
||||
- finger_joint
|
||||
command_interfaces:
|
||||
- position
|
||||
state_interfaces:
|
||||
- position
|
||||
- velocity
|
||||
state_publish_rate: 100.0
|
||||
action_monitor_rate: 20.0
|
||||
allow_partial_joints_goal: false
|
||||
constraints:
|
||||
stopped_velocity_tolerance: 0.2
|
||||
goal_time: 0.0
|
||||
finger_joint: { trajectory: 0.2, goal: 0.1 }
|
||||
joint: gripper_gap
|
||||
|
||||
robotiq_activation_controller:
|
||||
ros__parameters:
|
||||
|
||||
@ -118,13 +118,11 @@ def generate_launch_description():
|
||||
)
|
||||
|
||||
if not use_fake_hardware_bool:
|
||||
controller_spawner_names = ["robotiq_gripper_controller"]
|
||||
controller_spawner_inactive_names = ["forward_gripper_position_controller",
|
||||
"fake_gripper_controller"]
|
||||
controller_spawner_names = []
|
||||
controller_spawner_inactive_names = ["forward_gripper_position_controller"]
|
||||
else:
|
||||
controller_spawner_names = ["forward_gripper_position_controller"]
|
||||
controller_spawner_inactive_names = ["robotiq_gripper_controller",
|
||||
"fake_gripper_controller"]
|
||||
controller_spawner_inactive_names = []
|
||||
|
||||
controller_spawners = [controller_spawner(name) for name in controller_spawner_names] + [
|
||||
controller_spawner(name, active=False) for name in controller_spawner_inactive_names
|
||||
|
||||
@ -10,6 +10,7 @@
|
||||
robot_port
|
||||
min_position
|
||||
max_position
|
||||
max_angle
|
||||
min_speed
|
||||
max_speed
|
||||
min_force
|
||||
@ -35,6 +36,7 @@
|
||||
<param name="gripper_force_multiplier">0.5</param>
|
||||
<param name="min_position">${min_position}</param>
|
||||
<param name="max_position">${max_position}</param>
|
||||
<param name="max_angle">${max_angle}</param>
|
||||
<param name="min_speed">${min_speed}</param>
|
||||
<param name="max_speed">${max_speed}</param>
|
||||
<param name="min_force">${min_force}</param>
|
||||
@ -43,12 +45,17 @@
|
||||
|
||||
<!-- Joint interfaces -->
|
||||
<!-- With Hardware, they handle mimic joints, so we only need this command interface activated -->
|
||||
<joint name="${prefix}finger_joint">
|
||||
<joint name="${prefix}gripper_gap">
|
||||
<command_interface name="position" />
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">0.0</param>
|
||||
</state_interface>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
<joint name="${prefix}finger_joint">
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">0.695</param>
|
||||
</state_interface>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
<!-- When simulating we need to include the rest of the gripper joints -->
|
||||
<!-- <xacro:if value="${use_fake_hardware}"> -->
|
||||
|
||||
@ -17,6 +17,7 @@
|
||||
<xacro:property name="robotiq_2f_gripper_dict" value="${config_dict['robotiq_2f_gripper']}" />
|
||||
<xacro:property name="min_position" value="${robotiq_2f_gripper_dict['min_position']}" />
|
||||
<xacro:property name="max_position" value="${robotiq_2f_gripper_dict['max_position']}" />
|
||||
<xacro:property name="max_angle" value="${robotiq_2f_gripper_dict['max_angle']}" />
|
||||
<xacro:property name="min_speed" value="${robotiq_2f_gripper_dict['min_speed']}" />
|
||||
<xacro:property name="max_speed" value="${robotiq_2f_gripper_dict['max_speed']}" />
|
||||
<xacro:property name="min_force" value="${robotiq_2f_gripper_dict['min_force']}" />
|
||||
@ -33,6 +34,7 @@
|
||||
robot_port="${robot_port}"
|
||||
min_position="${min_position}"
|
||||
max_position="${max_position}"
|
||||
max_angle="${max_angle}"
|
||||
min_speed="${min_speed}"
|
||||
max_speed="${max_speed}"
|
||||
min_force="${min_force}"
|
||||
@ -433,5 +435,31 @@
|
||||
<child link="${prefix}robotiq_140_right_inner_finger_pad" />
|
||||
<axis xyz="0 0 1" />
|
||||
</joint>
|
||||
|
||||
<!-- Pseudo Link and Joint for gripper gap -->
|
||||
<link name="dummy_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.001 0.001 0.001"/> <!-- Very small, effectively invisible -->
|
||||
</geometry>
|
||||
<material name="transparent">
|
||||
<color rgba="0.0 0.0 0.0 0.0"/> <!-- Transparent color -->
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="base_to_dummy" type="fixed">
|
||||
<parent link="${prefix}robotiq_140_base_link"/>
|
||||
<child link="dummy_link"/>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/> <!-- Adjust origin as needed -->
|
||||
</joint>
|
||||
|
||||
<joint name="gripper_gap" type="prismatic">
|
||||
<parent link="dummy_link"/>
|
||||
<child link="${prefix}robotiq_140_right_inner_finger_pad"/>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/> <!-- Set origin appropriately -->
|
||||
<axis xyz="0 1 0"/> <!-- Axis of movement; adjust according to actual alignment -->
|
||||
<limit lower="0.0" upper="0.14" effort="235" velocity="0.15"/>
|
||||
</joint>
|
||||
</xacro:macro>
|
||||
</robot>
|
||||
|
||||
@ -67,8 +67,9 @@ protected: // Likely changes the access to protected from private
|
||||
static constexpr double NO_NEW_CMD_ = std::numeric_limits<double>::quiet_NaN();
|
||||
|
||||
// Conversion Methods
|
||||
double convertRawToPosition(int raw_position);
|
||||
int convertPositionToRaw(double position);
|
||||
double convertRawToGap(int raw_position);
|
||||
double convertRawToAngle(int raw_position);
|
||||
int convertGapToRaw(double gap);
|
||||
int convertSpeedToRaw(double speed);
|
||||
int convertForceToRaw(double force);
|
||||
|
||||
@ -80,9 +81,10 @@ protected: // Likely changes the access to protected from private
|
||||
|
||||
|
||||
// Gripper State Tracking
|
||||
double gripper_position_ = 0.0;
|
||||
double gripper_gap_ = 0.0;
|
||||
double gripper_angle_ = 0.0;
|
||||
double gripper_velocity_ = 0.0;
|
||||
double gripper_position_command_ = 0.0;
|
||||
double gripper_gap_command_ = 0.0;
|
||||
|
||||
rclcpp::Clock::SharedPtr clock_ = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
|
||||
rclcpp::Time last_update_time_ = clock_ -> now();
|
||||
@ -102,6 +104,7 @@ protected: // Likely changes the access to protected from private
|
||||
// Parameters for physical limits and configuration
|
||||
double min_position_;
|
||||
double max_position_;
|
||||
double max_angle_;
|
||||
double min_speed_;
|
||||
double max_speed_;
|
||||
double min_force_;
|
||||
|
||||
@ -18,6 +18,9 @@ const double min_position_default = 0.01;
|
||||
const std::string max_position_parameter_name = "max_position";
|
||||
const double max_position_default = 0.0;
|
||||
|
||||
const std::string max_angle_parameter_name = "max_angle";
|
||||
const double max_angle_default = 0.5;
|
||||
|
||||
const std::string min_speed_parameter_name = "min_speed";
|
||||
const double min_speed_default = 0.0;
|
||||
|
||||
@ -96,6 +99,14 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(cons
|
||||
}
|
||||
RCLCPP_INFO(logger_, "Accessing parameter: max position, Value: %f", max_position_);
|
||||
|
||||
if (info.hardware_parameters.count(max_angle_parameter_name) > 0) {
|
||||
max_angle_ = std::stod(info.hardware_parameters.at(max_angle_parameter_name));
|
||||
} else {
|
||||
RCLCPP_WARN(logger_, "max angle not found falling back to default value!");
|
||||
max_angle_ = max_angle_default;
|
||||
}
|
||||
RCLCPP_INFO(logger_, "Accessing parameter: max angle , Value: %f", max_angle_);
|
||||
|
||||
if (info.hardware_parameters.count(min_speed_parameter_name) > 0) {
|
||||
min_speed_ = std::stod(info.hardware_parameters.at(min_speed_parameter_name));
|
||||
} else {
|
||||
@ -130,49 +141,69 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(cons
|
||||
return CallbackReturn::ERROR;
|
||||
}
|
||||
|
||||
gripper_position_ = std::numeric_limits<double>::quiet_NaN();
|
||||
gripper_gap_ = std::numeric_limits<double>::quiet_NaN();
|
||||
gripper_angle_ = std::numeric_limits<double>::quiet_NaN();
|
||||
gripper_velocity_ = std::numeric_limits<double>::quiet_NaN();
|
||||
gripper_position_command_ = std::numeric_limits<double>::quiet_NaN();
|
||||
gripper_gap_command_ = std::numeric_limits<double>::quiet_NaN();
|
||||
reactivate_gripper_cmd_ = NO_NEW_CMD_;
|
||||
reactivate_gripper_async_cmd_.store(false);
|
||||
|
||||
const hardware_interface::ComponentInfo& joint = info_.joints[0];
|
||||
const hardware_interface::ComponentInfo& joint_gap = info_.joints[0];
|
||||
const hardware_interface::ComponentInfo& joint_angle = info_.joints[1];
|
||||
|
||||
// There is one command interface: position.
|
||||
if (joint.command_interfaces.size() != 1)
|
||||
if (joint_gap.command_interfaces.size() != 1)
|
||||
{
|
||||
RCLCPP_FATAL(logger_, "Joint '%s' has %zu command interfaces found. 1 expected.", joint.name.c_str(),
|
||||
joint.command_interfaces.size());
|
||||
RCLCPP_FATAL(logger_, "Joint '%s' has %zu command interfaces found. 1 expected.", joint_gap.name.c_str(),
|
||||
joint_gap.command_interfaces.size());
|
||||
return CallbackReturn::ERROR;
|
||||
}
|
||||
|
||||
if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION)
|
||||
if (joint_gap.command_interfaces[0].name != hardware_interface::HW_IF_POSITION)
|
||||
{
|
||||
RCLCPP_FATAL(logger_, "Joint '%s' has %s command interfaces found. '%s' expected.", joint.name.c_str(),
|
||||
joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION);
|
||||
RCLCPP_FATAL(logger_, "Joint '%s' has %s command interfaces found. '%s' expected.", joint_gap.name.c_str(),
|
||||
joint_gap.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION);
|
||||
return CallbackReturn::ERROR;
|
||||
}
|
||||
|
||||
// There are two state interfaces: position and velocity.
|
||||
if (joint.state_interfaces.size() != 2)
|
||||
// There are two state interfaces for the gripper gap: position and velocity.
|
||||
if (joint_gap.state_interfaces.size() != 2)
|
||||
{
|
||||
RCLCPP_FATAL(logger_, "Joint '%s' has %zu state interface. 2 expected.", joint.name.c_str(),
|
||||
joint.state_interfaces.size());
|
||||
RCLCPP_FATAL(logger_, "Joint '%s' has %zu state interface. 2 expected.", joint_gap.name.c_str(),
|
||||
joint_gap.state_interfaces.size());
|
||||
return CallbackReturn::ERROR;
|
||||
}
|
||||
|
||||
for (int i = 0; i < 2; ++i)
|
||||
{
|
||||
if (!(joint.state_interfaces[i].name == hardware_interface::HW_IF_POSITION ||
|
||||
joint.state_interfaces[i].name == hardware_interface::HW_IF_VELOCITY))
|
||||
if (!(joint_gap.state_interfaces[i].name == hardware_interface::HW_IF_POSITION ||
|
||||
joint_gap.state_interfaces[i].name == hardware_interface::HW_IF_VELOCITY))
|
||||
{
|
||||
RCLCPP_FATAL(logger_, "Joint '%s' has %s state interface. Expected %s or %s.", joint.name.c_str(),
|
||||
joint.state_interfaces[i].name.c_str(), hardware_interface::HW_IF_POSITION,
|
||||
RCLCPP_FATAL(logger_, "Joint '%s' has %s state interface. Expected %s or %s.", joint_gap.name.c_str(),
|
||||
joint_gap.state_interfaces[i].name.c_str(), hardware_interface::HW_IF_POSITION,
|
||||
hardware_interface::HW_IF_VELOCITY);
|
||||
return CallbackReturn::ERROR;
|
||||
}
|
||||
}
|
||||
|
||||
// There are two state interfaces for the gripper angle: position.
|
||||
if (joint_angle.state_interfaces.size() != 1)
|
||||
{
|
||||
RCLCPP_FATAL(logger_, "Joint '%s' has %zu state interface. 1 expected.", joint_angle.name.c_str(),
|
||||
joint_angle.state_interfaces.size());
|
||||
return CallbackReturn::ERROR;
|
||||
}
|
||||
|
||||
if (!(joint_angle.state_interfaces[0].name == hardware_interface::HW_IF_POSITION))
|
||||
{
|
||||
RCLCPP_FATAL(logger_, "Joint '%s' has %s state interface. Expected %s or %s.", joint_angle.name.c_str(),
|
||||
joint_angle.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION,
|
||||
hardware_interface::HW_IF_VELOCITY);
|
||||
return CallbackReturn::ERROR;
|
||||
}
|
||||
|
||||
|
||||
|
||||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
@ -207,11 +238,12 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_activate(
|
||||
RCLCPP_DEBUG(logger_, "on_activate");
|
||||
|
||||
// set some default values for joints
|
||||
if (std::isnan(gripper_position_))
|
||||
if (std::isnan(gripper_gap_))
|
||||
{
|
||||
gripper_position_ = 0;
|
||||
gripper_velocity_ = 0;
|
||||
gripper_position_command_ = 0;
|
||||
gripper_gap_ = 0.0;
|
||||
gripper_angle_ = 0.0;
|
||||
gripper_velocity_ = 0.0;
|
||||
gripper_gap_command_ = 0.0;
|
||||
}
|
||||
|
||||
// Activate the gripper.
|
||||
@ -272,12 +304,13 @@ std::vector<hardware_interface::StateInterface> RobotiqGripperHardwareInterface:
|
||||
std::vector<hardware_interface::StateInterface> state_interfaces;
|
||||
|
||||
state_interfaces.emplace_back(
|
||||
hardware_interface::StateInterface(info_.joints[0].name, hardware_interface::HW_IF_POSITION, &gripper_position_));
|
||||
hardware_interface::StateInterface(info_.joints[0].name, hardware_interface::HW_IF_POSITION, &gripper_gap_));
|
||||
state_interfaces.emplace_back(
|
||||
hardware_interface::StateInterface(info_.joints[0].name, hardware_interface::HW_IF_VELOCITY, &gripper_velocity_));
|
||||
|
||||
state_interfaces.emplace_back(
|
||||
hardware_interface::StateInterface(info_.joints[1].name, hardware_interface::HW_IF_POSITION, &gripper_angle_));
|
||||
for (const auto& interface : state_interfaces) {
|
||||
RCLCPP_DEBUG(logger_, "Exporting state interface for joint: %s type: %s",
|
||||
RCLCPP_INFO(logger_, "Exporting state interface for joint: %s type: %s",
|
||||
interface.get_name().c_str(), interface.get_interface_name().c_str());
|
||||
}
|
||||
|
||||
@ -292,7 +325,7 @@ std::vector<hardware_interface::CommandInterface> RobotiqGripperHardwareInterfac
|
||||
|
||||
command_interfaces.emplace_back(
|
||||
hardware_interface::CommandInterface(
|
||||
info_.joints[0].name, hardware_interface::HW_IF_POSITION, &gripper_position_command_
|
||||
info_.joints[0].name, hardware_interface::HW_IF_POSITION, &gripper_gap_command_
|
||||
)
|
||||
);
|
||||
|
||||
@ -337,16 +370,18 @@ hardware_interface::return_type RobotiqGripperHardwareInterface::read(const rclc
|
||||
|
||||
try {
|
||||
int raw_position = socket_adapter_->position();
|
||||
double new_position = convertRawToPosition(raw_position);
|
||||
double new_gap = convertRawToGap(raw_position);
|
||||
double new_angle = convertRawToAngle(raw_position);
|
||||
|
||||
if (!std::isnan(gripper_position_)) {
|
||||
if (!std::isnan(gripper_gap_)) {
|
||||
double time_difference = (current_time_ - last_update_time_).seconds();
|
||||
if (time_difference > 0) {
|
||||
gripper_velocity_ = (new_position - gripper_position_) / time_difference;
|
||||
gripper_velocity_ = (new_gap - gripper_gap_) / time_difference;
|
||||
}
|
||||
}
|
||||
|
||||
gripper_position_ = new_position;
|
||||
gripper_gap_ = new_gap;
|
||||
gripper_angle_ = new_angle;
|
||||
last_update_time_ = current_time_;
|
||||
|
||||
} catch (const std::exception& e) {
|
||||
@ -375,12 +410,12 @@ hardware_interface::return_type RobotiqGripperHardwareInterface::write(const rcl
|
||||
{
|
||||
try {
|
||||
// Use the conversion methods to scale the command values to the range expected by the hardware
|
||||
int scaled_position = convertPositionToRaw(gripper_position_command_);
|
||||
int scaled_gap = convertGapToRaw(gripper_gap_command_);
|
||||
int scaled_speed = convertSpeedToRaw(gripper_speed_);
|
||||
int scaled_force = convertForceToRaw(gripper_force_);
|
||||
|
||||
// Store the scaled values into atomic variables
|
||||
write_command_.store(scaled_position);
|
||||
write_command_.store(scaled_gap);
|
||||
write_speed_.store(scaled_speed);
|
||||
write_force_.store(scaled_force);
|
||||
|
||||
@ -425,17 +460,17 @@ void RobotiqGripperHardwareInterface::performRegularOperations()
|
||||
{
|
||||
try
|
||||
{
|
||||
int scaled_position = write_command_.load();
|
||||
int scaled_gap = write_command_.load();
|
||||
int scaled_speed = write_speed_.load();
|
||||
int scaled_force = write_force_.load();
|
||||
|
||||
auto result = socket_adapter_->move(scaled_position, scaled_speed, scaled_force);
|
||||
auto result = socket_adapter_->move(scaled_gap, scaled_speed, scaled_force);
|
||||
if (!std::get<0>(result)) {
|
||||
throw std::runtime_error("Failed to send move command to Robotiq gripper.");
|
||||
}
|
||||
|
||||
int raw_position = socket_adapter_->position();
|
||||
gripper_current_state_.store(convertRawToPosition(raw_position));
|
||||
int raw_gap = socket_adapter_->position();
|
||||
gripper_current_state_.store(convertRawToGap(raw_gap));
|
||||
}
|
||||
catch (const std::exception& e)
|
||||
{
|
||||
@ -457,19 +492,27 @@ void RobotiqGripperHardwareInterface::configureAdapter(bool useMock) {
|
||||
|
||||
|
||||
// Conversion methods
|
||||
double RobotiqGripperHardwareInterface::convertRawToPosition(int raw_position) {
|
||||
double RobotiqGripperHardwareInterface::convertRawToGap(int raw_position) {
|
||||
if (std::isnan(min_position_) || std::isnan(max_position_) || min_position_ >= max_position_) {
|
||||
throw std::runtime_error("Invalid gripper position limits: min_position_ or max_position_ are not configured correctly.");
|
||||
}
|
||||
double scaled_position = min_position_ + (static_cast<double>(raw_position) / 255.0) * (max_position_ - min_position_);
|
||||
double scaled_position = max_position_ - (static_cast<double>(raw_position) / 255.0) * (max_position_ - min_position_);
|
||||
return std::max(min_position_, std::min(scaled_position, max_position_));
|
||||
}
|
||||
|
||||
int RobotiqGripperHardwareInterface::convertPositionToRaw(double position) {
|
||||
double RobotiqGripperHardwareInterface::convertRawToAngle(int raw_position) {
|
||||
if (std::isnan(max_angle_) || 0.0 >= max_angle_) {
|
||||
throw std::runtime_error("Invalid gripper angle limits: max_angle is not configured correctly.");
|
||||
}
|
||||
double scaled_angle = (static_cast<double>(raw_position) / 255.0) * max_angle_;
|
||||
return std::max(0.0, std::min(scaled_angle, max_angle_));
|
||||
}
|
||||
|
||||
int RobotiqGripperHardwareInterface::convertGapToRaw(double gap) {
|
||||
if (std::isnan(min_position_) || std::isnan(max_position_) || min_position_ >= max_position_) {
|
||||
throw std::runtime_error("Invalid gripper position limits: min_position_ or max_position_ are not configured correctly.");
|
||||
}
|
||||
double scaled = (position - min_position_) / (max_position_ - min_position_) * 255.0;
|
||||
double scaled = 255.0 - (gap - min_position_) / (max_position_ - min_position_) * 255.0;
|
||||
return static_cast<uint8_t>(std::clamp(scaled, 0.0, 255.0));
|
||||
}
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user