added a gain parameter for position control for UR5e

Author: sidtalia

moveit_ros/moveit_servo/config/servo_parameters.yaml

@@ -340,3 +340,14 @@ servo:
       bounds<>: [0.0, 1.0]
     }
   }
+
+  position_control_gain: {
+    type: double,
+    default_value: 1.0,
+    description: "Gain multiplier applied to joint position deltas before publishing. \
+                  Useful when commanding a forward_position_controller where larger steps \
+                  are needed to achieve the desired Cartesian velocity.",
+    validation: {
+      gt<>: 0.0
+    }
+  }

moveit_ros/moveit_servo/src/servo.cpp

@@ -463,7 +463,10 @@ Eigen::VectorXd Servo::jointDeltaFromCommand(const ServoInput& command, const mo
     servo_status_ = StatusCode::INVALID;
     RCLCPP_WARN_STREAM(logger_, "Incoming servo command type does not match known command types.");
   }
+  // if we are in forward_position_controller, multiply the joint_position_deltas by 4
 
+  joint_position_deltas *= servo_params_.position_control_gain; 
+  
   return joint_position_deltas;
 }
 
@@ -521,7 +524,7 @@ KinematicState Servo::getNextJointState(const moveit::core::RobotStatePtr& robot
     target_state.velocities *= joint_velocity_limit_scale;
 
     // Adjust joint position based on scaled down velocity
-    target_state.positions = current_state.positions + (target_state.velocities * servo_params_.publish_period) * 20;
+    target_state.positions = current_state.positions + (target_state.velocities * servo_params_.publish_period);
 
     // Apply collision scaling to the joint position delta
     target_state.positions =

moveit_ros/moveit_servo/src/servo_node.cpp

@@ -367,7 +367,7 @@ void ServoNode::servoLoop()
         // if all joint_cmd_rolling_window_ is empty or all commands in it are outdated, use current robot state
         joint_cmd_rolling_window_.clear();
         current_state = servo_->getCurrentRobotState(false /* block for current robot state */);
-        current_state.velocities *= 0.0;
+        // current_state.velocities *= 0.0;
       }
 
       // update robot state values
@@ -376,7 +376,7 @@ void ServoNode::servoLoop()
 
       next_joint_state = std::nullopt;
       const CommandType expected_type = servo_->getCommandType();
-
+      
       if (expected_type == CommandType::JOINT_JOG && new_joint_jog_msg_)
       {
         next_joint_state = processJointJogCommand(robot_state);