From ca1d9f8050a7d4d3b206d2988005a3a70fc6b155 Mon Sep 17 00:00:00 2001 From: Georg Bartels Date: Thu, 16 Feb 2017 11:34:12 +0100 Subject: [PATCH 1/2] Using VelocityJointSaturationInterface to enforce joint limits when in simulation mode velocity. --- src/sim_hw_interface.cpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/src/sim_hw_interface.cpp b/src/sim_hw_interface.cpp index 7c5d597..fa52cc3 100644 --- a/src/sim_hw_interface.cpp +++ b/src/sim_hw_interface.cpp @@ -121,7 +121,18 @@ void SimHWInterface::write(ros::Duration &elapsed_time) void SimHWInterface::enforceLimits(ros::Duration &period) { // Enforces position and velocity - pos_jnt_sat_interface_.enforceLimits(period); + switch (sim_control_mode_) + { + case 0: // hardware_interface::MODE_POSITION: + pos_jnt_sat_interface_.enforceLimits(period); + break; + case 1: // hardware_interface::MODE_VELOCITY: + vel_jnt_sat_interface_.enforceLimits(period); + break; + case 2: // hardware_interface::MODE_EFFORT: + ROS_ERROR_STREAM_NAMED(name_, "Effort not implemented yet"); + break; + } } void SimHWInterface::positionControlSimulation(ros::Duration &elapsed_time, const std::size_t joint_id) From 67f6a80bcd61192c305970d47fb4eca1d57e4d40 Mon Sep 17 00:00:00 2001 From: Georg Bartels Date: Thu, 16 Feb 2017 15:39:47 +0100 Subject: [PATCH 2/2] Enforce joint limits during initialization of simulation. --- src/generic_hw_interface.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/generic_hw_interface.cpp b/src/generic_hw_interface.cpp index 7a6263f..ff7c8f4 100644 --- a/src/generic_hw_interface.cpp +++ b/src/generic_hw_interface.cpp @@ -216,6 +216,11 @@ void GenericHWInterface::registerJointLimits(const hardware_interface::JointHand joint_position_lower_limits_[joint_id] = joint_limits.min_position; joint_position_upper_limits_[joint_id] = joint_limits.max_position; + + // Enforce limits to not start simulation in invalid state + joint_position_[joint_id] = + joint_limits_interface::internal::saturate(joint_position_[joint_id], + joint_limits.min_position, joint_limits.max_position); } // Copy velocity limits if available