Skip to content

Commit

Permalink
Allow clutch only mode for omega6 (#29)
Browse files Browse the repository at this point in the history 
* add verbose

* default params

* Allow the scenario with clutch, but w/o the orientation
  • Loading branch information
@tpoignonec
tpoignonec authored Aug 21, 2024
1 parent 927cac5 commit 57454be
Show file tree
Hide file tree
Showing 2 changed files with 63 additions and 35 deletions.
2 changes: 2 additions & 0 deletions fd_description/config/fd.config.xacro
View file
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
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

0 comments on commit 57454be

Please sign in to comment.