Assertion error when using ChainIkSolverVel_wdls with a 5dof robot.

[dignakov]

I'm trying to use the ChainIkSolverVel_wdls solver with a 5-axis robot, and I'm getting this error:

/usr/include/eigen3/Eigen/src/Core/Block.h:146: Eigen::Block<XprType, BlockRows, BlockCols, InnerPanel>::Block(XprType&, Eigen::Index, Eigen::Index, Eigen::Index, Eigen::Index) [with XprType = Eigen::Matrix<double, -1, -1>; int BlockRows = -1; int BlockCols = -1; bool InnerPanel = false; Eigen::Index = long int]: Assertion `startRow >= 0 && blockRows >= 0 && startRow <= xpr.rows() - blockRows && startCol >= 0 && blockCols >= 0 && startCol <= xpr.cols() - blockCols' failed.

I'm loading the robot from a urdf, and I'm able to use other solvers such as ChainIkSolverVel_pinv successfully. I'm also able to use the same code with a 6-axis robot. There is a potentially related issue #77