diff --git a/franka_description/robots/hand.xacro b/franka_description/robots/hand.xacro index 22e6a087..b6760282 100644 --- a/franka_description/robots/hand.xacro +++ b/franka_description/robots/hand.xacro @@ -12,9 +12,16 @@ - + + + + + + + + @@ -33,32 +40,37 @@ - - + + + - + - + - + - + - + - - - - - - - + + + + + + + + + + + @@ -66,6 +78,18 @@ + + + + + + + + + + + + diff --git a/franka_description/robots/panda_arm.urdf.xacro b/franka_description/robots/panda_arm.urdf.xacro index 0b607f9c..4a1a317a 100644 --- a/franka_description/robots/panda_arm.urdf.xacro +++ b/franka_description/robots/panda_arm.urdf.xacro @@ -1,12 +1,12 @@ - - - - - - + + + + + + @@ -45,13 +45,13 @@ - - - - - - - + + + + + + + + + + + + @@ -309,12 +314,31 @@ + + + + + + + - + + + + + + + - + + + + + + + @@ -324,9 +348,21 @@ - + + + + + + + + + + + + + - + @@ -351,6 +387,7 @@ + diff --git a/franka_gazebo/config/franka_hw_sim.yaml b/franka_gazebo/config/franka_hw_sim.yaml index f5d154d7..151bb7a1 100644 --- a/franka_gazebo/config/franka_hw_sim.yaml +++ b/franka_gazebo/config/franka_hw_sim.yaml @@ -10,3 +10,26 @@ franka_gripper: finger2: gains: { p: 100, i: 25, d: 20 } + +# 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..30d4589f 100644 --- a/franka_gazebo/config/sim_controllers.yaml +++ b/franka_gazebo/config/sim_controllers.yaml @@ -39,6 +39,33 @@ cartesian_impedance_example_controller: - $(arg arm_id)_joint6 - $(arg arm_id)_joint7 +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 + panda_joint1: + goal: 0.05 + panda_joint2: + goal: 0.05 + panda_joint3: + goal: 0.05 + panda_joint4: + goal: 0.05 + panda_joint5: + goal: 0.05 + panda_joint6: + goal: 0.05 + panda_joint7: + goal: 0.05 + effort_joint_trajectory_controller: type: effort_controllers/JointTrajectoryController joints: @@ -50,19 +77,10 @@ effort_joint_trajectory_controller: - $(arg arm_id)_joint6 - $(arg arm_id)_joint7 gains: - $(arg arm_id)_joint1: { p: 600, d: 30, i: 0 } - $(arg arm_id)_joint2: { p: 600, d: 30, i: 0 } - $(arg arm_id)_joint3: { p: 600, d: 30, i: 0 } - $(arg arm_id)_joint4: { p: 600, d: 30, i: 0 } - $(arg arm_id)_joint5: { p: 250, d: 10, i: 0 } - $(arg arm_id)_joint6: { p: 150, d: 10, i: 0 } - $(arg arm_id)_joint7: { p: 50, d: 5, i: 0 } - 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 } + panda_joint1: { p: 600, d: 30, i: 0.0 } + panda_joint2: { p: 600, d: 30, i: 0.0 } + panda_joint3: { p: 600, d: 30, i: 0.0 } + panda_joint4: { p: 600, d: 30, i: 0.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/include/franka_gazebo/franka_hw_sim.h b/franka_gazebo/include/franka_gazebo/franka_hw_sim.h index a509f5d9..ab83f215 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 @@ -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 @@ -90,12 +93,20 @@ class FrankaHWSim : public gazebo_ros_control::RobotHWSim { void eStopActive(const bool active) override; private: + std::array gravity_earth_; + std::string arm_id_; 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 +123,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 92ae7337..99d587b7 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/launch/panda.launch b/franka_gazebo/launch/panda.launch index eba70f46..6fdf15ad 100644 --- a/franka_gazebo/launch/panda.launch +++ b/franka_gazebo/launch/panda.launch @@ -12,6 +12,7 @@ + @@ -36,12 +37,12 @@ - @@ -89,7 +90,6 @@ - - + diff --git a/franka_gazebo/src/franka_hw_sim.cpp b/franka_gazebo/src/franka_hw_sim.cpp index 661276c1..a8e997f9 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 @@ -41,6 +43,10 @@ bool FrankaHWSim::initSim(const std::string& robot_namespace, ROS_INFO_STREAM_NAMED("franka_hw_sim", "Using physics type " << physics->GetType()); + // Retrieve physics parameters + auto gravity_ = physics->World()->Gravity(); + this->gravity_earth_ = {gravity_.X(), gravity_.Y(), gravity_.Z()}; + // Generate a list of franka_gazebo::Joint to store all relevant information for (const auto& transmission : transmissions) { if (transmission.type_ != "transmission_interface/SimpleTransmission") { @@ -76,6 +82,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 +112,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; } } @@ -144,6 +177,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_); @@ -164,6 +199,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, @@ -306,26 +351,63 @@ void FrankaHWSim::readSim(ros::Time time, ros::Duration period) { this->updateRobotState(time); } -void FrankaHWSim::writeSim(ros::Time /*time*/, ros::Duration /*period*/) { - auto g = this->model_->gravity(this->robot_state_); +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; 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); } }