Is this a singularity or incorrect implementation of inverse kinematics?

Question

I at moment trying to compute the Q configuration that moves my robot from it current state described by this transformation matrix. with rotation

0.00549713  0.842013  -0.539429  
0.999983 -0.00362229 0.00453632 
0.00186567 -0.539445 -0.842019
                    

and position as this:

-0.0882761
-0.255069
 0.183645

To this rotatation

    0  0.942755 -0.333487
    1         0         0
    0 -0.333487 -0.942755

and this position

8.66654
19.809
115.771

Due to the drastic change in the Z direction, I thought i could split the path between start and end into small chunks by creating data inbetween by interpolating, and compute the inverse kinematics for each of these small position. Problem is that the output i am getting is pretty large.. Which making me suspect that some of the output might be wrong. The simulation i am using constrains the rotation to 360 degrees.. I think something goes wrong..

The only reason I could think would do this, would be if the jacobian i am was using had singularities... Which why i assumed that i was running into singualarity issue..

setQ{22.395444, 319.402231, 90.548314, -228.989836, -295.921218, -336.808799}
setQ{8.209388, 362.472468, 108.618073, -232.346755, -299.935844, -334.929518}
setQ{8.479842, 399.892521, 127.432982, -234.017882, -303.852583, -335.063821}
setQ{8.224516, 362.232497, 108.666778, -232.319554, -299.899932, -334.928688}
setQ{7.718908, 286.832458, 71.150606, -228.913831, -291.982659, -334.658147}
setQ{7.468625, 249.092444, 52.400638, -227.206436, -288.018036, -334.522738}
setQ{7.220023, 211.325766, 33.656081, -225.496018, -284.049424, -334.387237}
setQ{-6.134091, -2538.260148, -1283.375216, -96.331289, 7.920957, -324.531125}
setQ{-6.261661, -2577.946595, -1301.730132, -94.403263, 12.176863, -324.388990}
setQ{-6.634286, -2697.165915, -1356.762411, -88.601053, 24.968521, -323.962029}
setQ{-6.991781, -2816.625206, -1411.745985, -82.771641, 37.796090, -323.534239}
setQ{-7.334148, -2936.324468, -1466.680853, -76.915029, 50.659572, -323.105620}
setQ{-7.661386, -3056.263702, -1521.567017, -71.031215, 63.558965, -322.676171}
setQ{-8.642914, -3457.794271, -1704.169136, -51.222052, 106.816303, -321.238686}
setQ{-8.988457, -3619.153075, -1777.058457, -43.213761, 124.230964, -320.661112}
setQ{-9.382564, -3821.451508, -1868.048346, -33.135395, 146.089069, -319.937071}
setQ{-9.528439, -3902.557525, -1904.406419, -29.082892, 154.860242, -319.646810}
setQ{-9.667591, -3983.770196, -1940.742846, -25.018300, 163.647376, -319.356179}
setQ{-9.734645, -4024.416527, -1958.902942, -22.981471, 168.046928, -319.210726}
setQ{-9.986053, -4187.268484, -2031.489209, -14.803929, 185.685040, -318.627992}
setQ{-10.210564, -4350.547057, -2103.988889, -6.578030, 203.386994, -318.043783}
setQ{-10.312734, -4432.346324, -2140.206259, -2.446947, 212.261912, -317.751125}
setQ{-10.453381, -4555.245201, -2194.491727, 3.772345, 225.604215, -317.311448}
setQ{-10.496902, -4596.264820, -2212.576060, 5.851488, 230.059630, -317.164705}
setQ{-10.538741, -4637.311102, -2230.654980, 7.933652, 234.519035, -317.017869}
setQ{-10.617377, -4719.483658, -2266.796587, 12.107048, 243.449816, -316.723922}
setQ{-10.812941, -4966.641247, -2375.091527, 24.699772, 270.337923, -315.839868}
setQ{-10.839651, -5007.927501, -2393.121742, 26.809138, 274.833240, -315.692203}
setQ{-10.888029, -5090.579998, -2429.165939, 31.036936, 283.835844, -315.396596}

setQ is just a function for my simulation, the numbers are the actual Q values starting from 0 - 5. (I am using a 6 jointed robot (UR5))

Update

I am using a sphere to compute my desired transformation matrix.. The idea is that i want my arm be on this sphere, point inward to the center.

std::vector<Transform3D<>> pathPlanning::sphere(double dx, double dy, double dz)
{
    double r = 5.0; // Radius of the sphere -  set to 5.0 cm (TODO: has to be checked if that also is accurate)
    cout << "Create a sphere" << endl;

    double current_x = this->device->baseTframe(this->toolFrame,this->state).P()[0];
    double current_y = this->device->baseTframe(this->toolFrame,this->state).P()[1];
    double current_z = this->device->baseTframe(this->toolFrame,this->state).P()[2];


    // Formula for sphere (x-x0)²+(y-y0)²+(z-z0)²=r²
    // x: x = x_0 + rcos(theta)sin(phi)
    // y: y = y_0 + rsin(theta)sin(phi)
    // z: z = z_0 + rcos(phi)
    // Angle range: 0 <= theta <= 2M_PI ; 0 <= phi <= M_PI

    double obj_x = current_x + dx;
    double obj_y = current_y + dy;
    double obj_z = current_z + dz;

    std::vector<Transform3D<>> out;

    int count = 32;

    for(double azimuthal = 0; azimuthal <= M_PI ; azimuthal+=0.01 )
    {

        for(double polar = 0.35; polar <= M_PI-0.35 ; polar+=0.01 )
        {

            double sphere_x = obj_x + r*cos(azimuthal)*sin(polar);
            double sphere_y = obj_y + r*sin(azimuthal)*sin(polar);
            double sphere_z = obj_z + + r*cos(polar);

            //string text = to_string(sphere_x) + " , " + to_string(sphere_y)+ " , " + to_string(sphere_z);
            //positions << text << endl;

            Transform3D<> transformation_matrix = transform(obj_x,obj_y,obj_z,sphere_x,sphere_y,sphere_z);

            if(0.1<(transformation_matrix.P()[0] - current_x) || 0.1<(transformation_matrix.P()[1] - current_y) || 0.1<(transformation_matrix.P()[2] - current_z))
            {
                cout << "Interpolate: " << endl;
                
                std::vector<Transform3D<>> transformation_i = invKin_LargeDisplacement(transformation_matrix);
                out.insert(out.end(),transformation_i.begin(),transformation_i.end());
                cout << out.size() << endl;
                cout << "only returning one interpolation onto the sphere!" << endl;

                return transformation_i;
            }
            else
            {
                cout << "OK" << endl;
                out.push_back(transformation_matrix);

            }
            

            if(count == 32) //TODO: Why...... is this occuring?
            {
                //cout << "Theta: " << theta << " Phi: " << phi << endl;
                //cout << sphere_x << " , " << sphere_y <<" , "<< sphere_z << endl;
                count = 0;
            }
            else
            {
                count++;
            }
        }
    }

    return out;
}

This function provides me with the point on the sphere, which is use to create my rotation matrix using transform.

Transform3D<> pathPlanning::transform(double obj_x, double obj_y, double obj_z, double sphere_x, double sphere_y ,double sphere_z)
{
    // Z-axis should be oriented towards the object.
    // Rot consist of 3 direction vector [x,y,z] which describes how the axis should be oriented in the world space.
    // Looking at the simulation the z-axis is the camera out. X, and Y describes the orientation of the camera.
    // The vector are only for direction purposes, so they have to be normalized....
    // TODO: case [0  0 -1]... Why is it happening at what can be done to undo it?
    cout << "inside Transform" << endl;
    cout << obj_x << "," << sphere_x << " ; "  << obj_y << " , " << sphere_y  <<" ; "<< obj_z << " , " << sphere_z  << endl;
    Vector3D<> dir_z((obj_x - sphere_x), (obj_y - sphere_y), (obj_z - sphere_z));
    //Vector3D<> dir_z((sphere_x-obj_x), (sphere_y - obj_y), (sphere_z-obj_z));
    dir_z = normalize(dir_z);
    Vector3D<> downPlane(0.0,0.0,-1.0);
    Vector3D<> dir_x = cross(downPlane,dir_z);
    dir_x = normalize(dir_x);
    Vector3D<> dir_y = cross(dir_z,dir_x);
    dir_y = normalize(dir_y);
    Rotation3D<> rot_out (dir_x,dir_y,dir_z);  // [x y z]

    Vector3D<> pos_out(sphere_x,sphere_y,sphere_z);

    Transform3D<> out(pos_out,rot_out);
    cout << "desired: " << out << endl;

    return out;
}

The transform basically computes the rotation matrix. The math is based on the on this post by @Ben, which is an answer to a similar problem i am having..

Update

Error with the rotation matrix was due to the polar coordinate being 0 => sin(0) = 0.

I made this plot displaying the determinant of the jacobian, while i compute the inverse kinematics for the large displacement. For each inverse kinematics iteration, I set the robot to the new q_i and use that as current and continue computing until i reach the end configuration.

It seems that alot of them goes toward a singularity or in general a pretty low number..

Update

Again i think the singularities might be the culprit here..

determinant: 0.0424284
Q{13.0099, -46.6613, -18.9411, 2.38865, 5.39454, -4.53456}
determinant: -0.0150253
Q{47.1089, -0.790356, 6.89939, -2.725, -1.66168, 11.2271}
determinant: -0.0368926
Q{15.7475, 8.89658, 7.78122, -2.74134, -5.32446, 1.11023}
determinant: -0.0596228
Q{180.884, 66.3786, 17.5729, 9.21228, -14.9721, -12.9577}
determinant: -0.000910399
Q{5426.74, 5568.04, -524.078, 283.581, -316.499, -67.3459}
determinant: -0.0897656
Q{16.6649, -37.4239, -34.0747, -16.5337, -3.95636, -7.31064}
determinant: -0.00719097
Q{-1377.14, 167.281, -125.883, -10.4689, 179.78, 56.3877}
determinant: 0.0432689
Q{22.2983, -10.1491, -15.0894, -4.41318, -2.07675, -3.48763}
determinant: -0.0430843
Q{82.6984, -39.02, -24.5518, 13.6317, 4.17851, -14.0956}
determinant: -0.0137243
Q{425.189, -9.65443, 20.9752, 7.63067, 25.4944, -52.4964}

Everytime i compute a new Q I set the robot in that state, and perform inverse kinematics from that state.. Q is the joint angles for the 6 joints.

Update

Interpolation is done by lineary dividing the path from start to end into a specified amount of of data points.

This plot shows each tranformation matrices generated from the interpolation and with their the position part plotted. The red dots is the path (every 1000th position). The blue ball is the object in want to track, and green dots represents the sphere.. As I am only doing this for the first point on the sphere, it only hits one point on the sphere, which is the top point, which the plot also shows.

Rotation doesn't show that much change, which also makes sense based difference between the current and desired rotations.

Update

My InvKin Implementation for LargeDisplacements:

std::vector<Q> pathPlanning::invKin_largeDisplacement(std::vector<Transform3D<>> t_tool_base_desired_i)
{

     Device::Ptr device_backup = this->device;  //Read in device parameter
     WorkCell::Ptr workcell_backup = this->workcell; //Read in workcell parameter
     State state_backup = this->state;
     std::vector<Q> output;
     
     for(unsigned int i = 0; i<t_tool_base_desired_i.size();  ++i)
     {
         Transform3D<> T_tool_base_current_i = device_backup->baseTframe(this->toolFrame,state_backup); //Read in Current transformation matrix

         Eigen::MatrixXd jq(device_backup->baseJframe(this->toolFrame,state_backup).e().cols(), this->device.get()->baseJframe(this->toolFrame,state_backup).e().rows());

         jq =  this->device.get()->baseJframe(this->toolFrame,state_backup).e(); // Get the jacobian for current_configuration

         //Least square solver - dq = [j(q)]T (j(q)[j(q)]T)⁻1 du  <=> dq = A*du
         Eigen::MatrixXd A (6,6);
        
         //A = jq.transpose()*(jq*jq.transpose()).inverse();
         A = (jq*jq.transpose()).inverse()*jq.transpose();

         Vector3D<> dif_p = t_tool_base_desired_i[i].P()-T_tool_base_current_i.P();  //Difference in position

         Eigen::Matrix3d dif = t_tool_base_desired_i[i].R().e()- T_tool_base_current_i.R().e(); //Differene in rotation
         Rotation3D<> dif_r(dif); //Making a rotation matrix the the difference of rotation
         RPY<> dif_rot(dif_r);    //RPY of the rotation matrix. 

         Eigen::VectorXd du(6); //Creating du
         du(0) = dif_p[0];
         du(1) = dif_p[1];
         du(2) = dif_p[2];

         du(3) = dif_rot[0];
         du(4) = dif_rot[1];
         du(5) = dif_rot[2];

         Eigen::VectorXd q(6);
         q = A*du; // computing dq

         Q q_current;
         q_current = this->device->getQ(this->state);
         Q dq(q); 
         Q q_new = q_current+ dq; // computing the new Q angles
         output.push_back(q_new); store it in the output vector
         device_backup->setQ(q_new,state_backup); //Set the robot to the calculated state. 
     }
     return output;
}

I am pretty sure that my interpolation works, as the plot shows. My inverse kinematics on the other hand not so sure..

Update

@Chuck mentions in his answer that it would be a good idea to check the core functionality, which might shed some light on what could be going wrong.

I tried it with an inv.kin function i know would work, which didn't return any result, which make me doubt whether my transformation function i create is accurate?

The robot simulation is the one shown above.. The Transform function shown above, is the function which i use to compute my desired, and provide my inverse kinematics.. Is something incorrectly setup?

Update

@Chuck came up with an different approach to my problem, which only has 3 DOF, being the position. I choose change track, and peform a simple inverse kinematics given a distance dx,dy,dz.. Which for some reason isn't working quite good for me? even for small differences...

Here is my code:

    std::vector<Q>pathPlanning::invKin(double dx, double dy , double dz)
{

    kinematics::State state =  this->state;
    Transform3D<> t_tool_base_current =  this->device.get()->baseTframe(this->toolFrame,state);

    cout <<"Current: "<< t_tool_base_current.P().e()<< endl;

    Vector3D<> P_desired(0.000001+t_tool_base_current.P().e()[0],t_tool_base_current.P().e()[1],t_tool_base_current.P().e()[2]);
    cout <<"Desired: " <<P_desired << endl;

    Transform3D<> t_tool_base_desired(P_desired,t_tool_base_current.R());
    Eigen::MatrixXd jq(this->device.get()->baseJframe(this->toolFrame,state).e().cols(), this->device.get()->baseJframe(this->toolFrame,state).e().rows());
    jq =  this->device.get()->baseJframe(this->toolFrame,state).e();

    
    //Least square solver - dq = [j(q)]T (j(q)[j(q)]T)⁻1 du  <=> dq = A*du
    Eigen::MatrixXd A (6,6);

    //A = jq.transpose()*(jq*jq.transpose()).inverse();
    A = (jq*jq.transpose()).inverse()*jq.transpose();

    Vector3D<> dif_p = t_tool_base_desired.P()-t_tool_base_current.P();
    cout <<"difference: " <<dif_p << endl;

    Eigen::VectorXd du(6);
    du(0) = dif_p[0];
    du(1) = dif_p[1];
    du(2) = dif_p[2];

    du(3) = 0;
    du(4) = 0;
    du(5) = 0;

    Eigen::VectorXd q(6);
    q = A*du;

    Q q_current;
    q_current = this->device->getQ(this->state);
    Q dq(q);
    Q q_new = q_current+ dq;
    std::vector<rw::math::Q> output;
    if(!collision(q_new))
    {
        output.push_back(q_new);
    }
    else
    {
        cout << endl;      
        cout << q_new << endl;
    }

    return output;
}

which outputs this

Current: -0.000799058
-0.282
0.99963
Desired: Vector3D(-0.000789058, -0.282, 0.99963)
difference: Vector3D(1e-05, 0, 0)
setQ{1.559142, 110474925659325248.000000, -1834.776226, 55426871347211368.000000, 0.068436, 88275880260745.328125}

setQ is the state which moves the robot to the desires state.. Either is something wrong with my implementation, or it is a singularity..

Especially because i am not moving it that much (0.00001)!!!

Updates

I think I have solved the mystery.. It must be the sphere function which creates points that outside the reach of the robot.!!

Answer 1

You still haven't described:

1. How you are doing interpolation, and
2. What the Q values are.

Are the Q values angles? Are they distances? What do they mean? Can you post a picture of your arm and label the axes of motion and describe how the drawing relates to the Q values?

I post this as an answer because I hope I have a lead on your problem. You state, "The idea is that i want my arm be on this sphere, point inward to the center," but, while your pathPlanning::sphere function sets a radius of 5, neither your start nor end positions are a distance of 5 from the origin. They are also greater than 10 from each other (the end radius from origin is 117!!), meaning that this is not a sphere at the origin but it is also not a sphere anywhere else in space.

My suggestion would be to fix the start and end coordinates at a value such that $\sqrt{x^2 + y^2 + z^2} = r_{\mbox{sphere}}$ and see if that makes anything better.

Current it appears that you're starting off of the sphere, and you're trying to get to somewhere else (really far) off the sphere, but you're trying to do path planning to keep the end effector on the sphere. This means that you're trying to generate an impossible path, which could explain the results you're getting.

:EDIT:

I was going to add this as a comment, but there's enough content that I figured I'd alter the answer.

If you start at a point on a sphere, and you end at a point on a sphere, and you linearly increment the start path to the end path, then you will traverse a trajectory of a line through the sphere. You need to be "smarter" about interpolating by choosing points that all fall on the surface of the sphere.

The way I would attempt to do this is to find the angle between the two vectors that describe the start and end points (via a dot or cross product), then interpolate the angle. Find the plane that contains the two vectors, apply the interpolated angle and constrain the interpolated points to that plane.

It's a little involved, but not fantastically complicated. That said, you need to make sure that your core functionality is working correctly to begin with. To that end, I would probably hand-pick some points first, to ensure that your problem isn't with interpolating, and try your inverse kinematics with those.

Then, once you've got the inverse kinematics working correctly, then write the interpolate function. If you try to solve all of your problems at the same time it becomes impossible to tell what is working and what isn't.