diff --git a/CHANGELOG.md b/CHANGELOG.md index 353abc4b..4a075741 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -13,6 +13,7 @@ Requires `libfranka` >= 0.8.0 * `franka_gazebo`: `FrankaHWSim` only acts on joints belonging to a Franka robot. This allows to combine a Franka robot and others (like mobile platforms) in same URDF ([#313](https://github.com/frankaemika/franka_ros/issues/313)) * `franka_description`: `` macro now supports to customize the `parent` frame and its `xyz` + `rpy` offset * `franka_hw`: Fix the bug where the previous controller is still running after switching the controller. ([#326](https://github.com/frankaemika/franka_ros/issues/326)) + * `franka_gazebo`: Add `set_franka_model_configuration` service. ## 0.10.1 - 2022-09-15 diff --git a/franka_gazebo/include/franka_gazebo/franka_hw_sim.h b/franka_gazebo/include/franka_gazebo/franka_hw_sim.h index d94b9019..a316da50 100644 --- a/franka_gazebo/include/franka_gazebo/franka_hw_sim.h +++ b/franka_gazebo/include/franka_gazebo/franka_hw_sim.h @@ -158,8 +158,10 @@ class FrankaHWSim : public gazebo_ros_control::RobotHWSim { ros::ServiceServer service_set_load_; ros::ServiceServer service_collision_behavior_; ros::ServiceServer service_user_stop_; + ros::ServiceServer service_set_model_configuration_; ros::ServiceClient service_controller_list_; ros::ServiceClient service_controller_switch_; + ros::ServiceClient service_gazebo_set_model_configuration_; std::unique_ptr> action_recovery_; std::vector lower_force_thresholds_nominal_; diff --git a/franka_gazebo/include/franka_gazebo/joint.h b/franka_gazebo/include/franka_gazebo/joint.h index a6e9bdea..b301e8b7 100644 --- a/franka_gazebo/include/franka_gazebo/joint.h +++ b/franka_gazebo/include/franka_gazebo/joint.h @@ -167,9 +167,19 @@ struct Joint { /// gains are ignored. control_toolbox::Pid velocity_controller; + /** + * Sets the joint position. + */ + void setJointPosition(const double joint_position); + private: double lastVelocity = std::numeric_limits::quiet_NaN(); double lastAcceleration = std::numeric_limits::quiet_NaN(); + + // Track joint position set requests + bool setPositionRequested_ = false; + double requestedPosition_ = 0.0; + std::mutex requestedPositionMutex_; }; } // namespace franka_gazebo diff --git a/franka_gazebo/src/franka_hw_sim copy.cpp b/franka_gazebo/src/franka_hw_sim copy.cpp new file mode 100644 index 00000000..43a9c09d --- /dev/null +++ b/franka_gazebo/src/franka_hw_sim copy.cpp @@ -0,0 +1,897 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "ros/ros.h" + +namespace franka_gazebo { + +using actionlib::SimpleActionServer; +using boost::sml::state; + +FrankaHWSim::FrankaHWSim() : sm_(this->robot_state_, this->joints_) {} + +bool FrankaHWSim::initSim(const std::string& robot_namespace, + ros::NodeHandle model_nh, + gazebo::physics::ModelPtr parent, + const urdf::Model* const urdf, + std::vector transmissions) { + model_nh.param("arm_id", this->arm_id_, robot_namespace); + if (this->arm_id_ != robot_namespace) { + ROS_WARN_STREAM_NAMED( + "franka_hw_sim", + "Caution: Robot names differ! Read 'arm_id: " + << this->arm_id_ << "' from parameter server but URDF defines '" + << robot_namespace << "'. Will use '" << this->arm_id_ << "'!"); + } + + this->robot_ = parent; + this->robot_initialized_ = false; + + this->robot_initialized_pub_ = model_nh.advertise("initialized", 1); + std_msgs::Bool msg; + msg.data = static_cast(false); + this->robot_initialized_pub_.publish(msg); + + this->action_recovery_ = std::make_unique>( + model_nh, "franka_control/error_recovery", + [&](const franka_msgs::ErrorRecoveryGoalConstPtr& goal) { + if (this->robot_state_.robot_mode == franka::RobotMode::kUserStopped) { + ROS_WARN_STREAM_NAMED("franka_hw_sim", + "Cannot recover errors since the user stop seems still pressed"); + this->action_recovery_->setSucceeded(); + return; + } + try { + restartControllers(); + ROS_INFO_NAMED("franka_hw_sim", "Recovered from error"); + this->sm_.process_event(ErrorRecovery()); + this->action_recovery_->setSucceeded(); + } catch (const std::runtime_error& e) { + ROS_WARN_STREAM_NAMED("franka_hw_sim", "Error recovery failed: " << e.what()); + this->action_recovery_->setAborted(); + } + }, + false); + this->action_recovery_->start(); + +#if GAZEBO_MAJOR_VERSION >= 8 + gazebo::physics::PhysicsEnginePtr physics = gazebo::physics::get_world()->Physics(); +#else + gazebo::physics::PhysicsEnginePtr physics = gazebo::physics::get_world()->GetPhysicsEngine(); +#endif + + ROS_INFO_STREAM_NAMED("franka_hw_sim", "Using physics type " << physics->GetType()); + + // Retrieve initial gravity vector from Gazebo + // NOTE: Can be overwritten by the user via the 'gravity_vector' ROS parameter. + auto gravity = physics->World()->Gravity(); + this->gravity_earth_ = {gravity.X(), gravity.Y(), gravity.Z()}; + + model_nh.param("tau_ext_lowpass_filter", this->tau_ext_lowpass_filter_, + kDefaultTauExtLowpassFilter); + + // Generate a list of franka_gazebo::Joint to store all relevant information + for (const auto& transmission : transmissions) { + if (transmission.type_ != "transmission_interface/SimpleTransmission") { + continue; + } + if (transmission.joints_.empty()) { + ROS_WARN_STREAM_NAMED("franka_hw_sim", + "Transmission " << transmission.name_ << " has no associated joints."); + return false; + } + if (transmission.joints_.size() > 1) { + ROS_WARN_STREAM_NAMED( + "franka_hw_sim", + "Transmission " + << transmission.name_ + << " has more than one joint. Currently the franka robot hardware simulation " + << " interface only supports one."); + return false; + } + + // Fill a 'Joint' struct which holds all necessary data + auto joint = std::make_shared(); + joint->name = transmission.joints_[0].name_; + + if (std::none_of(kRobotJointSuffixes.begin(), kRobotJointSuffixes.end(), + [&](auto suffix) { return joint->name == arm_id_ + suffix; })) { + ROS_WARN_STREAM_NAMED("franka_hw_sim", + "Joint '" << joint->name << "' contains a '" << transmission.type_ + << "' transmission, but it's not part of the Franka robot. " + "Ignoring this joint in FrankaHWSim"); + continue; + } + + if (urdf == nullptr) { + ROS_ERROR_STREAM_NAMED( + "franka_hw_sim", "Could not find any URDF model. Was it loaded on the parameter server?"); + return false; + } + auto urdf_joint = urdf->getJoint(joint->name); + if (not urdf_joint) { + ROS_ERROR_STREAM_NAMED("franka_hw_sim", + "Could not get joint '" << joint->name << "' from URDF"); + return false; + } + joint->type = urdf_joint->type; + joint_limits_interface::getJointLimits(urdf_joint, joint->limits); + joint->axis = Eigen::Vector3d(urdf_joint->axis.x, urdf_joint->axis.y, urdf_joint->axis.z); + + // Get a handle to the underlying Gazebo Joint + gazebo::physics::JointPtr handle = parent->GetJoint(joint->name); + if (not handle) { + ROS_ERROR_STREAM_NAMED("franka_hw_sim", "This robot has a joint named '" + << joint->name + << "' which is not in the gazebo model."); + return false; + } + joint->handle = handle; + // set the control method for finger joints to effort + if (joint->name.find(arm_id_ + "_finger_joint") != std::string::npos) { + joint->control_method = EFFORT; + } + this->joints_.emplace(joint->name, joint); + } + + // After the joint data containers have been fully initialized and their memory address don't + // change anymore, get the respective addresses to pass them to the handles + + for (auto& pair : this->joints_) { + initJointStateHandle(pair.second); + } + + // Register all supported command interfaces + for (const auto& transmission : transmissions) { + for (const auto& k_interface : transmission.joints_[0].hardware_interfaces_) { + auto name = transmission.joints_[0].name_; + if (this->joints_.count(name) == 0) { + continue; + } + auto joint = this->joints_[name]; + if (transmission.type_ == "transmission_interface/SimpleTransmission") { + ROS_INFO_STREAM_NAMED("franka_hw_sim", "Found transmission interface of joint '" + << joint->name << "': " << k_interface); + if (k_interface == "hardware_interface/EffortJointInterface") { + initEffortCommandHandle(joint); + continue; + } + if (k_interface == "hardware_interface/PositionJointInterface") { + // Initiate position motion generator (PID controller) + joint->position_controller.initParam(robot_namespace + + "/motion_generators/position/gains/" + joint->name); + initPositionCommandHandle(joint); + continue; + } + if (k_interface == "hardware_interface/VelocityJointInterface") { + // Initiate velocity motion generator (PID controller) + joint->velocity_controller.initParam(robot_namespace + + "/motion_generators/velocity/gains/" + joint->name); + initVelocityCommandHandle(joint); + continue; + } + } + + if (transmission.type_ == "franka_hw/FrankaStateInterface") { + ROS_INFO_STREAM_NAMED("franka_hw_sim", + "Found transmission interface '" << transmission.type_ << "'"); + try { + initFrankaStateHandle(this->arm_id_, *urdf, transmission); + continue; + + } catch (const std::invalid_argument& e) { + ROS_ERROR_STREAM_NAMED("franka_hw_sim", e.what()); + return false; + } + } + + if (transmission.type_ == "franka_hw/FrankaModelInterface") { + ROS_INFO_STREAM_NAMED("franka_hw_sim", + "Found transmission interface '" << transmission.type_ << "'"); + double singularity_threshold; + model_nh.param("singularity_warning_threshold", singularity_threshold, -1); + try { + initFrankaModelHandle(this->arm_id_, *urdf, transmission, singularity_threshold); + continue; + + } catch (const std::invalid_argument& e) { + ROS_ERROR_STREAM_NAMED("franka_hw_sim", e.what()); + return false; + } + } + ROS_WARN_STREAM_NAMED("franka_hw_sim", "Unsupported transmission interface of joint '" + << joint->name << "': " << k_interface); + } + } + + // After all handles have been assigned to interfaces, register them + assert(this->eji_.getNames().size() >= 7); + assert(this->pji_.getNames().size() == 7); + assert(this->vji_.getNames().size() == 7); + assert(this->jsi_.getNames().size() >= 7); + assert(this->fsi_.getNames().size() == 1); + assert(this->fmi_.getNames().size() == 1); + + registerInterface(&this->eji_); + registerInterface(&this->pji_); + registerInterface(&this->vji_); + registerInterface(&this->jsi_); + registerInterface(&this->fsi_); + registerInterface(&this->fmi_); + + // Initialize ROS Services + initServices(model_nh); + verifier_ = std::make_unique(joints_, arm_id_); + return readParameters(model_nh, *urdf); +} + +void FrankaHWSim::initJointStateHandle(const std::shared_ptr& joint) { + this->jsi_.registerHandle(hardware_interface::JointStateHandle(joint->name, &joint->position, + &joint->velocity, &joint->effort)); +} + +void FrankaHWSim::initEffortCommandHandle(const std::shared_ptr& joint) { + this->eji_.registerHandle( + hardware_interface::JointHandle(this->jsi_.getHandle(joint->name), &joint->command)); +} + +void FrankaHWSim::initPositionCommandHandle(const std::shared_ptr& joint) { + this->pji_.registerHandle( + hardware_interface::JointHandle(this->jsi_.getHandle(joint->name), &joint->desired_position)); +} + +void FrankaHWSim::initVelocityCommandHandle(const std::shared_ptr& joint) { + this->vji_.registerHandle( + hardware_interface::JointHandle(this->jsi_.getHandle(joint->name), &joint->desired_velocity)); +} + +void FrankaHWSim::initFrankaStateHandle( + const std::string& robot, + const urdf::Model& urdf, + const transmission_interface::TransmissionInfo& transmission) { + if (transmission.joints_.size() != 7) { + throw std::invalid_argument( + "Cannot create franka_hw/FrankaStateInterface for robot '" + robot + "_robot' because " + + std::to_string(transmission.joints_.size()) + + " joints were found beneath the tag, but 7 are required."); + } + + // Initialize robot_mode to "Idle". Once a controller is started, we will switch to "Move" + this->robot_state_.robot_mode = franka::RobotMode::kIdle; + + // Check if all joints defined in the actually exist in the URDF + for (const auto& joint : transmission.joints_) { + if (not urdf.getJoint(joint.name_)) { + throw std::invalid_argument("Cannot create franka_hw/FrankaStateInterface for robot '" + + robot + "_robot' because the specified joint '" + joint.name_ + + "' in the tag cannot be found in the URDF"); + } + ROS_DEBUG_STREAM_NAMED("franka_hw_sim", + "Found joint " << joint.name_ << " to belong to a Panda robot"); + } + this->fsi_.registerHandle(franka_hw::FrankaStateHandle(robot + "_robot", this->robot_state_)); +} + +void FrankaHWSim::initFrankaModelHandle( + const std::string& robot, + const urdf::Model& urdf, + const transmission_interface::TransmissionInfo& transmission, + double singularity_threshold) { + if (transmission.joints_.size() != 2) { + throw std::invalid_argument( + "Cannot create franka_hw/FrankaModelInterface for robot '" + robot + "_model' because " + + std::to_string(transmission.joints_.size()) + + " joints were found beneath the tag, but 2 are required."); + } + + for (const auto& joint : transmission.joints_) { + if (not urdf.getJoint(joint.name_)) { + if (not urdf.getJoint(joint.name_)) { + throw std::invalid_argument("Cannot create franka_hw/FrankaModelInterface for robot '" + + robot + "_model' because the specified joint '" + joint.name_ + + "' in the tag cannot be found in the URDF"); + } + } + } + auto root = + std::find_if(transmission.joints_.begin(), transmission.joints_.end(), + [&](const transmission_interface::JointInfo& i) { return i.role_ == "root"; }); + if (root == transmission.joints_.end()) { + throw std::invalid_argument("Cannot create franka_hw/FrankaModelInterface for robot '" + robot + + "_model' because no with root can be found " + "in the "); + } + auto tip = + std::find_if(transmission.joints_.begin(), transmission.joints_.end(), + [&](const transmission_interface::JointInfo& i) { return i.role_ == "tip"; }); + if (tip == transmission.joints_.end()) { + throw std::invalid_argument("Cannot create franka_hw/FrankaModelInterface for robot '" + robot + + "_model' because no with tip can be found " + "in the "); + } + try { + auto root_link = urdf.getJoint(root->name_)->parent_link_name; + auto tip_link = urdf.getJoint(tip->name_)->child_link_name; + + this->model_ = + std::make_unique(urdf, root_link, tip_link, singularity_threshold); + + } catch (const std::invalid_argument& e) { + throw std::invalid_argument("Cannot create franka_hw/FrankaModelInterface for robot '" + robot + + "_model'. " + e.what()); + } + this->fmi_.registerHandle( + franka_hw::FrankaModelHandle(robot + "_model", *this->model_, this->robot_state_)); +} + +void FrankaHWSim::initServices(ros::NodeHandle& nh) { + this->service_set_ee_ = + nh.advertiseService( + "franka_control/set_EE_frame", [&](auto& request, auto& response) { + ROS_INFO_STREAM_NAMED("franka_hw_sim", + this->arm_id_ << ": Setting NE_T_EE transformation"); + std::copy(request.NE_T_EE.cbegin(), request.NE_T_EE.cend(), + this->robot_state_.NE_T_EE.begin()); + this->updateRobotStateDynamics(); + response.success = true; + return true; + }); + this->service_set_k_ = franka_hw::advertiseService( + nh, "franka_control/set_K_frame", [&](auto& request, auto& response) { + ROS_INFO_STREAM_NAMED("franka_hw_sim", this->arm_id_ << ": Setting EE_T_K transformation"); + std::copy(request.EE_T_K.cbegin(), request.EE_T_K.cend(), + this->robot_state_.EE_T_K.begin()); + this->updateRobotStateDynamics(); + response.success = true; + return true; + }); + this->service_set_load_ = franka_hw::advertiseService( + nh, "franka_control/set_load", [&](auto& request, auto& response) { + ROS_INFO_STREAM_NAMED("franka_hw_sim", this->arm_id_ << ": Setting Load"); + this->robot_state_.m_load = request.mass; + std::copy(request.F_x_center_load.cbegin(), request.F_x_center_load.cend(), + this->robot_state_.F_x_Cload.begin()); + std::copy(request.load_inertia.cbegin(), request.load_inertia.cend(), + this->robot_state_.I_load.begin()); + this->updateRobotStateDynamics(); + response.success = true; + return true; + }); + this->service_collision_behavior_ = + franka_hw::advertiseService( + nh, "franka_control/set_force_torque_collision_behavior", + [&](auto& request, auto& response) { + ROS_INFO_STREAM_NAMED("franka_hw_sim", this->arm_id_ << ": Setting Collision Behavior"); + + for (int i = 0; i < 7; i++) { + std::string name = this->arm_id_ + "_joint" + std::to_string(i + 1); + this->joints_[name]->contact_threshold = + request.lower_torque_thresholds_nominal.at(i); + this->joints_[name]->collision_threshold = + request.upper_torque_thresholds_nominal.at(i); + } + + std::move(request.lower_force_thresholds_nominal.begin(), + request.lower_force_thresholds_nominal.end(), + this->lower_force_thresholds_nominal_.begin()); + std::move(request.upper_force_thresholds_nominal.begin(), + request.upper_force_thresholds_nominal.end(), + this->upper_force_thresholds_nominal_.begin()); + + response.success = true; + return true; + }); + this->service_user_stop_ = + nh.advertiseService( + "franka_control/set_user_stop", [&](auto& request, auto& response) { + this->sm_.process_event(UserStop{static_cast(request.data)}); + response.success = true; + return true; + }); + this->service_set_model_configuration_ = franka_hw::advertiseService< + franka_msgs::SetJointConfiguration>( + nh, "set_franka_model_configuration", [&](auto& request, auto& response) { + // Check if joints are specified + if (request.configuration.name.empty()) { + ROS_ERROR_STREAM_NAMED("franka_hw_sim", + "Failed to set Franka model configuration: the request is " + "invalid because no joints were specified."); + response.success = false; + response.error = "no joints specified"; + return false; + } + + // Check if 'position' and 'name' fields have different lengths + if (request.configuration.name.size() != request.configuration.position.size()) { + ROS_ERROR_STREAM_NAMED( + "franka_hw_sim", + "Failed to set Franka model configuration: the request is invalid because the " + "'position' and 'name' fields have different lengths."); + response.success = false; + response.error = "'position' and 'name' fields length unequal"; + return false; + } + + // Log request information + std::ostringstream requested_configuration_stream; + for (std::size_t ii = 0; ii < request.configuration.name.size(); ii++) { + requested_configuration_stream << request.configuration.name[ii] << ": " + << request.configuration.position[ii] << ", "; + } + std::string requested_configuration_string = requested_configuration_stream.str(); + requested_configuration_string = requested_configuration_string.substr( + 0, requested_configuration_string.length() - 2); // Remove last 2 characters + ROS_INFO_STREAM_NAMED("franka_hw_sim", + "Setting joint configuration: " << requested_configuration_string); + + // Validate joint names and joint limits + std::map model_configuration; + for (std::size_t ii = 0; ii < request.configuration.name.size(); ii++) { + std::string joint(request.configuration.name[ii]); + double position(request.configuration.position[ii]); + + auto joint_iterator = this->joints_.find(joint); + if (joint_iterator != this->joints_.end()) { // If joint exists + double min_position(joint_iterator->second->limits.min_position); + double max_position(joint_iterator->second->limits.max_position); + + // Check if position is within joint limits + if (position == boost::algorithm::clamp(position, min_position, max_position)) { + model_configuration.emplace(std::make_pair(joint, position)); + } else { + ROS_WARN_STREAM_NAMED("franka_hw_sim", + "Joint configuration of joint '" + << joint + << "' was not set since the requested joint position (" + << position << ") is not within joint limits (i.e. " + << min_position << " - " << max_position << ")."); + } + } else { + ROS_WARN_STREAM_NAMED("franka_hw_sim", + "Joint configuration of joint '" + << joint << "' not set since it is not a valid panda joint."); + } + } + + // Return if no valid positions were found + if (model_configuration.empty()) { + ROS_ERROR_STREAM_NAMED("franka_hw_sim", + "Setting of Franka model configuration aborted since no valid " + "joint configuration were found."); + response.success = false; + response.error = "no valid joint configurations"; + return false; + } + + // Throw warnings about unused request fields + if (!request.configuration.effort.empty()) { + ROS_WARN_STREAM_ONCE_NAMED( + "franka_hw_sim", + "The 'set_franka_model_configuration' service does not use the 'effort' field."); + } + if (!request.configuration.velocity.empty()) { + ROS_WARN_STREAM_ONCE_NAMED( + "franka_hw_sim", + "The 'set_franka_model_configuration' service does not use the 'velocity' field."); + } + + // Call Gazebo 'set_model_configuration' service and update Franka joint positions + gazebo_msgs::SetModelConfiguration gazebo_model_configuration; + gazebo_model_configuration.request.model_name = "panda"; + for (const auto& pair : model_configuration) { + gazebo_model_configuration.request.joint_names.push_back(pair.first); + gazebo_model_configuration.request.joint_positions.push_back(pair.second); + } + if (this->service_gazebo_set_model_configuration_.call(gazebo_model_configuration)) { + // Update franka positions + for (const auto& pair : model_configuration) { + this->joints_[pair.first]->setJointPosition(pair.second); + } + + response.success = true; + return true; + } else { + ROS_WARN_STREAM_NAMED( + "franka_hw_sim", + "Setting of Franka model configuration failed since " + "a problem occurred when setting the joint configuration in Gazebo."); + response.success = false; + return false; + } + }); + this->service_controller_list_ = nh.serviceClient( + "controller_manager/list_controllers"); + this->service_controller_switch_ = nh.serviceClient( + "controller_manager/switch_controller"); + this->service_gazebo_set_model_configuration_ = + nh.serviceClient("/gazebo/set_model_configuration"); +} + +void FrankaHWSim::restartControllers() { + // Restart controllers by stopping and starting all running ones + auto name = this->service_controller_list_.getService(); + if (not this->service_controller_list_.waitForExistence(ros::Duration(3))) { + throw std::runtime_error("Cannot find service '" + name + + "'. Is the controller_manager running?"); + } + + controller_manager_msgs::ListControllers list; + if (not this->service_controller_list_.call(list)) { + throw std::runtime_error("Service call '" + name + "' failed"); + } + + controller_manager_msgs::SwitchController swtch; + for (const auto& controller : list.response.controller) { + if (controller.state != "running") { + continue; + } + swtch.request.stop_controllers.push_back(controller.name); + swtch.request.start_controllers.push_back(controller.name); + } + swtch.request.start_asap = static_cast(true); + swtch.request.strictness = controller_manager_msgs::SwitchControllerRequest::STRICT; + if (not this->service_controller_switch_.call(swtch) or + not static_cast(swtch.response.ok)) { + throw std::runtime_error("Service call '" + this->service_controller_switch_.getService() + + "' failed"); + } +} + +void FrankaHWSim::readSim(ros::Time time, ros::Duration period) { + for (const auto& pair : this->joints_) { + auto joint = pair.second; + joint->update(period); + } + this->updateRobotState(time); +} + +double FrankaHWSim::positionControl(Joint& joint, double setpoint, const ros::Duration& period) { + double error; + const double kJointLowerLimit = joint.limits.min_position; + const double kJointUpperLimit = joint.limits.max_position; + switch (joint.type) { + case urdf::Joint::REVOLUTE: + angles::shortest_angular_distance_with_limits(joint.position, setpoint, kJointLowerLimit, + kJointUpperLimit, error); + break; + case urdf::Joint::PRISMATIC: + error = + boost::algorithm::clamp(setpoint - joint.position, kJointLowerLimit, kJointUpperLimit); + break; + default: + std::string error_message = + "Only revolute or prismatic joints are allowed for position control right now"; + ROS_FATAL("%s", error_message.c_str()); + throw std::invalid_argument(error_message); + } + + return boost::algorithm::clamp(joint.position_controller.computeCommand(error, period), + -joint.limits.max_effort, joint.limits.max_effort); +} + +double FrankaHWSim::velocityControl(Joint& joint, double setpoint, const ros::Duration& period) { + return boost::algorithm::clamp( + joint.velocity_controller.computeCommand(setpoint - joint.velocity, period), + -joint.limits.max_effort, joint.limits.max_effort); +} + +void FrankaHWSim::writeSim(ros::Time /*time*/, ros::Duration period) { + auto g = this->model_->gravity(this->robot_state_, this->gravity_earth_); + + for (auto& pair : this->joints_) { + auto joint = pair.second; + + // Retrieve effort control command + double effort = 0; + + // Finger joints must still be controllable from franka_gripper_sim controller + if (not sm_.is(state) and not contains(pair.first, "finger_joint")) { + effort = positionControl(*joint, joint->stop_position, period); + } else if (joint->control_method == POSITION) { + effort = positionControl(*joint, joint->desired_position, period); + } else if (joint->control_method == VELOCITY) { + effort = velocityControl(*joint, joint->desired_velocity, period); + } else if (joint->control_method == EFFORT) { + // Feed-forward commands in effort control + effort = joint->command; + } + + // Check if this joint is affected by gravity compensation + std::string prefix = this->arm_id_ + "_joint"; + if (pair.first.rfind(prefix, 0) != std::string::npos) { + int i = std::stoi(pair.first.substr(prefix.size())) - 1; + joint->gravity = g.at(i); + } + effort += joint->gravity; + + // Send control effort control command + if (not std::isfinite(effort)) { + ROS_WARN_STREAM_NAMED("franka_hw_sim", + "Command for " << joint->name << "is not finite, won't send to robot"); + continue; + } + joint->handle->SetForce(0, effort); + } +} + +void FrankaHWSim::eStopActive(bool /* active */) {} + +bool FrankaHWSim::readParameters(const ros::NodeHandle& nh, const urdf::Model& urdf) { + try { + guessEndEffector(nh, urdf); + + nh.param("m_load", this->robot_state_.m_load, 0); + + std::string I_load; // NOLINT [readability-identifier-naming] + nh.param("I_load", I_load, "0 0 0 0 0 0 0 0 0"); + this->robot_state_.I_load = readArray<9>(I_load, "I_load"); + + std::string F_x_Cload; // NOLINT [readability-identifier-naming] + nh.param("F_x_Cload", F_x_Cload, "0 0 0"); + this->robot_state_.F_x_Cload = readArray<3>(F_x_Cload, "F_x_Cload"); + + std::string NE_T_EE; // NOLINT [readability-identifier-naming] + nh.param("NE_T_EE", NE_T_EE, "1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1"); + this->robot_state_.NE_T_EE = readArray<16>(NE_T_EE, "NE_T_EE"); + + std::string EE_T_K; // NOLINT [readability-identifier-naming] + nh.param("EE_T_K", EE_T_K, "1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1"); + this->robot_state_.EE_T_K = readArray<16>(EE_T_K, "EE_T_K"); + + std::string gravity_vector; + if (nh.getParam("gravity_vector", gravity_vector)) { + this->gravity_earth_ = readArray<3>(gravity_vector, "gravity_vector"); + } + + // Only nominal cases supported for now + std::vector lower_torque_thresholds = franka_hw::FrankaHW::getCollisionThresholds( + "lower_torque_thresholds_nominal", nh, {20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}); + + std::vector upper_torque_thresholds = franka_hw::FrankaHW::getCollisionThresholds( + "upper_torque_thresholds_nominal", nh, {20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}); + + this->lower_force_thresholds_nominal_ = franka_hw::FrankaHW::getCollisionThresholds( + "lower_force_thresholds_nominal", nh, {20.0, 20.0, 20.0, 25.0, 25.0, 25.0}); + this->upper_force_thresholds_nominal_ = franka_hw::FrankaHW::getCollisionThresholds( + "upper_force_thresholds_nominal", nh, {20.0, 20.0, 20.0, 25.0, 25.0, 25.0}); + + for (int i = 0; i < 7; i++) { + std::string name = this->arm_id_ + "_joint" + std::to_string(i + 1); + this->joints_[name]->contact_threshold = lower_torque_thresholds.at(i); + this->joints_[name]->collision_threshold = upper_torque_thresholds.at(i); + } + + } catch (const std::invalid_argument& e) { + ROS_ERROR_STREAM_NAMED("franka_hw_sim", e.what()); + return false; + } + updateRobotStateDynamics(); + return true; +} + +void FrankaHWSim::guessEndEffector(const ros::NodeHandle& nh, const urdf::Model& urdf) { + auto hand_link = this->arm_id_ + "_hand"; + auto hand = urdf.getLink(hand_link); + if (hand != nullptr) { + ROS_INFO_STREAM_NAMED("franka_hw_sim", + "Found link '" << hand_link + << "' in URDF. Assuming it is defining the kinematics & " + "inertias of a Franka Hand Gripper."); + } + + // By absolute default unless URDF or ROS params say otherwise, assume no end-effector. + double def_m_ee = 0; + std::string def_i_ee = "0.0 0 0 0 0.0 0 0 0 0.0"; + std::string def_f_x_cee = "0 0 0"; + std::string def_f_t_ne = "1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1"; + if (not nh.hasParam("F_T_NE") and hand != nullptr) { + // NOTE: We cannot interprete the Joint pose from the URDF directly, because + // its _link is mounted at the flange directly and not at NE + def_f_t_ne = "0.7071 -0.7071 0 0 0.7071 0.7071 0 0 0 0 1 0 0 0 0.1034 1"; + } + std::string F_T_NE; // NOLINT [readability-identifier-naming] + nh.param("F_T_NE", F_T_NE, def_f_t_ne); + this->robot_state_.F_T_NE = readArray<16>(F_T_NE, "F_T_NE"); + + if (not nh.hasParam("m_ee") and hand != nullptr) { + if (hand->inertial == nullptr) { + throw std::invalid_argument("Trying to use inertia of " + hand_link + + " but this link has no tag defined in it."); + } + def_m_ee = hand->inertial->mass; + } + nh.param("m_ee", this->robot_state_.m_ee, def_m_ee); + + if (not nh.hasParam("I_ee") and hand != nullptr) { + if (hand->inertial == nullptr) { + throw std::invalid_argument("Trying to use inertia of " + hand_link + + " but this link has no tag defined in it."); + } + // clang-format off + def_i_ee = std::to_string(hand->inertial->ixx) + " " + std::to_string(hand->inertial->ixy) + " " + std::to_string(hand->inertial->ixz) + " " + + std::to_string(hand->inertial->ixy) + " " + std::to_string(hand->inertial->iyy) + " " + std::to_string(hand->inertial->iyz) + " " + + std::to_string(hand->inertial->ixz) + " " + std::to_string(hand->inertial->iyz) + " " + std::to_string(hand->inertial->izz); + // clang-format on + } + std::string I_ee; // NOLINT [readability-identifier-naming] + nh.param("I_ee", I_ee, def_i_ee); + this->robot_state_.I_ee = readArray<9>(I_ee, "I_ee"); + + if (not nh.hasParam("F_x_Cee") and hand != nullptr) { + if (hand->inertial == nullptr) { + throw std::invalid_argument("Trying to use inertia of " + hand_link + + " but this link has no tag defined in it."); + } + def_f_x_cee = std::to_string(hand->inertial->origin.position.x) + " " + + std::to_string(hand->inertial->origin.position.y) + " " + + std::to_string(hand->inertial->origin.position.z); + } + std::string F_x_Cee; // NOLINT [readability-identifier-naming] + nh.param("F_x_Cee", F_x_Cee, def_f_x_cee); + this->robot_state_.F_x_Cee = readArray<3>(F_x_Cee, "F_x_Cee"); +} + +void FrankaHWSim::updateRobotStateDynamics() { + this->robot_state_.m_total = this->robot_state_.m_ee + this->robot_state_.m_load; + + Eigen::Map(this->robot_state_.F_T_EE.data()) = + Eigen::Matrix4d(this->robot_state_.F_T_NE.data()) * + Eigen::Matrix4d(this->robot_state_.NE_T_EE.data()); + + Eigen::Map(this->robot_state_.I_total.data()) = + shiftInertiaTensor(Eigen::Matrix3d(this->robot_state_.I_ee.data()), this->robot_state_.m_ee, + Eigen::Vector3d(this->robot_state_.F_x_Cload.data())); +} + +void FrankaHWSim::updateRobotState(ros::Time time) { + // This is ensured, because a FrankaStateInterface checks for at least seven joints in the URDF + assert(this->joints_.size() >= 7); + + auto mode = this->robot_state_.robot_mode; + for (int i = 0; i < 7; i++) { + std::string name = this->arm_id_ + "_joint" + std::to_string(i + 1); + const auto& joint = this->joints_.at(name); + this->robot_state_.q[i] = joint->position; + this->robot_state_.dq[i] = joint->velocity; + this->robot_state_.tau_J[i] = joint->effort; + this->robot_state_.dtau_J[i] = joint->jerk; + + this->robot_state_.q_d[i] = joint->getDesiredPosition(mode); + this->robot_state_.dq_d[i] = joint->getDesiredVelocity(mode); + this->robot_state_.ddq_d[i] = joint->getDesiredAcceleration(mode); + this->robot_state_.tau_J_d[i] = joint->getDesiredTorque(mode); + + // For now we assume no flexible joints + this->robot_state_.theta[i] = joint->position; + this->robot_state_.dtheta[i] = joint->velocity; + + // first time initialization of the desired position + if (not this->robot_initialized_) { + joint->desired_position = joint->position; + joint->stop_position = joint->position; + } + + if (this->robot_initialized_) { + double tau_ext = joint->effort - joint->command + joint->gravity; + + // Exponential moving average filter from tau_ext -> tau_ext_hat_filtered + this->robot_state_.tau_ext_hat_filtered[i] = + this->tau_ext_lowpass_filter_ * tau_ext + + (1 - this->tau_ext_lowpass_filter_) * this->robot_state_.tau_ext_hat_filtered[i]; + } + + this->robot_state_.joint_contact[i] = static_cast(joint->isInContact()); + this->robot_state_.joint_collision[i] = static_cast(joint->isInCollision()); + } + + // Calculate estimated wrenches in Task frame from external joint torques with jacobians + Eigen::Map> tau_ext(this->robot_state_.tau_ext_hat_filtered.data()); + Eigen::MatrixXd j0_transpose_pinv; + Eigen::MatrixXd jk_transpose_pinv; + Eigen::Matrix j0( + this->model_->zeroJacobian(franka::Frame::kStiffness, this->robot_state_).data()); + Eigen::Matrix jk( + this->model_->bodyJacobian(franka::Frame::kStiffness, this->robot_state_).data()); + franka_example_controllers::pseudoInverse(j0.transpose(), j0_transpose_pinv); + franka_example_controllers::pseudoInverse(jk.transpose(), jk_transpose_pinv); + + Eigen::VectorXd f_ext_0 = j0_transpose_pinv * tau_ext; + Eigen::VectorXd f_ext_k = jk_transpose_pinv * tau_ext; + Eigen::VectorXd::Map(&this->robot_state_.O_F_ext_hat_K[0], 6) = f_ext_0; + Eigen::VectorXd::Map(&this->robot_state_.K_F_ext_hat_K[0], 6) = f_ext_k; + + for (int i = 0; i < this->robot_state_.cartesian_contact.size(); i++) { + // Evaluate the cartesian contacts/collisions in K frame + double fi = std::abs(f_ext_k(i)); + this->robot_state_.cartesian_contact[i] = + static_cast(fi > this->lower_force_thresholds_nominal_.at(i)); + this->robot_state_.cartesian_collision[i] = + static_cast(fi > this->upper_force_thresholds_nominal_.at(i)); + } + + this->robot_state_.control_command_success_rate = 1.0; + this->robot_state_.time = franka::Duration(time.toNSec() / 1e6 /*ms*/); + this->robot_state_.O_T_EE = this->model_->pose(franka::Frame::kEndEffector, this->robot_state_); +#ifdef ENABLE_BASE_ACCELERATION + // This will always be {0,0,-9.81} on the real robot as it cannot be mounted differently for now + this->robot_state_.O_ddP_O = this->gravity_earth_; +#endif + + std_msgs::Bool msg; + msg.data = static_cast(true); + this->robot_initialized_ = true; + this->robot_initialized_pub_.publish(msg); +} + +bool FrankaHWSim::prepareSwitch( + const std::list& start_list, + const std::list& /*stop_list*/) { + return std::all_of(start_list.cbegin(), start_list.cend(), [this](const auto& controller) { + return verifier_->isValidController(controller); + }); +} + +void FrankaHWSim::doSwitch(const std::list& start_list, + const std::list& stop_list) { + forControlledJoint(stop_list, [](franka_gazebo::Joint& joint, const ControlMethod& /*method*/) { + joint.control_method = boost::none; + joint.stop_position = joint.position; + joint.desired_position = joint.position; + joint.desired_velocity = 0; + }); + forControlledJoint(start_list, [](franka_gazebo::Joint& joint, const ControlMethod& method) { + joint.control_method = method; + // sets the desired joint position once for the effort interface + joint.desired_position = joint.position; + joint.desired_velocity = 0; + }); + + this->sm_.process_event(SwitchControl()); +} + +void FrankaHWSim::forControlledJoint( + const std::list& controllers, + const std::function& f) { + for (const auto& controller : controllers) { + if (not verifier_->isClaimingArmController(controller)) { + continue; + } + for (const auto& resource : controller.claimed_resources) { + auto control_method = ControllerVerifier::determineControlMethod(resource.hardware_interface); + if (not control_method) { + continue; + } + for (const auto& joint_name : resource.resources) { + auto& joint = joints_.at(joint_name); + f(*joint, control_method.value()); + } + } + } +} + +} // namespace franka_gazebo + +PLUGINLIB_EXPORT_CLASS(franka_gazebo::FrankaHWSim, gazebo_ros_control::RobotHWSim) diff --git a/franka_gazebo/src/franka_hw_sim.cpp b/franka_gazebo/src/franka_hw_sim.cpp index fb80907b..bb11a069 100644 --- a/franka_gazebo/src/franka_hw_sim.cpp +++ b/franka_gazebo/src/franka_hw_sim.cpp @@ -9,8 +9,10 @@ #include #include #include +#include #include #include +#include #include #include #include @@ -22,6 +24,7 @@ #include #include #include +#include "ros/ros.h" namespace franka_gazebo { @@ -408,10 +411,122 @@ void FrankaHWSim::initServices(ros::NodeHandle& nh) { response.success = true; return true; }); + this->service_set_model_configuration_ = franka_hw::advertiseService< + franka_msgs::SetJointConfiguration>( + nh, "set_franka_model_configuration", [&](auto& request, auto& response) { + // Check if joints are specified + if (request.configuration.name.size() == 1 && request.configuration.name[0].empty()) { + ROS_ERROR_STREAM_NAMED("franka_hw_sim", + "Failed to set Franka model configuration: the request is " + "invalid because no joints were specified."); + response.success = false; + response.error = "no joints specified"; + return false; + } + + // Check if 'position' and 'name' fields have different lengths + if (request.configuration.name.size() != request.configuration.position.size()) { + ROS_ERROR_STREAM_NAMED( + "franka_hw_sim", + "Failed to set Franka model configuration: the request is invalid because the " + "'position' and 'name' fields have different lengths."); + response.success = false; + response.error = "'position' and 'name' fields length unequal"; + return false; + } + + // Log request information + std::ostringstream requested_configuration_stream; + for (std::size_t ii = 0; ii < request.configuration.name.size(); ii++) { + requested_configuration_stream << request.configuration.name[ii] << ": " + << request.configuration.position[ii] << ", "; + } + std::string requested_configuration_string = requested_configuration_stream.str(); + requested_configuration_string = requested_configuration_string.substr( + 0, requested_configuration_string.length() - 2); // Remove last 2 characters + ROS_INFO_STREAM_NAMED("franka_hw_sim", + "Setting joint configuration: " << requested_configuration_string); + + // Validate joint names and joint limits + std::unordered_map model_configuration; + for (std::size_t ii = 0; ii < request.configuration.name.size(); ii++) { + std::string joint(request.configuration.name[ii]); + double position(request.configuration.position[ii]); + + auto joint_iterator = this->joints_.find(joint); + if (joint_iterator != this->joints_.end()) { // If joint exists + double min_position(joint_iterator->second->limits.min_position); + double max_position(joint_iterator->second->limits.max_position); + + // Check if position is within joint limits + if (position == boost::algorithm::clamp(position, min_position, max_position)) { + model_configuration.emplace(std::make_pair(joint, position)); + } else { + ROS_WARN_STREAM_NAMED("franka_hw_sim", + "Joint configuration of joint '" + << joint + << "' was not set since the requested joint position (" + << position << ") is not within joint limits (i.e. " + << min_position << " - " << max_position << ")."); + } + } else { + ROS_WARN_STREAM_NAMED("franka_hw_sim", + "Joint configuration of joint '" + << joint << "' not set since it is not a valid panda joint."); + } + } + + // Return if no valid positions were found + if (model_configuration.empty()) { + ROS_ERROR_STREAM_NAMED("franka_hw_sim", + "Setting of Franka model configuration aborted since no valid " + "joint configuration were found."); + response.success = false; + response.error = "no valid joint configurations"; + return false; + } + + // Throw warnings about unused request fields + if (!request.configuration.effort.empty()) { + ROS_WARN_STREAM_ONCE_NAMED( + "franka_hw_sim", + "The 'set_franka_model_configuration' service does not use the 'effort' field."); + } + if (!request.configuration.velocity.empty()) { + ROS_WARN_STREAM_ONCE_NAMED( + "franka_hw_sim", + "The 'set_franka_model_configuration' service does not use the 'velocity' field."); + } + + // Call Gazebo 'set_model_configuration' service and update Franka joint positions + gazebo_msgs::SetModelConfiguration gazebo_model_configuration; + gazebo_model_configuration.request.model_name = "panda"; + for (const auto& pair : model_configuration) { + gazebo_model_configuration.request.joint_names.push_back(pair.first); + gazebo_model_configuration.request.joint_positions.push_back(pair.second); + } + if (this->service_gazebo_set_model_configuration_.call(gazebo_model_configuration)) { + // Update franka positions + for (const auto& pair : model_configuration) { + this->joints_[pair.first]->setJointPosition(pair.second); + } + + response.success = true; + return true; + } + + ROS_WARN_STREAM_NAMED("franka_hw_sim", + "Setting of Franka model configuration failed since " + "a problem occurred when setting the joint configuration in Gazebo."); + response.success = false; + return false; + }); this->service_controller_list_ = nh.serviceClient( "controller_manager/list_controllers"); this->service_controller_switch_ = nh.serviceClient( "controller_manager/switch_controller"); + this->service_gazebo_set_model_configuration_ = + nh.serviceClient("/gazebo/set_model_configuration"); } void FrankaHWSim::restartControllers() { diff --git a/franka_gazebo/src/joint.cpp b/franka_gazebo/src/joint.cpp index 186f1192..c12b3fe9 100644 --- a/franka_gazebo/src/joint.cpp +++ b/franka_gazebo/src/joint.cpp @@ -9,6 +9,13 @@ void Joint::update(const ros::Duration& dt) { return; } + // Apply 'setJointPosition' position request. + if (this->setPositionRequested_) { + std::lock_guard lock(this->requestedPositionMutex_); + this->position = this->requestedPosition_; + this->setPositionRequested_ = false; + } + this->velocity = this->handle->GetVelocity(0); #if GAZEBO_MAJOR_VERSION >= 8 double position = this->handle->Position(0); @@ -111,4 +118,12 @@ bool Joint::isInCollision() const { bool Joint::isInContact() const { return std::abs(this->effort - this->command) > this->contact_threshold; } + +void Joint::setJointPosition(const double joint_position) { + // NOTE: Joint position is set in update() method to prevent racing conditions. + std::lock_guard lock(this->requestedPositionMutex_); + this->requestedPosition_ = joint_position; + this->setPositionRequested_ = true; +} + } // namespace franka_gazebo diff --git a/franka_msgs/CMakeLists.txt b/franka_msgs/CMakeLists.txt index 1f6f9265..94b42334 100644 --- a/franka_msgs/CMakeLists.txt +++ b/franka_msgs/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 3.4) project(franka_msgs) -find_package(catkin REQUIRED COMPONENTS message_generation std_msgs actionlib_msgs) +find_package(catkin REQUIRED COMPONENTS message_generation std_msgs sensor_msgs actionlib_msgs) add_message_files(FILES Errors.msg FrankaState.msg) @@ -10,6 +10,7 @@ add_service_files(FILES SetEEFrame.srv SetForceTorqueCollisionBehavior.srv SetFullCollisionBehavior.srv + SetJointConfiguration.srv SetJointImpedance.srv SetKFrame.srv SetLoad.srv @@ -17,6 +18,6 @@ add_service_files(FILES add_action_files(FILES ErrorRecovery.action) -generate_messages(DEPENDENCIES std_msgs actionlib_msgs) +generate_messages(DEPENDENCIES std_msgs sensor_msgs actionlib_msgs) -catkin_package(CATKIN_DEPENDS message_runtime std_msgs actionlib_msgs) +catkin_package(CATKIN_DEPENDS message_runtime std_msgs sensor_msgs actionlib_msgs) diff --git a/franka_msgs/package.xml b/franka_msgs/package.xml index 04fc8207..41792d62 100644 --- a/franka_msgs/package.xml +++ b/franka_msgs/package.xml @@ -16,6 +16,7 @@ message_generation std_msgs + sensor_msgs actionlib_msgs message_runtime diff --git a/franka_msgs/srv/SetJointConfiguration.srv b/franka_msgs/srv/SetJointConfiguration.srv new file mode 100644 index 00000000..f33b2bb6 --- /dev/null +++ b/franka_msgs/srv/SetJointConfiguration.srv @@ -0,0 +1,4 @@ +sensor_msgs/JointState configuration +--- +bool success +string error