diff --git a/franka_description/robots/panda_arm.urdf.xacro b/franka_description/robots/panda_arm.urdf.xacro index d5b8d0061..36133af9b 100644 --- a/franka_description/robots/panda_arm.urdf.xacro +++ b/franka_description/robots/panda_arm.urdf.xacro @@ -4,8 +4,10 @@ + + @@ -44,13 +46,13 @@ - - - - - - - + + + + + + + #include #include +#include #include #include #include @@ -29,6 +30,8 @@ namespace franka_gazebo { * ### transmission_interface/SimpleTransmission * - hardware_interface/JointStateInterface * - hardware_interface/EffortJointInterface + * - hardware_interface/PositionJointInterface + * - hardware_interface/VelocityJointInterface * * ### franka_hw/FrankaStateInterface * ### franka_hw/FrankaModelInterface @@ -94,8 +97,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_; @@ -112,6 +121,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 92ae7337c..99acd8664 100644 --- a/franka_gazebo/include/franka_gazebo/joint.h +++ b/franka_gazebo/include/franka_gazebo/joint.h @@ -4,6 +4,7 @@ #include #include #include +#include namespace franka_gazebo { @@ -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/launch/panda.launch b/franka_gazebo/launch/panda.launch index 8cf57e5b2..c8d0c4ef6 100644 --- a/franka_gazebo/launch/panda.launch +++ b/franka_gazebo/launch/panda.launch @@ -12,6 +12,7 @@ + @@ -42,6 +43,7 @@ gazebo:=true hand:=$(arg use_gripper) arm_id:=$(arg arm_id) + transmission:=$(arg transmission) xyz:='$(arg x) $(arg y) $(arg z)' rpy:='$(arg roll) $(arg pitch) $(arg yaw)'"> diff --git a/franka_gazebo/src/franka_hw_sim.cpp b/franka_gazebo/src/franka_hw_sim.cpp index b2bf83162..c877c715b 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 @@ -76,6 +78,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 @@ -105,8 +108,34 @@ 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; + } else if (k_interface == "hardware_interface/PositionJointInterface") { + this->joint_control_methods_.emplace(joint->name, POSITION); + + // Initiate position motion generator (PID controller) + const ros::NodeHandle pos_pid_gains_nh( + 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(pos_pid_gains_nh); + + initPositionCommandHandle(joint); + continue; + } else if (k_interface == "hardware_interface/VelocityJointInterface") { + this->joint_control_methods_.emplace(joint->name, VELOCITY); + + // Initiate velocity motion generator (PID controller) + const ros::NodeHandle vel_pid_gains_nh( + 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(vel_pid_gains_nh); + + initVelocityCommandHandle(joint); + continue; } } @@ -142,6 +171,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_); @@ -162,6 +193,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, @@ -302,26 +343,63 @@ 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_); for (auto& pair : this->joints_) { auto joint = pair.second; auto command = 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; - command += g.at(i); + auto& effort = command; + + // Retrieve effort control command + 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; + command += g.at(i); + } + effort = command; + } break; + case POSITION: { + // Use position motion generator + double error; + const double joint_lower_limit_ = joint->limits.min_position; + const double joint_upper_limit_ = joint->limits.max_position; + switch (joint->type) { + case urdf::Joint::REVOLUTE: + angles::shortest_angular_distance_with_limits( + joint->position, command, joint_lower_limit_, joint_upper_limit_, error); + break; + case urdf::Joint::CONTINUOUS: + error = angles::shortest_angular_distance(joint->position, command); + break; + default: + error = command - joint->position; + } + const double effort_limit = joint->limits.max_effort; + effort = + boost::algorithm::clamp(pid_controllers_[joint->name].computeCommand(error, period), + -effort_limit, effort_limit); + } break; + case VELOCITY: { + // Use velocity motion generator + const double error = command - joint->velocity; + const double effort_limit = joint->limits.max_effort; + effort = + boost::algorithm::clamp(pid_controllers_[joint->name].computeCommand(error, period), + -effort_limit, effort_limit); + } break; } - 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); } }