Merge branch 'main' into dynamic-servo-collision-params

moveit_py/moveit/core/collision_detection.pyi

@@ -2,6 +2,7 @@ from typing import Any
 
 class AllowedCollisionMatrix:
     def __init__(self, *args, **kwargs) -> None: ...
+    def clear(self, *args, **kwargs) -> Any: ...
     def get_entry(self, *args, **kwargs) -> Any: ...
     def remove_entry(self, *args, **kwargs) -> Any: ...
     def set_entry(self, *args, **kwargs) -> Any: ...

moveit_py/moveit/planning.pyi

@@ -34,6 +34,7 @@ class PlanRequestParameters:
 
 class PlanningComponent:
     def __init__(self, *args, **kwargs) -> None: ...
+    def get_named_target_state_values(self, *args, **kwargs) -> Any: ...
     def get_start_state(self, *args, **kwargs) -> Any: ...
     def plan(self, *args, **kwargs) -> Any: ...
     def set_goal_state(self, *args, **kwargs) -> Any: ...
@@ -43,8 +44,6 @@ class PlanningComponent:
     def set_workspace(self, *args, **kwargs) -> Any: ...
     def unset_workspace(self, *args, **kwargs) -> Any: ...
     @property
-    def named_target_state_values(self) -> Any: ...
-    @property
     def named_target_states(self) -> Any: ...
     @property
     def planning_group_name(self) -> Any: ...

moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.cpp

@@ -261,6 +261,11 @@ void init_planning_component(py::module& m)
                 moveit_py_instance (:py:class:`moveit_py.core.MoveItPy`): The MoveItPy instance to use.
         )")
 
+      .def("get_named_target_state_values", &moveit_cpp::PlanningComponent::getNamedTargetStateValues, py::arg("name"),
+           R"(
+           dict: The joint values for targets specified by name.
+           )")
+
       .def_property("planning_group_name", &moveit_cpp::PlanningComponent::getPlanningGroupName, nullptr,
                     R"(
                     str: The name of the planning group to plan for.
@@ -271,13 +276,6 @@ void init_planning_component(py::module& m)
                     list of str: The names of the named robot states available as targets.
                     )")
 
-      // TODO (peterdavidfagan): write test case for this method.
-      .def_property("named_target_state_values", &moveit_cpp::PlanningComponent::getNamedTargetStateValues, nullptr,
-                    py::return_value_policy::move,
-                    R"(
-                    dict: The joint values for targets specified by name.
-                    )")
-
       // start state methods
       .def("set_start_state_to_current_state", &moveit_cpp::PlanningComponent::setStartStateToCurrentState,
            R"(

moveit_ros/moveit_servo/config/panda_simulated_config.yaml

@@ -1,7 +1,6 @@
 ###############################################
 # Modify all parameters related to servoing here
 ###############################################
-use_gazebo: false # Whether the robot is started in a Gazebo simulation environment
 
 ## Properties of incoming commands
 command_in_type: "unitless" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s

moveit_ros/moveit_servo/config/panda_simulated_config_pose_tracking.yaml

@@ -1,7 +1,6 @@
 ###############################################
 # Modify all parameters related to servoing here
 ###############################################
-use_gazebo: false # Whether the robot is started in a Gazebo simulation environment
 
 ## Properties of incoming commands
 command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s

moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h

@@ -49,8 +49,6 @@
 #include <geometry_msgs/msg/twist_stamped.hpp>
 #include <moveit/planning_scene_monitor/planning_scene_monitor.h>
 #include <moveit/robot_model_loader/robot_model_loader.h>
-#include <moveit_msgs/srv/change_drift_dimensions.hpp>
-#include <moveit_msgs/srv/change_control_dimensions.hpp>
 #include <pluginlib/class_loader.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/joint_state.hpp>
@@ -193,56 +191,11 @@ class ServoCalcs
   bool internalServoUpdate(Eigen::ArrayXd& delta_theta, trajectory_msgs::msg::JointTrajectory& joint_trajectory,
                            const ServoType servo_type);
 
-  /** \brief Gazebo simulations have very strict message timestamp requirements.
-   * Satisfy Gazebo by stuffing multiple messages into one.
-   */
-  void insertRedundantPointsIntoTrajectory(trajectory_msgs::msg::JointTrajectory& joint_trajectory, int count) const;
-
-  /**
-   * Remove the Jacobian row and the delta-x element of one Cartesian dimension, to take advantage of task redundancy
-   *
-   * @param matrix The Jacobian matrix.
-   * @param delta_x Vector of Cartesian delta commands, should be the same size as matrix.rows()
-   * @param row_to_remove Dimension that will be allowed to drift, e.g. row_to_remove = 2 allows z-translation drift.
-   */
-  void removeDimension(Eigen::MatrixXd& matrix, Eigen::VectorXd& delta_x, unsigned int row_to_remove) const;
-
-  /**
-   * Removes all of the drift dimensions from the jacobian and delta-x element
-   *
-   * @param matrix The Jacobian matrix.
-   * @param delta_x Vector of Cartesian delta commands, should be the same size as matrix.rows()
-   */
-  void removeDriftDimensions(Eigen::MatrixXd& matrix, Eigen::VectorXd& delta_x);
-
-  /**
-   * Uses control_dimensions_ to set the incoming twist command values to 0 in uncontrolled directions
-   *
-   * @param command TwistStamped msg being used in the Cartesian calcs process
-   */
-  void enforceControlDimensions(geometry_msgs::msg::TwistStamped& command);
-
   /* \brief Command callbacks */
   void twistStampedCB(const geometry_msgs::msg::TwistStamped::ConstSharedPtr& msg);
   void jointCmdCB(const control_msgs::msg::JointJog::ConstSharedPtr& msg);
   void collisionVelocityScaleCB(const std_msgs::msg::Float64::ConstSharedPtr& msg);
 
-  /**
-   * Allow drift in certain dimensions. For example, may allow the wrist to rotate freely.
-   * This can help avoid singularities.
-   *
-   * @param request the service request
-   * @param response the service response
-   * @return true if the adjustment was made
-   */
-  void changeDriftDimensions(const std::shared_ptr<moveit_msgs::srv::ChangeDriftDimensions::Request>& req,
-                             const std::shared_ptr<moveit_msgs::srv::ChangeDriftDimensions::Response>& res);
-
-  /** \brief Start the main calculation timer */
-  // Service callback for changing servoing dimensions
-  void changeControlDimensions(const std::shared_ptr<moveit_msgs::srv::ChangeControlDimensions::Request>& req,
-                               const std::shared_ptr<moveit_msgs::srv::ChangeControlDimensions::Response>& res);
-
   // Pointer to the ROS node
   std::shared_ptr<rclcpp::Node> node_;
 
@@ -288,8 +241,6 @@ class ServoCalcs
   rclcpp::Publisher<std_msgs::msg::Int8>::SharedPtr status_pub_;
   rclcpp::Publisher<trajectory_msgs::msg::JointTrajectory>::SharedPtr trajectory_outgoing_cmd_pub_;
   rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr multiarray_outgoing_cmd_pub_;
-  rclcpp::Service<moveit_msgs::srv::ChangeControlDimensions>::SharedPtr control_dimensions_server_;
-  rclcpp::Service<moveit_msgs::srv::ChangeDriftDimensions>::SharedPtr drift_dimensions_server_;
 
   // Main tracking / result publisher loop
   std::thread thread_;
@@ -304,16 +255,8 @@ class ServoCalcs
   // Use ArrayXd type to enable more coefficient-wise operations
   Eigen::ArrayXd delta_theta_;
 
-  const int gazebo_redundant_message_count_ = 30;
-
   unsigned int num_joints_;
 
-  // True -> allow drift in this dimension. In the command frame. [x, y, z, roll, pitch, yaw]
-  std::array<bool, 6> drift_dimensions_ = { { false, false, false, false, false, false } };
-
-  // The dimensions to control. In the command frame. [x, y, z, roll, pitch, yaw]
-  std::array<bool, 6> control_dimensions_ = { { true, true, true, true, true, true } };
-
   // main_loop_mutex_ is used to protect the input state and dynamic parameters
   mutable std::mutex main_loop_mutex_;
   Eigen::Isometry3d tf_moveit_to_robot_cmd_frame_;

moveit_ros/moveit_servo/src/servo_calcs.cpp

@@ -101,22 +101,6 @@ ServoCalcs::ServoCalcs(const rclcpp::Node::SharedPtr& node,
       servo_params_.joint_command_in_topic, rclcpp::SystemDefaultsQoS(),
       [this](const control_msgs::msg::JointJog::ConstSharedPtr& msg) { return jointCmdCB(msg); });
 
-  // ROS Server for allowing drift in some dimensions
-  drift_dimensions_server_ = node_->create_service<moveit_msgs::srv::ChangeDriftDimensions>(
-      "~/change_drift_dimensions",
-      [this](const std::shared_ptr<moveit_msgs::srv::ChangeDriftDimensions::Request>& req,
-             const std::shared_ptr<moveit_msgs::srv::ChangeDriftDimensions::Response>& res) {
-        return changeDriftDimensions(req, res);
-      });
-
-  // ROS Server for changing the control dimensions
-  control_dimensions_server_ = node_->create_service<moveit_msgs::srv::ChangeControlDimensions>(
-      "~/change_control_dimensions",
-      [this](const std::shared_ptr<moveit_msgs::srv::ChangeControlDimensions::Request>& req,
-             const std::shared_ptr<moveit_msgs::srv::ChangeControlDimensions::Response>& res) {
-        return changeControlDimensions(req, res);
-      });
-
   // Subscribe to the collision_check topic
   collision_velocity_scale_sub_ = node_->create_subscription<std_msgs::msg::Float64>(
       "~/collision_velocity_scale", rclcpp::SystemDefaultsQoS(),
@@ -520,9 +504,6 @@ bool ServoCalcs::cartesianServoCalcs(geometry_msgs::msg::TwistStamped& cmd,
   if (!checkValidCommand(cmd))
     return false;
 
-  // Set uncontrolled dimensions to 0 in command frame
-  enforceControlDimensions(cmd);
-
   // Transform the command to the MoveGroup planning frame
   if (cmd.header.frame_id != servo_params_.planning_frame)
   {
@@ -533,8 +514,6 @@ bool ServoCalcs::cartesianServoCalcs(geometry_msgs::msg::TwistStamped& cmd,
 
   Eigen::MatrixXd jacobian = current_state_->getJacobian(joint_model_group_);
 
-  removeDriftDimensions(jacobian, delta_x);
-
   Eigen::JacobiSVD<Eigen::MatrixXd> svd =
       Eigen::JacobiSVD<Eigen::MatrixXd>(jacobian, Eigen::ComputeThinU | Eigen::ComputeThinV);
   Eigen::MatrixXd matrix_s = svd.singularValues().asDiagonal();
@@ -657,34 +636,10 @@ bool ServoCalcs::internalServoUpdate(Eigen::ArrayXd& delta_theta,
   // compose outgoing message
   composeJointTrajMessage(next_joint_state_, joint_trajectory);
 
-  // Modify the output message if we are using gazebo
-  if (servo_params_.use_gazebo)
-  {
-    insertRedundantPointsIntoTrajectory(joint_trajectory, gazebo_redundant_message_count_);
-  }
-
   previous_joint_state_ = current_joint_state_;
   return true;
 }
 
-// Spam several redundant points into the trajectory. The first few may be skipped if the
-// time stamp is in the past when it reaches the client. Needed for gazebo simulation.
-// Start from 1 because the first point's timestamp is already 1*servo_parameters_.publish_period
-void ServoCalcs::insertRedundantPointsIntoTrajectory(trajectory_msgs::msg::JointTrajectory& joint_trajectory,
-                                                     int count) const
-{
-  if (count < 2)
-    return;
-  joint_trajectory.points.resize(count);
-  auto point = joint_trajectory.points[0];
-  // Start from 1 because we already have the first point. End at count+1 so (total #) == count
-  for (int i = 1; i < count; ++i)
-  {
-    point.time_from_start = rclcpp::Duration::from_seconds((i + 1) * servo_params_.publish_period);
-    joint_trajectory.points[i] = point;
-  }
-}
-
 void ServoCalcs::resetLowPassFilters(const sensor_msgs::msg::JointState& joint_state)
 {
   smoother_->reset(joint_state.position);
@@ -962,54 +917,6 @@ Eigen::VectorXd ServoCalcs::scaleJointCommand(const control_msgs::msg::JointJog&
   return result;
 }
 
-void ServoCalcs::removeDimension(Eigen::MatrixXd& jacobian, Eigen::VectorXd& delta_x, unsigned int row_to_remove) const
-{
-  unsigned int num_rows = jacobian.rows() - 1;
-  unsigned int num_cols = jacobian.cols();
-
-  if (row_to_remove < num_rows)
-  {
-    jacobian.block(row_to_remove, 0, num_rows - row_to_remove, num_cols) =
-        jacobian.block(row_to_remove + 1, 0, num_rows - row_to_remove, num_cols);
-    delta_x.segment(row_to_remove, num_rows - row_to_remove) =
-        delta_x.segment(row_to_remove + 1, num_rows - row_to_remove);
-  }
-  jacobian.conservativeResize(num_rows, num_cols);
-  delta_x.conservativeResize(num_rows);
-}
-
-void ServoCalcs::removeDriftDimensions(Eigen::MatrixXd& matrix, Eigen::VectorXd& delta_x)
-{
-  // May allow some dimensions to drift, based on drift_dimensions
-  // i.e. take advantage of task redundancy.
-  // Remove the Jacobian rows corresponding to True in the vector drift_dimensions
-  // Work backwards through the 6-vector so indices don't get out of order
-  for (auto dimension = matrix.rows() - 1; dimension >= 0; --dimension)
-  {
-    if (drift_dimensions_[dimension] && matrix.rows() > 1)
-    {
-      removeDimension(matrix, delta_x, dimension);
-    }
-  }
-}
-
-void ServoCalcs::enforceControlDimensions(geometry_msgs::msg::TwistStamped& command)
-{
-  // Can't loop through the message, so check them all
-  if (!control_dimensions_[0])
-    command.twist.linear.x = 0;
-  if (!control_dimensions_[1])
-    command.twist.linear.y = 0;
-  if (!control_dimensions_[2])
-    command.twist.linear.z = 0;
-  if (!control_dimensions_[3])
-    command.twist.angular.x = 0;
-  if (!control_dimensions_[4])
-    command.twist.angular.y = 0;
-  if (!control_dimensions_[5])
-    command.twist.angular.z = 0;
-}
-
 bool ServoCalcs::getCommandFrameTransform(Eigen::Isometry3d& transform)
 {
   const std::lock_guard<std::mutex> lock(main_loop_mutex_);
@@ -1087,29 +994,4 @@ void ServoCalcs::collisionVelocityScaleCB(const std_msgs::msg::Float64::ConstSha
   collision_velocity_scale_ = msg->data;
 }
 
-void ServoCalcs::changeDriftDimensions(const std::shared_ptr<moveit_msgs::srv::ChangeDriftDimensions::Request>& req,
-                                       const std::shared_ptr<moveit_msgs::srv::ChangeDriftDimensions::Response>& res)
-{
-  drift_dimensions_[0] = req->drift_x_translation;
-  drift_dimensions_[1] = req->drift_y_translation;
-  drift_dimensions_[2] = req->drift_z_translation;
-  drift_dimensions_[3] = req->drift_x_rotation;
-  drift_dimensions_[4] = req->drift_y_rotation;
-  drift_dimensions_[5] = req->drift_z_rotation;
-
-  res->success = true;
-}
-
-void ServoCalcs::changeControlDimensions(const std::shared_ptr<moveit_msgs::srv::ChangeControlDimensions::Request>& req,
-                                         const std::shared_ptr<moveit_msgs::srv::ChangeControlDimensions::Response>& res)
-{
-  control_dimensions_[0] = req->control_x_translation;
-  control_dimensions_[1] = req->control_y_translation;
-  control_dimensions_[2] = req->control_z_translation;
-  control_dimensions_[3] = req->control_x_rotation;
-  control_dimensions_[4] = req->control_y_rotation;
-  control_dimensions_[5] = req->control_z_rotation;
-
-  res->success = true;
-}
 }  // namespace moveit_servo

moveit_ros/moveit_servo/src/servo_parameters.yaml

@@ -96,12 +96,6 @@ servo:
                   2. robot_link_command_frame"
   }
 
-  use_gazebo: {
-    type: bool,
-    default_value: false,
-    description: "Whether the robot is started in a Gazebo simulation environment"
-  }
-
   linear_scale: {
     type: double,
     default_value: 0.4,

moveit_ros/moveit_servo/test/config/servo_settings.yaml

@@ -1,7 +1,6 @@
 ###############################################
 # Modify all parameters related to servoing here
 ###############################################
-use_gazebo: false # Whether the robot is started in a Gazebo simulation environment
 
 ## Properties of incoming commands
 command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s