diff --git a/moveit_ros/moveit_servo/config/panda_simulated_config.yaml b/moveit_ros/moveit_servo/config/panda_simulated_config.yaml index dc3f319245..e65036135a 100644 --- a/moveit_ros/moveit_servo/config/panda_simulated_config.yaml +++ b/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 diff --git a/moveit_ros/moveit_servo/config/panda_simulated_config_pose_tracking.yaml b/moveit_ros/moveit_servo/config/panda_simulated_config_pose_tracking.yaml index 902ec2db13..d0ee8d6b40 100644 --- a/moveit_ros/moveit_servo/config/panda_simulated_config_pose_tracking.yaml +++ b/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 diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h b/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h index 5ff2c5bfd3..60a7c477f7 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h @@ -49,8 +49,6 @@ #include #include #include -#include -#include #include #include #include @@ -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& req, - const std::shared_ptr& res); - - /** \brief Start the main calculation timer */ - // Service callback for changing servoing dimensions - void changeControlDimensions(const std::shared_ptr& req, - const std::shared_ptr& res); - // Pointer to the ROS node std::shared_ptr node_; @@ -288,8 +241,6 @@ class ServoCalcs rclcpp::Publisher::SharedPtr status_pub_; rclcpp::Publisher::SharedPtr trajectory_outgoing_cmd_pub_; rclcpp::Publisher::SharedPtr multiarray_outgoing_cmd_pub_; - rclcpp::Service::SharedPtr control_dimensions_server_; - rclcpp::Service::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 drift_dimensions_ = { { false, false, false, false, false, false } }; - - // The dimensions to control. In the command frame. [x, y, z, roll, pitch, yaw] - std::array 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_; diff --git a/moveit_ros/moveit_servo/src/servo_calcs.cpp b/moveit_ros/moveit_servo/src/servo_calcs.cpp index 58cbbfe0cd..be284ea907 100644 --- a/moveit_ros/moveit_servo/src/servo_calcs.cpp +++ b/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( - "~/change_drift_dimensions", - [this](const std::shared_ptr& req, - const std::shared_ptr& res) { - return changeDriftDimensions(req, res); - }); - - // ROS Server for changing the control dimensions - control_dimensions_server_ = node_->create_service( - "~/change_control_dimensions", - [this](const std::shared_ptr& req, - const std::shared_ptr& res) { - return changeControlDimensions(req, res); - }); - // Subscribe to the collision_check topic collision_velocity_scale_sub_ = node_->create_subscription( "~/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 svd = Eigen::JacobiSVD(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 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& req, - const std::shared_ptr& 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& req, - const std::shared_ptr& 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 diff --git a/moveit_ros/moveit_servo/src/servo_parameters.yaml b/moveit_ros/moveit_servo/src/servo_parameters.yaml index 41c59921e3..57d005cdb7 100644 --- a/moveit_ros/moveit_servo/src/servo_parameters.yaml +++ b/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, diff --git a/moveit_ros/moveit_servo/test/config/servo_settings.yaml b/moveit_ros/moveit_servo/test/config/servo_settings.yaml index 27f7fbded6..007aa8002f 100644 --- a/moveit_ros/moveit_servo/test/config/servo_settings.yaml +++ b/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 diff --git a/moveit_ros/moveit_servo/test/servo_launch_test_common.hpp b/moveit_ros/moveit_servo/test/servo_launch_test_common.hpp index 72eeb20d61..86e260def9 100644 --- a/moveit_ros/moveit_servo/test/servo_launch_test_common.hpp +++ b/moveit_ros/moveit_servo/test/servo_launch_test_common.hpp @@ -51,8 +51,6 @@ #include #include #include -#include -#include // Testing #include @@ -162,40 +160,6 @@ class ServoFixture : public ::testing::Test return true; } - bool setupControlDimsClient() - { - client_change_control_dims_ = node_->create_client( - resolveServoTopicName("~/change_control_dimensions")); - while (!client_change_control_dims_->service_is_ready()) - { - if (!rclcpp::ok()) - { - RCLCPP_ERROR(LOGGER, "Interrupted while waiting for the service. Exiting."); - return false; - } - RCLCPP_INFO(LOGGER, "client_change_control_dims_ service not available, waiting again..."); - rclcpp::sleep_for(std::chrono::milliseconds(500)); - } - return true; - } - - bool setupDriftDimsClient() - { - client_change_drift_dims_ = node_->create_client( - resolveServoTopicName("~/change_drift_dimensions")); - while (!client_change_drift_dims_->service_is_ready()) - { - if (!rclcpp::ok()) - { - RCLCPP_ERROR(LOGGER, "Interrupted while waiting for the service. Exiting."); - return false; - } - RCLCPP_INFO(LOGGER, "client_change_drift_dims_ service not available, waiting again..."); - rclcpp::sleep_for(std::chrono::milliseconds(500)); - } - return true; - } - bool setupCollisionScaleSub() { sub_collision_scale_ = node_->create_subscription( @@ -427,8 +391,6 @@ class ServoFixture : public ::testing::Test // Service Clients rclcpp::Client::SharedPtr client_servo_start_; rclcpp::Client::SharedPtr client_servo_stop_; - rclcpp::Client::SharedPtr client_change_control_dims_; - rclcpp::Client::SharedPtr client_change_drift_dims_; // Publishers rclcpp::Publisher::SharedPtr pub_twist_cmd_;