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.!!
setQ? What are these numbers? Are they degrees? I can see that it looks like columns 2 and 3 have a pretty dramatic shift at the end, but with no diagram or description as to what these numbers are I don't know that anyone can really tell you what you've got. What are you interpolating? Waypoints along the trajectory? How are you calculating those? Why interpolate at all? $\endgroup$