// assumes cv::Mats are of type <float>
int PoseEstimator::estimate(const matches_t &matches,
const cv::Mat &train_kpts, const cv::Mat &query_kpts,
const cv::Mat &train_pts, const cv::Mat &query_pts,
const cv::Mat &K, const double baseline)
{
// make sure we're using floats
if (train_kpts.depth() != CV_32F ||
query_kpts.depth() != CV_32F ||
train_pts.depth() != CV_32F ||
query_pts.depth() != CV_32F)
throw std::runtime_error("Expected input to be of floating point (CV_32F)");
int nmatch = matches.size();
cout << endl << "[pe3d] Matches: " << nmatch << endl;
// set up data structures for fast processing
// indices to good matches
std::vector<Eigen::Vector3f> t3d, q3d;
std::vector<Eigen::Vector2f> t2d, q2d;
std::vector<int> m; // keep track of matches
for (int i=0; i<nmatch; i++)
{
const float* ti = train_pts.ptr<float>(matches[i].trainIdx);
const float* qi = query_pts.ptr<float>(matches[i].queryIdx);
const float* ti2 = train_kpts.ptr<float>(matches[i].trainIdx);
const float* qi2 = query_kpts.ptr<float>(matches[i].queryIdx);
// make sure we have points within range; eliminates NaNs as well
if (matches[i].distance < 60 && // TODO: get this out of the estimator
ti[2] < maxMatchRange &&
qi[2] < maxMatchRange)
{
m.push_back(i);
t2d.push_back(Eigen::Vector2f(ti2[0],ti2[1]));
q2d.push_back(Eigen::Vector2f(qi2[0],qi2[1]));
t3d.push_back(Eigen::Vector3f(ti[0],ti[1],ti[2]));
q3d.push_back(Eigen::Vector3f(qi[0],qi[1],qi[2]));
}
}
nmatch = m.size();
cout << "[pe3d] Filtered matches: " << nmatch << endl;
if (nmatch < 3) return 0; // can't do it...
int bestinl = 0;
Matrix3f Kf;
cv::cv2eigen(K,Kf); // camera matrix
float fb = Kf(0,0)*baseline; // focal length times baseline
float maxInlierXDist2 = maxInlierXDist*maxInlierXDist;
float maxInlierDDist2 = maxInlierDDist*maxInlierDDist;
// set up minimum hyp pixel distance
float minpdist = minPDist * Kf(0,2) * 2.0;
// RANSAC loop
int numchoices = 1 + numRansac / 10;
for (int ii=0; ii<numRansac; ii++)
{
// find a candidate
int k;
int a=rand()%nmatch;
int b;
for (k=0; k<numchoices; k++)
{
b=rand()%nmatch;
if (a!=b && (t2d[a]-t2d[b]).norm() > minpdist
&& (q2d[a]-q2d[b]).norm() > minpdist)
// TODO: add distance check
break;
}
if (k >= numchoices) continue;
int c;
for (k=0; k<numchoices+numchoices; k++)
{
c=rand()%nmatch;
if (c!=b && c!=a && (t2d[a]-t2d[c]).norm() > minpdist
&& (t2d[b]-t2d[c]).norm() > minpdist
&& (q2d[a]-q2d[c]).norm() > minpdist
&& (q2d[b]-q2d[c]).norm() > minpdist)
// TODO: add distance check
break;
}
if (k >= numchoices+numchoices) continue;
// get centroids
Vector3f p0a = t3d[a];
Vector3f p0b = t3d[b];
Vector3f p0c = t3d[c];
Vector3f p1a = q3d[a];
Vector3f p1b = q3d[b];
Vector3f p1c = q3d[c];
Vector3f c0 = (p0a+p0b+p0c)*(1.0/3.0);
Vector3f c1 = (p1a+p1b+p1c)*(1.0/3.0);
// subtract out
p0a -= c0;
p0b -= c0;
p0c -= c0;
p1a -= c1;
p1b -= c1;
p1c -= c1;
Matrix3f Hf = p1a*p0a.transpose() + p1b*p0b.transpose() +
p1c*p0c.transpose();
Matrix3d H = Hf.cast<double>();
#if 0
cout << p0a.transpose() << endl;
cout << p0b.transpose() << endl;
cout << p0c.transpose() << endl;
#endif
// do the SVD thang
JacobiSVD<Matrix3d> svd(H, ComputeFullU | ComputeFullV);
Matrix3d V = svd.matrixV();
Matrix3d R = V * svd.matrixU().transpose();
double det = R.determinant();
//ntot++;
if (det < 0.0)
{
//nneg++;
V.col(2) = V.col(2)*-1.0;
R = V * svd.matrixU().transpose();
}
Vector3d cd0 = c0.cast<double>();
Vector3d cd1 = c1.cast<double>();
Vector3d tr = cd0-R*cd1; // translation
// cout << "[PE test] R: " << endl << R << endl;
// cout << "[PE test] t: " << tr.transpose() << endl;
Vector3f trf = tr.cast<float>(); // convert to floats
Matrix3f Rf = R.cast<float>();
Rf = Kf*Rf;
trf = Kf*trf;
// find inliers, based on image reprojection
int inl = 0;
for (int i=0; i<nmatch; i++)
{
const Vector3f &pt1 = q3d[i];
Vector3f ipt = Rf*pt1+trf; // transform query point
float iz = 1.0/ipt.z();
Vector2f &kp = t2d[i];
// cout << kp.transpose() << " " << pt1.transpose() << " " << ipt.transpose() << endl;
float dx = kp.x() - ipt.x()*iz;
float dy = kp.y() - ipt.y()*iz;
float dd = fb/t3d[i].z() - fb/ipt.z(); // diff of disparities, could pre-compute t3d
if (dx*dx < maxInlierXDist2 && dy*dy < maxInlierXDist2
&& dd*dd < maxInlierDDist2)
// inl+=(int)fsqrt(ipt.z()); // clever way to weight closer points
inl++;
}
if (inl > bestinl)
{
bestinl = inl;
trans = tr.cast<float>(); // convert to floats
rot = R.cast<float>();
}
}
cout << "[pe3d] Best inliers: " << bestinl << endl;
// printf("Total ransac: %d Neg det: %d\n", ntot, nneg);
// reduce matches to inliers
matches_t inls; // temporary for current inliers
inliers.clear();
Matrix3f Rf = Kf*rot;
Vector3f trf = Kf*trans;
// cout << "[pe3e] R: " << endl << rot << endl;
cout << "[pe3d] t: " << trans.transpose() << endl;
AngleAxisf aa;
aa.fromRotationMatrix(rot);
cout << "[pe3d] AA: " << aa.angle()*180.0/M_PI << " " << aa.axis().transpose() << endl;
for (int i=0; i<nmatch; i++)
{
Vector3f &pt1 = q3d[i];
Vector3f ipt = Rf*pt1+trf; // transform query point
float iz = 1.0/ipt.z();
Vector2f &kp = t2d[i];
// cout << kp.transpose() << " " << pt1.transpose() << " " << ipt.transpose() << endl;
float dx = kp.x() - ipt.x()*iz;
float dy = kp.y() - ipt.y()*iz;
float dd = fb/t3d[i].z() - fb/ipt.z(); // diff of disparities, could pre-compute t3d
if (dx*dx < maxInlierXDist2 && dy*dy < maxInlierXDist2 &&
dd*dd < maxInlierDDist2)
inls.push_back(matches[m[i]]);
}
cout << "[pe3d] Final inliers: " << inls.size() << endl;
// polish with SVD
if (polish)
{
Matrix3d Rd = rot.cast<double>();
Vector3d trd = trans.cast<double>();
StereoPolish pol(5,false);
pol.polish(inls,train_kpts,query_kpts,train_pts,query_pts,K,baseline,
Rd,trd);
AngleAxisd aa;
aa.fromRotationMatrix(Rd);
cout << "[pe3d] Polished t: " << trd.transpose() << endl;
cout << "[pe3d] Polished AA: " << aa.angle()*180.0/M_PI << " " << aa.axis().transpose() << endl;
int num = inls.size();
// get centroids
Vector3f c0(0,0,0);
Vector3f c1(0,0,0);
for (int i=0; i<num; i++)
{
c0 += t3d[i];
c1 += q3d[i];
}
c0 = c0 / (float)num;
c1 = c1 / (float)num;
// subtract out and create H matrix
Matrix3f Hf;
Hf.setZero();
for (int i=0; i<num; i++)
{
Vector3f p0 = t3d[i]-c0;
Vector3f p1 = q3d[i]-c1;
Hf += p1*p0.transpose();
}
Matrix3d H = Hf.cast<double>();
// do the SVD thang
JacobiSVD<Matrix3d> svd(H, ComputeFullU | ComputeFullV);
Matrix3d V = svd.matrixV();
Matrix3d R = V * svd.matrixU().transpose();
double det = R.determinant();
//ntot++;
if (det < 0.0)
{
//nneg++;
V.col(2) = V.col(2)*-1.0;
R = V * svd.matrixU().transpose();
}
Vector3d cd0 = c0.cast<double>();
Vector3d cd1 = c1.cast<double>();
Vector3d tr = cd0-R*cd1; // translation
aa.fromRotationMatrix(R);
cout << "[pe3d] t: " << tr.transpose() << endl;
cout << "[pe3d] AA: " << aa.angle()*180.0/M_PI << " " << aa.axis().transpose() << endl;
#if 0
// system
SysSBA sba;
sba.verbose = 0;
// set up nodes
// should have a frame => node function
Vector4d v0 = Vector4d(0,0,0,1);
Quaterniond q0 = Quaternion<double>(Vector4d(0,0,0,1));
sba.addNode(v0, q0, f0.cam, true);
Quaterniond qr1(rot); // from rotation matrix
Vector4d temptrans = Vector4d(trans(0), trans(1), trans(2), 1.0);
// sba.addNode(temptrans, qr1.normalized(), f1.cam, false);
qr1.normalize();
sba.addNode(temptrans, qr1, f1.cam, false);
int in = 3;
if (in > (int)inls.size())
in = inls.size();
// set up projections
for (int i=0; i<(int)inls.size(); i++)
{
// add point
int i0 = inls[i].queryIdx;
int i1 = inls[i].trainIdx;
Vector4d pt = query_pts[i0];
sba.addPoint(pt);
// projected point, ul,vl,ur
Vector3d ipt;
ipt(0) = f0.kpts[i0].pt.x;
ipt(1) = f0.kpts[i0].pt.y;
ipt(2) = ipt(0)-f0.disps[i0];
sba.addStereoProj(0, i, ipt);
// projected point, ul,vl,ur
ipt(0) = f1.kpts[i1].pt.x;
ipt(1) = f1.kpts[i1].pt.y;
ipt(2) = ipt(0)-f1.disps[i1];
sba.addStereoProj(1, i, ipt);
}
sba.huber = 2.0;
sba.doSBA(5,10e-4,SBA_DENSE_CHOLESKY);
int nbad = sba.removeBad(2.0);
// cout << endl << "Removed " << nbad << " projections > 2 pixels error" << endl;
sba.doSBA(5,10e-5,SBA_DENSE_CHOLESKY);
// cout << endl << sba.nodes[1].trans.transpose().head(3) << endl;
// get the updated transform
trans = sba.nodes[1].trans.head(3);
Quaterniond q1;
q1 = sba.nodes[1].qrot;
rot = q1.toRotationMatrix();
// set up inliers
inliers.clear();
for (int i=0; i<(int)inls.size(); i++)
{
ProjMap &prjs = sba.tracks[i].projections;
if (prjs[0].isValid && prjs[1].isValid) // valid track
inliers.push_back(inls[i]);
}
#if 0
printf("Inliers: %d After polish: %d\n", (int)inls.size(), (int)inliers.size());
#endif
#endif
}
inliers = inls;
return (int)inls.size();
}
void Omega3::AcquisitionTask::run(){
while(1){
//! qDebug()<< "try to open the first available device";
if (drdOpen () < 0) {
//qDebug()<<"no device available...";
//dhdSleep (2.0);
omegaDetected = false;
}
else{
omegaDetected = true;
}
if(omegaDetected){
break;
}
sleep(1);
}
done = 0;
double p[DHD_MAX_DOF];
double r[3][3];
double v[DHD_MAX_DOF];
double f[DHD_MAX_DOF];
double normf, normt;
double t0 = dhdGetTime ();
double t1 = t0;
bool base = false;
bool wrist = false;
bool grip = false;
//! Eigen objects (mapped to the arrays above)
Map<Vector3d> position(&p[0], 3);
Map<Vector3d> force (&f[0], 3);
Map<Vector3d> torque (&f[3], 3);
Map<Vector3d> velpos (&v[0], 3);
Map<Vector3d> velrot (&v[3], 3);
Matrix3d center;
Matrix3d rotation;
//! center of workspace
center.setIdentity (); // rotation (matrix)
double nullPose[DHD_MAX_DOF] = { 0.0, 0.0, 0.0, // translation
0.0, 0.0, 0.0, // rotation (joint angles)
0.0 }; // gripper
//! print out device identifier
if (!drdIsSupported ()) {
dhdSleep (2.0);
drdClose ();
}
//! perform auto-initialization
if (!drdIsInitialized () && drdAutoInit () < 0) {
qDebug()<<"error: auto-initialization failed"<<dhdErrorGetLastStr ();
dhdSleep (2.0);
}
else if (drdStart () < 0) {
printf ("error: regulation thread failed to start (%s)\n", dhdErrorGetLastStr ());
dhdSleep (2.0);
}
//! move to center
drdMoveTo (nullPose);
// request a null force (only gravity compensation will be applied)
// this will only apply to unregulated axis
for (int i=0; i<DHD_MAX_DOF; i++) f[i] = 0.0;
drdSetForceAndTorqueAndGripperForce (f);
// disable all axis regulation (but leave regulation thread running)
drdRegulatePos (base);
drdRegulateRot (wrist);
drdRegulateGrip (grip);
//! save current pos
drdGetPositionAndOrientation (p, r);
for (int i=0; i<3; i++) rotation.row(i) = Vector3d::Map(&r[i][0], 3);
this->omega->patientHandling->setOmegaPosition(p[0],p[1],p[2]);
long long cpt = 0;
// loop while the button is not pushed
while (!done) {
// synchronize with regulation thread
drdWaitForTick ();
// get position/orientation/gripper and update Eigen rotation matrix
drdGetPositionAndOrientation (p, r);
for (int i=0; i<3; i++) rotation.row(i) = Vector3d::Map(&r[i][0], 3);
if(cpt%31==0){
this->omega->patientHandling->setOmegaPosition(p[0],p[1],p[2]);
}
else{
cpt += 1;
}
// get position/orientation/gripper velocity
drdGetVelocity (v);
// compute base centering force
force = - KP * position;
// compute wrist centering torque
AngleAxisd deltaRotation (rotation.transpose() * center);
torque = rotation * (KR * deltaRotation.angle() * deltaRotation.axis());
// compute gripper centering force
f[6] = - KG * (p[6] - 0.015);
// scale force to a pre-defined ceiling
if ((normf = force.norm()) > MAXF) force *= MAXF/normf;
// scale torque to a pre-defined ceiling
if ((normt = torque.norm()) > MAXT) torque *= MAXT/normt;
// scale gripper force to a pre-defined ceiling
if (f[6] > MAXG) f[6] = MAXG;
if (f[6] < -MAXG) f[6] = -MAXG;
// add damping
force -= KVP * velpos;
torque -= KWR * velrot;
f[6] -= KVG * v[6];
// apply centering force with damping
if (drdSetForceAndTorqueAndGripperForce (f, -1) < DHD_NO_ERROR) {
printf ("error: cannot set force (%s)\n", dhdErrorGetLastStr ());
done = 1;
}
// local loop refresh rate
//count++;
// display refresh rate and position at 10Hz
t1 = drdGetTime ();
if ((t1-t0) > REFRESH_INTERVAL) {
// retrieve/compute information to display
// double freq = (double)count/(t1-t0)*1e-3;
// count = 0;
// t0 = t1;
// qDebug()<<freq<<cpt;
if (dhdGetButtonMask()) {
qDebug()<<"stop";
dhdClose ();
done = 1;
}
}
//qDebug()<<cpt<<t1;
//dhdSleep (0.1);
}
}
void ikTest()
{
cout << "--------------------------------------------" << endl;
cout << "| Testing Standard Damped Least Squares IK |" << endl;
cout << "--------------------------------------------" << endl;
Robot parseTest("../urdf/huboplus.urdf");
parseTest.linkage("Body_RSP").name("RightArm");
parseTest.linkage("Body_LSP").name("LeftArm");
parseTest.linkage("Body_RHY").name("RightLeg");
parseTest.linkage("Body_LHY").name("LeftLeg");
string limb = "LeftArm";
VectorXd targetValues, jointValues, restValues, storedVals;
targetValues.resize(parseTest.linkage(limb).nJoints());
jointValues.resize(parseTest.linkage(limb).nJoints());
restValues.resize(parseTest.linkage(limb).nJoints());
restValues.setZero();
if(limb == "RightLeg" || limb == "LeftLeg")
{
restValues[0] = 0;
restValues[1] = 0;
restValues[2] = -0.15;
restValues[3] = 0.3;
restValues[4] = -0.15;
restValues[5] = 0;
}
else if(limb == "RightArm" || limb == "LeftArm")
{
if(limb=="RightArm")
restValues[0] = -30*M_PI/180;
else
restValues[0] = 30*M_PI/180;
restValues[1] = 0;
restValues[2] = 0;
restValues[3] = -30*M_PI/180;
restValues[4] = 0;
restValues[5] = 0;
}
bool totallyRandom = false;
int resolution = 1000;
double scatterScale = 0.05;
int tests = 10000;
Constraints constraints;
constraints.restingValues(restValues);
constraints.performNullSpaceTask = false;
constraints.maxAttempts = 1;
constraints.maxIterations = 50;
constraints.convergenceTolerance = 0.001;
// constraints.wrapToJointLimits = true;
// constraints.wrapSolutionToJointLimits = true;
constraints.wrapToJointLimits = false;
constraints.wrapSolutionToJointLimits = false;
TRANSFORM target;
int wins = 0, jumps = 0;
srand(time(NULL));
clock_t time;
time = clock();
for(int k=0; k<tests; k++)
{
for(int i=0; i<parseTest.linkage(limb).nJoints(); i++)
{
int randomVal = rand();
targetValues(i) = ((double)(randomVal%resolution))/((double)resolution-1)
*(parseTest.linkage(limb).joint(i).max() - parseTest.linkage(limb).joint(i).min())
+ parseTest.linkage(limb).joint(i).min();
// cout << "vals: " << randomVal;
randomVal = rand();
// cout << "\t:\t" << randomVal << endl;
/////////////////////// TOTALLY RANDOM TEST ///////////////////////////
if(totallyRandom)
jointValues(i) = ((double)(randomVal%resolution))/((double)resolution-1)
*(parseTest.linkage(limb).joint(i).max() - parseTest.linkage(limb).joint(i).min())
+ parseTest.linkage(limb).joint(i).min();
////////////////////// NOT COMPLETELY RANDOM TEST //////////////////////
else
// jointValues(i) = targetValues(i) +
// ((double)(2*rand()%resolution)/((double)resolution-1)-1)*scatterScale
// *(parseTest.linkage(limb).joint(i).max() - parseTest.linkage(limb).joint(i).min());
jointValues(i) = targetValues(i) +
((double)(2*rand()%resolution)/((double)resolution-1)-1)*scatterScale;
parseTest.linkage(limb).joint(i).value(targetValues(i));
}
target = parseTest.linkage(limb).tool().respectToRobot();
parseTest.linkage(limb).values(jointValues);
// cout << "Start:" << endl;
// cout << parseTest.linkage(limb).tool().respectToRobot().matrix() << endl;
// cout << "Target:" << endl;
// cout << target.matrix() << endl;
storedVals = jointValues;
rk_result_t result = parseTest.dampedLeastSquaresIK_linkage(limb, jointValues, target, constraints);
if( result == RK_SOLVED )
wins++;
// if(false)
if(!totallyRandom && (result != RK_SOLVED || (storedVals-jointValues).norm() > 1.1*(storedVals-targetValues).norm()) )
{
if( result == RK_SOLVED )
cout << "SOLVED" << endl;
else
{
Vector3d Terr = target.translation() - parseTest.linkage(limb).tool().respectToRobot().translation();
AngleAxisd aaerr;
aaerr = target.rotation()*parseTest.linkage(limb).tool().respectToRobot().rotation().transpose();
Vector3d Rerr;
if(fabs(aaerr.angle()) <= M_PI)
Rerr = aaerr.angle()*aaerr.axis();
else
Rerr = (aaerr.angle()-2*M_PI)*aaerr.axis();
cout << "FAILED (" << Terr.norm() << ", " << Rerr.norm() << ")" << endl;
}
cout << "Start: " << storedVals.transpose() << endl;
cout << "Solve: " << jointValues.transpose() << endl;
cout << "Goal : " << targetValues.transpose() << endl;
cout << "Delta: " << (storedVals-targetValues).transpose() << "\t(" << (storedVals-targetValues).norm() << ")" << endl;
cout << "Diff : " << (storedVals-jointValues).transpose() << "\t(" << (storedVals-jointValues).norm() << ")" << endl << endl;
if( result == RK_SOLVED )
jumps++;
}
// cout << "End:" << endl;
// cout << parseTest.linkage(limb).tool().respectToRobot().matrix() << endl;
// cout << "Target Joint Values: " << targetValues.transpose() << endl;
// if(result != RK_SOLVED)
if(false)
{
cout << "Start values: ";
for(int p=0; p<storedVals.size(); p++)
cout << storedVals[p] << "\t\t";
cout << endl;
cout << "Target values: ";
for(int p=0; p<targetValues.size(); p++)
cout << targetValues[p] << "\t\t";
cout << endl;
cout << "Final Joint Values: ";
for(int p=0; p<jointValues.size(); p++)
cout << jointValues[p] << "\t\t";
cout << endl;
cout << "Norm: " << ( jointValues - targetValues ).norm() << endl;
cout << "Error: " << (parseTest.linkage(limb).tool().respectToRobot().translation()
- target.translation()).norm() << endl;
}
}
clock_t endTime;
endTime = clock();
cout << "Time: " << (endTime - time)/((double)CLOCKS_PER_SEC*tests) << " : " <<
(double)CLOCKS_PER_SEC*tests/(endTime-time) << endl;
cout << "Win: " << ((double)wins)/((double)tests)*100 << "%" << endl;
if(!totallyRandom)
cout << "Jump: " << ((double)jumps)/((double)tests)*100 << "%" << endl;
}
rk_result_t Robot::dampedLeastSquaresIK_chain(const vector<size_t> &jointIndices, VectorXd &jointValues, const Isometry3d &target, const Isometry3d &finalTF)
{
vector<Linkage::Joint*> joints;
joints.resize(jointIndices.size());
// FIXME: Add in safety checks
for(int i=0; i<joints.size(); i++)
joints[i] = joints_[jointIndices[i]];
// ~~ Declarations ~~
MatrixXd J;
MatrixXd Jinv;
Isometry3d pose;
AngleAxisd aagoal(target.rotation());
AngleAxisd aastate;
Vector3d Terr;
Vector3d Rerr;
Vector6d err;
VectorXd delta(jointValues.size());
VectorXd f(jointValues.size());
tolerance = 0.001;
maxIterations = 50; // TODO: Put this in the constructor so the user can set it arbitrarily
damp = 0.05;
values(jointIndices, jointValues);
pose = joint(jointIndices.back()).respectToRobot()*finalTF;
aastate = pose.rotation();
Terr = target.translation()-pose.translation();
Rerr = aagoal.angle()*aagoal.axis()-aastate.angle()*aastate.axis();
err << Terr, Rerr;
size_t iterations = 0;
do {
jacobian(J, joints, joints.back()->respectToRobot().translation()+finalTF.translation(), this);
f = (J*J.transpose() + damp*damp*Matrix6d::Identity()).colPivHouseholderQr().solve(err);
delta = J.transpose()*f;
jointValues += delta;
values(jointIndices, jointValues);
pose = joint(jointIndices.back()).respectToRobot()*finalTF;
aastate = pose.rotation();
Terr = target.translation()-pose.translation();
Rerr = aagoal.angle()*aagoal.axis()-aastate.angle()*aastate.axis();
err << Terr, Rerr;
iterations++;
} while(err.norm() > tolerance && iterations < maxIterations);
}
rk_result_t Robot::jacobianTransposeIK_chain(const vector<size_t> &jointIndices, VectorXd &jointValues, const Isometry3d &target, const Isometry3d &finalTF)
{
return RK_SOLVER_NOT_READY;
// FIXME: Make this solver work
vector<Linkage::Joint*> joints;
joints.resize(jointIndices.size());
// FIXME: Add in safety checks
for(int i=0; i<joints.size(); i++)
joints[i] = joints_[jointIndices[i]];
// ~~ Declarations ~~
MatrixXd J;
MatrixXd Jinv;
Isometry3d pose;
AngleAxisd aagoal;
AngleAxisd aastate;
Vector6d state;
Vector6d err;
VectorXd delta(jointValues.size());
Vector6d gamma;
double alpha;
aagoal = target.rotation();
double Tscale = 3; // TODO: Put these as a class member in the constructor
double Rscale = 0;
tolerance = 1*M_PI/180; // TODO: Put this in the constructor so the user can set it arbitrarily
maxIterations = 100; // TODO: Put this in the constructor so the user can set it arbitrarily
size_t iterations = 0;
do {
values(jointIndices, jointValues);
jacobian(J, joints, joints.back()->respectToRobot().translation()+finalTF.translation(), this);
pose = joint(jointIndices.back()).respectToRobot()*finalTF;
aastate = pose.rotation();
state << pose.translation(), aastate.axis()*aastate.angle();
err << (target.translation()-pose.translation()).normalized()*Tscale,
(aagoal.angle()*aagoal.axis()-aastate.angle()*aastate.axis()).normalized()*Rscale;
gamma = J*J.transpose()*err;
alpha = err.dot(gamma)/gamma.norm();
delta = alpha*J.transpose()*err;
jointValues += delta;
iterations++;
std::cout << iterations << " | Norm:" << delta.norm()
// << "\tdelta: " << delta.transpose() << "\tJoints:" << jointValues.transpose() << std::endl;
<< " | " << (target.translation() - pose.translation()).norm()
<< "\tErr: " << (target.translation()-pose.translation()).transpose() << std::endl;
} while(err.norm() > tolerance && iterations < maxIterations);
}
rk_result_t Robot::pseudoinverseIK_chain(const vector<size_t> &jointIndices, VectorXd &jointValues,
const Isometry3d &target, const Isometry3d &finalTF)
{
return RK_SOLVER_NOT_READY;
// FIXME: Make this solver work
vector<Linkage::Joint*> joints;
joints.resize(jointIndices.size());
// FIXME: Add in safety checks
for(int i=0; i<joints.size(); i++)
joints[i] = joints_[jointIndices[i]];
// ~~ Declarations ~~
MatrixXd J;
MatrixXd Jinv;
Isometry3d pose;
AngleAxisd aagoal;
AngleAxisd aastate;
Vector6d goal;
Vector6d state;
Vector6d err;
VectorXd delta(jointValues.size());
MatrixXd Jsub;
aagoal = target.rotation();
goal << target.translation(), aagoal.axis()*aagoal.angle();
tolerance = 1*M_PI/180; // TODO: Put this in the constructor so the user can set it arbitrarily
maxIterations = 100; // TODO: Put this in the constructor so the user can set it arbitrarily
errorClamp = 0.25; // TODO: Put this in the constructor
deltaClamp = M_PI/4; // TODO: Put this in the constructor
size_t iterations = 0;
do {
values(jointIndices, jointValues);
jacobian(J, joints, joints.back()->respectToRobot().translation()+finalTF.translation(), this);
Jsub = J.block(0,0,3,jointValues.size());
pinv(Jsub, Jinv);
pose = joint(jointIndices.back()).respectToRobot()*finalTF;
aastate = pose.rotation();
state << pose.translation(), aastate.axis()*aastate.angle();
err = goal-state;
for(int i=3; i<6; i++)
err[i] *= 0;
err.normalize();
Vector3d e = (target.translation() - pose.translation()).normalized()*0.005;
// delta = Jinv*err*0.1;
// clampMag(delta, deltaClamp);
VectorXd d = Jinv*e;
// jointValues += delta;
jointValues += d;
std::cout << iterations << " | Norm:" << delta.norm()
// << "\tdelta: " << delta.transpose() << "\tJoints:" << jointValues.transpose() << std::endl;
<< " | " << (target.translation() - pose.translation()).norm()
<< "\tErr: " << (goal-state).transpose() << std::endl;
iterations++;
} while(delta.norm() > tolerance && iterations < maxIterations);
}
// Based on a paper by Samuel R. Buss and Jin-Su Kim // TODO: Cite the paper properly
rk_result_t Robot::selectivelyDampedLeastSquaresIK_chain(const vector<size_t> &jointIndices, VectorXd &jointValues,
const Isometry3d &target, const Isometry3d &finalTF)
{
return RK_SOLVER_NOT_READY;
// FIXME: Make this work
// Arbitrary constant for maximum angle change in one step
gammaMax = M_PI/4; // TODO: Put this in the constructor so the user can change it at a whim
vector<Linkage::Joint*> joints;
joints.resize(jointIndices.size());
// FIXME: Add in safety checks
for(int i=0; i<joints.size(); i++)
joints[i] = joints_[jointIndices[i]];
// ~~ Declarations ~~
MatrixXd J;
JacobiSVD<MatrixXd> svd;
Isometry3d pose;
AngleAxisd aagoal;
AngleAxisd aastate;
Vector6d goal;
Vector6d state;
Vector6d err;
Vector6d alpha;
Vector6d N;
Vector6d M;
Vector6d gamma;
VectorXd delta(jointValues.size());
VectorXd tempPhi(jointValues.size());
// ~~~~~~~~~~~~~~~~~~
// cout << "\n\n" << endl;
tolerance = 1*M_PI/180; // TODO: Put this in the constructor so the user can set it arbitrarily
maxIterations = 1000; // TODO: Put this in the constructor so the user can set it arbitrarily
size_t iterations = 0;
do {
values(jointIndices, jointValues);
jacobian(J, joints, joints.back()->respectToRobot().translation()+finalTF.translation(), this);
svd.compute(J, ComputeFullU | ComputeThinV);
// cout << "\n\n" << svd.matrixU() << "\n\n\n" << svd.singularValues().transpose() << "\n\n\n" << svd.matrixV() << endl;
// for(int i=0; i<svd.matrixU().cols(); i++)
// cout << "u" << i << " : " << svd.matrixU().col(i).transpose() << endl;
// std::cout << "Joint name: " << joint(jointIndices.back()).name()
// << "\t Number: " << jointIndices.back() << std::endl;
pose = joint(jointIndices.back()).respectToRobot()*finalTF;
// std::cout << "Pose: " << std::endl;
// std::cout << pose.matrix() << std::endl;
// AngleAxisd aagoal(target.rotation());
aagoal = target.rotation();
goal << target.translation(), aagoal.axis()*aagoal.angle();
aastate = pose.rotation();
state << pose.translation(), aastate.axis()*aastate.angle();
err = goal-state;
// std::cout << "state: " << state.transpose() << std::endl;
// std::cout << "err: " << err.transpose() << std::endl;
for(int i=0; i<6; i++)
alpha[i] = svd.matrixU().col(i).dot(err);
// std::cout << "Alpha: " << alpha.transpose() << std::endl;
for(int i=0; i<6; i++)
{
N[i] = svd.matrixU().block(0,i,3,1).norm();
N[i] += svd.matrixU().block(3,i,3,1).norm();
}
// std::cout << "N: " << N.transpose() << std::endl;
double tempMik = 0;
for(int i=0; i<svd.matrixV().cols(); i++)
{
M[i] = 0;
for(int k=0; k<svd.matrixU().cols(); k++)
{
tempMik = 0;
for(int j=0; j<svd.matrixV().cols(); j++)
tempMik += fabs(svd.matrixV()(j,i))*J(k,j);
M[i] += 1/svd.singularValues()[i]*tempMik;
}
}
// std::cout << "M: " << M.transpose() << std::endl;
for(int i=0; i<svd.matrixV().cols(); i++)
gamma[i] = minimum(1, N[i]/M[i])*gammaMax;
// std::cout << "Gamma: " << gamma.transpose() << std::endl;
delta.setZero();
for(int i=0; i<svd.matrixV().cols(); i++)
{
// std::cout << "1/sigma: " << 1/svd.singularValues()[i] << std::endl;
tempPhi = 1/svd.singularValues()[i]*alpha[i]*svd.matrixV().col(i);
// std::cout << "Phi: " << tempPhi.transpose() << std::endl;
clampMaxAbs(tempPhi, gamma[i]);
delta += tempPhi;
// std::cout << "delta " << i << ": " << delta.transpose() << std::endl;
}
clampMaxAbs(delta, gammaMax);
jointValues += delta;
std::cout << iterations << " | Norm:" << delta.norm() << "\tdelta: "
<< delta.transpose() << "\tJoints:" << jointValues.transpose() << std::endl;
iterations++;
} while(delta.norm() > tolerance && iterations < maxIterations);
}