C++ Transform::translation方法代码示例

void toGeometryMsg(geometry_msgs::Transform& out, const Eigen::Transform<double, 3, TransformType>& pose) {
    // convert accumulated_pose_ to transformStamped
    Eigen::Quaterniond rot_quat(pose.rotation());

    out.translation.x = pose.translation().x();
    out.translation.y = pose.translation().y();
    out.translation.z = pose.translation().z();

    out.rotation.x = rot_quat.x();
    out.rotation.y = rot_quat.y();
    out.rotation.z = rot_quat.z();
    out.rotation.w = rot_quat.w();
}

void X3DConverter::startShape(const std::array<float, 12>& matrix) {

    // Finding axis/angle from matrix using Eigen for its bullet proof implementation.
    Eigen::Transform<float, 3, Eigen::Affine> t;
    t.setIdentity();
    for (unsigned int i = 0; i < 3; i++) {
        for (unsigned int j = 0; j < 4; j++) {
            t(i, j) = matrix[i+j*3];
        }
    }

    Eigen::Matrix3f rotationMatrix;
	Eigen::Matrix3f scaleMatrix;
    t.computeRotationScaling(&rotationMatrix, &scaleMatrix);
	Eigen::Quaternionf q;
	Eigen::AngleAxisf aa;
	q = rotationMatrix;
    aa = q;

	Eigen::Vector3f scale = scaleMatrix.diagonal();
	Eigen::Vector3f translation = t.translation();

    startNode(ID::Transform);
    m_writers.back()->setSFVec3f(ID::translation, translation.x(), translation.y() , translation.z());
    m_writers.back()->setSFRotation(ID::rotation, aa.axis().x(), aa.axis().y(), aa.axis().z(), aa.angle());
    m_writers.back()->setSFVec3f(ID::scale, scale.x(), scale.y(), scale.z());
    startNode(ID::Shape);
    startNode(ID::Appearance);
    startNode(ID::Material);
    m_writers.back()->setSFColor<vector<float> >(ID::diffuseColor, RVMColorHelper::color(m_materials.back()));
    endNode(ID::Material); // Material
    endNode(ID::Appearance); // Appearance

}

/**
 * @function heuristicCost
 * @brief
 */
double M_RRT::heuristicCost( Eigen::VectorXd node )
{

  Eigen::Transform<double, 3, Eigen::Affine> T;

  // Calculate the EE position
  robinaLeftArm_fk( node, TWBase, Tee, T );

  Eigen::VectorXd trans_ee = T.translation();
  Eigen::VectorXd x_ee = T.rotation().col(0);
  Eigen::VectorXd y_ee = T.rotation().col(1);
  Eigen::VectorXd z_ee = T.rotation().col(2);

  Eigen::VectorXd GH = ( goalPosition - trans_ee );

  double fx1 = GH.norm() ;

  GH = GH/GH.norm();

  double fx2 = abs( GH.dot( x_ee ) - 1 );

  double fx3 = abs( GH.dot( z_ee ) );

  double heuristic = w1*fx1 + w2*fx2 + w3*fx3;     

  return heuristic;
}

Eigen::Matrix<Scalar, HOMOGENEOUS_TRANSFORM_SIZE, DerivedDT::ColsAtCompileTime> dHomogTransInv(
    const Eigen::Transform<Scalar, 3, Eigen::Isometry>& T,
    const Eigen::MatrixBase<DerivedDT>& dT) {
  const int nq_at_compile_time = DerivedDT::ColsAtCompileTime;
  int nq = dT.cols();

  const auto& R = T.linear();
  const auto& p = T.translation();

  std::array<int, 3> rows {0, 1, 2};
  std::array<int, 3> R_cols {0, 1, 2};
  std::array<int, 1> p_cols {3};

  auto dR = getSubMatrixGradient(dT, rows, R_cols, T.Rows);
  auto dp = getSubMatrixGradient(dT, rows, p_cols, T.Rows);

  auto dinvT_R = transposeGrad(dR, R.rows());
  auto dinvT_p = (-R.transpose() * dp - matGradMult(dinvT_R, p)).eval();

  const int numel = HOMOGENEOUS_TRANSFORM_SIZE;
  Eigen::Matrix<Scalar, numel, nq_at_compile_time> ret(numel, nq);
  setSubMatrixGradient(ret, dinvT_R, rows, R_cols, T.Rows);
  setSubMatrixGradient(ret, dinvT_p, rows, p_cols, T.Rows);

  // zero out gradient of elements in last row:
  const int last_row = 3;
  for (int col = 0; col < T.HDim; col++) {
    ret.row(last_row + col * T.Rows).setZero();
  }

  return ret;
}

void TranslationRotation3D::setF(const std::vector<double> &F_in) {
  if (F_in.size() != 16)
    throw std::runtime_error(
        "TranslationRotation3D::setF: F_in requires 16 elements");

  if ((F_in.at(12) != 0.0) || (F_in.at(13) != 0.0) || (F_in.at(14) != 0.0) ||
      (F_in.at(15) != 1.0))
    throw std::runtime_error(
        "TranslationRotation3D::setF: bottom row of F_in should be [0 0 0 1]");

  Eigen::Map<const Eigen::Matrix<double, 4, 4, Eigen::RowMajor> > F_in_eig(
      F_in.data());

  Eigen::Transform<double, 3, Eigen::Affine> F;
  F = F_in_eig;

  double tmpT[3];
  Eigen::Map<Eigen::Vector3d> tra_eig(tmpT);
  tra_eig = F.translation();

  double tmpR_mat[9];
  Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > rot_eig(tmpR_mat);
  rot_eig = F.rotation();

  setT(tmpT);
  setR_mat(tmpR_mat);
  updateR_mat(); // for stability
}

	operator Eigen::Transform<double, 3, Eigen::Affine, _Options>() const
	{
	    Eigen::Transform<double, 3, Eigen::Affine, _Options> ret;
	    ret.setIdentity();
	    ret.rotate(this->orientation);
	    ret.translation() = this->position;
	    return ret;
	}

/**
 * @function wsDiff
 */
Eigen::VectorXd JG_RRT::wsDiff( Eigen::VectorXd q )
{
    Eigen::Transform<double, 3, Eigen::Affine> T;
    robinaLeftArm_fk( q, TWBase, Tee, T );
    Eigen::VectorXd ws_diff = ( goalPosition - T.translation() );

    return ws_diff;
}

/**
 * @function wsDistance
 */
double goWSOrient::wsDistance( Eigen::VectorXd q )
{
  Eigen::Transform<double, 3, Eigen::Affine> T;
  robinaLeftArm_fk( q, TWBase, Tee, T );
  Eigen::VectorXd ws_diff = ( goalPos - T.translation() );
  double ws_dist = ws_diff.norm();
    
  return ws_dist;
}

/**
 * @function plan
 * @brief
 */
bool JG_RRT::plan( const Eigen::VectorXd &_startConfig,
                   const Eigen::Transform<double, 3, Eigen::Affine> &_goalPose,
                   const std::vector< Eigen::VectorXd > &_guidingNodes,
                   std::vector<Eigen::VectorXd> &path )
{
    /** Store information */
    this->startConfig = _startConfig;
    this->goalPose = _goalPose;
    this->goalPosition = _goalPose.translation();


    //-- Initialize the search tree
    addNode( startConfig, -1 );

    //-- Add the guiding nodes
    addGuidingNodes( _guidingNodes );

    double p;
    while( goalDistVector[activeNode] > distanceThresh )
    {
        //-- Generate the probability
        p = RANDNM( 0.0, 1.0 );

        //-- Apply either extension to goal (J) or random connection
        if( p < p_goal )
        {
            if( extendGoal() == true )
            {
                break;
            }
        }
        else
        {
            tryStep(); /*extendRandom();*/
        }

        // Check if we are still inside
        if( configVector.size() > maxNodes )
        {   cout<<"-- Exceeded "<<maxNodes<<" allowed - ws_dist: "<<goalDistVector[rankingVector[0]]<<endl;
            break;
        }
    }

    //-- If a path is found
    if( goalDistVector[activeNode] < distanceThresh )
    {   tracePath( activeNode, path );
        cout<<"JG - Got a path! - nodes: "<<path.size()<<" tree size: "<<configVector.size()<<endl;
        return true;
    }
    else
    {   cout<<"--(!) JG :No successful path found! "<<endl;
        return false;
    }
}

/**
 * @function Basic_M_RRT
 * @brief
 */
bool M_RRT::BasicPlan( std::vector<Eigen::VectorXd> &path, 
			Eigen::VectorXd &_startConfig, 
			Eigen::Transform<double, 3, Eigen::Affine> &_goalPose, 
                        Eigen::Transform<double, 3, Eigen::Affine> _TWBase,
                        Eigen::Transform<double, 3, Eigen::Affine> _Tee )
{
  printf("Basic Plan \n");

  /** Store information */
  this->startConfig = _startConfig;
  this->goalPose = _goalPose;
  this->goalPosition = _goalPose.translation();
  this->TWBase = _TWBase;
  this->Tee = _Tee;

  //-- Initialize the search tree
  addNode( startConfig, -1 );

  //-- Calculate the heuristicThreshold
  heuristicThresh = w1*distThresh + w2*abs( cos( xAlignThresh ) - 1 ) +w3*abs( cos( yAlignThresh ) );

  //-- Let's start the loop
  double p;
  double heuristic = heuristicVector[0];
  int gc = 0; int rc = 0;
  while( heuristic > heuristicThresh )
   { 
     //-- Probability
     p = rand()%100; 

     //-- Either extends towards goal or random
     if( p < pGoal )
       { printf("Goal \n");extendGoal(); gc++; }
     else
       { printf("Random \n"); extendRandom(); rc++;}

     //-- If bigger than maxNodes, get out loop 
     if( maxNodes > 0 && configVector.size() > maxNodes )
       { 
         cout<<"** Exceeded "<<maxNodes<<" MinCost: "<<heuristicVector[minHeuristicIndex()]<<"MinRankingCost: "<<heuristicVector[rankingVector[0]]<<endl;
         printf("Goal counter: %d, random counter: %d \n", gc, rc );
         return false; }

     heuristic = heuristicVector[ rankingVector[0] ];
   }
  printf("Goal counter: %d, random counter: %d \n", gc, rc );
  printf( "-- Plan successfully generated with %d nodes \n", configVector.size() );
  tracePath( activeNode, path );
  return true;
}

/**
 * @function workspaceDist
 * @brief
 */
Eigen::VectorXd M_RRT::workspaceDist( Eigen::VectorXd node, Eigen::VectorXd ws_target )
{
  Eigen::Transform<double, 3, Eigen::Affine> T;
  Eigen::VectorXd diff;

  // Calculate the EE position
  robinaLeftArm_fk( node, TWBase, Tee, T );
  Eigen::VectorXd ws_node = T.translation();

  // Calculate the workspace distance to goal
  diff = ( ws_target - ws_node );

  return diff;
}

typename Gradient<DerivedX, DerivedDX::ColsAtCompileTime>::type dTransformAdjointTranspose(
    const Eigen::Transform<Scalar, 3, Eigen::Isometry>& T,
    const Eigen::MatrixBase<DerivedX>& X,
    const Eigen::MatrixBase<DerivedDT>& dT,
    const Eigen::MatrixBase<DerivedDX>& dX) {
  assert(dT.cols() == dX.cols());
  int nq = dT.cols();

  const auto& R = T.linear();
  const auto& p = T.translation();

  std::array<int, 3> rows {0, 1, 2};
  std::array<int, 3> R_cols {0, 1, 2};
  std::array<int, 1> p_cols {3};

  auto dR = getSubMatrixGradient(dT, rows, R_cols, T.Rows);
  auto dp = getSubMatrixGradient(dT, rows, p_cols, T.Rows);

  auto Rtranspose = R.transpose();
  auto dRtranspose = transposeGrad(dR, R.rows());

  typename Gradient<DerivedX, DerivedDX::ColsAtCompileTime>::type ret(X.size(), nq);
  std::array<int, 3> Xomega_rows {0, 1, 2};
  std::array<int, 3> Xv_rows {3, 4, 5};
  for (int col = 0; col < X.cols(); col++) {
    auto Xomega_col = X.template block<3, 1>(0, col);
    auto Xv_col = X.template block<3, 1>(3, col);

    std::array<int, 1> col_array {col};
    auto dXomega_col = getSubMatrixGradient(dX, Xomega_rows, col_array, X.rows());
    auto dXv_col = getSubMatrixGradient(dX, Xv_rows, col_array, X.rows());

    auto dp_hatXv_col = (dp.colwise().cross(Xv_col) - dXv_col.colwise().cross(p)).eval();
    auto Xomega_col_minus_p_cross_Xv_col = (Xomega_col - p.cross(Xv_col)).eval();
    auto dXomega_transformed_col = (Rtranspose * (dXomega_col - dp_hatXv_col) + matGradMult(dRtranspose, Xomega_col_minus_p_cross_Xv_col)).eval();
    auto dRtransposeXv_col = (Rtranspose * dXv_col + matGradMult(dRtranspose, Xv_col)).eval();

    setSubMatrixGradient(ret, dXomega_transformed_col, Xomega_rows, col_array, X.rows());
    setSubMatrixGradient(ret, dRtransposeXv_col, Xv_rows, col_array, X.rows());
  }
  return ret;
}

 CalibrateKinectCheckerboard()
   : nh_("~"), it_(nh_), calibrated(false)
 {
   // Load parameters from the server.
   nh_.param<std::string>("fixed_frame", fixed_frame, "/base_link");
   nh_.param<std::string>("camera_frame", camera_frame, "/camera_link");
   nh_.param<std::string>("target_frame", target_frame, "/calibration_pattern");
   nh_.param<std::string>("tip_frame", tip_frame, "/gripper_link");
   
   nh_.param<int>("checkerboard_width", checkerboard_width, 6);
   nh_.param<int>("checkerboard_height", checkerboard_height, 7);
   nh_.param<double>("checkerboard_grid", checkerboard_grid, 0.027);
   
   // Set pattern detector sizes
   pattern_detector_.setPattern(cv::Size(checkerboard_width, checkerboard_height), checkerboard_grid, CHESSBOARD);
   
   transform_.translation().setZero();
   transform_.matrix().topLeftCorner<3, 3>() = Quaternionf().setIdentity().toRotationMatrix();
   
   // Create subscriptions
   info_sub_ = nh_.subscribe("/camera/rgb/camera_info", 1, &CalibrateKinectCheckerboard::infoCallback, this);
   
   // Also publishers
   pub_ = it_.advertise("calibration_pattern_out", 1);
   detector_pub_ = nh_.advertise<pcl::PointCloud<pcl::PointXYZ> >("detector_cloud", 1);
   physical_pub_ = nh_.advertise<pcl::PointCloud<pcl::PointXYZ> >("physical_points_cloud", 1);
   
   // Create ideal points
   ideal_points_.push_back( pcl::PointXYZ(0, 0, 0) );
   ideal_points_.push_back( pcl::PointXYZ((checkerboard_width-1)*checkerboard_grid, 0, 0) );
   ideal_points_.push_back( pcl::PointXYZ(0, (checkerboard_height-1)*checkerboard_grid, 0) );
   ideal_points_.push_back( pcl::PointXYZ((checkerboard_width-1)*checkerboard_grid, (checkerboard_height-1)*checkerboard_grid, 0) );
   
   // Create proper gripper tip point
   nh_.param<double>("gripper_tip_x", gripper_tip.point.x, 0.0);
   nh_.param<double>("gripper_tip_y", gripper_tip.point.y, 0.0);
   nh_.param<double>("gripper_tip_z", gripper_tip.point.z, 0.0);
   gripper_tip.header.frame_id = tip_frame;
   
   ROS_INFO("[calibrate] Initialized.");
 }

template <typename PointT, typename Scalar> double
pcl::getPrincipalTransformation (const pcl::PointCloud<PointT> &cloud, 
                                 Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
{
  EIGEN_ALIGN16 Eigen::Matrix<Scalar, 3, 3> covariance_matrix;
  Eigen::Matrix<Scalar, 4, 1> centroid;
  
  pcl::computeMeanAndCovarianceMatrix (cloud, covariance_matrix, centroid);

  EIGEN_ALIGN16 Eigen::Matrix<Scalar, 3, 3> eigen_vects;
  Eigen::Matrix<Scalar, 3, 1> eigen_vals;
  pcl::eigen33 (covariance_matrix, eigen_vects, eigen_vals);

  double rel1 = eigen_vals.coeff (0) / eigen_vals.coeff (1);
  double rel2 = eigen_vals.coeff (1) / eigen_vals.coeff (2);
  
  transform.translation () = centroid.head (3);
  transform.linear () = eigen_vects;
  
  return (std::min (rel1, rel2));
}

int main(int argc, char** argv){
  ros::init(argc, argv, "workspace_transformation");

  ros::NodeHandle node;
  ros::Rate loop_rate(loop_rate_in_hz);


  // Topics
  //  Publishers
  ros::Publisher pub = node.advertise<geometry_msgs::PoseStamped>("poseSlaveWorkspace", 1);
  ros::Publisher pub_poseSlaveWSOrigin = node.advertise<geometry_msgs::PoseStamped>("poseSlaveWorkspaceOrigin", 1);
  ros::Publisher pub_set_camera_pose = node.advertise<geometry_msgs::PoseStamped>("Set_ActiveCamera_Pose", 1);
  pub_OmniForceFeedback = node.advertise<geometry_msgs::Vector3>("set_forces", 1);
  //  Subscribers
  ros::Subscriber sub_PoseMasterWS = node.subscribe("poseMasterWorkspace", 1, &PoseMasterWSCallback);
  ros::Subscriber sub_BaseMasterWS = node.subscribe("baseMasterWorkspace", 1, &BaseMasterWSCallback);
  ros::Subscriber sub_BaseSlaveWS = node.subscribe("baseSlaveWorkspace", 1, &BaseSlaveWSCallback);
  ros::Subscriber sub_OriginSlaveWS = node.subscribe("originSlaveWorkspace", 1, &OriginSlaveWSCallback); 
  ros::Subscriber sub_scale = node.subscribe("scale", 1, &scaleCallback);
  ros::Subscriber subOmniButtons = node.subscribe("button", 1, &omniButtonCallback);
  ros::Subscriber sub_HapticAngles = node.subscribe("angles", 1, &HapticAnglesCallback);
  //  Services
  ros::ServiceServer service_server_algorithm = node.advertiseService("set_algorithm", algorithmSrvCallback);

  
  // INITIALIZATION ------------------------------------------------------------------------
  
  //  Haptic
  omni_white_button_pressed = omni_grey_button_pressed = first_haptic_angle_read = false;  
  
  //  Algorithm
  algorithm_type = 0;

  for (unsigned int i=0; i<3; i++)	scale[i] = 1.0;  
  
  m_init = s_init = mi_init = s0_init = algorithm_set = false;

  dm << 0,0,0;
  ds << 0,0,0;
  pm_im1 << 0,0,0;
  ps_im1 << 0,0,0;
  vm_i << 0,0,0;
  
  ps_0 << 0.0, 0.0, 0.0;
  quats_0.x() = quats_0.y() = quats_0.z() = 0.0;
  quats_0.w() = 1.0;
  Rs_0 = quats_0.toRotationMatrix();  

  //   Auxiliary pose
  geometry_msgs::PoseStamped outputPose;  
  outputPose.pose.position.x = outputPose.pose.position.y = outputPose.pose.position.z = 0.0;
  outputPose.pose.orientation.x = outputPose.pose.orientation.y = outputPose.pose.orientation.z = 0.0;
  outputPose.pose.orientation.w = 1.0;  
  
  //   Workspace boundaries
  Xmin = -5.0;
  Ymin = -5.0;
  Zmin = 0.0;
  Xmax = 5.0;
  Ymax = 5.0;
  Zmax = 2.0;
  
  // Default camera pose
  geometry_msgs::PoseStamped cameraPose;  
  cameraPose.pose.position.x = cameraPose.pose.position.y = cameraPose.pose.position.z = 0.0;
  cameraPose.pose.orientation.x = cameraPose.pose.orientation.y = cameraPose.pose.orientation.z = 0.0;
  cameraPose.pose.orientation.w = 1.0;
  
  Eigen::Vector3d origin_cam = 10.0 * Eigen::Vector3d(1.0, 0.0, 0.5);
  T_camPose_S.translation()  = Eigen::Vector3d(0.0, 0.0, 1.0) + origin_cam;

  Eigen::Vector3d eigen_cam_axis_z = origin_cam.normalized();
  Eigen::Vector3d eigen_cam_axis_x = ( Eigen::Vector3d::UnitZ().cross( eigen_cam_axis_z ) ).normalized();
  Eigen::Vector3d eigen_cam_axis_y = ( eigen_cam_axis_z.cross( eigen_cam_axis_x ) ).normalized();

  T_camPose_S.linear() << eigen_cam_axis_x(0), eigen_cam_axis_y(0), eigen_cam_axis_z(0), 
			  eigen_cam_axis_x(1), eigen_cam_axis_y(1), eigen_cam_axis_z(1),
			  eigen_cam_axis_x(2), eigen_cam_axis_y(2), eigen_cam_axis_z(2);
  
  // Time management
  period = 1.0/(double)loop_rate_in_hz;
  timeval past_time_, current_time_;  
  gettimeofday(&current_time_, NULL);  
  time_increment_ = 0;
  
  // File management
  std::ofstream WTdataRecord;  
  WTdataRecord.open("/home/users/josep.a.claret/data/WTdataRecord.txt", std::ofstream::trunc);
    
  
  // UPDATE -------------------------------------------------------------------------------
  while (ros::ok())
  {
    if (m_init && s_init && mi_init)
    {
      // Time management
//       past_time_ = current_time_;
//       gettimeofday(&current_time_, NULL);
//       time_increment_ = ( (current_time_.tv_sec*1e6 + current_time_.tv_usec) - (past_time_.tv_sec*1e6 + past_time_.tv_usec) ) / 1e6;
      time_increment_ = period;
//.........这里部分代码省略.........