Error with EigenSolver.h when giving joystick commands.

[ManolisCh]

Hi,

I get an error when I try to give a joystick command to jog in any of the axis. So, first I run the arm drivers which they work fine. Then I run moveit and I make sure it works fine. Then I run the default launch file included in the jog_arm package. Everything seems to work fine until the moment I press any of the buttons/axis of the joystick to actually move the arm. I get this error:

[ INFO] [1528296917.040967380, 110.045000000]: Waiting for first joint msg.
[ INFO] [1528296927.519803045, 120.511000000]: Received first joint msg.
jog_arm_server: /usr/include/eigen3/Eigen/src/Eigenvalues/EigenSolver.h:384: Eigen::EigenSolver<MatrixType>& Eigen::EigenSolver<_MatrixType>::compute(const Eigen::EigenBase<OtherDerived>&, bool) [with InputType = Eigen::Matrix<double, -1, -1>; _MatrixType = Eigen::Matrix<double, -1, -1>]: Assertion `matrix.cols() == matrix.rows()' failed.
[jog_arm_server-3] process has died [pid 10019, exit code -6, cmd /home/manolis/exp1_ws/devel/lib/jog_arm/jog_arm_server __name:=jog_arm_server __log:=/home/manolis/.ros/log/5cd24bfc-6999-11e8-95ee-9cb6d01cdafb/jog_arm_server-3.log].

My setup is ubuntu 16.04, ROS kinetic, xbox joystick and schunk lwa4d arm. The jog_arm settings I use are the following:

jog_arm_server:
  simulation: false # Whether the robot is started in simulation environment
  collision_check: false # Check collisions?
  command_in_topic:  jog_arm_server/delta_jog_cmds #topic for incoming commands from joy_to_twist node towards jog_arm_server
  command_frame:  /world  # TF frame that incoming cmds are given in
  incoming_command_timeout:  5  # Stop jogging if X seconds elapse without a new cmd
  joint_topic:  /arm/joint_states #where does the arm driver publish the joint names/values? Typically 'joint_states'.
  move_group_name:  arm    # Must be a serial chain since the Jacobian is used.
  singularity_threshold:  5.5  # Slow down when the condition number hits this (close to singularity)
  hard_stop_singularity_threshold: 12. # Stop when the condition number hits this
  command_out_topic:  /arm/joint_trajectory_controller/command # name of the outgoing trajectory_msgs::JointTrajectory topic. Your robot driver should be subscribed to this.
  planning_frame:  /world # typically should match the MoveIt! planning frame
  low_pass_filter_coeff:  2.  # Larger --> trust the filtered data more, trust the measurements less.
  publish_period:  0.01  # 1/Nominal publish rate [seconds]
  scale:
    linear:  0.0004  # Max linear velocity. Meters per publish_period. Units is [m/s]
    rotational:  0.0008  # Max angular velocity. Rads per publish_period. Units is [rad/s]
  # Publish boolean warnings to this topic
  warning_topic:  jog_arm_server/warning

thanks in advance for your time!

[AndyZe]

Hi @ManolisCh,

This is probably caused by a 7-DOF arm rather than 6-DOF. I'll have to look into it more. Theoretically, the algorithm should work but I'll have to do some debugging.

[AndyZe]

Can you please zip your files (including the Schunk files) and email them to me? That would save some time. andyz at utexas.edu

[ManolisCh]

Yes the arm is 7-DOF idd. I have send you the files. Thank you very much!

[AndyZe]

I have a simulation running with a Motoman SIA5 - also 7DOF, and it has the same issue. So it's not just you ;) Now, to fix it...

[AndyZe]

I'll push some code later today, but here's the fix in a nutshell. Replace checkConditionNumber() with this:

// Calculate the condition number of the jacobian, to check for singularities
double JogCalcs::checkConditionNumber(const Eigen::MatrixXd& matrix) const
{
  Eigen::JacobiSVD<Eigen::MatrixXd> svd(matrix);

  return svd.singularValues()(0) 
    / svd.singularValues()(svd.singularValues().size()-1);;
}

I was trying to calculate the eigenvalues of a non-square matrix. No can do.

[ManolisCh]

thats great! Many thanks! I will check it out once you have pushed the fix and I will let you know.

[AndyZe]

318ebfb