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

void testDHomogTransInv(int ntests, bool check) {
  Isometry3d T;
  std::default_random_engine generator;
  for (int testnr = 0; testnr < ntests; testnr++) {
    Vector4d q = uniformlyRandomQuat(generator);
//    T = Quaterniond(q(0), q(1), q(2), q(3)) * Translation3d(Vector3d::Random());
    T = Quaterniond(q(0), q(1), q(2), q(3));

    const int nv = 6;
    const int nq = 7;

    auto S = Matrix<double, 6, Dynamic>::Random(6, nv).eval();
    auto qdot_to_v = MatrixXd::Random(nv, nq).eval();

    auto dT = dHomogTrans(T, S, qdot_to_v).eval();
    auto dTInv = dHomogTransInv(T, dT);
    volatile auto vol = dTInv;

    if (check) {
      auto dTInvInv = dHomogTransInv(T.inverse(), dTInv);

      if (!dT.matrix().isApprox(dTInvInv.matrix(), 1e-10)) {
        std::cout << "dTInv:\n" << dTInv << "\n\n";
        std::cout << "dT:\n" << dT << "\n\n";
        std::cout << "dTInvInv:\n" << dTInvInv << "\n\n";
        std::cout << "dTInvInv - dT:\n" << dTInvInv - dT << "\n\n";

        throw std::runtime_error("wrong");
      }
    }
  }
}

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

}

Vector6d bodySpatialMotionPD(
    const RigidBodyTree &r, const DrakeRobotState &robot_state,
    const int body_index, const Isometry3d &body_pose_des,
    const Ref<const Vector6d> &body_v_des,
    const Ref<const Vector6d> &body_vdot_des, const Ref<const Vector6d> &Kp,
    const Ref<const Vector6d> &Kd, const Isometry3d &T_task_to_world) {
  // @param body_pose_des  desired pose in the task frame, this is the
  // homogeneous transformation from desired body frame to task frame
  // @param body_v_des    desired [xyzdot;angular_velocity] in task frame
  // @param body_vdot_des    desired [xyzddot;angular_acceleration] in task
  // frame
  // @param Kp     The gain in task frame
  // @param Kd     The gain in task frame
  // @param T_task_to_world  The homogeneous transform from task to world
  // @retval twist_dot, [angular_acceleration, xyz_acceleration] in body frame

  Isometry3d T_world_to_task = T_task_to_world.inverse();
  KinematicsCache<double> cache = r.doKinematics(robot_state.q, robot_state.qd);

  auto body_pose = r.relativeTransform(cache, 0, body_index);
  const auto &body_xyz = body_pose.translation();
  Vector3d body_xyz_task = T_world_to_task * body_xyz;
  Vector4d body_quat = rotmat2quat(body_pose.linear());
  std::vector<int> v_indices;
  auto J_geometric =
      r.geometricJacobian(cache, 0, body_index, body_index, true, &v_indices);
  VectorXd v_compact(v_indices.size());
  for (size_t i = 0; i < v_indices.size(); i++) {
    v_compact(i) = robot_state.qd(v_indices[i]);
  }
  Vector6d body_twist = J_geometric * v_compact;
  Matrix3d R_body_to_world = quat2rotmat(body_quat);
  Matrix3d R_world_to_body = R_body_to_world.transpose();
  Matrix3d R_body_to_task = T_world_to_task.linear() * R_body_to_world;
  Vector3d body_angular_vel =
      R_body_to_world *
      body_twist.head<3>();  // body_angular velocity in world frame
  Vector3d body_xyzdot =
      R_body_to_world * body_twist.tail<3>();  // body_xyzdot in world frame
  Vector3d body_angular_vel_task = T_world_to_task.linear() * body_angular_vel;
  Vector3d body_xyzdot_task = T_world_to_task.linear() * body_xyzdot;

  Vector3d body_xyz_des = body_pose_des.translation();
  Vector3d body_angular_vel_des = body_v_des.tail<3>();
  Vector3d body_angular_vel_dot_des = body_vdot_des.tail<3>();

  Vector3d xyz_err_task = body_xyz_des - body_xyz_task;

  Matrix3d R_des = body_pose_des.linear();
  Matrix3d R_err_task = R_des * R_body_to_task.transpose();
  Vector4d angleAxis_err_task = rotmat2axis(R_err_task);
  Vector3d angular_err_task =
      angleAxis_err_task.head<3>() * angleAxis_err_task(3);

  Vector3d xyzdot_err_task = body_v_des.head<3>() - body_xyzdot_task;
  Vector3d angular_vel_err_task = body_angular_vel_des - body_angular_vel_task;

  Vector3d Kp_xyz = Kp.head<3>();
  Vector3d Kd_xyz = Kd.head<3>();
  Vector3d Kp_angular = Kp.tail<3>();
  Vector3d Kd_angular = Kd.tail<3>();
  Vector3d body_xyzddot_task =
      (Kp_xyz.array() * xyz_err_task.array()).matrix() +
      (Kd_xyz.array() * xyzdot_err_task.array()).matrix() +
      body_vdot_des.head<3>();
  Vector3d body_angular_vel_dot_task =
      (Kp_angular.array() * angular_err_task.array()).matrix() +
      (Kd_angular.array() * angular_vel_err_task.array()).matrix() +
      body_angular_vel_dot_des;

  Vector6d twist_dot = Vector6d::Zero();
  Vector3d body_xyzddot = T_task_to_world.linear() * body_xyzddot_task;
  Vector3d body_angular_vel_dot =
      T_task_to_world.linear() * body_angular_vel_dot_task;
  twist_dot.head<3>() = R_world_to_body * body_angular_vel_dot;
  twist_dot.tail<3>() = R_world_to_body * body_xyzddot -
                        body_twist.head<3>().cross(body_twist.tail<3>());
  return twist_dot;
}

Vector6d bodySpatialMotionPD(RigidBodyManipulator *r, DrakeRobotState &robot_state, const int body_index, const Isometry3d &body_pose_des, const Ref<const Vector6d> &body_v_des, const Ref<const Vector6d> &body_vdot_des, const Ref<const Vector6d> &Kp, const Ref<const Vector6d> &Kd, const Isometry3d &T_task_to_world)
{
  // @param body_pose_des  desired pose in the task frame, this is the homogeneous transformation from desired body frame to task frame 
  // @param body_v_des    desired [xyzdot;angular_velocity] in task frame
  // @param body_vdot_des    desired [xyzddot;angular_acceleration] in task frame
  // @param Kp     The gain in task frame
  // @param Kd     The gain in task frame 
  // @param T_task_to_world  The homogeneous transform from task to world
  // @retval twist_dot, [angular_acceleration, xyz_acceleration] in body frame

  if(!r->getUseNewKinsol())
  {
    throw std::runtime_error("bodySpatialMotionPD requires new kinsol format");
  }
  Isometry3d T_world_to_task = T_task_to_world.inverse();
  r->doKinematicsNew(robot_state.q,robot_state.qd,false);

  Vector3d origin = Vector3d::Zero();
  auto body_pose = r->forwardKinNew(origin,body_index,0,2,0);
  Vector3d body_xyz = body_pose.value().head<3>();
  Vector3d body_xyz_task = T_world_to_task * body_xyz.colwise().homogeneous();
  Vector4d body_quat = body_pose.value().tail<4>();
  std::vector<int> v_indices;
  auto J_geometric = r->geometricJacobian<double>(0,body_index,body_index,0,true,&v_indices);
  VectorXd v_compact(v_indices.size());
  for(size_t i = 0;i<v_indices.size();i++)
  {
    v_compact(i) = robot_state.qd(v_indices[i]);
  }
  Vector6d body_twist = J_geometric.value() * v_compact;
  Matrix3d R_body_to_world = quat2rotmat(body_quat);
  Matrix3d R_world_to_body = R_body_to_world.transpose();
  Matrix3d R_body_to_task = T_world_to_task.linear() * R_body_to_world;
  Vector3d body_angular_vel = R_body_to_world * body_twist.head<3>();// body_angular velocity in world frame
  Vector3d body_xyzdot = R_body_to_world * body_twist.tail<3>();// body_xyzdot in world frame
  Vector3d body_angular_vel_task = T_world_to_task.linear() * body_angular_vel;
  Vector3d body_xyzdot_task = T_world_to_task.linear() * body_xyzdot;

  Vector3d body_xyz_des = body_pose_des.translation();
  Vector3d body_angular_vel_des = body_v_des.tail<3>();
  Vector3d body_angular_vel_dot_des = body_vdot_des.tail<3>();

  Vector3d xyz_err_task = body_xyz_des-body_xyz_task;

  Matrix3d R_des = body_pose_des.linear();
  Matrix3d R_err_task = R_des * R_body_to_task.transpose();
  Vector4d angleAxis_err_task = rotmat2axis(R_err_task); 
  Vector3d angular_err_task = angleAxis_err_task.head<3>() * angleAxis_err_task(3);

  Vector3d xyzdot_err_task = body_v_des.head<3>() - body_xyzdot_task;
  Vector3d angular_vel_err_task = body_angular_vel_des - body_angular_vel_task;

  Vector3d Kp_xyz = Kp.head<3>();
  Vector3d Kd_xyz = Kd.head<3>();
  Vector3d Kp_angular = Kp.tail<3>();
  Vector3d Kd_angular = Kd.tail<3>();
  Vector3d body_xyzddot_task = (Kp_xyz.array() * xyz_err_task.array()).matrix() + (Kd_xyz.array() * xyzdot_err_task.array()).matrix() + body_vdot_des.head<3>();
  Vector3d body_angular_vel_dot_task = (Kp_angular.array() * angular_err_task.array()).matrix() + (Kd_angular.array() * angular_vel_err_task.array()).matrix() + body_angular_vel_dot_des;
  
  Vector6d twist_dot = Vector6d::Zero();
  Vector3d body_xyzddot = T_task_to_world.linear() * body_xyzddot_task;
  Vector3d body_angular_vel_dot = T_task_to_world.linear() * body_angular_vel_dot_task;
  twist_dot.head<3>() = R_world_to_body * body_angular_vel_dot;
  twist_dot.tail<3>() = R_world_to_body * body_xyzddot - body_twist.head<3>().cross(body_twist.tail<3>());
  return twist_dot;
}

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

}