diff --git a/franka_gazebo/include/franka_gazebo/franka_hw_sim.h b/franka_gazebo/include/franka_gazebo/franka_hw_sim.h index 2cbd1fdd3..4e710526b 100644 --- a/franka_gazebo/include/franka_gazebo/franka_hw_sim.h +++ b/franka_gazebo/include/franka_gazebo/franka_hw_sim.h @@ -115,6 +115,7 @@ class FrankaHWSim : public gazebo_ros_control::RobotHWSim { ros::ServiceServer service_set_k_; ros::ServiceServer service_set_load_; ros::ServiceServer service_collision_behavior_; + ros::ServiceServer service_reset_joint_states_; std::vector lower_force_thresholds_nominal_; std::vector upper_force_thresholds_nominal_; diff --git a/franka_gazebo/include/franka_gazebo/joint.h b/franka_gazebo/include/franka_gazebo/joint.h index be8afcf3c..79898fa75 100644 --- a/franka_gazebo/include/franka_gazebo/joint.h +++ b/franka_gazebo/include/franka_gazebo/joint.h @@ -87,9 +87,17 @@ struct Joint { */ bool isInCollision() const; + /** + * Makes sure the joint states are reset to zero. + */ + void resetJointStates(); + private: double lastVelocity = std::numeric_limits::quiet_NaN(); double lastAcceleration = std::numeric_limits::quiet_NaN(); + + // Used to track whether a reset of the joint states was requested + bool resetRequested_ = false; }; } // namespace franka_gazebo diff --git a/franka_gazebo/src/franka_hw_sim.cpp b/franka_gazebo/src/franka_hw_sim.cpp index d34d344fe..85e148b9e 100644 --- a/franka_gazebo/src/franka_hw_sim.cpp +++ b/franka_gazebo/src/franka_hw_sim.cpp @@ -5,11 +5,13 @@ #include #include #include +#include #include #include #include #include #include +#include #include #include #include @@ -305,6 +307,18 @@ void FrankaHWSim::initServices(ros::NodeHandle& nh) { response.success = true; return true; }); + this->service_reset_joint_states_ = franka_hw::advertiseService( + nh, "reset_joint_states", [&](auto& request, auto& response) { + ROS_INFO_STREAM_NAMED("franka_hw_sim", "Resetting joint_states"); + + for (int i = 0; i < 7; i++) { + std::string name = this->arm_id_ + "_joint" + std::to_string(i + 1); + this->joints_[name]->resetJointStates(); + } + + response.success = true; + return true; + }); } void FrankaHWSim::readSim(ros::Time time, ros::Duration period) { diff --git a/franka_gazebo/src/joint.cpp b/franka_gazebo/src/joint.cpp index d733539db..f2f456141 100644 --- a/franka_gazebo/src/joint.cpp +++ b/franka_gazebo/src/joint.cpp @@ -9,6 +9,12 @@ void Joint::update(const ros::Duration& dt) { return; } + // Reset joint states if requested + if (this->resetRequested_) { + this->position = 0; + this->resetRequested_ = false; + } + this->velocity = this->handle->GetVelocity(0); #if GAZEBO_MAJOR_VERSION >= 8 double position = this->handle->Position(0); @@ -70,4 +76,9 @@ bool Joint::isInCollision() const { bool Joint::isInContact() const { return std::abs(this->effort - this->command) > this->contact_threshold; } + +void Joint::resetJointStates() { + this->resetRequested_ = true; +} + } // namespace franka_gazebo diff --git a/franka_msgs/CMakeLists.txt b/franka_msgs/CMakeLists.txt index 1f6f92659..a5a1c2ad1 100644 --- a/franka_msgs/CMakeLists.txt +++ b/franka_msgs/CMakeLists.txt @@ -6,6 +6,7 @@ find_package(catkin REQUIRED COMPONENTS message_generation std_msgs actionlib_ms add_message_files(FILES Errors.msg FrankaState.msg) add_service_files(FILES + ResetJointStates.srv SetCartesianImpedance.srv SetEEFrame.srv SetForceTorqueCollisionBehavior.srv diff --git a/franka_msgs/srv/ResetJointStates.srv b/franka_msgs/srv/ResetJointStates.srv new file mode 100644 index 000000000..54b7e0cfd --- /dev/null +++ b/franka_msgs/srv/ResetJointStates.srv @@ -0,0 +1,3 @@ +--- +bool success +string error