diff --git a/CHANGELOG.md b/CHANGELOG.md index 933e6bdd..4a7aeb8e 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,5 +1,13 @@ # CHANGELOG +## 0.x - UNRELEASED + +* `franka_gazebo`: + - Add JointPosition and JointVelocity Interface + - Fix: Robot now keeps position when no controller is running + - joint_{position,velocity}_example controller are now available in `franka_gazebo` + + ## 0.8.2 - 2022-02-22 Requires `libfranka` >= 0.8.0 diff --git a/franka_description/robots/panda_arm.urdf.xacro b/franka_description/robots/panda_arm.urdf.xacro index 2e663d71..ce154776 100644 --- a/franka_description/robots/panda_arm.urdf.xacro +++ b/franka_description/robots/panda_arm.urdf.xacro @@ -44,6 +44,21 @@ + + + + + + + + + + + + + + + diff --git a/franka_gazebo/CMakeLists.txt b/franka_gazebo/CMakeLists.txt index d3aa771a..615d7c36 100644 --- a/franka_gazebo/CMakeLists.txt +++ b/franka_gazebo/CMakeLists.txt @@ -80,6 +80,7 @@ add_library(franka_hw_sim src/franka_hw_sim.cpp src/joint.cpp src/model_kdl.cpp + src/controller_verifier.cpp ) target_link_libraries(franka_hw_sim ${catkin_LIBRARIES} diff --git a/franka_gazebo/config/franka_hw_sim.yaml b/franka_gazebo/config/franka_hw_sim.yaml index 6703a168..7e0e0b3d 100644 --- a/franka_gazebo/config/franka_hw_sim.yaml +++ b/franka_gazebo/config/franka_hw_sim.yaml @@ -11,3 +11,24 @@ franka_gripper: finger2: gains: { p: 100, i: 0, d: 1.0 } + +# Motion generators PID gains +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: 30, d: 0, i: 0 } + panda_joint2: { p: 30, d: 0, i: 0 } + panda_joint3: { p: 30, d: 0, i: 0 } + panda_joint4: { p: 30, d: 0, i: 0 } + panda_joint5: { p: 10, d: 0, i: 0 } + panda_joint6: { p: 10, d: 0, i: 0 } + panda_joint7: { p: 5, d: 0, i: 0 } diff --git a/franka_gazebo/config/sim_controllers.yaml b/franka_gazebo/config/sim_controllers.yaml index cb5d2fdd..1601c24c 100644 --- a/franka_gazebo/config/sim_controllers.yaml +++ b/franka_gazebo/config/sim_controllers.yaml @@ -66,3 +66,45 @@ 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} + +joint_velocity_example_controller: + type: franka_example_controllers/JointVelocityExampleController + joint_names: + - $(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 + +joint_position_example_controller: + type: franka_example_controllers/JointPositionExampleController + joint_names: + - $(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 \ No newline at end of file diff --git a/franka_gazebo/include/franka_gazebo/controller_verifier.h b/franka_gazebo/include/franka_gazebo/controller_verifier.h new file mode 100644 index 00000000..c24949e6 --- /dev/null +++ b/franka_gazebo/include/franka_gazebo/controller_verifier.h @@ -0,0 +1,46 @@ +#pragma once +#include +#include +#include +#include +#include + +namespace franka_gazebo { + +/// Can be used to check controllers in franka_gazebo. It also can distinguish +/// between gripper and arm controllers. +class ControllerVerifier { + public: + /// Creates a ControllerVerifier object to check controllers for franka_gazebo + /// @param joints map of joint names and joints including gripper joints + /// @param arm_id prefix of the joints + ControllerVerifier(const std::map>& joints, + const std::string& arm_id); + + /// checks if a set of joint_names only contains the joints that are used for the arm + bool areArmJoints(const std::set& resources) const; + + /// checks if a controller can be used in the franka_hw gazebo plugin + bool isValidController(const hardware_interface::ControllerInfo& controller) const; + + /// checks if a set of joint_names only contains the joints that are used for the gripper + bool areFingerJoints(const std::set& resources) const; + + /// checks if a controller wants to use the finger joints with the effort interface + bool isClaimingGripperController(const hardware_interface::ControllerInfo& info) const; + + /// checks if a controller that uses the joints of the arm (not gripper joints) claims a position, + /// velocity or effort interface. + bool isClaimingArmController(const hardware_interface::ControllerInfo& info) const; + + /// returns the control method of a hardware interface + static boost::optional determineControlMethod( + const std::string& hardware_interface); + + private: + std::vector joint_names_; + std::string arm_id_; + + static bool hasControlMethodAndValidSize(const hardware_interface::InterfaceResources& resource); +}; +} // namespace franka_gazebo \ No newline at end of file diff --git a/franka_gazebo/include/franka_gazebo/franka_hw_sim.h b/franka_gazebo/include/franka_gazebo/franka_hw_sim.h index 2cbd1fdd..6872d4b0 100644 --- a/franka_gazebo/include/franka_gazebo/franka_hw_sim.h +++ b/franka_gazebo/include/franka_gazebo/franka_hw_sim.h @@ -1,6 +1,8 @@ #pragma once +#include #include +#include #include #include #include @@ -12,6 +14,7 @@ #include #include #include +#include #include #include #include @@ -31,6 +34,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 @@ -91,9 +96,28 @@ class FrankaHWSim : public gazebo_ros_control::RobotHWSim { */ void eStopActive(const bool active) override; + /** + * Switches the control mode of the robot arm + * @param start_list list of controllers to start + * @param stop_list list of controllers to stop + */ + void doSwitch(const std::list& start_list, + const std::list& stop_list) override; + + /** + * Check (in non-realtime) if given controllers could be started and stopped from the current + * state of the RobotHW with regard to necessary hardware interface switches and prepare the + * switching. Start and stop list are disjoint. This handles the check and preparation, the actual + * switch is commited in doSwitch(). + */ + bool prepareSwitch(const std::list& start_list, + const std::list& /*stop_list*/) override; + private: /// If gazebo::Joint::GetForceTorque() yielded already a non-zero value - bool efforts_initialized_; + bool robot_initialized_; + + std::unique_ptr verifier_; std::array gravity_earth_; @@ -101,8 +125,13 @@ class FrankaHWSim : public gazebo_ros_control::RobotHWSim { gazebo::physics::ModelPtr robot_; std::map> joints_; + std::map position_pid_controllers_; + std::map velocity_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 +150,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); @@ -192,6 +223,10 @@ class FrankaHWSim : public gazebo_ros_control::RobotHWSim { Eigen::Matrix3d Ip = I + m * P.transpose() * P; return Ip; } + + void forControlledJoint( + const std::list& controllers, + const std::function& f); }; } // namespace franka_gazebo diff --git a/franka_gazebo/include/franka_gazebo/joint.h b/franka_gazebo/include/franka_gazebo/joint.h index be8afcf3..3770f537 100644 --- a/franka_gazebo/include/franka_gazebo/joint.h +++ b/franka_gazebo/include/franka_gazebo/joint.h @@ -1,16 +1,22 @@ #pragma once #include +#include #include #include #include namespace franka_gazebo { +/** + * Specifies the current control method of the joint. + */ +enum ControlMethod { EFFORT, POSITION, VELOCITY }; + /** * A data container holding all relevant information about a robotic joint. * - * Calling @ref update on this object will compute its internal state based on the all currenlty + * Calling @ref update on this object will compute its internal state based on the all currently * supplied information such as position, efforts etc. */ struct Joint { @@ -20,7 +26,7 @@ struct Joint { Joint(const Joint&) = delete; /** - * Calculate all members such as accelerations, jerks velocities by differention + * Calculate all members such as accelerations, jerks velocities by differentiation * @param[in] dt the current time step since last time this method was called */ void update(const ros::Duration& dt); @@ -35,6 +41,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; @@ -42,6 +52,21 @@ struct Joint { /// \f$Nm\f$ without gravity double command = 0; + /// The current desired position that is used for the PID controller when the joints control + /// method is "POSITION". When the control method is not "POSITION", this value will only be + /// updated once at the start of the controller and stay the same until a new controller is + /// started. + double desired_position = 0; + + /// The current desired velocity that is used for the PID controller when the joints control + /// method is "VELOCITY". When the control method is not "VELOCITY", this value will be set to + /// zero. + double desired_velocity = 0; + + /// Decides whether the joint is doing torque control or if the position or velocity should + /// be controlled. + ControlMethod control_method = POSITION; + /// The currently acting gravity force or torque acting on this joint in \f$N\f$ or \f$Nm\f$ double gravity = 0; @@ -58,7 +83,7 @@ struct Joint { /// \f$\frac{rad}{s^3}\f$ double jerk = 0; - /// The currenlty acting acceleration on this joint in either \f$\frac{m}{s^2}\f$ or + /// The currently acting acceleration on this joint in either \f$\frac{m}{s^2}\f$ or /// \f$\frac{rad}{s^2}\f$ double acceleration = 0; diff --git a/franka_gazebo/launch/panda.launch b/franka_gazebo/launch/panda.launch index 92b1a7d3..a15c27e0 100644 --- a/franka_gazebo/launch/panda.launch +++ b/franka_gazebo/launch/panda.launch @@ -11,7 +11,7 @@ - + @@ -21,9 +21,9 @@ - + + +namespace franka_gazebo { + +ControllerVerifier::ControllerVerifier( + const std::map>& joints, + const std::string& arm_id) + : arm_id_(arm_id) { + for (const auto& joint : joints) { + joint_names_.push_back(joint.first); + } +} + +bool ControllerVerifier::isValidController( + const hardware_interface::ControllerInfo& controller) const { + if (isClaimingGripperController(controller) or isClaimingArmController(controller)) { + return true; + } + return std::none_of( + controller.claimed_resources.begin(), controller.claimed_resources.end(), + [](const auto& resource) { + return ControllerVerifier::determineControlMethod(resource.hardware_interface); + }); +} + +bool ControllerVerifier::isClaimingArmController( + const hardware_interface::ControllerInfo& info) const { + for (const auto& claimed_resource : info.claimed_resources) { + if (hasControlMethodAndValidSize(claimed_resource)) { + return areArmJoints(claimed_resource.resources); + } + } + return false; +} + +bool ControllerVerifier::isClaimingGripperController( + const hardware_interface::ControllerInfo& info) const { + for (const auto& claimed_resource : info.claimed_resources) { + if (not areFingerJoints(claimed_resource.resources) or claimed_resource.resources.size() != 2) { + continue; + } + auto control_method = + ControllerVerifier::determineControlMethod(claimed_resource.hardware_interface); + if (not control_method) { + continue; + } + if (control_method.value() == EFFORT) { + return true; + } + } + return false; +} + +bool ControllerVerifier::hasControlMethodAndValidSize( + const hardware_interface::InterfaceResources& resource) { + return ControllerVerifier::determineControlMethod(resource.hardware_interface) + .is_initialized() and + resource.resources.size() == 7; +} + +bool ControllerVerifier::areArmJoints(const std::set& resources) const { + return std::all_of(resources.begin(), resources.end(), [this](const std::string& joint_name) { + return std::find_if(joint_names_.begin(), joint_names_.end(), + [&joint_name, this](const std::string& joint) { + // make sure that the joint is not a finger joint + if (joint.find(arm_id_ + "_finger_joint") != std::string::npos) { + return false; + } + return joint == joint_name; + }) != joint_names_.end(); + }); +} + +bool ControllerVerifier::areFingerJoints(const std::set& resources) const { + return std::all_of(resources.begin(), resources.end(), [this](const std::string& joint_name) { + return joint_name.find(arm_id_ + "_finger_joint") != std::string::npos; + }); +} + +boost::optional ControllerVerifier::determineControlMethod( + const std::string& hardware_interface) { + if (hardware_interface.find("hardware_interface::PositionJointInterface") != std::string::npos) { + return POSITION; + } + if (hardware_interface.find("hardware_interface::VelocityJointInterface") != std::string::npos) { + return VELOCITY; + } + if (hardware_interface.find("hardware_interface::EffortJointInterface") != std::string::npos) { + return EFFORT; + } + return boost::none; +} + +} // namespace franka_gazebo diff --git a/franka_gazebo/src/franka_hw_sim.cpp b/franka_gazebo/src/franka_hw_sim.cpp index d34d344f..68d84eb1 100644 --- a/franka_gazebo/src/franka_hw_sim.cpp +++ b/franka_gazebo/src/franka_hw_sim.cpp @@ -1,7 +1,7 @@ -#include - +#include #include #include +#include #include #include #include @@ -10,7 +10,10 @@ #include #include #include +#include #include +#include +#include #include #include #include @@ -32,7 +35,7 @@ bool FrankaHWSim::initSim(const std::string& robot_namespace, } this->robot_ = parent; - this->efforts_initialized_ = false; + this->robot_initialized_ = false; #if GAZEBO_MAJOR_VERSION >= 8 gazebo::physics::PhysicsEnginePtr physics = gazebo::physics::get_world()->Physics(); @@ -85,6 +88,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 @@ -96,6 +100,10 @@ bool FrankaHWSim::initSim(const std::string& robot_namespace, 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); } @@ -107,7 +115,7 @@ bool FrankaHWSim::initSim(const std::string& robot_namespace, } // Register all supported command interfaces - for (auto& transmission : transmissions) { + for (const auto& transmission : transmissions) { for (const auto& k_interface : transmission.joints_[0].hardware_interfaces_) { auto joint = this->joints_[transmission.joints_[0].name_]; if (transmission.type_ == "transmission_interface/SimpleTransmission") { @@ -117,6 +125,25 @@ bool FrankaHWSim::initSim(const std::string& robot_namespace, initEffortCommandHandle(joint); continue; } + if (k_interface == "hardware_interface/PositionJointInterface") { + // Initiate position motion generator (PID controller) + control_toolbox::Pid pid; + pid.initParam(robot_namespace + "/motion_generators/position/gains/" + joint->name); + this->position_pid_controllers_.emplace(joint->name, pid); + + initPositionCommandHandle(joint); + continue; + } + if (k_interface == "hardware_interface/VelocityJointInterface") { + // Initiate velocity motion generator (PID controller) + control_toolbox::Pid pid_velocity; + pid_velocity.initParam(robot_namespace + "/motion_generators/velocity/gains/" + + joint->name); + this->velocity_pid_controllers_.emplace(joint->name, pid_velocity); + + initVelocityCommandHandle(joint); + continue; + } } if (transmission.type_ == "franka_hw/FrankaStateInterface") { @@ -152,14 +179,23 @@ bool FrankaHWSim::initSim(const std::string& robot_namespace, } // 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); } @@ -173,6 +209,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->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, @@ -315,27 +361,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; + // Retrieve effort control command + double effort = 0; // 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); } + auto& control_method = joint->control_method; + if (control_method == EFFORT) { + effort = joint->command + joint->gravity; + } else if (control_method == 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->desired_position, + kJointLowerLimit, kJointUpperLimit, error); + break; + default: + std::string error_message = + "Only revolute joints are allowed for position control right now"; + ROS_FATAL("%s", error_message.c_str()); + throw std::invalid_argument(error_message); + } - auto command = joint->command + joint->gravity; + const double kEffortLimit = joint->limits.max_effort; + effort = boost::algorithm::clamp( + position_pid_controllers_[joint->name].computeCommand(error, period), + -kEffortLimit, kEffortLimit) + + joint->gravity; + } else if (control_method == VELOCITY) { + // Use velocity motion generator + const double kError = joint->desired_velocity - joint->velocity; + const double kEffortLimit = joint->limits.max_effort; + effort = boost::algorithm::clamp( + velocity_pid_controllers_[joint->name].computeCommand(kError, period), + -kEffortLimit, kEffortLimit) + + joint->gravity; + } - if (std::isnan(command)) { + // Send control effort control command + if (not std::isfinite(effort)) { ROS_WARN_STREAM_NAMED("franka_hw_sim", - "Command for " << joint->name << "is NaN, won't send to robot"); + "Command for " << joint->name << "is not finite, won't send to robot"); continue; } - joint->handle->SetForce(0, command); + joint->handle->SetForce(0, effort); } } @@ -480,18 +561,30 @@ 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_.tau_J_d[i] = joint->command; + if (joint->control_method == EFFORT) { + this->robot_state_.q_d[i] = joint->desired_position; + this->robot_state_.dq_d[i] = 0; + this->robot_state_.ddq_d[i] = 0; + this->robot_state_.tau_J_d[i] = joint->command; + } else { + this->robot_state_.q_d[i] = + joint->control_method == POSITION ? joint->desired_position : joint->position; + this->robot_state_.dq_d[i] = + joint->control_method == VELOCITY ? joint->desired_velocity : joint->velocity; + this->robot_state_.ddq_d[i] = joint->acceleration; + this->robot_state_.tau_J_d[i] = 0; + } // For now we assume no flexible joints this->robot_state_.theta[i] = joint->position; this->robot_state_.dtheta[i] = joint->velocity; - if (this->efforts_initialized_) { + // first time initialization of the desired position + if (not this->robot_initialized_) { + joint->desired_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 @@ -533,7 +626,50 @@ void FrankaHWSim::updateRobotState(ros::Time time) { 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_); - this->efforts_initialized_ = true; + this->robot_initialized_ = true; +} + +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 = 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; + }); +} + +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 diff --git a/franka_gazebo/test/CMakeLists.txt b/franka_gazebo/test/CMakeLists.txt index 8ffb29eb..d8ccc242 100644 --- a/franka_gazebo/test/CMakeLists.txt +++ b/franka_gazebo/test/CMakeLists.txt @@ -81,3 +81,22 @@ target_link_libraries(franka_gripper_sim_test_with_object target_include_directories(franka_gripper_sim_test_with_object PUBLIC ${catkin_INCLUDE_DIRS} ) + +catkin_add_gtest(franka_hw_sim_controller_verifier_test + main.cpp + controller_verifier_test.cpp +) + +add_dependencies(franka_hw_sim_controller_verifier_test + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} +) + +target_link_libraries(franka_hw_sim_controller_verifier_test + ${catkin_LIBRARIES} + franka_hw_sim +) + +target_include_directories(franka_hw_sim_controller_verifier_test PUBLIC + ${catkin_INCLUDE_DIRS} +) \ No newline at end of file diff --git a/franka_gazebo/test/controller_verifier_test.cpp b/franka_gazebo/test/controller_verifier_test.cpp new file mode 100644 index 00000000..7c4a18ce --- /dev/null +++ b/franka_gazebo/test/controller_verifier_test.cpp @@ -0,0 +1,239 @@ +#include +#include + +static const std::string effort = "hardware_interface::EffortJointInterface"; +static const std::string position = "hardware_interface::PositionJointInterface"; +static const std::string velocity = "hardware_interface::VelocityJointInterface"; + +class ControllerVerifierFixture : public ::testing::Test { + protected: + std::unique_ptr verifier; + + virtual void SetUp() { + std::map> joint_map; + for (int i = 1; i < 8; i++) { + joint_map.emplace("panda_joint" + std::to_string(i), nullptr); + } + for (int i = 1; i < 3; i++) { + joint_map.emplace("panda_finger_joint" + std::to_string(i), nullptr); + } + verifier = std::make_unique(joint_map, "panda"); + } + + virtual void TearDown() {} +}; +class ValidArmControllerTest + : public ControllerVerifierFixture, + public testing::WithParamInterface { + protected: + void SetUp() override { + ControllerVerifierFixture::SetUp(); + controller = GetParam(); + } + + hardware_interface::ControllerInfo controller; +}; + +class NonClaimingArmControllerTest : public ValidArmControllerTest {}; +class MalformedArmControllerTest : public ValidArmControllerTest {}; +class NonClaimingGripperControllerTest : public ValidArmControllerTest {}; +class InvalidGripperControllerTest : public ValidArmControllerTest {}; +class ValidGripperControllerTest : public ValidArmControllerTest {}; +class MalformedGripperControllerTest : public ValidArmControllerTest {}; + +hardware_interface::ControllerInfo generate_arm_controller(const std::string& hw_interface, + int num_joints = 7) { + hardware_interface::ControllerInfo controller; + hardware_interface::InterfaceResources resource; + resource.hardware_interface = hw_interface; + for (int i = 1; i <= num_joints; i++) { + resource.resources.emplace("panda_joint" + std::to_string(i)); + } + controller.claimed_resources.push_back(resource); + return controller; +} + +hardware_interface::ControllerInfo generate_gripper_controller(const std::string& hw_interface, + int num_joints = 2) { + hardware_interface::ControllerInfo controller; + hardware_interface::InterfaceResources resource; + resource.hardware_interface = hw_interface; + for (int i = 1; i <= num_joints; i++) { + resource.resources.emplace("panda_finger_joint" + std::to_string(i)); + } + controller.claimed_resources.push_back(resource); + return controller; +} + +hardware_interface::ControllerInfo generate_arm_and_gripper_controller( + const std::string& hw_interface) { + hardware_interface::ControllerInfo controller; + hardware_interface::InterfaceResources resource; + resource.hardware_interface = hw_interface; + for (int i = 1; i <= 7; i++) { + resource.resources.emplace("panda_joint" + std::to_string(i)); + } + for (int i = 1; i <= 2; i++) { + resource.resources.emplace("panda_finger_joint" + std::to_string(i)); + } + controller.claimed_resources.push_back(resource); + return controller; +} + +TEST_P(ValidArmControllerTest, IsArmController) { + EXPECT_TRUE(verifier->isClaimingArmController(controller)); +} + +TEST_P(ValidArmControllerTest, HasArmJoints) { + for (const auto& claimed_resources : controller.claimed_resources) { + EXPECT_TRUE(verifier->areArmJoints(claimed_resources.resources)); + } +} + +TEST_P(ValidArmControllerTest, DoesNotHaveFingerJoints) { + for (const auto& claimed_resources : controller.claimed_resources) { + EXPECT_FALSE(verifier->areFingerJoints(claimed_resources.resources)); + } +} + +TEST_P(ValidArmControllerTest, IsNotGripperController) { + EXPECT_FALSE(verifier->isClaimingGripperController(controller)); +} + +TEST_P(ValidArmControllerTest, IsValidController) { + EXPECT_TRUE(verifier->isValidController(controller)); +} + +TEST_P(NonClaimingArmControllerTest, IsValidController) { + EXPECT_TRUE(verifier->isValidController(controller)); +} + +TEST_P(NonClaimingArmControllerTest, IsNotArmController) { + EXPECT_FALSE(verifier->isClaimingArmController(controller)); +} + +TEST_P(NonClaimingArmControllerTest, IsNotGripperController) { + EXPECT_FALSE(verifier->isClaimingGripperController(controller)); +} + +TEST_P(MalformedArmControllerTest, IsValidController) { + EXPECT_FALSE(verifier->isValidController(controller)); +} + +TEST_P(MalformedArmControllerTest, IsNotArmController) { + EXPECT_FALSE(verifier->isClaimingArmController(controller)); +} + +TEST_P(MalformedArmControllerTest, IsNotGripperController) { + EXPECT_FALSE(verifier->isClaimingGripperController(controller)); +} + +TEST_P(ValidGripperControllerTest, IsValidController) { + EXPECT_TRUE(verifier->isValidController(controller)); +} + +TEST_P(ValidGripperControllerTest, IsGripperController) { + EXPECT_TRUE(verifier->isClaimingGripperController(controller)); +} + +TEST_P(ValidGripperControllerTest, HasFingerJoints) { + for (const auto& claimed_resources : controller.claimed_resources) { + EXPECT_TRUE(verifier->areFingerJoints(claimed_resources.resources)); + } +} + +TEST_P(ValidGripperControllerTest, DoesNotHaveArmJoints) { + for (const auto& claimed_resources : controller.claimed_resources) { + EXPECT_FALSE(verifier->areArmJoints(claimed_resources.resources)); + } +} + +TEST_P(NonClaimingGripperControllerTest, DoesNotHaveArmJoints) { + for (const auto& claimed_resources : controller.claimed_resources) { + EXPECT_FALSE(verifier->areArmJoints(claimed_resources.resources)); + } +} + +TEST_P(NonClaimingGripperControllerTest, IsNotGripperController) { + EXPECT_FALSE(verifier->isClaimingGripperController(controller)); +} + +TEST_P(NonClaimingGripperControllerTest, IsNotArmController) { + EXPECT_FALSE(verifier->isClaimingArmController(controller)); +} + +TEST_P(NonClaimingGripperControllerTest, IsValidController) { + EXPECT_TRUE(verifier->isValidController(controller)); +} + +TEST_P(InvalidGripperControllerTest, DoesNotHaveArmJoints) { + for (const auto& claimed_resources : controller.claimed_resources) { + EXPECT_FALSE(verifier->areArmJoints(claimed_resources.resources)); + } +} + +TEST_P(InvalidGripperControllerTest, IsNotGripperController) { + EXPECT_FALSE(verifier->isClaimingGripperController(controller)); +} + +TEST_P(InvalidGripperControllerTest, IsNotArmController) { + EXPECT_FALSE(verifier->isClaimingArmController(controller)); +} + +TEST_P(InvalidGripperControllerTest, IsNotValidController) { + EXPECT_FALSE(verifier->isValidController(controller)); +} + +TEST_P(MalformedGripperControllerTest, IsNotValidController) { + EXPECT_FALSE(verifier->isValidController(controller)); +} + +TEST_P(MalformedGripperControllerTest, IsNotArmController) { + EXPECT_FALSE(verifier->isClaimingArmController(controller)); +} + +TEST_P(MalformedGripperControllerTest, IsNotGripperController) { + EXPECT_FALSE(verifier->isClaimingGripperController(controller)); +} + +INSTANTIATE_TEST_CASE_P(ValidArmControllerTest, // NOLINT(cert-err58-cpp) + ValidArmControllerTest, + ::testing::Values(generate_arm_controller(effort), + generate_arm_controller(position), + generate_arm_controller(velocity))); + +INSTANTIATE_TEST_CASE_P(NonClaimingArmControllerTest, // NOLINT(cert-err58-cpp) + NonClaimingArmControllerTest, + ::testing::Values(generate_arm_controller(""), + generate_arm_controller(" "), + generate_arm_controller("safsdafsdafsad"), + generate_arm_and_gripper_controller("asfdasf"))); + +INSTANTIATE_TEST_CASE_P(MalformedArmControllerTest, // NOLINT(cert-err58-cpp) + MalformedArmControllerTest, + ::testing::Values(generate_arm_controller(effort, 6), + generate_arm_controller(position, 8), + generate_arm_controller(velocity, 2), + generate_arm_and_gripper_controller(effort), + generate_arm_and_gripper_controller(position), + generate_arm_and_gripper_controller(velocity))); + +INSTANTIATE_TEST_CASE_P(ValidGripperControllerTest, // NOLINT(cert-err58-cpp) + ValidGripperControllerTest, + ::testing::Values(generate_gripper_controller(effort))); + +INSTANTIATE_TEST_CASE_P(NonClaimingGripperControllerTest, // NOLINT(cert-err58-cpp) + NonClaimingGripperControllerTest, + ::testing::Values(generate_gripper_controller(""), + generate_gripper_controller(" "), + generate_gripper_controller("afdasfas"))); + +INSTANTIATE_TEST_CASE_P(InvalidGripperControllerTest, // NOLINT(cert-err58-cpp) + InvalidGripperControllerTest, + ::testing::Values(generate_gripper_controller(velocity), + generate_gripper_controller(position))); + +INSTANTIATE_TEST_CASE_P(MalformedGripperControllerTest, // NOLINT(cert-err58-cpp) + MalformedGripperControllerTest, + ::testing::Values(generate_gripper_controller(effort, 1), + generate_gripper_controller(effort, 3))); \ No newline at end of file