C++ Isometry3d::rotation方法代码示例

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;
            }
        }

//.........这里部分代码省略.........