diff --git a/franka_gazebo/config/franka_hw_sim.yaml b/franka_gazebo/config/franka_hw_sim.yaml index 6703a168..f6e17f38 100644 --- a/franka_gazebo/config/franka_hw_sim.yaml +++ b/franka_gazebo/config/franka_hw_sim.yaml @@ -11,3 +11,26 @@ franka_gripper: finger2: gains: { p: 100, i: 0, d: 1.0 } + +# Motion generators PID gains +# NOTE: Needed since the gazebo_ros_control currently only directly support effort hardware +# interfaces (see #161). +motion_generators: + position: + gains: + panda_joint1: { p: 600, d: 30, i: 0 } + panda_joint2: { p: 600, d: 30, i: 0 } + panda_joint3: { p: 600, d: 30, i: 0 } + panda_joint4: { p: 600, d: 30, i: 0 } + panda_joint5: { p: 250, d: 10, i: 0 } + panda_joint6: { p: 150, d: 10, i: 0 } + panda_joint7: { p: 50, d: 5, i: 0 } + velocity: + gains: + panda_joint1: { p: 600, d: 30, i: 0 } + panda_joint2: { p: 600, d: 30, i: 0 } + panda_joint3: { p: 600, d: 30, i: 0 } + panda_joint4: { p: 600, d: 30, i: 0 } + panda_joint5: { p: 250, d: 10, i: 0 } + panda_joint6: { p: 150, d: 10, i: 0 } + panda_joint7: { p: 50, d: 5, i: 0 } diff --git a/franka_gazebo/config/sim_controllers.yaml b/franka_gazebo/config/sim_controllers.yaml index cb5d2fdd..ad1c0240 100644 --- a/franka_gazebo/config/sim_controllers.yaml +++ b/franka_gazebo/config/sim_controllers.yaml @@ -66,3 +66,23 @@ effort_joint_trajectory_controller: $(arg arm_id)_joint5: { goal: 0.05 } $(arg arm_id)_joint6: { goal: 0.05 } $(arg arm_id)_joint7: { goal: 0.05 } + +position_joint_trajectory_controller: + type: position_controllers/JointTrajectoryController + joints: + - $(arg arm_id)_joint1 + - $(arg arm_id)_joint2 + - $(arg arm_id)_joint3 + - $(arg arm_id)_joint4 + - $(arg arm_id)_joint5 + - $(arg arm_id)_joint6 + - $(arg arm_id)_joint7 + constraints: + goal_time: 0.5 + $(arg arm_id)_joint1: { goal: 0.05} + $(arg arm_id)_joint2: { goal: 0.05} + $(arg arm_id)_joint3: { goal: 0.05} + $(arg arm_id)_joint4: { goal: 0.05} + $(arg arm_id)_joint5: { goal: 0.05} + $(arg arm_id)_joint6: { goal: 0.05} + $(arg arm_id)_joint7: { goal: 0.05} diff --git a/franka_gazebo/include/franka_gazebo/franka_hw_sim.h b/franka_gazebo/include/franka_gazebo/franka_hw_sim.h index 2cbd1fdd..bb040aa5 100644 --- a/franka_gazebo/include/franka_gazebo/franka_hw_sim.h +++ b/franka_gazebo/include/franka_gazebo/franka_hw_sim.h @@ -1,5 +1,6 @@ #pragma once +#include #include #include #include @@ -31,6 +32,8 @@ const double kDefaultTauExtLowpassFilter = 1.0; // no filtering per default of * ### transmission_interface/SimpleTransmission * - hardware_interface/JointStateInterface * - hardware_interface/EffortJointInterface + * - hardware_interface/PositionJointInterface + * - hardware_interface/VelocityJointInterface * * ### franka_hw/FrankaStateInterface * ### franka_hw/FrankaModelInterface @@ -101,8 +104,14 @@ class FrankaHWSim : public gazebo_ros_control::RobotHWSim { gazebo::physics::ModelPtr robot_; std::map> joints_; + enum ControlMethod { EFFORT, POSITION, VELOCITY }; + std::map joint_control_methods_; + std::map pid_controllers_; + hardware_interface::JointStateInterface jsi_; hardware_interface::EffortJointInterface eji_; + hardware_interface::PositionJointInterface pji_; + hardware_interface::VelocityJointInterface vji_; franka_hw::FrankaStateInterface fsi_; franka_hw::FrankaModelInterface fmi_; @@ -121,6 +130,8 @@ class FrankaHWSim : public gazebo_ros_control::RobotHWSim { void initJointStateHandle(const std::shared_ptr& joint); void initEffortCommandHandle(const std::shared_ptr& joint); + void initPositionCommandHandle(const std::shared_ptr& joint); + void initVelocityCommandHandle(const std::shared_ptr& joint); void initFrankaStateHandle(const std::string& robot, const urdf::Model& urdf, const transmission_interface::TransmissionInfo& transmission); diff --git a/franka_gazebo/include/franka_gazebo/joint.h b/franka_gazebo/include/franka_gazebo/joint.h index be8afcf3..bc425b57 100644 --- a/franka_gazebo/include/franka_gazebo/joint.h +++ b/franka_gazebo/include/franka_gazebo/joint.h @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include #include @@ -35,6 +36,10 @@ struct Joint { /// http://docs.ros.org/en/diamondback/api/urdf/html/classurdf_1_1Joint.html int type; + /// Joint limits @see + /// https://docs.ros.org/en/diamondback/api/urdf/html/classurdf_1_1JointLimits.html + joint_limits_interface::JointLimits limits; + /// The axis of rotation/translation of this joint in local coordinates Eigen::Vector3d axis; diff --git a/franka_gazebo/src/franka_hw_sim.cpp b/franka_gazebo/src/franka_hw_sim.cpp index d34d344f..ae9154fc 100644 --- a/franka_gazebo/src/franka_hw_sim.cpp +++ b/franka_gazebo/src/franka_hw_sim.cpp @@ -10,7 +10,9 @@ #include #include #include +#include #include +#include #include #include #include @@ -85,6 +87,7 @@ bool FrankaHWSim::initSim(const std::string& robot_namespace, 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 @@ -114,9 +117,37 @@ bool FrankaHWSim::initSim(const std::string& robot_namespace, ROS_INFO_STREAM_NAMED("franka_hw_sim", "Found transmission interface of joint '" << joint->name << "': " << k_interface); if (k_interface == "hardware_interface/EffortJointInterface") { + this->joint_control_methods_.emplace(joint->name, EFFORT); + initEffortCommandHandle(joint); continue; } + if (k_interface == "hardware_interface/PositionJointInterface") { + this->joint_control_methods_.emplace(joint->name, POSITION); + + // Initiate position motion generator (PID controller) + const ros::NodeHandle kPosPidGainsNh(robot_namespace + + "/motion_generators/position/gains/" + joint->name); + control_toolbox::Pid pid; + this->pid_controllers_.emplace(joint->name, pid); + this->pid_controllers_[joint->name].init(kPosPidGainsNh); + + initPositionCommandHandle(joint); + continue; + } + if (k_interface == "hardware_interface/VelocityJointInterface") { + this->joint_control_methods_.emplace(joint->name, VELOCITY); + + // Initiate velocity motion generator (PID controller) + const ros::NodeHandle kVelPidGainsNh(robot_namespace + + "/motion_generators/velocity/gains/" + joint->name); + control_toolbox::Pid pid; + this->pid_controllers_.emplace(joint->name, pid); + this->pid_controllers_[joint->name].init(kVelPidGainsNh); + + initVelocityCommandHandle(joint); + continue; + } } if (transmission.type_ == "franka_hw/FrankaStateInterface") { @@ -153,6 +184,8 @@ bool FrankaHWSim::initSim(const std::string& robot_namespace, // After all handles have been assigned to interfaces, register them registerInterface(&this->eji_); + registerInterface(&this->pji_); + registerInterface(&this->vji_); registerInterface(&this->jsi_); registerInterface(&this->fsi_); registerInterface(&this->fmi_); @@ -173,6 +206,16 @@ void FrankaHWSim::initEffortCommandHandle(const std::shared_ptrjsi_.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->command)); +} + +void FrankaHWSim::initVelocityCommandHandle(const std::shared_ptr& joint) { + this->vji_.registerHandle( + hardware_interface::JointHandle(this->jsi_.getHandle(joint->name), &joint->command)); +} + void FrankaHWSim::initFrankaStateHandle( const std::string& robot, const urdf::Model& urdf, @@ -315,27 +358,62 @@ void FrankaHWSim::readSim(ros::Time time, ros::Duration period) { this->updateRobotState(time); } -void FrankaHWSim::writeSim(ros::Time /*time*/, ros::Duration /*period*/) { +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; - // 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); + // Retrieve effort control command + double effort; + switch (joint_control_methods_[joint->name]) { + case EFFORT: { + // 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->command + joint->gravity; + } break; + case POSITION: { + // Use position motion generator + 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, joint->command, kJointLowerLimit, kJointUpperLimit, error); + break; + case urdf::Joint::CONTINUOUS: + error = angles::shortest_angular_distance(joint->position, joint->command); + break; + default: + error = joint->command - joint->position; + } + const double kEffortLimit = joint->limits.max_effort; + effort = + boost::algorithm::clamp(pid_controllers_[joint->name].computeCommand(error, period), + -kEffortLimit, kEffortLimit); + } break; + case VELOCITY: { + // Use velocity motion generator + const double kError = joint->command - joint->velocity; + const double kEffortLimit = joint->limits.max_effort; + effort = + boost::algorithm::clamp(pid_controllers_[joint->name].computeCommand(kError, period), + -kEffortLimit, kEffortLimit); + } break; } - auto command = joint->command + joint->gravity; - - if (std::isnan(command)) { + // Send control effort control command + if (std::isnan(effort)) { ROS_WARN_STREAM_NAMED("franka_hw_sim", "Command for " << joint->name << "is NaN, won't send to robot"); continue; } - joint->handle->SetForce(0, command); + joint->handle->SetForce(0, effort); } } @@ -480,11 +558,9 @@ void FrankaHWSim::updateRobotState(ros::Time time) { this->robot_state_.tau_J[i] = joint->effort; this->robot_state_.dtau_J[i] = joint->jerk; - // since we don't support position or velocity interface yet, we set the desired joint - // trajectory to zero indicating we are in torque control mode - this->robot_state_.q_d[i] = joint->position; // special case for resetting motion generators - this->robot_state_.dq_d[i] = 0; - this->robot_state_.ddq_d[i] = 0; + this->robot_state_.q_d[i] = joint->position; + this->robot_state_.dq_d[i] = joint->velocity; + this->robot_state_.ddq_d[i] = joint->acceleration; this->robot_state_.tau_J_d[i] = joint->command; // For now we assume no flexible joints