C++ Vector6d::tail方法代码示例

Vector6d gc_asd_to_av(Vector4d asd) {
  Vector6d av;

  Vector3d aa = asd.head(3);

  // double d_inv = asd(3);

  // double sig_d_inv = (1.0 - exp(-asd(3))) / (2.0 * (1.0 + exp(-asd(3))));

  // double sig_d_inv = -log(1.0/asd(3) - 1.0);
  // double sig_d_inv = log( (2.0 * asd(3) + 1.0) / (1.0 - 2.0*asd(3)) );
  // double sig_d_inv = atan(asd(3)) / 2.0;
  // double sig_d_inv = atan2(asd(3), 1.0) / 2.0;
  // double sig_d_inv = atan2(asd(3), 1.0);

  // double sig_d_inv = atan2(asd(3), 1.0) * 1.0;
  
  // double sig_d_inv = tan(4.0 * asd(3));
  double sig_d_inv = log(asd(3));
  // cout << "sig_d_inv = " << sig_d_inv << endl;

  // double sig_d_inv = cos(asd(3)) / sin(asd(3));

  // double sig_d_inv = sin(asd(3)) / cos(asd(3));
  // double sig_d_inv = sin(asd(3)) / cos(asd(3));

  Matrix3d R = gc_Rodriguez(aa);

  // av.head(3) = R.col(2) / sig_d_inv;
  av.head(3) = R.col(2) * sig_d_inv;
  av.tail(3) = R.col(0);

  return av;
}

Vector6d gc_aid_to_av(Vector4d aid) {

  Vector6d av;
  Vector3d aa = aid.head(3);
  double d = 1.0 / aid(3);
  Matrix3d R = gc_Rodriguez(aa);
  av.head(3) = R.col(2) * d;
  av.tail(3) = R.col(0);

//  Vector6d av;
//  double a = aid[0];
//  double b = aid[1];
//  double g = aid[2];
//  double t = aid[3];
//
//  double s1 = sin(a);
//  double c1 = cos(a);
//  double s2 = sin(b);
//  double c2 = cos(b);
//  double s3 = sin(g);
//  double c3 = cos(g);
//
//  Matrix3d R;
//  R <<
//      c2 * c3,   s1 * s2 * c3 - c1 * s3,   c1 * s2 * c3 + s1 * s3,
//      c2 * s3,   s1 * s2 * s3 + c1 * c3,   c1 * s2 * s3 - s1 * c3,
//      -s2,                  s1 * c2,                  c1 * c2;
//
//  double d = 1.0 / t;
//  av.head(3) = -R.col(2) * d;
//  av.tail(3) = R.col(1);

  return av;
}

Vector6d gc_Rt_to_wt( pose_t Rt ) {
  Vector6d wt;
  wt.head(3) = gc_Rodriguez( Rt.R );
  wt.tail(3) = Rt.t;

  return wt;
}

static Vector6d apply(const Matrix4d &T, const Vector6d &q, double w) {
	Vector6d q_t;
	q_t << q.head(3), w, 0, 0;
	q_t.head(4) = T * q_t.head(4);
	q_t.tail(3) = q.tail(3);
	return q_t;
}

Vector6d logmap_se3(Matrix4d T){
    Matrix3d R, Id3 = Matrix3d::Identity();
    Vector3d Vt, t, w;
    Matrix3d V = Matrix3d::Identity(), w_hat = Matrix3d::Zero();
    Vector6d x;
    Vt << T(0,3), T(1,3), T(2,3);
    w  << 0.f, 0.f, 0.f;
    R = T.block(0,0,3,3);
    double cosine = (R.trace() - 1.f)/2.f;
    if(cosine > 1.f)
        cosine = 1.f;
    else if (cosine < -1.f)
        cosine = -1.f;
    double sine = sqrt(1.0-cosine*cosine);
    if(sine > 1.f)
        sine = 1.f;
    else if (sine < -1.f)
        sine = -1.f;
    double theta  = acos(cosine);
    if( theta > 0.000001 ){
        w_hat = theta*(R-R.transpose())/(2.f*sine);
        w = skewcoords(w_hat);
        Matrix3d s;
        s = skew(w) / theta;
        V = Id3 + s * (1.f-cosine) / theta + s * s * (theta - sine) / theta;
    }
    t = V.inverse() * Vt;
    x.head(3) = t;
    x.tail(3) = w;
    return x;
}

Vector4d gc_av_to_asd(Vector6d av) {
  Vector4d asd;

  Vector3d a = av.head(3);
  Vector3d x = av.tail(3);  // v
  Vector3d y = a.cross(x);  // n

//  Vector2d w(y.norm(), x.norm());
//  w /= w.norm();
//  asd(3) = asin(w(1));
  
  double depth = x.norm() / y.norm();
  // double sig_d = log( (2.0*depth + 1.0) / (1.0 - 2.0*depth));

//  double quotient = depth / 0.5;
//  int integer_quotient = (int)quotient;
//  double floating_quotient = quotient - (double)integer_quotient;
//  depth = depth * floating_quotient;
//  double sig_d = log(2.0*depth + 1.0) - log(1.0 - 2.0*depth);

// double sig_d = atan(1.0 / (1.0*depth) );
// double sig_d = atan(depth);
// double sig_d = atan2(1.0, depth);
// double sig_d = atan(depth);

// double sig_d = atan2(1.0, depth) / 4.0;
  double sig_d = 1.0 / exp(-depth);
  asd(3) = sig_d;
  // cout << "sig_d = " << sig_d << endl;

  // asd(3) = depth;

  // double sig_d = tan(1.0/depth);
  // double sig_d = tan(2.0/depth);
  // double sig_d = tan(2.0*depth);
  // double sig_d = (1.0 - exp(-1.0/depth)) / (2.0*(1.0 + exp(-1.0/depth)));
  // double sig_d = (1.0 - exp(-depth)) / (2.0*(1.0 + exp(-depth)));
  // double sig_d = 1.0 / (1.0 + exp(-1.0/depth));
  // double sig_d = 1.0 / (1.0 + exp(-depth));

  x /= x.norm();
  y /= y.norm();
  Vector3d z = x.cross(y);

  Matrix3d R;
  R.col(0) = x;
  R.col(1) = y;
  R.col(2) = z;

  Vector3d aa = gc_Rodriguez(R);

  asd(0) = aa(0);
  asd(1) = aa(1);
  asd(2) = aa(2);

  return asd;
}

Vector4d gc_av_to_orth(Vector6d av) {
  Vector4d orth;

  Vector3d a = av.head(3);
  Vector3d v = av.tail(3);  // v
  Vector3d n = a.cross(v);  // n

  Vector3d x = n / n.norm();
  Vector3d y = v / v.norm();
  Vector3d z = x.cross(y);

  orth[0] = atan2( y(2), z(2) );
  orth[1] = asin( - x(2) );
  orth[2] = atan2( x(1), x(0) );

  Vector2d w( n.norm(), v.norm() );
  w = w / w.norm();

  orth[3] = asin( w(1) );

//   MatrixXd A(3,2), Q, R;
// 
//   A.col(0) = n;
//   A.col(1) = v;
// 
//   Eigen::FullPivHouseholderQR<MatrixXd> qr(A);
//   // Q = qr.householderQ();
//   Q = qr.matrixQ();
//   R = qr.matrixQR().triangularView<Upper>();
//   // std::cout << Q << "\n\n" << R << "\n\n" << Q * R - A << "\n";
// 
// //  double sigma1 = R(0,0);
// //  double sigma2 = R(1,1);
// 
// //  cout << "\ntheta from sigma1 = " << acos(sigma1) << endl;
// //  cout << "theta from sigma2 = " << asin(sigma2) << endl;
// 
// // cout << "\nsigma1 = " << sigma1<< endl;
// // cout << "sigma2 = " << sigma2<< endl;
// 
// // sigma2 /= sqrt(sigma1*sigma1 + sigma2*sigma2);
// 
//   Vector3d x = Q.col(0);
//   Vector3d y = Q.col(1);
//   Vector3d z = Q.col(2);
// 
//   orth[0] = atan2( y(2), z(2) );
//   orth[1] = asin( - x(2) );
//   orth[2] = atan2( x(1), x(0) );
//   // orth[3] = asin(sigma2);
// 
//   Vector2d w( n.norm(), v.norm() );
//   w = w / w.norm();
//   orth[3] = asin( w(1) );

  return orth;
}

Vector6d gc_plk_to_pose( Vector6d plk_w, pose_t T ) {
  Vector3d nw = plk_w.head(3);
  Vector3d vw = plk_w.tail(3);

  Vector3d nc = T.R * nw + gc_skew_symmetric(T.t) * T.R * vw;
  Vector3d vc = T.R * vw;

  Vector6d plk_c;
  plk_c.head(3) = nc;
  plk_c.tail(3) = vc;
  return plk_c;
}

Vector6d gc_line_to_pose(Vector6d line_w, pose_t T) {
  Vector6d line_c;

  Vector3d cp0, dv0;
  cp0 = line_w.head(3);
  dv0 = line_w.tail(3);

  Vector3d cp1 = gc_point_to_pose( T, cp0 );
  Vector3d dv1 = T.R * dv0;

  line_c.head(3) = cp1;
  line_c.tail(3) = dv1;

  return line_c;
}

Matrix4d expmap_se3(Vector6d x){
    Matrix3d R, V, s, I = Matrix3d::Identity();
    Vector3d t, w;
    Matrix4d T = Matrix4d::Identity();
    w = x.tail(3);
    t = x.head(3);
    double theta = w.norm();
    if( theta < 0.000001 )
        R = I;
    else{
        s = skew(w)/theta;
        R = I + s * sin(theta) + s * s * (1.0f-cos(theta));
        V = I + s * (1.0f - cos(theta)) / theta + s * s * (theta - sin(theta)) / theta;
        t = V * t;
    }
    T.block(0,0,3,4) << R, t;
    return T;
}

pair<Matrix4d, Matrix4d> C3Trajectory::transformation_pair(const Vector6d &q) {
	Matrix4d R;
	R.block<3,3>(0, 0) = AttitudeHelpers::EulerToRotation(q.tail(3));
	R.block<1,3>(3, 0).fill(0);
	R.block<3,1>(0, 3).fill(0);
	R(3, 3) = 1;

	Matrix4d T = Matrix4d::Identity();
	T.block<3,1>(0, 3) = -q.head(3);

	pair<Matrix4d, Matrix4d> result;
	result.first = R.transpose()*T; // NED -> BODY

	T.block<3,1>(0, 3) = q.head(3);
	result.second = T*R; // BODY -> NED

	return result;
}

Vector6d gc_orth_to_av(Vector4d auth) {
  Vector6d av;

  double a = auth[0];
  double b = auth[1];
  double g = auth[2];
  double t = auth[3];

  double s1 = sin(a);
  double c1 = cos(a);
  double s2 = sin(b);
  double c2 = cos(b);
  double s3 = sin(g);
  double c3 = cos(g);

  Matrix3d R;
  R <<
      c2 * c3,   s1 * s2 * c3 - c1 * s3,   c1 * s2 * c3 + s1 * s3,
      c2 * s3,   s1 * s2 * s3 + c1 * c3,   c1 * s2 * s3 - s1 * c3,
      -s2,                  s1 * c2,                  c1 * c2;

  double d = cos(t) / sin(t);
  av.head(3) = -R.col(2) * d;
  av.tail(3) = R.col(1);

//   Vector3d u1 = R.col(0);
//   Vector3d u2 = R.col(1);
// 
//   double sigma1 = cos(t);
//   double sigma2 = sin(t);
// 
//   double d = cos(t) / sin(t);
// 
// //  Vector3d n = sigma1 * u1;
// //  Vector3d v = sigma2 * u2;
//   Vector3d n = u1;
//   Vector3d v = u2;
//   av.head(3) = -v.cross(n) * d;
//   av.tail(3) = v;

  return av;
}

Vector4d gc_av_to_aid(Vector6d av) {

  Vector4d aid;
  Vector3d a = av.head(3);
  Vector3d x = av.tail(3);  // v
  Vector3d y = a.cross(x);  // n
  aid(3) = x.norm() / y.norm();
  x /= x.norm();
  y /= y.norm();
  Vector3d z = x.cross(y);

  Matrix3d R;
  R.col(0) = x;
  R.col(1) = y;
  R.col(2) = z;

  Vector3d aa = gc_Rodriguez(R);

  aid(0) = aa(0);
  aid(1) = aa(1);
  aid(2) = aa(2);

//  Vector4d aid;
//  Vector3d a = av.head(3);
//  Vector3d v = av.tail(3);  // v
//  Vector3d n = a.cross(v);  // n
//
//  aid(3) = v.norm() / n.norm();
//
//  Vector3d x = n / n.norm();
//  Vector3d y = v / v.norm();
//  Vector3d z = x.cross(y);
//
//  aid[0] = atan2( y(2), z(2) );
//  aid[1] = asin( - x(2) );
//  aid[2] = atan2( x(1), x(0) );

  return aid;
}

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
  if (nrhs<1) mexErrMsgTxt("usage: ptr = pelvisMotionControlmex(0,robot_obj,alpha,nominal_pelvis_height,Kp,Kd); y=pelvisMotionControlmex(ptr,x)");
  if (nlhs<1) mexErrMsgTxt("take at least one output... please.");
  
  struct PelvisMotionControlData* pdata;

  if (mxGetScalar(prhs[0])==0) { // then construct the data object and return
    pdata = new struct PelvisMotionControlData;
    
    // get robot mex model ptr
    if (!mxIsNumeric(prhs[1]) || mxGetNumberOfElements(prhs[1])!=1)
      mexErrMsgIdAndTxt("DRC:pelvisMotionControlmex:BadInputs","the second argument should be the robot mex ptr");
    memcpy(&(pdata->r),mxGetData(prhs[1]),sizeof(pdata->r));
        
    if (!mxIsNumeric(prhs[2]) || mxGetNumberOfElements(prhs[2])!=1)
    mexErrMsgIdAndTxt("DRC:pelvisMotionControlmex:BadInputs","the third argument should be alpha");
    memcpy(&(pdata->alpha),mxGetPr(prhs[2]),sizeof(pdata->alpha));

    if (!mxIsNumeric(prhs[3]) || mxGetNumberOfElements(prhs[3])!=1)
    mexErrMsgIdAndTxt("DRC:pelvisMotionControlmex:BadInputs","the fourth argument should be nominal_pelvis_height");
    memcpy(&(pdata->nominal_pelvis_height),mxGetPr(prhs[3]),sizeof(pdata->nominal_pelvis_height));

    if (!mxIsNumeric(prhs[4]) || mxGetM(prhs[4])!=6 || mxGetN(prhs[4])!=1)
    mexErrMsgIdAndTxt("DRC:pelvisMotionControlmex:BadInputs","the fifth argument should be Kp");
    memcpy(&(pdata->Kp),mxGetPr(prhs[4]),sizeof(pdata->Kp));

    if (!mxIsNumeric(prhs[5]) || mxGetM(prhs[5])!=6 || mxGetN(prhs[5])!=1)
    mexErrMsgIdAndTxt("DRC:pelvisMotionControlmex:BadInputs","the sixth argument should be Kd");
    memcpy(&(pdata->Kd),mxGetPr(prhs[5]),sizeof(pdata->Kd));

    
    mxClassID cid;
    if (sizeof(pdata)==4) cid = mxUINT32_CLASS;
    else if (sizeof(pdata)==8) cid = mxUINT64_CLASS;
    else mexErrMsgIdAndTxt("Drake:pelvisMotionControlmex:PointerSize","Are you on a 32-bit machine or 64-bit machine??");
     
    pdata->pelvis_height_previous = -1;

    pdata->pelvis_body_index = pdata->r->findLinkInd("pelvis", 0);
    pdata->rfoot_body_index = pdata->r->findLinkInd("r_foot", 0);
    pdata->lfoot_body_index = pdata->r->findLinkInd("l_foot", 0);

    plhs[0] = mxCreateNumericMatrix(1,1,cid,mxREAL);
    memcpy(mxGetData(plhs[0]),&pdata,sizeof(pdata));
     
    return;
  }
  
  // first get the ptr back from matlab
  if (!mxIsNumeric(prhs[0]) || mxGetNumberOfElements(prhs[0])!=1)
    mexErrMsgIdAndTxt("DRC:pelvisMotionControlmex:BadInputs","the first argument should be the ptr");
  memcpy(&pdata,mxGetData(prhs[0]),sizeof(pdata));

  int nq = pdata->r->num_dof;
  int narg = 1;
  double *q = mxGetPr(prhs[narg++]);
  double *qd = &q[nq];
  Map<VectorXd> qdvec(qd,nq);
  double lfoot_yaw = mxGetScalar(prhs[narg++]);
  double rfoot_yaw = mxGetScalar(prhs[narg++]);
 
  pdata->r->doKinematics(q,false,qd);

  // TODO: this must be updated to use quaternions/spatial velocity
  Vector6d pelvis_pose,rfoot_pose,lfoot_pose;
  MatrixXd Jpelvis = MatrixXd::Zero(6,pdata->r->num_dof);
  Vector4d zero = Vector4d::Zero();
  zero(3) = 1.0;
  pdata->r->forwardKin(pdata->pelvis_body_index,zero,1,pelvis_pose);
  pdata->r->forwardJac(pdata->pelvis_body_index,zero,1,Jpelvis);
  pdata->r->forwardKin(pdata->rfoot_body_index,zero,1,rfoot_pose);
  pdata->r->forwardKin(pdata->lfoot_body_index,zero,1,lfoot_pose);

  if (pdata->pelvis_height_previous<0) {
    pdata->pelvis_height_previous = pelvis_pose(2);
  }

  double min_foot_z = std::min(lfoot_pose(2),rfoot_pose(2));
  double mean_foot_yaw = angleAverage(lfoot_yaw,rfoot_yaw);

  double pelvis_height_desired = pdata->alpha*pdata->pelvis_height_previous + (1.0-pdata->alpha)*(min_foot_z + pdata->nominal_pelvis_height); 
  pdata->pelvis_height_previous = pelvis_height_desired;
      
  Vector6d body_des;
  double nan = std::numeric_limits<double>::quiet_NaN();
  body_des << nan,nan,pelvis_height_desired,0,0,mean_foot_yaw; 
  Vector6d error;
  error.head<3>()= body_des.head<3>()-pelvis_pose.head<3>();

  Vector3d error_rpy,pose_rpy,des_rpy;
  pose_rpy = pelvis_pose.tail<3>();
  des_rpy = body_des.tail<3>();
  angleDiff(pose_rpy,des_rpy,error_rpy);
  error.tail(3) = error_rpy;

  Vector6d body_vdot = (pdata->Kp.array()*error.array()).matrix() - (pdata->Kd.array()*(Jpelvis*qdvec).array()).matrix();
  
  plhs[0] = eigenToMatlab(body_vdot);
}

MatrixXd der_logarithm_map(Matrix4d T)
{

    MatrixXd dlogT_dT = MatrixXd::Zero(6,12);

    // Approximate derivative of the logarithm_map wrt the transformation matrix
    Matrix3d L1 = Matrix3d::Zero();
    Matrix3d L2 = Matrix3d::Zero();
    Matrix3d L3 = Matrix3d::Zero();
    Matrix3d Vinv = Matrix3d::Identity();
    Vector6d x = logmap_se3(T);

    // estimates the cosine, sine, and theta
    double b;
    double cos_ = 0.5 * (T.block(0,0,3,3).trace() - 1.0 );
    if(cos_ > 1.f)
        cos_ = 1.f;
    else if (cos_ < -1.f)
        cos_ = -1.f;
    double theta  = acos(cos_);
    double theta2 = theta*theta;
    double sin_   = sin(theta);
    double cot_   = 1.0 / tan( 0.5*theta );
    double csc2_  = pow( 1.0/sin(0.5*theta) ,2);

    // if the angle is small...
    if( cos_ > 0.9999 )
    {
        b = 0.5;
        L1(1,2) = -b;
        L1(2,1) =  b;
        L2(0,2) =  b;
        L2(2,0) = -b;
        L3(0,1) = -b;
        L3(1,0) =  b;
        // form the full derivative
        dlogT_dT.block(3,0,3,3) = L1;
        dlogT_dT.block(3,3,3,3) = L2;
        dlogT_dT.block(3,6,3,3) = L3;
        dlogT_dT.block(0,9,3,3) = Vinv;
    }
    // if not...
    else
    {
        // rotation part
        double k;
        Vector3d a;
        a(0) = T(2,1) - T(1,2);
        a(1) = T(0,2) - T(2,0);
        a(2) = T(1,0) - T(0,1);
        k = ( theta * cos_ - sin_ ) / ( 4 * pow(sin_,3) );
        a = k * a;
        L1.block(0,0,3,1) = a;
        L2.block(0,1,3,1) = a;
        L3.block(0,2,3,1) = a;
        // translation part
        Matrix3d w_skew = skew( x.tail(3) );
        Vinv += w_skew * (1.f-cos_) / theta2 + w_skew * w_skew * (theta - sin_) / pow(theta,3);
        Vinv  = Vinv.inverse().eval();
        // dVinv_dR
        Vector3d t;
        Matrix3d B, skew_t;
        MatrixXd dVinv_dR(3,9);
        t = T.block(0,3,3,1);
        skew_t = skew( t );
        // - form a
        a =  (theta*cos_-sin_)/(8.0*pow(sin_,3)) * w_skew * t
            + ( (theta*sin_-theta2*cos_)*(0.5*theta*cot_-1.0) - theta*sin_*(0.25*theta*cot_+0.125*theta2*csc2_-1.0))/(4.0*theta2*pow(sin_,4)) * w_skew * w_skew * t;
        // - form B
        Vector3d w;
        Matrix3d dw_dR;
        w = x.tail(3);
        dw_dR.row(0) << -w(1)*t(1)-w(2)*t(2), 2.0*w(1)*t(0)-w(0)*t(1), 2.0*w(2)*t(0)-w(0)*t(2);
        dw_dR.row(1) << -w(1)*t(0)+2.0*w(0)*t(1), -w(0)*t(0)-w(2)*t(2), 2.0*w(2)*t(1)-w(1)*t(2);
        dw_dR.row(2) << -w(2)*t(0)+2.0*w(0)*t(2), -w(2)*t(1)+2.0*w(1)*t(2), -w(0)*t(0)-w(1)*t(1);
        B = -0.5*theta*skew_t/sin_ - (theta*cot_-2.0)*dw_dR/(8.0*pow(sin_,2));
        // - form dVinv_dR
        dVinv_dR.col(0) = a;
        dVinv_dR.col(1) = -B.col(2);
        dVinv_dR.col(2) = B.col(1);
        dVinv_dR.col(3) = B.col(2);
        dVinv_dR.col(4) = a;
        dVinv_dR.col(5) = -B.col(0);
        dVinv_dR.col(6) = -B.col(1);
        dVinv_dR.col(7) = B.col(0);
        dVinv_dR.col(8) = a;
        // form the full derivative
        dlogT_dT.block(3,0,3,3) = L1;
        dlogT_dT.block(3,3,3,3) = L2;
        dlogT_dT.block(3,6,3,3) = L3;
        dlogT_dT.block(0,9,3,3) = Vinv;
        dlogT_dT.block(0,0,3,9) = dVinv_dR;
    }

    return dlogT_dT;

}