[Servo] Add override parameter to set constant velocity scaling in Se…

…rvo (#1169)

moveit_ros/moveit_servo/config/panda_simulated_config.yaml

@@ -12,6 +12,9 @@ scale:
   # Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic.
   joint: 0.5
 
+# Optionally override Servo's internal velocity scaling when near singularity or collision (0.0 = use internal velocity scaling)
+# override_velocity_scaling_factor = 0.0 # valid range [0.0:1.0]
+
 ## Properties of outgoing commands
 publish_period: 0.034  # 1/Nominal publish rate [seconds]
 low_latency_mode: false  # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored)

moveit_ros/moveit_servo/config/panda_simulated_config_pose_tracking.yaml

@@ -12,6 +12,9 @@ scale:
   # Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic.
   joint: 0.01
 
+# Optionally override Servo's internal velocity scaling when near singularity or collision (0.0 = use internal velocity scaling)
+# override_velocity_scaling_factor = 0.0 # valid range [0.0:1.0]
+
 ## Properties of outgoing commands
 publish_period: 0.034  # 1/Nominal publish rate [seconds]
 

moveit_ros/moveit_servo/include/moveit_servo/enforce_limits.hpp

@@ -49,8 +49,11 @@ namespace moveit_servo
  * @brief Decrease robot position change and velocity, if needed, to satisfy joint velocity limits
  * @param joint_model_group Active joint group. Used to retrieve limits.
  * @param joint_state The command that will go to the robot.
+ * @param override_velocity_scaling_factor Option if the user wants a constant override of the velocity scaling.
+ *        a value greater than 0 will override the internal calculations done by getVelocityScalingFactor.
  */
 void enforceVelocityLimits(const moveit::core::JointModelGroup* joint_model_group, const double publish_period,
-                           sensor_msgs::msg::JointState& joint_state);
+                           sensor_msgs::msg::JointState& joint_state,
+                           const double override_velocity_scaling_factor = 0.0);
 
 }  // namespace moveit_servo

moveit_ros/moveit_servo/include/moveit_servo/servo_parameters.h

@@ -70,6 +70,8 @@ struct ServoParameters
   double linear_scale{ 0.4 };
   double rotational_scale{ 0.8 };
   double joint_scale{ 0.5 };
+  // Properties of Servo calculations
+  double override_velocity_scaling_factor{ 0.0 };
   // Properties of outgoing commands
   std::string command_out_topic{ "/panda_arm_controller/joint_trajectory" };
   double publish_period{ 0.034 };

moveit_ros/moveit_servo/src/enforce_limits.cpp

@@ -70,12 +70,15 @@ double getVelocityScalingFactor(const moveit::core::JointModelGroup* joint_model
 }  // namespace
 
 void enforceVelocityLimits(const moveit::core::JointModelGroup* joint_model_group, const double publish_period,
-                           sensor_msgs::msg::JointState& joint_state)
+                           sensor_msgs::msg::JointState& joint_state, const double override_velocity_scaling_factor)
 {
   // Get the velocity scaling factor
   Eigen::VectorXd velocity =
       Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(joint_state.velocity.data(), joint_state.velocity.size());
-  double velocity_scaling_factor = getVelocityScalingFactor(joint_model_group, velocity);
+  double velocity_scaling_factor = override_velocity_scaling_factor;
+  // if the override velocity scaling factor is approximately zero then the user is not overriding the value.
+  if (override_velocity_scaling_factor < 0.01)
+    velocity_scaling_factor = getVelocityScalingFactor(joint_model_group, velocity);
 
   // Take a smaller step if the velocity scaling factor is less than 1
   if (velocity_scaling_factor < 1)

moveit_ros/moveit_servo/src/servo_calcs.cpp

@@ -626,7 +626,8 @@ bool ServoCalcs::internalServoUpdate(Eigen::ArrayXd& delta_theta,
   updated_filters_ = true;
 
   // Enforce SRDF velocity limits
-  enforceVelocityLimits(joint_model_group_, parameters_->publish_period, internal_joint_state_);
+  enforceVelocityLimits(joint_model_group_, parameters_->publish_period, internal_joint_state_,
+                        parameters_->override_velocity_scaling_factor);
 
   // Enforce SRDF position limits, might halt if needed, set prev_vel to 0
   const auto joints_to_halt = enforcePositionLimits(internal_joint_state_);

moveit_ros/moveit_servo/src/servo_parameters.cpp

@@ -135,6 +135,14 @@ void ServoParameters::declare(const std::string& ns,
                                          .description("Max joint angular/linear velocity. Only used for joint "
                                                       "commands on joint_command_in_topic."));
 
+  // Properties of Servo calculations
+  node_parameters->declare_parameter(ns + ".override_velocity_scaling_factor",
+                                     ParameterValue{ parameters.override_velocity_scaling_factor },
+                                     ParameterDescriptorBuilder{}
+                                         .type(PARAMETER_DOUBLE)
+                                         .description("Override constant scalar of how fast the robot should jog."
+                                                      "Valid values are between 0-1.0"));
+
   // Properties of outgoing commands
   node_parameters->declare_parameter(
       ns + ".command_out_topic", ParameterValue{ parameters.command_out_topic },
@@ -265,6 +273,10 @@ ServoParameters ServoParameters::get(const std::string& ns,
   parameters.rotational_scale = node_parameters->get_parameter(ns + ".scale.rotational").as_double();
   parameters.joint_scale = node_parameters->get_parameter(ns + ".scale.joint").as_double();
 
+  // Properties of Servo calculations
+  parameters.override_velocity_scaling_factor =
+      node_parameters->get_parameter(ns + ".override_velocity_scaling_factor").as_double();
+
   // Properties of outgoing commands
   parameters.command_out_topic = node_parameters->get_parameter(ns + ".command_out_topic").as_string();
   parameters.publish_period = node_parameters->get_parameter(ns + ".publish_period").as_double();