add current state to command queue if it is empty. This is needed sin…

…ce the time stamp is set in the updateSlidingWindow function now.

Signed-off-by: Paul Gesel <paulgesel@gmail.com>

moveit_ros/moveit_servo/demos/cpp_interface/demo_joint_jog.cpp

@@ -93,8 +93,10 @@ int main(int argc, char* argv[])
   std::chrono::seconds time_elapsed(0);
   auto start_time = std::chrono::steady_clock::now();
 
-  // create command queue to build trajectory message
+  // create command queue to build trajectory message and add current robot state
   std::deque<KinematicState> joint_cmd_rolling_window;
+  KinematicState current_state = servo.getCurrentRobotState();
+  updateSlidingWindow(current_state, joint_cmd_rolling_window, 0.0, demo_node->now());
 
   RCLCPP_INFO_STREAM(demo_node->get_logger(), servo.getStatusMessage());
   while (rclcpp::ok())

moveit_ros/moveit_servo/demos/cpp_interface/demo_pose.cpp

@@ -115,12 +115,12 @@ int main(int argc, char* argv[])
 
   RCLCPP_INFO_STREAM(demo_node->get_logger(), servo.getStatusMessage());
 
-  KinematicState joint_state;
-  KinematicState current_state = servo.getCurrentRobotState();
   rclcpp::WallRate servo_rate(1 / servo_params.publish_period);
 
-  // create command queue to build trajectory message
+  // create command queue to build trajectory message and add current robot state
   std::deque<KinematicState> joint_cmd_rolling_window;
+  KinematicState current_state = servo.getCurrentRobotState();
+  updateSlidingWindow(current_state, joint_cmd_rolling_window, 0.0, demo_node->now());
 
   bool satisfies_linear_tolerance = false;
   bool satisfies_angular_tolerance = false;
@@ -146,7 +146,7 @@ int main(int argc, char* argv[])
     }
 
     // get next servo command
-    joint_state = servo.getNextJointState(robot_state, target_pose);
+    KinematicState joint_state = servo.getNextJointState(robot_state, target_pose);
     StatusCode status = servo.getStatus();
     if (status != StatusCode::INVALID)
     {

moveit_ros/moveit_servo/demos/cpp_interface/demo_twist.cpp

@@ -96,8 +96,10 @@ int main(int argc, char* argv[])
   std::chrono::seconds time_elapsed(0);
   auto start_time = std::chrono::steady_clock::now();
 
-  // create command queue to build trajectory message
+  // create command queue to build trajectory message and add current robot state
   std::deque<KinematicState> joint_cmd_rolling_window;
+  KinematicState current_state = servo.getCurrentRobotState();
+  updateSlidingWindow(current_state, joint_cmd_rolling_window, 0.0, demo_node->now());
 
   RCLCPP_INFO_STREAM(demo_node->get_logger(), servo.getStatusMessage());
   while (rclcpp::ok())

moveit_ros/moveit_servo/include/moveit_servo/utils/common.hpp

@@ -162,17 +162,6 @@ std::pair<double, StatusCode> velocityScalingFactorForSingularity(const moveit::
 double jointLimitVelocityScalingFactor(const Eigen::VectorXd& velocities,
                                        const moveit::core::JointBoundsVector& joint_bounds, double scaling_override);
 
-/**
- * \brief Apply acceleration scaling based on joint limits. If the robot model does not have acceleration limits
- * defined, then a scale factor of 1.0 will be returned.
- * @param accelerations The commanded accelerations.
- * @param joint_bounds The bounding information for the robot joints.
- * @param scaling_override The user defined acceleration scaling override.
- * @return The acceleration scaling factor.
- */
-double jointLimitAccelerationScalingFactor(const Eigen::VectorXd& accelerations,
-                                           const moveit::core::JointBoundsVector& joint_bounds,
-                                           double scaling_override);
 /**
  * \brief Finds the joints that are exceeding allowable position limits.
  * @param positions The joint positions.

moveit_ros/moveit_servo/src/servo.cpp

@@ -521,6 +521,9 @@ KinematicState Servo::getNextJointState(const moveit::core::RobotStatePtr& robot
     // Compute the next joint positions based on the joint position deltas
     target_state.positions = current_state.positions + joint_position_delta;
 
+    // Compute the joint velocities required to reach positions
+    target_state.velocities = joint_position_delta / servo_params_.publish_period;
+
     // Apply smoothing to the positions if a smoother was provided.
     doSmoothing(target_state);
 
@@ -540,19 +543,6 @@ KinematicState Servo::getNextJointState(const moveit::core::RobotStatePtr& robot
     }
     target_state.velocities *= joint_velocity_limit_scale;
 
-    // Scale down the acceleration based on joint acceleration limit or user defined scaling if applicable.
-    Eigen::VectorXd joint_accelerations =
-        (target_state.velocities - current_state.velocities) / (servo_params_.publish_period);
-    const double joint_acceleration_limit_scale =
-        jointLimitAccelerationScalingFactor(joint_accelerations, joint_bounds, 1.0);
-    if (joint_acceleration_limit_scale < 1.0)  // 1.0 means no scaling.
-    {
-      RCLCPP_DEBUG_STREAM(logger_,
-                          "Joint acceleration limit scaling applied by a factor of " << joint_acceleration_limit_scale);
-    }
-    target_state.velocities =
-        current_state.velocities + joint_acceleration_limit_scale * joint_accelerations * servo_params_.publish_period;
-
     // Adjust joint position based on scaled down velocity
     target_state.positions = current_state.positions + (target_state.velocities * servo_params_.publish_period);
 
@@ -676,15 +666,11 @@ std::pair<bool, KinematicState> Servo::smoothHalt(const moveit::core::RobotState
   bool stopped = false;
   auto target_state = halt_state;
 
-  // Get joint bounds from robot state for current move group.
-  const moveit::core::JointModelGroup* joint_model_group =
-      robot_state->getJointModelGroup(servo_params_.move_group_name);
-  const moveit::core::JointBoundsVector joint_bounds = joint_model_group->getActiveJointModelsBounds();
+  // set target velocity
+  target_state.velocities *= 0.0;
 
-  // apply scaling to target velocity based on robot's limits
-  Eigen::VectorXd acceleration = -target_state.velocities / servo_params_.publish_period;
-  const double joint_acceleration_limit_scale = jointLimitAccelerationScalingFactor(acceleration, joint_bounds, 1.0);
-  target_state.velocities += joint_acceleration_limit_scale * acceleration * servo_params_.publish_period;
+  // apply smoothing: this will change target position/velocity to make slow down gradual
+  doSmoothing(target_state);
 
   // scale velocity in case of obstacle
   target_state.velocities *= collision_velocity_scale_;
@@ -698,8 +684,6 @@ std::pair<bool, KinematicState> Servo::smoothHalt(const moveit::core::RobotState
         (target_state.velocities[i] - halt_state.velocities[i]) / servo_params_.publish_period;
   }
 
-  doSmoothing(target_state);
-
   // If all velocities are near zero, robot has decelerated to a stop.
   stopped = (target_state.velocities.cwiseAbs().array() < STOPPED_VELOCITY_EPS).all();
 

moveit_ros/moveit_servo/src/servo_node.cpp

@@ -332,7 +332,9 @@ void ServoNode::servoLoop()
     else
     {
       current_state = servo_->getCurrentRobotState();
+      current_state.velocities *= 0.0;
       joint_cmd_rolling_window_.clear();
+      updateSlidingWindow(current_state, joint_cmd_rolling_window_, 0.0, cur_time);
     }
 
     // Get the robot state and joint model group info.

moveit_ros/moveit_servo/src/utils/common.cpp

@@ -418,22 +418,6 @@ double jointLimitVelocityScalingFactor(const Eigen::VectorXd& velocities,
   return jointLimitScalingFactorCommon(velocities, joint_bounds, scaling_override, calculate_joint_scaling_factor);
 }
 
-double jointLimitAccelerationScalingFactor(const Eigen::VectorXd& accelerations,
-                                           const moveit::core::JointBoundsVector& joint_bounds, double scaling_override)
-{
-  auto calculate_joint_scaling_factor = [](moveit::core::VariableBounds variable_bound, double target_accel) {
-    if (variable_bound.acceleration_bounded_ && target_accel != 0.0)
-    {
-      // Find the ratio of clamped acceleration to original acceleration
-      const auto bounded_vel =
-          std::clamp(target_accel, variable_bound.min_acceleration_, variable_bound.max_acceleration_);
-      return bounded_vel / target_accel;
-    }
-    return 1.0;
-  };
-  return jointLimitScalingFactorCommon(accelerations, joint_bounds, scaling_override, calculate_joint_scaling_factor);
-}
-
 std::vector<int> jointsToHalt(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities,
                               const moveit::core::JointBoundsVector& joint_bounds, const std::vector<double>& margins)
 {

moveit_ros/moveit_servo/tests/test_utils.cpp

@@ -95,60 +95,6 @@ TEST(ServoUtilsUnitTests, JointLimitVelocityScaling)
   ASSERT_NEAR(scaling_factor, 1.0 / 1.1, tol);
 }
 
-TEST(ServoUtilsUnitTests, JointLimitAccelerationScaling)
-{
-  using moveit::core::loadTestingRobotModel;
-  moveit::core::RobotModelPtr robot_model = loadTestingRobotModel("panda");
-  const auto joint_model_group = robot_model->getJointModelGroup("panda_arm");
-  auto const_joint_bounds = joint_model_group->getActiveJointModelsBounds();
-
-  // Get the upper bound for the accelerations of each joint.
-  Eigen::VectorXd incoming_accelerations(const_joint_bounds.size());
-
-  // The URDF does not have accelerations, so add values manually
-  incoming_accelerations << 3.75, 1.875, 2.5, 3.125, 3.75, 5.0, 5.0;
-
-  // Build joint_bounds with acceleration data
-  std::vector<const moveit::core::JointModel::Bounds*> joint_bounds;
-  std::vector<moveit::core::JointModel::Bounds> bounds_vector;
-  bounds_vector.resize(const_joint_bounds.size());
-  for (size_t i = 0; i < const_joint_bounds.size(); i++)
-  {
-    moveit::core::JointModel::Bounds* b = &bounds_vector[i];
-    *b = *const_joint_bounds[i];
-    (*b)[0].acceleration_bounded_ = true;
-    (*b)[0].max_acceleration_ = incoming_accelerations(i);
-    joint_bounds.emplace_back(b);
-  }
-
-  // Create incoming accelerations with only joint 1 and joint 2 over limit by a factor of 0.1 and 0.05
-  // Scale down all other joint accelerations by 0.3 to keep it within limits.
-  incoming_accelerations(0) *= 1.1;
-  incoming_accelerations(1) *= 1.05;
-  incoming_accelerations.tail<5>() *= 0.7;
-
-  constexpr double tol = 0.001;
-
-  // The resulting scaling factor from joints should be 1 / 1.1 = 0.90909
-  double user_acceleration_override = 0.0;
-  double scaling_factor = moveit_servo::jointLimitAccelerationScalingFactor(incoming_accelerations, joint_bounds,
-                                                                            user_acceleration_override);
-  ASSERT_NEAR(scaling_factor, 1.0 / 1.1, tol);
-
-  // With a scaling override lower than the joint limit scaling, it should use the override value.
-  user_acceleration_override = 0.5;
-  scaling_factor = moveit_servo::jointLimitAccelerationScalingFactor(incoming_accelerations, joint_bounds,
-                                                                     user_acceleration_override);
-  ASSERT_NEAR(scaling_factor, 0.5, tol);
-
-  // With a scaling override higher than the joint limit scaling, it should still use the joint limits.
-  // Safety always first!
-  user_acceleration_override = 1.0;
-  scaling_factor = moveit_servo::jointLimitAccelerationScalingFactor(incoming_accelerations, joint_bounds,
-                                                                     user_acceleration_override);
-  ASSERT_NEAR(scaling_factor, 1.0 / 1.1, tol);
-}
-
 TEST(ServoUtilsUnitTests, validVector)
 {
   Eigen::VectorXd valid_vector(7);