[moveit servo]: Scara-like robot always giving "Very close to a singularity, emergency stop"

[jintaoChan]

Problem details

I followed the direction moveit servo to setup my scara-like robot.
However the twist command keep giving "Very close to a singularity, emergency stop".

Solutions I've tried

1. Test the panda_arm in tutorial if it works fine. It works.
2. try bio_ik. I tried. It didn't change anything.

Way to reproduce

My moveit_config is upload. My control program is list followed.

1. build the package
2. ros2 launch moveit_config launch.py
3. run the control program

Here the key part of my code

launch file

    servo_params = {
        "moveit_servo": ParameterBuilder("moveit_config")
        .yaml("config/moveit_servo.yaml")
        .to_dict()
    }
    acceleration_filter_update_period = {"update_period": 0.01}
    planning_group_name = {"planning_group_name": "store_manipulator"}
    ld.add_action(
        Node(
            package="moveit_servo",
            executable="servo_node",
            name="servo_node",
            output="screen",
            parameters=[
                servo_params,
                acceleration_filter_update_period,
                planning_group_name,
                moveit_config.robot_description,
                moveit_config.robot_description_semantic,
                moveit_config.robot_description_kinematics,
                moveit_config.joint_limits,
            ],
        )
    )

control program(modified this file)

#include
#include <control_msgs/msg/joint_jog.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <moveit_msgs/srv/servo_command_type.hpp>
#include <rclcpp/rclcpp.hpp>
#include <signal.h>
#include <stdio.h>
#ifndef WIN32
#include <termios.h>
#include <unistd.h>
#else
#include <conio.h>
#endif

// Define used keys
namespace
{
constexpr int8_t KEYCODE_RIGHT = 0x43;
constexpr int8_t KEYCODE_LEFT = 0x44;
constexpr int8_t KEYCODE_UP = 0x41;
constexpr int8_t KEYCODE_DOWN = 0x42;
constexpr int8_t KEYCODE_PERIOD = 0x2E;
constexpr int8_t KEYCODE_SEMICOLON = 0x3B;
constexpr int8_t KEYCODE_1 = 0x31;
constexpr int8_t KEYCODE_2 = 0x32;
constexpr int8_t KEYCODE_3 = 0x33;
constexpr int8_t KEYCODE_4 = 0x34;
constexpr int8_t KEYCODE_5 = 0x35;
constexpr int8_t KEYCODE_6 = 0x36;
constexpr int8_t KEYCODE_7 = 0x37;
constexpr int8_t KEYCODE_Q = 0x71;
constexpr int8_t KEYCODE_R = 0x72;
constexpr int8_t KEYCODE_J = 0x6A;
constexpr int8_t KEYCODE_T = 0x74;
constexpr int8_t KEYCODE_W = 0x77;
constexpr int8_t KEYCODE_E = 0x65;
} // namespace

// Some constants used in the Servo Teleop demo
namespace
{
const std::string TWIST_TOPIC = "/servo_node/delta_twist_cmds";
const std::string JOINT_TOPIC = "/servo_node/delta_joint_cmds";
const size_t ROS_QUEUE_SIZE = 10;
const std::string PLANNING_FRAME_ID = "base_link";
const std::string EE_FRAME_ID = "link5";
} // namespace

// A class for reading the key inputs from the terminal
class KeyboardReader
{
public:
KeyboardReader() : file_descriptor_(0)
{
#ifndef WIN32
// get the console in raw mode
tcgetattr(file_descriptor_, &cooked_);
struct termios raw;
memcpy(&raw, &cooked_, sizeof(struct termios));
raw.c_lflag &= ~(ICANON | ECHO);
// Setting a new line, then end of file
raw.c_cc[VEOL] = 1;
raw.c_cc[VEOF] = 2;
tcsetattr(file_descriptor_, TCSANOW, &raw);
#endif
}
void readOne(char* c)
{
#ifndef WIN32
int rc = read(file_descriptor_, c, 1);
if (rc < 0)
{
throw std::runtime_error("read failed");
}
#else
*c = static_cast(getch());
#endif
}
void shutdown()
{
#ifndef WIN32
tcsetattr(file_descriptor, TCSANOW, &cooked_);
#endif
}

private:
int file_descriptor_;
#ifndef WIN32
struct termios cooked_;
#endif
};

// Converts key-presses to Twist or Jog commands for Servo, in lieu of a controller
class KeyboardServo
{
public:
KeyboardServo();
int keyLoop();

private:
void spin();

rclcpp::Node::SharedPtr nh_;

rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr twist_pub_;
rclcpp::Publisher<control_msgs::msg::JointJog>::SharedPtr joint_pub_;
rclcpp::Client<moveit_msgs::srv::ServoCommandType>::SharedPtr switch_input_;

std::shared_ptr<moveit_msgs::srv::ServoCommandType::Request> request_;
double joint_vel_cmd_;
std::string command_frame_id_;
};

KeyboardServo::KeyboardServo() : joint_vel_cmd_(1.0), command_frame_id_{ PLANNING_FRAME_ID }
{
nh_ = rclcpp::Node::make_shared("servo_keyboard_input");

twist_pub_ = nh_->create_publisher<geometry_msgs::msg::TwistStamped>(TWIST_TOPIC, ROS_QUEUE_SIZE);
joint_pub_ = nh_->create_publisher<control_msgs::msg::JointJog>(JOINT_TOPIC, ROS_QUEUE_SIZE);

// Client for switching input types
switch_input_ = nh_->create_client<moveit_msgs::srv::ServoCommandType>("servo_node/switch_command_type");
}

KeyboardReader input;

void quit(int sig)
{
(void)sig;
input.shutdown();
rclcpp::shutdown();
exit(0);
}

int main(int argc, char** argv)
{
rclcpp::init(argc, argv);
KeyboardServo keyboard_servo;

signal(SIGINT, quit);

int rc = keyboard_servo.keyLoop();
input.shutdown();
rclcpp::shutdown();

return rc;
}

void KeyboardServo::spin()
{
while (rclcpp::ok())
{
rclcpp::spin_some(nh_);
}
}

int KeyboardServo::keyLoop()
{
char c;
bool publish_twist = false;
bool publish_joint = false;

std::thread{ this { return spin(); } }.detach();

puts("Reading from keyboard");
puts("---------------------------");
puts("All commands are in the planning frame");
puts("Use arrow keys and the '.' and ';' keys to Cartesian jog");
puts("Use 1|2|3|4|5|6|7 keys to joint jog. 'r' to reverse the direction of jogging.");
puts("Use 'j' to select joint jog. ");
puts("Use 't' to select twist ");
puts("Use 'w' and 'e' to switch between sending command in planning frame or end effector frame");
puts("'Q' to quit.");

for (;;)
{
// get the next event from the keyboard
try
{
input.readOne(&c);
}
catch (const std::runtime_error&)
{
perror("read():");
return -1;
}

RCLCPP_DEBUG(nh_->get_logger(), "value: 0x%02X\n", c);

// // Create the messages we might publish
auto twist_msg = std::make_unique<geometry_msgs::msg::TwistStamped>();
auto joint_msg = std::make_unique<control_msgs::msg::JointJog>();

joint_msg->joint_names.resize(6);
joint_msg->joint_names = { "joint0", "joint1", "joint2", "joint3", "joint4", "joint5" };

joint_msg->velocities.resize(6);
std::fill(joint_msg->velocities.begin(), joint_msg->velocities.end(), 0.0);
// Use read key-press
switch (c)
{
  case KEYCODE_LEFT:
    RCLCPP_DEBUG(nh_->get_logger(), "LEFT");
    twist_msg->twist.linear.y = -0.01;
    publish_twist = true;
    break;
  case KEYCODE_RIGHT:
    RCLCPP_DEBUG(nh_->get_logger(), "RIGHT");
    twist_msg->twist.linear.y = 0.01;
    publish_twist = true;
    break;
  case KEYCODE_UP:
    RCLCPP_DEBUG(nh_->get_logger(), "UP");
    twist_msg->twist.linear.x = 0.01;
    publish_twist = true;
    break;
  case KEYCODE_DOWN:
    RCLCPP_DEBUG(nh_->get_logger(), "DOWN");
    twist_msg->twist.linear.x = -0.01;
    publish_twist = true;
    break;
  case KEYCODE_PERIOD:
    RCLCPP_DEBUG(nh_->get_logger(), "PERIOD");
    twist_msg->twist.linear.z = -0.01;
    publish_twist = true;
    break;
  case KEYCODE_SEMICOLON:
    RCLCPP_DEBUG(nh_->get_logger(), "SEMICOLON");
    twist_msg->twist.linear.z = 0.01;
    publish_twist = true;
    break;
  case KEYCODE_1:
    RCLCPP_DEBUG(nh_->get_logger(), "1");
    joint_msg->velocities[0] = joint_vel_cmd_;
    publish_joint = true;
    break;
  case KEYCODE_2:
    RCLCPP_DEBUG(nh_->get_logger(), "2");
    joint_msg->velocities[1] = joint_vel_cmd_;
    publish_joint = true;
    break;
  case KEYCODE_3:
    RCLCPP_DEBUG(nh_->get_logger(), "3");
    joint_msg->velocities[2] = joint_vel_cmd_;
    publish_joint = true;
    break;
  case KEYCODE_4:
    RCLCPP_DEBUG(nh_->get_logger(), "4");
    joint_msg->velocities[3] = joint_vel_cmd_;
    publish_joint = true;
    break;
  case KEYCODE_5:
    RCLCPP_DEBUG(nh_->get_logger(), "5");
    joint_msg->velocities[4] = joint_vel_cmd_;
    publish_joint = true;
    break;
  case KEYCODE_6:
    RCLCPP_DEBUG(nh_->get_logger(), "6");
    joint_msg->velocities[5] = joint_vel_cmd_;
    publish_joint = true;
    break;
  case KEYCODE_7:
    RCLCPP_DEBUG(nh_->get_logger(), "7");
    joint_msg->velocities[6] = joint_vel_cmd_;
    publish_joint = true;
    break;
  case KEYCODE_R:
    RCLCPP_DEBUG(nh_->get_logger(), "r");
    joint_vel_cmd_ *= -1;
    break;
  case KEYCODE_J:
    RCLCPP_DEBUG(nh_->get_logger(), "j");
    request_ = std::make_shared<moveit_msgs::srv::ServoCommandType::Request>();
    request_->command_type = moveit_msgs::srv::ServoCommandType::Request::JOINT_JOG;
    if (switch_input_->wait_for_service(std::chrono::seconds(1)))
    {
      auto result = switch_input_->async_send_request(request_);
      if (result.get()->success)
      {
        RCLCPP_INFO_STREAM(nh_->get_logger(), "Switched to input type: JointJog");
      }
      else
      {
        RCLCPP_WARN_STREAM(nh_->get_logger(), "Could not switch input to: JointJog");
      }
    }
    break;
  case KEYCODE_T:
    RCLCPP_DEBUG(nh_->get_logger(), "t");
    request_ = std::make_shared<moveit_msgs::srv::ServoCommandType::Request>();
    request_->command_type = moveit_msgs::srv::ServoCommandType::Request::TWIST;
    if (switch_input_->wait_for_service(std::chrono::seconds(1)))
    {
      auto result = switch_input_->async_send_request(request_);
      if (result.get()->success)
      {
        RCLCPP_INFO_STREAM(nh_->get_logger(), "Switched to input type: Twist");
      }
      else
      {
        RCLCPP_WARN_STREAM(nh_->get_logger(), "Could not switch input to: Twist");
      }
    }
    break;
  case KEYCODE_W:
    RCLCPP_DEBUG(nh_->get_logger(), "w");
    RCLCPP_INFO_STREAM(nh_->get_logger(), "Command frame set to: " << PLANNING_FRAME_ID);
    command_frame_id_ = PLANNING_FRAME_ID;
    break;
  case KEYCODE_E:
    RCLCPP_DEBUG(nh_->get_logger(), "e");
    RCLCPP_INFO_STREAM(nh_->get_logger(), "Command frame set to: " << EE_FRAME_ID);
    command_frame_id_ = EE_FRAME_ID;
    break;
  case KEYCODE_Q:
    RCLCPP_DEBUG(nh_->get_logger(), "quit");
    return 0;
}

// If a key requiring a publish was pressed, publish the message now
if (publish_twist)
{
  twist_msg->header.stamp = nh_->now();
  twist_msg->header.frame_id = command_frame_id_;
  twist_pub_->publish(std::move(twist_msg));
  publish_twist = false;
}
else if (publish_joint)
{
  joint_msg->header.stamp = nh_->now();
  joint_msg->header.frame_id = PLANNING_FRAME_ID;
  joint_pub_->publish(std::move(joint_msg));
  publish_joint = false;
}

}

return 0;
}

servo config yaml

publish_period: 0.01  # 1/Nominal publish rate [seconds]
max_expected_latency: 0.1  # delay between sending a command and the robot executing it [seconds]

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
scale:
  # Scale parameters are only used if command_in_type=="unitless"
  linear:  0.5  # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands.
  rotational:  0.5  # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands.
  # Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic.
  joint: 0.5

# What type of topic does your robot driver expect?
# Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory
command_out_type: trajectory_msgs/JointTrajectory

# What to publish? Can save some bandwidth as most robots only require positions or velocities
publish_joint_positions: true
publish_joint_velocities: true
publish_joint_accelerations: false

## Plugins for smoothing outgoing commands
use_smoothing: true
smoothing_filter_plugin_name: "online_signal_smoothing::AccelerationLimitedPlugin"

# If is_primary_planning_scene_monitor is set to true, the Servo server's PlanningScene advertises the /get_planning_scene service,
# which other nodes can use as a source for information about the planning environment.
# NOTE: If a different node in your system is responsible for the "primary" planning scene instance (e.g. the MoveGroup node),
# then is_primary_planning_scene_monitor needs to be set to false.
is_primary_planning_scene_monitor: true
check_octomap_collisions: false  # Check collision against the octomap (if a 3D sensor plugin is available)

## MoveIt properties
move_group_name: store_manipulator  # Often 'manipulator' or 'arm'

## Configure handling of singularities and joint limits
lower_singularity_threshold: 100.0  # Start decelerating when the condition number hits this (close to singularity)
hard_stop_singularity_threshold: 300.0  # Stop when the condition number hits this
leaving_singularity_threshold_multiplier: 2.0  # Multiply the hard stop limit by this when leaving singularity (see https://github.com/moveit/moveit2/pull/620)
# Added as a buffer to joint variable position bounds [in that joint variable's respective units].
# Can be of size 1, which applies the margin to all joints, or the same size as the number of degrees of freedom of the active joint group.
# If moving quickly, make these values larger.
joint_limit_margins: [0.12]

## Topic names
cartesian_command_in_topic: ~/delta_twist_cmds  # Topic for incoming Cartesian twist commands
joint_command_in_topic: ~/delta_joint_cmds  # Topic for incoming joint angle commands
joint_topic: /joint_states  # Get joint states from this topic
status_topic: ~/status  # Publish status to this topic
command_out_topic: /store_manipulator_controller/joint_trajectory  # Publish outgoing commands here

## Collision checking for the entire robot body
check_collisions: false  # Check collisions?
collision_check_rate: 10.0  # [Hz] Collision-checking can easily bog down a CPU if done too often.
self_collision_proximity_threshold: 0.01  # Start decelerating when a self-collision is this far [m]
scene_collision_proximity_threshold: 0.02  # Start decelerating when a scene collision is this far [m]

A shot of the robot

moveit_config.zip