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

 void tfToPose(tf::Transform &trans, geometry_msgs::PoseStamped &msg)
 {
   tf::quaternionTFToMsg(trans.getRotation(), msg.pose.orientation);
   msg.pose.position.x = trans.getOrigin().x();
   msg.pose.position.y = trans.getOrigin().y();
   msg.pose.position.z = trans.getOrigin().z();
 }

static void print_transform(tf::Transform transform)
{	
	carmen_orientation_3D_t orientation = get_carmen_orientation_from_tf_transform(transform);
	printf("y:% lf p:% lf r:% lf\n", orientation.yaw, orientation.pitch, orientation.roll);
	printf("x:% lf y:% lf z:% lf\n", transform.getOrigin().x(), transform.getOrigin().y(), transform.getOrigin().z());
	printf("\n");
}

void
fromTF(const tf::Transform &source, Eigen::Matrix4f &dest, geometry_msgs::Pose &pose_dest)
{
    Eigen::Quaternionf q(source.getRotation().getW(),source.getRotation().getX(), source.getRotation().getY(),source.getRotation().getZ());
    q.normalize();
    Eigen::Vector3f t(source.getOrigin().x(), source.getOrigin().y(), source.getOrigin().z());
    Eigen::Matrix3f R(q.toRotationMatrix());
    dest(0,0) = R(0,0);
    dest(0,1) = R(0,1);
    dest(0,2) = R(0,2);
    dest(1,0) = R(1,0);
    dest(1,1) = R(1,1);
    dest(1,2) = R(1,2);
    dest(2,0) = R(2,0);
    dest(2,1) = R(2,1);
    dest(2,2) = R(2,2);
    dest(3,0) = dest(3,1)= dest(3,2) = 0;
    dest(3,3) = 1;
    dest(0,3) = t(0);
    dest(1,3) = t(1);
    dest(2,3) = t(2);
    pose_dest.orientation.x = q.x();
    pose_dest.orientation.y = q.y();
    pose_dest.orientation.z = q.z();
    pose_dest.orientation.w = q.w();
    pose_dest.position.x = t(0);
    pose_dest.position.y = t(1);
    pose_dest.position.z = t(2);
}

void TransformROSBridge::calculateTwist(const tf::Transform& _current_trans, const tf::Transform& _prev_trans, tf::Vector3& _linear_twist, tf::Vector3& _angular_twist, double _dt)
{
    // current rotation matrix
    tf::Matrix3x3 current_basis = _current_trans.getBasis();

    // linear twist
    tf::Vector3 current_origin = _current_trans.getOrigin();
    tf::Vector3 prev_origin = _prev_trans.getOrigin();
    _linear_twist = current_basis.transpose() * ((current_origin - prev_origin) / _dt);

    // angular twist
    // R = exp(omega_w*dt) * prev_R
    // omega_w is described in global coordinates in relationships of twist transformation.
    // it is easier to calculate omega using hrp functions than tf functions
    tf::Matrix3x3 prev_basis = _prev_trans.getBasis();
    double current_rpy[3], prev_rpy[3];
    current_basis.getRPY(current_rpy[0], current_rpy[1], current_rpy[2]);
    prev_basis.getRPY(prev_rpy[0], prev_rpy[1], prev_rpy[2]);
    hrp::Matrix33 current_hrpR = hrp::rotFromRpy(current_rpy[0], current_rpy[1], current_rpy[2]);
    hrp::Matrix33 prev_hrpR = hrp::rotFromRpy(prev_rpy[0], prev_rpy[1], prev_rpy[2]);
    hrp::Vector3 hrp_omega = current_hrpR.transpose() * hrp::omegaFromRot(current_hrpR * prev_hrpR.transpose()) / _dt;
    _angular_twist.setX(hrp_omega[0]);
    _angular_twist.setY(hrp_omega[1]);
    _angular_twist.setZ(hrp_omega[2]);

    return;
}

// Move the end effector (tf jaco_tool_position) to a certain position
void JacoCustom::moveToPoint(tf::Transform tf_){

    geometry_msgs::Twist arm;
    arm.linear.x = tf_.getOrigin().getX();
    arm.linear.y = tf_.getOrigin().getY();
    arm.linear.z = tf_.getOrigin().getZ();

//    double roll, pitch, yaw;
//    tf_.getBasis().getRPY(roll,pitch, yaw);
//    arm.angular.x = roll;
//    arm.angular.y = pitch;
//    arm.angular.z = yaw;



    wpi_jaco_msgs::QuaternionToEuler conv;
    geometry_msgs::Quaternion quat;
    tf::quaternionTFToMsg(tf_.getRotation(), quat);
    conv.request.orientation = quat;
    if(quaternion_to_euler_service_client.call(conv)){
        arm.angular.x = conv.response.roll;
        arm.angular.y = conv.response.pitch;
        arm.angular.z = conv.response.yaw;
    }

    moveToPoint(arm);
}

inline tf::Transform interpolateTF(tf::Transform start, tf::Transform end, double ratio)
{
	tf::Vector3 lerp_pos = start.getOrigin().lerp(end.getOrigin(), ratio);
	tf::Quaternion lerp_rot = start.getRotation().slerp(end.getRotation(), ratio);
	
	return tf::Transform(lerp_rot, lerp_pos);
}

void add_edge(GraphOptimizer3D* optimizer, int id1, int id2, tf::Transform pose,
		double sigma_position, double sigma_orientation) {
	Vector6 p;
	double roll, pitch, yaw;
	MAT_to_RPY(pose.getBasis(), roll, pitch, yaw);
	p[0] = pose.getOrigin().x();
	p[1] = pose.getOrigin().y();
	p[2] = pose.getOrigin().z();
	p[3] = roll;
	p[4] = pitch;
	p[5] = yaw;

	Matrix6 m = Matrix6::eye(1.0);
	double ip = 1 / SQR(sigma_position);
	double io = 1 / SQR(sigma_orientation);
	m[0][0] = ip;
	m[1][1] = ip;
	m[2][2] = ip;
	m[3][3] = io;
	m[4][4] = io;
	m[5][5] = io;
	GraphOptimizer3D::Vertex* v1 = optimizer->vertex(id1);
	GraphOptimizer3D::Vertex* v2 = optimizer->vertex(id2);
	if (!v1) {
		cerr << "adding edge, id1=" << id1 << " not found" << endl;
	}
	if (!v2) {
		cerr << "adding edge, id2=" << id2 << " not found" << endl;
	}
	Transformation3 t = Transformation3::fromVector(p);
	GraphOptimizer3D::Edge* e = optimizer->addEdge(v1, v2, t, m);
	if (!e) {
		cerr << "adding edge failed" << endl;
	}
}

void
save_localize_transform()
{
    std::ofstream filed_transform;
	std::stringstream file_name;

	std::string path = ros::package::getPath("prx_localizer");
	file_name << path << "/transform/localize_transform.bin";
    filed_transform.open(file_name.str().c_str(), std::ofstream::binary|std::ofstream::trunc);

    tfScalar x, y, z, w;
    x = g_localize_transform.getRotation().getX();
    y = g_localize_transform.getRotation().getY();
    z = g_localize_transform.getRotation().getZ();
    w = g_localize_transform.getRotation().getW();
    filed_transform.write((char *) &x, sizeof(tfScalar));
    filed_transform.write((char *) &y, sizeof(tfScalar));
    filed_transform.write((char *) &z, sizeof(tfScalar));
    filed_transform.write((char *) &w, sizeof(tfScalar));

    x = g_localize_transform.getOrigin().getX();
    y = g_localize_transform.getOrigin().getY();
    z = g_localize_transform.getOrigin().getZ();
    filed_transform.write((char *) &x, sizeof(tfScalar));
    filed_transform.write((char *) &y, sizeof(tfScalar));
    filed_transform.write((char *) &z, sizeof(tfScalar));

    filed_transform.close();
}

geometry_msgs::Pose transformToPose(tf::Transform &trans)
{
    geometry_msgs::Pose pose;
    pose.position.x = trans.getOrigin().x();
    pose.position.y = trans.getOrigin().y();
    pose.position.z = trans.getOrigin().z();
    tf::quaternionTFToMsg(trans.getRotation(), pose.orientation);
    return pose;
}

void leatherman::btTransformToPoseMsg(const tf::Transform &bt, geometry_msgs::Pose &pose)
{
  pose.position.x = bt.getOrigin().getX();
  pose.position.y = bt.getOrigin().getY();
  pose.position.z = bt.getOrigin().getZ();
  pose.orientation.x = bt.getRotation().x();
  pose.orientation.y = bt.getRotation().y();
  pose.orientation.z = bt.getRotation().z();
  pose.orientation.w = bt.getRotation().w();
}

void
fromTF(const tf::Transform &source, geometry_msgs::Pose &dest)
{
    dest.orientation.x = source.getRotation().getX();
    dest.orientation.y = source.getRotation().getY();
    dest.orientation.z = source.getRotation().getZ();
    dest.orientation.w = source.getRotation().getW();
    dest.position.x = source.getOrigin().x();
    dest.position.y = source.getOrigin().y();
    dest.position.z = source.getOrigin().z();
}

bool ROSToKlampt(const tf::Transform& T,RigidTransform& kT)
{
  kT.t.set(T.getOrigin().x(),T.getOrigin().y(),T.getOrigin().z());
  Vector3 row1(T.getBasis()[0].x(),T.getBasis()[0].y(),T.getBasis()[0].z());
  Vector3 row2(T.getBasis()[1].x(),T.getBasis()[1].y(),T.getBasis()[1].z());
  Vector3 row3(T.getBasis()[2].x(),T.getBasis()[2].y(),T.getBasis()[2].z());
  kT.R.setRow1(row1);
  kT.R.setRow2(row2);
  kT.R.setRow3(row3);
  return true;
}

std::vector<double> planning_models::KinematicModel::FloatingJointModel::computeJointStateValues(const tf::Transform& transform) const 
{
  std::vector<double> ret;
  ret.push_back(transform.getOrigin().x());
  ret.push_back(transform.getOrigin().y());
  ret.push_back(transform.getOrigin().z());
  ret.push_back(transform.getRotation().x());
  ret.push_back(transform.getRotation().y());
  ret.push_back(transform.getRotation().z());
  ret.push_back(transform.getRotation().w());
  return ret;
}

void tfToXYZRPY(
  const tf::Transform& t,
  double& x,    double& y,     double& z,
  double& roll, double& pitch, double& yaw)
{
  x = t.getOrigin().getX();
  y = t.getOrigin().getY();
  z = t.getOrigin().getZ();

  tf::Matrix3x3  m(t.getRotation());
  m.getRPY(roll, pitch, yaw);
}

geometry_msgs::PoseWithCovarianceStamped
tfToPoseCovarianceStamped (const tf::Transform &pose)
{
	geometry_msgs::PoseWithCovarianceStamped pocv;
	pocv.pose.pose.position.x = pose.getOrigin().x();
	pocv.pose.pose.position.y = pose.getOrigin().y();
	pocv.pose.pose.position.z = pose.getOrigin().z();
	pocv.pose.pose.orientation.x = pose.getRotation().x();
	pocv.pose.pose.orientation.y = pose.getRotation().y();
	pocv.pose.pose.orientation.z = pose.getRotation().z();
	pocv.pose.pose.orientation.w = pose.getRotation().w();
	return pocv;
}