void computeEdgeSE3PriorGradient(Isometry3d& E,
Matrix6d& J,
const Isometry3d& Z,
const Isometry3d& X,
const Isometry3d& P){
// compute the error at the linearization point
const Isometry3d A = Z.inverse()*X;
const Isometry3d B = P;
const Matrix3d Ra = A.rotation();
const Matrix3d Rb = B.rotation();
const Vector3d tb = B.translation();
E = A*B;
const Matrix3d Re=E.rotation();
Matrix<double, 3 , 9 > dq_dR;
compute_dq_dR (dq_dR,
Re(0,0),Re(1,0),Re(2,0),
Re(0,1),Re(1,1),Re(2,1),
Re(0,2),Re(1,2),Re(2,2));
J.setZero();
// dte/dt
J.block<3,3>(0,0)=Ra;
// dte/dq =0
// dte/dqj
{
Matrix3d S;
skew(S,tb);
J.block<3,3>(0,3)=Ra*S;
}
// dre/dt =0
// dre/dq
{
double buf[27];
Map<Matrix<double, 9,3> > M(buf);
Matrix3d Sx,Sy,Sz;
skew(Sx,Sy,Sz,Rb);
Map<Matrix3d> Mx(buf); Mx = Ra*Sx;
Map<Matrix3d> My(buf+9); My = Ra*Sy;
Map<Matrix3d> Mz(buf+18); Mz = Ra*Sz;
J.block<3,3>(3,3) = dq_dR * M;
}
}
void computeEdgeSE3Gradient(Isometry3d& E,
Matrix6d& Ji,
Matrix6d& Jj,
const Isometry3d& Z,
const Isometry3d& Xi,
const Isometry3d& Xj,
const Isometry3d& Pi,
const Isometry3d& Pj
){
// compute the error at the linearization point
const Isometry3d A=Z.inverse()*Pi.inverse();
const Isometry3d B=Xi.inverse()*Xj;
const Isometry3d C=Pj;
const Isometry3d AB=A*B;
const Isometry3d BC=B*C;
E=AB*C;
const Matrix3d Re=E.rotation();
const Matrix3d Ra=A.rotation();
const Matrix3d Rb=B.rotation();
const Matrix3d Rc=C.rotation();
const Vector3d tc=C.translation();
//const Vector3d tab=AB.translation();
const Matrix3d Rab=AB.rotation();
const Vector3d tbc=BC.translation();
const Matrix3d Rbc=BC.rotation();
Matrix<double, 3 , 9 > dq_dR;
compute_dq_dR (dq_dR,
Re(0,0),Re(1,0),Re(2,0),
Re(0,1),Re(1,1),Re(2,1),
Re(0,2),Re(1,2),Re(2,2));
Ji.setZero();
Jj.setZero();
// dte/dti
Ji.block<3,3>(0,0)=-Ra;
// dte/dtj
Jj.block<3,3>(0,0)=Ra*Rb;
// dte/dqi
{
Matrix3d S;
skewT(S,tbc);
Ji.block<3,3>(0,3)=Ra*S;
}
// dte/dqj
{
Matrix3d S;
skew(S,tc);
Jj.block<3,3>(0,3)=Rab*S;
}
// dre/dqi
{
double buf[27];
Map<Matrix<double, 9,3> > M(buf);
Matrix3d Sxt,Syt,Szt;
skewT(Sxt,Syt,Szt,Rbc);
Map<Matrix3d> Mx(buf); Mx = Ra*Sxt;
Map<Matrix3d> My(buf+9); My = Ra*Syt;
Map<Matrix3d> Mz(buf+18); Mz = Ra*Szt;
Ji.block<3,3>(3,3) = dq_dR * M;
}
// dre/dqj
{
double buf[27];
Map<Matrix<double, 9,3> > M(buf);
Matrix3d Sx,Sy,Sz;
skew(Sx,Sy,Sz,Rc);
Map<Matrix3d> Mx(buf); Mx = Rab*Sx;
Map<Matrix3d> My(buf+9); My = Rab*Sy;
Map<Matrix3d> Mz(buf+18); Mz = Rab*Sz;
Jj.block<3,3>(3,3) = dq_dR * M;
}
}
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);
}
