Singularity even after using bio_ik

[Akumar201]

Hi, I am using MoveitServoing on real arm to track a fiduciary marker, however it stops in between and keeps on complaining about Very close to a singularity, emergency stop . I have already used bio_ik as suggested and also increased it to 100 hard_stop_singularity_threshold: 100.0 # Stop when the condition number hits this

I am using xarm6

Kinematics.yaml

xarm6:
  kinematics_solver: bio_ik/BioIKKinematicsPlugin
  # kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
  # kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin
  kinematics_solver_search_resolution: 0.005
  kinematics_solver_timeout: 0.005
  kinematics_solver_attempts: 3

config.yaml

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

## Properties of outgoing commands
publish_period: 0.1  # 1/Nominal publish rate [seconds]
low_latency_mode: true  # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored)

# 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: false
publish_joint_accelerations: false

## Incoming Joint State properties
low_pass_filter_coeff: 2.0  # Larger --> trust the filtered data more, trust the measurements less.

## MoveIt properties
move_group_name:  xarm6   # rewrite by robot dof
planning_frame: link_base  # The MoveIt planning frame. Often 'base_link' or 'world'

## Other frames
ee_frame_name: link_eef  # The name of the end effector link, used to return the EE pose
robot_link_command_frame:  link_eef  # commands must be given in the frame of a robot link. Usually either the base or end effector

## Stopping behaviour
incoming_command_timeout:  0.2  # Stop servoing if X seconds elapse without a new command
# If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish.
# Important because ROS may drop some messages and we need the robot to halt reliably.
num_outgoing_halt_msgs_to_publish: 4

## Configure handling of singularities and joint limits
lower_singularity_threshold:  17.0  # Start decelerating when the condition number hits this (close to singularity)
hard_stop_singularity_threshold: 100.0 # Stop when the condition number hits this
joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger.

## Topic names
cartesian_command_in_topic: servo_server/delta_twist_cmds  # Topic for incoming Cartesian twist commands
joint_command_in_topic: servo_server/delta_joint_cmds # Topic for incoming joint angle commands
joint_topic: /joint_states
status_topic: servo_server/status # Publish status to this topic
command_out_topic: /xarm/xarm6_traj_controller/command # rewrite by robot dof

## Collision checking for the entire robot body
check_collisions: true # Check collisions?
collision_check_rate: 10.0 # [Hz] Collision-checking can easily bog down a CPU if done too often.
# Two collision check algorithms are available:
# "threshold_distance" begins slowing down when nearer than a specified distance. Good if you want to tune collision thresholds manually.
# "stop_distance" stops if a collision is nearer than the worst-case stopping distance and the distance is decreasing. Requires joint acceleration limits
collision_check_type: threshold_distance
# Parameters for "threshold_distance"-type collision checking
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]
# Parameters for "stop_distance"-type collision checking
collision_distance_safety_factor: 1000.0 # Must be >= 1. A large safety factor is recommended to account for latency
min_allowable_collision_distance: 0.01 # Stop if a collision is closer than this [m]

I am not sure where I am doing wrong. As I have seen people using these techniques to resolve this problem. My goal is to achieve visual servoing.

[AndyZe]

Well, you might increase hard_stop_singularity_threshold to something more than 100. Can you attach a picture of the arm when it's at the singularity? This might be unavoidable if the arm really is at a singularity.

You could also tweak the settings of bio_ik in kinematics.yaml.

This would be advanced functionality but you could also define a custom cost function for bio_ik. I'm sorry I don't know how to do that off the top of my head.

[Akumar201]

Hi @AndyZe thank you so much for your reply. I narrow down the reasons that might be causing it. Let me explain the for future users seeking for the answer, I think I am doing following things wrong

1. When I run the program the distance between the target and end effector was around 15cm to 20cm. So as far as I think moveit servoing is using Cartesian planning under the hood which when taking a straight line approach to reach desirable goal may cause singularity. Any suggestions on the step size you would like to give me ? Like I should define a voxel attached to eef in 3d space such that when the target is near to end effector then it moves to avoid long range of motion.
2. I have modified Pose_tracking_example.cpp to track aruco marker. My code is showing a unusual behavior , when I run it moves the end effector near to marker position in x but stops after few seconds , when I rerun it , it agains move the eef in x litle bit and stops. IT moves like 5 cm then stops next time 3cm then stops. The more close it gets to the goal it is stopping.

 // Get the current EE transform
  geometry_msgs::TransformStamped current_ee_tf;
  tracker.getCommandFrameTransform(current_ee_tf);
  std::cout << current_ee_tf << std::endl;

  // Convert it to a Pose
  geometry_msgs::PoseStamped target_pose;
  target_pose.header.frame_id = current_ee_tf.header.frame_id;
  target_pose.pose.position.x = current_ee_tf.transform.translation.x;
  target_pose.pose.position.y = current_ee_tf.transform.translation.y;
  target_pose.pose.position.z = current_ee_tf.transform.translation.z;
  target_pose.pose.orientation = current_ee_tf.transform.rotation;

  // Modify it a little bit
  target_pose.pose.position.x += 0.01;

  // resetTargetPose() can be used to clear the target pose and wait for a new one, e.g. when moving between multiple
  // waypoints
  tracker.resetTargetPose();

  // Publish target pose
  target_pose.header.stamp = ros::Time::now();
  target_pose_pub.publish(target_pose);
  // Run the pose tracking in a new thread
  std::thread move_to_pose_thread(
      [&tracker, &lin_tol, &rot_tol] { tracker.moveToPose(lin_tol, rot_tol, 0.1 /* target pose timeout */); });

  ros::Rate loop_rate(50);
  while(ros::ok())
  {  std_msgs::Float64MultiArray val = listener.aruco_data;

  if (!val.data.empty()) {

    // Modify the pose target a little bit each cycle
    // This is a dynamic pose target
    target_pose.pose.position.x = current_ee_tf.transform.translation.x;
    std::cout << "Current position is " << target_pose.pose.position.x << std::endl;
    target_pose.pose.position.x += val.data.at(0);
    std::cout << "Tareget position in x is " << val.data.at(0) << std::endl;

    target_pose.header.stamp = ros::Time::now();
    target_pose_pub.publish(target_pose);
    std::cout << "Message has been published" << std::endl;
    loop_rate.sleep();
  }
  else{
    std::cout << "data is not received" << std::endl;
    loop_rate.sleep();
  }}

  // Make sure the tracker is stopped and clean up
  tracker.stopMotion();
  move_to_pose_thread.join();

  return EXIT_SUCCESS;
}

Any suggestions on which way I should go about tweaking the kinematics.yaml as I think going too far in terms of attempts and timeout can bring some latency

kinematics_solver_search_resolution: 0.005 kinematics_solver_timeout: 0.005 kinematics_solver_attempts: 3

I want to follow the aruco marker to perform visual servoing. Any help/ suggestion would be much appreciated.

[AndyZe]

Read up on the available bio_ik parameters here:

https://github.com/TAMS-Group/bio_ik

The ones you posted above seem fine to me.

[Akumar201]

Thank you @AndyZe for your help. Arm is moving now seems like needed to remove one line in the loop

target_pose.pose.position.x = current_ee_tf.transform.translation.x; causing the problem, I am still not sure why but It's fine.

I am closing this issue as it has been solved. If the problem persists, please comment and the issue will be reopened if appropriate.

[manasvinisrini]

Hey I'm trying to use moveit_servo to track an aruco marker as well and it's not working as well as I would want it to.

Do you have any suggestions to help the arm reach the target marker ?

[Akumar201]

@ManasviniSrini , can you please elaborate on where you are getting stuck and what is the error you are getting now?