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.

Sign up for GitHub

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

Allow clutch only mode for omega6 #29

Merged
merged 4 commits into from
Aug 21, 2024
Merged

Allow clutch only mode for omega6 #29

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
c020111
add verbose
Aug 21, 2024
275039b
default params
Aug 21, 2024
fb5e545
some more verbose
Aug 21, 2024
b1ad2bd
Allow the scenario with clutch, but w/o the orientation
Aug 21, 2024
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
2 changes: 2 additions & 0 deletions fd_description/config/fd.config.xacro
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,10 +17,12 @@
<!-- Import and setup interface ros2_control description -->
<xacro:include filename="$(find fd_description)/ros2_control/fd.r2c_hardware.xacro" />
<xacro:fd_ros2_control
interface_id="-1"
robot_id= "$(arg robot_id)"
use_fake_hardware="$(arg use_fake_hardware)"
use_orientation="$(arg use_orientation)"
orientation_is_actuated="$(arg use_orientation)"
use_clutch="$(arg use_clutch)"
emulate_button="true"
/> <!-- orientation_is_actuated = use_orientation for now (see HW impl.) -->
</robot>
96 changes: 61 additions & 35 deletions fd_hardware/src/fd_effort_hi.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -167,6 +167,7 @@ CallbackReturn FDEffortHardwareInterface::on_init(
} else {
emulate_button_ = false;
}
RCLCPP_INFO(LOGGER, "Emulating button: %s", emulate_button_ ? "true" : "false");

auto it_fd_inertia = info_.hardware_parameters.find("inertia_interface_name");
if (it_fd_inertia != info_.hardware_parameters.end()) {
Expand Down Expand Up @@ -281,40 +282,63 @@ hardware_interface::return_type FDEffortHardwareInterface::read(
flag += dhdGetPosition(
&hw_states_position_[0], &hw_states_position_[1], &hw_states_position_[2],
interface_ID_);
if (dhdHasWrist(interface_ID_) && hw_states_position_.size() > 3) {
if (dhdHasWrist(interface_ID_) && hw_states_position_.size() == 4) {
// No orientation, skip!
} else if (dhdHasWrist(interface_ID_) && hw_states_position_.size() > 3) {
flag += dhdGetOrientationRad(
&hw_states_position_[3], &hw_states_position_[4],
&hw_states_position_[5], interface_ID_);
}
if (dhdHasGripper(interface_ID_) && hw_states_position_.size() > 6) {

// Get gripper angle
if (dhdHasGripper(interface_ID_) && hw_states_position_.size() == 4) {
flag += dhdGetGripperAngleRad(&hw_states_position_[3], interface_ID_);
} else if (dhdHasGripper(interface_ID_) && hw_states_position_.size() > 6) {
flag += dhdGetGripperAngleRad(&hw_states_position_[6], interface_ID_);
}

// Get velocity
flag += dhdGetLinearVelocity(
&hw_states_velocity_[0], &hw_states_velocity_[1],
&hw_states_velocity_[2], interface_ID_);
if (dhdHasWrist(interface_ID_) && hw_states_velocity_.size() > 3) {
if (dhdHasWrist(interface_ID_) && hw_states_velocity_.size() == 4) {
// No orientation, skip!
} else if (dhdHasWrist(interface_ID_) && hw_states_velocity_.size() > 3) {
flag += dhdGetAngularVelocityRad(
&hw_states_velocity_[3], &hw_states_velocity_[4],
&hw_states_velocity_[5], interface_ID_);
}
if (dhdHasGripper(interface_ID_) && hw_states_velocity_.size() > 6) {

// Get gripper angular velocity
if (dhdHasGripper(interface_ID_) && hw_states_velocity_.size() == 4) {
flag += dhdGetGripperAngularVelocityRad(&hw_states_velocity_[3], interface_ID_);
} else if (dhdHasGripper(interface_ID_) && hw_states_velocity_.size() > 6) {
flag += dhdGetGripperAngularVelocityRad(&hw_states_velocity_[6], interface_ID_);
}

// Get forces
double torque[3];
double gripper_force;
flag += dhdGetForceAndTorqueAndGripperForce(
&hw_states_effort_[0], &hw_states_effort_[1],
&hw_states_effort_[2], &torque[0], &torque[1],
&torque[2], &gripper_force, interface_ID_);
if (dhdHasWrist(interface_ID_) && hw_states_effort_.size() > 3) {
&hw_states_effort_[0], &hw_states_effort_[1], &hw_states_effort_[2],
&torque[0], &torque[1], &torque[2],
&gripper_force,
interface_ID_
);

// Get torques
if (dhdHasWrist(interface_ID_) && hw_states_velocity_.size() == 4) {
// No orientation, skip!
} else if (dhdHasWrist(interface_ID_) && hw_states_effort_.size() > 3) {
hw_states_effort_[3] = torque[0];
hw_states_effort_[4] = torque[1];
hw_states_effort_[5] = torque[2];
}
if (dhdHasGripper(interface_ID_) && hw_states_effort_.size() > 6) {

// Get gripper force
if (dhdHasGripper(interface_ID_) && hw_states_effort_.size() == 4) {
hw_states_effort_[3] = gripper_force;
} else if (dhdHasGripper(interface_ID_) && hw_states_effort_.size() > 6) {
hw_states_effort_[6] = gripper_force;
}
// Get inertia
Expand Down Expand Up @@ -342,6 +366,7 @@ hardware_interface::return_type FDEffortHardwareInterface::read(
} else if (button_status == 0) {
hw_button_state_[0] = 0.0;
} else {
RCLCPP_ERROR(LOGGER, "Invalid button reading!");
flag += -1;
}

Expand Down Expand Up @@ -370,12 +395,18 @@ hardware_interface::return_type FDEffortHardwareInterface::write(
hw_commands_effort_[0], hw_commands_effort_[1], hw_commands_effort_[2],
hw_commands_effort_[3], hw_commands_effort_[4], hw_commands_effort_[5],
hw_commands_effort_[6], interface_ID_);
} else if (dhdHasWrist(interface_ID_) && hw_states_effort_.size() == 4) {
dhdSetForceAndTorqueAndGripperForce(
hw_commands_effort_[0], hw_commands_effort_[1], hw_commands_effort_[2],
0.0, 0.0, 0.0, hw_commands_effort_[3], interface_ID_);
} else if (dhdHasWrist(interface_ID_) && hw_states_effort_.size() > 3) {
// No clutch joint
dhdSetForceAndTorqueAndGripperForce(
hw_commands_effort_[0], hw_commands_effort_[1], hw_commands_effort_[2],
hw_commands_effort_[3], hw_commands_effort_[4], hw_commands_effort_[5],
0, interface_ID_);
} else {
// Only translation is actuated
dhdSetForceAndTorqueAndGripperForce(
hw_commands_effort_[0], hw_commands_effort_[1], hw_commands_effort_[2],
0, 0, 0, 0, interface_ID_);
Expand All @@ -395,9 +426,7 @@ bool FDEffortHardwareInterface::connectToDevice()

// Open connection
if (dhdOpen() >= 0) {
RCLCPP_INFO(
rclcpp::get_logger(
"FDEffortHardwareInterface"), "dhd : %s device detected", dhdGetSystemName());
RCLCPP_INFO(LOGGER, "dhd : %s device detected", dhdGetSystemName());

// Check if the device has 3 dof or more
if (dhdHasWrist(interface_ID_)) {
Expand All @@ -409,56 +438,55 @@ bool FDEffortHardwareInterface::connectToDevice()
// Retrieve the mass of the device
double effector_mass = 0.0;
if (dhdGetEffectorMass(&effector_mass, interface_ID_) == DHD_NO_ERROR) {
RCLCPP_INFO(
rclcpp::get_logger(
"FDEffortHardwareInterface"), "dhd : Effector Mass = %f (g)", effector_mass * 1000.0);
RCLCPP_INFO(LOGGER, "dhd : Effector Mass = %f (g)", effector_mass * 1000.0);
} else {
RCLCPP_WARN(
rclcpp::get_logger(
"FDEffortHardwareInterface"), "dhd : Impossible to retrieve effector mass !");
RCLCPP_WARN(LOGGER, "dhd : Impossible to retrieve effector mass !");
}

// Set force limit & enable force
double forceMax = 12; // N
if (dhdSetMaxForce(forceMax, interface_ID_) < DHD_NO_ERROR) {
RCLCPP_ERROR(LOGGER, "dhd : Could not set max force!");
disconnectFromDevice();
}
// apply zero force
dhdSetBrakes(DHD_OFF, interface_ID_);

if (dhdEnableForce(DHD_ON, interface_ID_) < DHD_NO_ERROR) {
RCLCPP_ERROR(LOGGER, "dhd : Could not enable force control!");
disconnectFromDevice();
return false;
}
// Gravity compensation
if (dhdSetGravityCompensation(DHD_ON, interface_ID_) < DHD_NO_ERROR) {
RCLCPP_WARN(
rclcpp::get_logger(
"FDEffortHardwareInterface"), "dhd : Could not enable the gravity compensation !");
RCLCPP_WARN(LOGGER, "dhd : Could not enable the gravity compensation !");
disconnectFromDevice();
return false;
} else {
RCLCPP_INFO(
rclcpp::get_logger(
"FDEffortHardwareInterface"), "dhd : Gravity compensation enabled");
RCLCPP_INFO(LOGGER, "dhd : Gravity compensation enabled");
}
RCLCPP_INFO(LOGGER, "dhd : Device connected !");

if (emulate_button_ && dhdHasGripper(interface_ID_)) {
RCLCPP_INFO(
rclcpp::get_logger(
"FDEffortHardwareInterface"), "dhd : Emulating button from clutch joint !");
// Emulate button
if (emulate_button_ && !dhdHasGripper(interface_ID_)) {
RCLCPP_ERROR(LOGGER, "dhd : Could not enable button emulation, no gripper found!");
} else if (emulate_button_ && dhdHasGripper(interface_ID_)) {
RCLCPP_INFO(LOGGER, "dhd : Emulating button from clutch joint...");
if (dhdEmulateButton(DHD_ON, interface_ID_) < DHD_NO_ERROR) {
RCLCPP_WARN(
rclcpp::get_logger(
"FDEffortHardwareInterface"), "dhd : Could not enable button emulation!");
RCLCPP_ERROR(LOGGER, "dhd : Could not enable button emulation!");
disconnectFromDevice();
return false;
}
RCLCPP_INFO(LOGGER, "dhd : OK, button will be emulated from clutch joint.");
}
// Set force to zero
if (dhdSetForceAndTorqueAndGripperForce(
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
interface_ID_) < DHD_NO_ERROR)
{
RCLCPP_ERROR(LOGGER, "dhd : Could not initialize force control!");
disconnectFromDevice();
return false;
}

// Sleep 100 ms
Expand All @@ -467,20 +495,18 @@ bool FDEffortHardwareInterface::connectToDevice()

return isConnected_;
} else {
RCLCPP_ERROR(
rclcpp::get_logger(
"FDEffortHardwareInterface"), "dhd : Could not connect to device !");
RCLCPP_ERROR(LOGGER, "dhd : Could not connect to device !");
isConnected_ = false;
return isConnected_;
}
}
// ------------------------------------------------------------------------------------------
bool FDEffortHardwareInterface::disconnectFromDevice()
{
RCLCPP_INFO(LOGGER, "dhd : stopping the device.");
// Stop the device: disables the force on the haptic device and puts it into BRAKE mode.
int hasStopped = -1;
while (hasStopped < 0) {
RCLCPP_INFO(LOGGER, "dhd : stopping the device, please wait...");
hasStopped = dhdStop(interface_ID_);
// Sleep 100 ms
dhdSleep(0.1);
Expand Down
Loading