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

geometry_msgs::Pose PoseAffineToGeomMsg(const Eigen::Affine3d &e) {
    geometry_msgs::Pose m;
    m.position.x = e.translation().x();
    m.position.y = e.translation().y();
    m.position.z = e.translation().z();
    // This is a column major vs row major matrice faux pas!
#if 0
    MatrixEXd em = e.rotation();

    Eigen::Quaterniond q = EMatrix2Quaterion(em);
#endif
    Eigen::Quaterniond q(e.rotation());
    m.orientation.x = q.x();
    m.orientation.y = q.y();
    m.orientation.z = q.z();
    m.orientation.w = q.w();
#if 0
    if (m.orientation.w < 0) {
        m.orientation.x *= -1;
        m.orientation.y *= -1;
        m.orientation.z *= -1;
        m.orientation.w *= -1;
    }
#endif
}

bool kinematic_constraints::VisibilityConstraint::equal(const KinematicConstraint &other, double margin) const
{
  if (other.getType() != type_)
    return false;
  const VisibilityConstraint &o = static_cast<const VisibilityConstraint&>(other);
  
  if (target_frame_id_ == o.target_frame_id_ && sensor_frame_id_ == o.sensor_frame_id_ &&
      cone_sides_ == o.cone_sides_ && sensor_view_direction_ == o.sensor_view_direction_)
  {
    if (fabs(max_view_angle_ - o.max_view_angle_) > margin ||
        fabs(target_radius_ - o.target_radius_) > margin)
      return false;
    Eigen::Affine3d diff = sensor_pose_.inverse() * o.sensor_pose_;
    if (diff.translation().norm() > margin)
      return false;
    if (!diff.rotation().isIdentity(margin))
      return false;
    diff = target_pose_.inverse() * o.target_pose_;
    if (diff.translation().norm() > margin)
      return false;
    if (!diff.rotation().isIdentity(margin))
      return false;
    return true;
  }
  return false;
}

/*!
* \brief affine3d2UrdfPose converts an  Eigen affine 4x4 matrix  o represent the pose into a urdf pose 
* vparam pose   eigen Affine3d pose 
* \return   urdf pose with position and rotation.
*/
RCS::Pose Affine3d2UrdfPose(const  Eigen::Affine3d &pose) {
    RCS::Pose p;
    p.getOrigin().setX(pose.translation().x());
    p.getOrigin().setY(pose.translation().y());
    p.getOrigin().setZ(pose.translation().z());

    Eigen::Quaterniond q (pose.rotation());
    tf::Quaternion qtf(q.x(),q.y(),q.z(),q.w());
    //std::cout <<  "Affine3d2UrdfPose Quaterion = \n" << q.x() << ":" << q.y() << ":" << q.z() << ":" << q.w() << std::endl;

    p.setRotation(qtf);
    //std::cout <<  "After Affine3d2UrdfPose Quaterion = \n" << p.getRotation().x() << ":" << p.getRotation().y() << ":" << p.getRotation().z() << ":" << p.getRotation().w() << std::endl;

#if 0
    MatrixEXd m = pose.rotation();
    Eigen::Quaterniond q = EMatrix2Quaterion(m);

    Eigen::Quaterniond q(pose.rotation());
    p.getRotation().setX(q.x());
    p.getRotation().setY(q.y());
    p.getRotation().setZ(q.z());
    p.getRotation().setW(q.w());
    #endif
   return p;
}

void UpperBodyPlanner::pose_moveTo2(const std::string &link_name,
                                    const Eigen::Affine3d& current_pose,
                                    const Eigen::Affine3d& desired_pose, const int &step_num,
                                    std::vector<geometry_msgs::Pose> &pose_sequence) {
    //Eigen::Affine3d current_pose = kinematic_state->getGlobalLinkTransform(link_name);
    Eigen::Quaterniond current_q = Rmat2Quaternion(current_pose.rotation());
    Eigen::Quaterniond desired_q = Rmat2Quaternion(desired_pose.rotation());
    Eigen::Quaterniond step_q;

    std::cout << "**********************************" << std::endl;
    std::cout << "end_effector_name is: " << link_name << std::endl;
    std::cout << "Current translation is: " << current_pose.translation().transpose() << std::endl;
    std::cout << "current rotation is: " << std::endl;
    std::cout << current_pose.rotation() << std::endl;
    std::cout << "current quaternion is: "<< std::endl;
    std::cout << "w = " << current_q.w() << ", x = " << current_q.x() << ", y = " << current_q.y() << ", z = " << current_q.z() << std::endl;
    std::cout << "Desired translation is: " << desired_pose.translation().transpose() << std::endl;
    std::cout << "Desired rotation is: " << std::endl;
    std::cout << desired_pose.rotation() << std::endl;
    std::cout << "Desired quaternion is: " << std::endl;
    std::cout << "w = " << desired_q.w() << ", x = " << desired_q.x() << ", y = " << desired_q.y() << ", z = " << desired_q.z() << std::endl;
    std::cout << "**********************************" << std::endl;

    Eigen::Vector3d step_translation_movement = (desired_pose.translation() - current_pose.translation()) / step_num;
    for (int i = 0; i < step_num; i++) {
        Eigen::Affine3d step_target;
        step_target.translation() = current_pose.translation() + (i + 1) * step_translation_movement;
        step_q = current_q.slerp((i + 1.0) / step_num, desired_q);
        geometry_msgs::Pose target_pose = Eigen2msgPose(step_target.translation(), step_q);
        pose_sequence.push_back(target_pose);
    }
}

void UpperBodyPlanner::addPose_moveTo(const Eigen::Affine3d& start_pose, const Eigen::Affine3d& end_pose, const int& step_num, std::vector<geometry_msgs::Pose>& pose_sequence) {
    Eigen::Quaterniond start_q = Rmat2Quaternion(start_pose.rotation());
    Eigen::Quaterniond end_q = Rmat2Quaternion(end_pose.rotation());
    Eigen::Quaterniond step_q;

    std::cout << "**********************************" << std::endl;
    std::cout << "start_pose translation is: " << start_pose.translation().transpose() << std::endl;
    std::cout << "start_pose rotation is: " << std::endl;
    std::cout << start_pose.rotation() << std::endl;
    std::cout << "start_pose quaternion is: "<< std::endl;
    std::cout << "w = " << start_q.w() << ", x = " << start_q.x() << ", y = " << start_q.y() << ", z = " << start_q.z() << std::endl;
    std::cout << "end_pose translation is: " << end_pose.translation().transpose() << std::endl;
    std::cout << "end_pose rotation is: " << std::endl;
    std::cout << end_pose.rotation() << std::endl;
    std::cout << "Desired quaternion is: " << std::endl;
    std::cout << "w = " << end_q.w() << ", x = " << end_q.x() << ", y = " << end_q.y() << ", z = " << end_q.z() << std::endl;
    std::cout << "**********************************" << std::endl;

    Eigen::Vector3d step_translation_movement = (end_pose.translation() - start_pose.translation()) / step_num;
    for (int i = 0; i < step_num; i++) {
        Eigen::Affine3d step_target;
        step_target.translation() = start_pose.translation() + (i + 1) * step_translation_movement;
        step_q = start_q.slerp((i + 1.0) / step_num, end_q);
        geometry_msgs::Pose target_pose = Eigen2msgPose(step_target.translation(), step_q);
        pose_sequence.push_back(target_pose);
    }
}

bool SensorProcessorBase::updateTransformations(const std::string& sensorFrameId,
                                                const ros::Time& timeStamp)
{
  try {
    transformListener_.waitForTransform(sensorFrameId, mapFrameId_, timeStamp, ros::Duration(1.0));

    tf::StampedTransform transformTf;
    transformListener_.lookupTransform(mapFrameId_, sensorFrameId, timeStamp, transformTf);
    poseTFToEigen(transformTf, transformationSensorToMap_);

    transformListener_.lookupTransform(robotBaseFrameId_, sensorFrameId, timeStamp, transformTf);  // TODO Why wrong direction?
    Eigen::Affine3d transform;
    poseTFToEigen(transformTf, transform);
    rotationBaseToSensor_.setMatrix(transform.rotation().matrix());
    translationBaseToSensorInBaseFrame_.toImplementation() = transform.translation();

    transformListener_.lookupTransform(mapFrameId_, robotBaseFrameId_, timeStamp, transformTf);  // TODO Why wrong direction?
    poseTFToEigen(transformTf, transform);
    rotationMapToBase_.setMatrix(transform.rotation().matrix());
    translationMapToBaseInMapFrame_.toImplementation() = transform.translation();

    return true;
  } catch (tf::TransformException &ex) {
    ROS_ERROR("%s", ex.what());
    return false;
  }
}

TEST_F(LoadPlanningModelsPr2, InitOK)
{
  ASSERT_TRUE(urdf_ok_);
  ASSERT_EQ(urdf_model_->getName(), "pr2_test");

  robot_model::RobotModelPtr kmodel(new robot_model::RobotModel(urdf_model_, srdf_model_));
  robot_state::RobotState ks(kmodel);
  ks.setToRandomValues();
  ks.setToDefaultValues();


  robot_state::Transforms tf(kmodel->getModelFrame());

  Eigen::Affine3d t1;
  t1.setIdentity();
  t1.translation() = Eigen::Vector3d(10.0, 1.0, 0.0);
  tf.setTransform(t1, "some_frame_1");

  Eigen::Affine3d t2(Eigen::Translation3d(10.0, 1.0, 0.0)*Eigen::AngleAxisd(0.5, Eigen::Vector3d::UnitY()));
  tf.setTransform(t2, "some_frame_2");

  Eigen::Affine3d t3;
  t3.setIdentity();
  t3.translation() = Eigen::Vector3d(0.0, 1.0, -1.0);
  tf.setTransform(t3, "some_frame_3");


  EXPECT_TRUE(tf.isFixedFrame("some_frame_1"));
  EXPECT_FALSE(tf.isFixedFrame("base_footprint"));
  EXPECT_TRUE(tf.isFixedFrame(kmodel->getModelFrame()));

  Eigen::Affine3d x;
  x.setIdentity();
  tf.transformPose(ks, "some_frame_2", x, x);
  
  EXPECT_TRUE(t2.translation() == x.translation());
  EXPECT_TRUE(t2.rotation() == x.rotation());

  tf.transformPose(ks, kmodel->getModelFrame(), x, x);
  EXPECT_TRUE(t2.translation() == x.translation());
  EXPECT_TRUE(t2.rotation() == x.rotation());

  x.setIdentity();
  tf.transformPose(ks, "r_wrist_roll_link", x, x);

  EXPECT_NEAR(x.translation().x(), 0.585315, 1e-4);
  EXPECT_NEAR(x.translation().y(), -0.188, 1e-4);
  EXPECT_NEAR(x.translation().z(), 1.24001, 1e-4);
}

void planning_models::KinematicModel::RevoluteJointModel::computeJointStateValues(const Eigen::Affine3d& transf, std::vector<double> &joint_values) const
{
  joint_values.resize(1);
  Eigen::Quaterniond q(transf.rotation());
  q.normalize();
  joint_values[0] = acos(q.w())*2.0f;
}

Eigen::Isometry3d toIsometry(const Eigen::Affine3d& pose)
{
  Eigen::Isometry3d p(pose.rotation());
  p.translation() = pose.translation();

  return p;
}

bool kinematic_constraints::PositionConstraint::equal(const KinematicConstraint &other, double margin) const
{
  if (other.getType() != type_)
    return false;
  const PositionConstraint &o = static_cast<const PositionConstraint&>(other);
  
  if (link_model_ == o.link_model_ && constraint_frame_id_ == o.constraint_frame_id_)
  {
    if ((offset_ - o.offset_).norm() > margin)
      return false;
    if (constraint_region_.size() != o.constraint_region_.size())
      return false;
    for (std::size_t i = 0 ; i < constraint_region_.size() ; ++i)
    {
      Eigen::Affine3d diff = constraint_region_pose_[i].inverse() * o.constraint_region_pose_[i];
      if (diff.translation().norm() > margin)
        return false;
      if (!diff.rotation().isIdentity(margin))
        return false;
      if (fabs(constraint_region_[i]->computeVolume() - o.constraint_region_[i]->computeVolume()) >= margin)
        return false;
    }
    return true;
  }
  return false;
}

bool CartesianTrajectoryAction::checkTolerance(Eigen::Affine3d err, cartesian_trajectory_msgs::CartesianTolerance tol) {
  if ((tol.position.x > 0.0) && (fabs(err.translation().x()) > tol.position.x)) {
    return false;
  }

  if ((tol.position.y > 0.0) && (fabs(err.translation().y()) > tol.position.y)) {
    return false;
  }

  if ((tol.position.z > 0.0) && (fabs(err.translation().z()) > tol.position.z)) {
    return false;
  }

  Eigen::AngleAxisd ax(err.rotation());
  Eigen::Vector3d rot = ax.axis() * ax.angle();

  if ((tol.rotation.x > 0.0) && (fabs(rot(0)) > tol.rotation.x)) {
    return false;
  }

  if ((tol.rotation.y > 0.0) && (fabs(rot(1)) > tol.rotation.y)) {
    return false;
  }

  if ((tol.rotation.z > 0.0) && (fabs(rot(2)) > tol.rotation.z)) {
    return false;
  }

  return true;
}

void MotionPlanningFrame::updateCollisionObjectPose(bool update_marker_position)
{
  QList<QListWidgetItem *> sel = ui_->collision_objects_list->selectedItems();
  if (sel.empty())
    return;
  planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW();
  if (ps)
  {
    collision_detection::CollisionWorld::ObjectConstPtr obj = ps->getWorld()->getObject(sel[0]->text().toStdString());
    if (obj && obj->shapes_.size() == 1)
    {
      Eigen::Affine3d p;
      p.translation()[0] = ui_->object_x->value();
      p.translation()[1] = ui_->object_y->value();
      p.translation()[2] = ui_->object_z->value();

      p = Eigen::Translation3d(p.translation()) *
        (Eigen::AngleAxisd(ui_->object_rx->value(), Eigen::Vector3d::UnitX()) *
         Eigen::AngleAxisd(ui_->object_ry->value(), Eigen::Vector3d::UnitY()) *
         Eigen::AngleAxisd(ui_->object_rz->value(), Eigen::Vector3d::UnitZ()));

      ps->getWorldNonConst()->moveShapeInObject(obj->id_, obj->shapes_[0], p);
      planning_display_->queueRenderSceneGeometry();

      // Update the interactive marker pose to match the manually introduced one
      if (update_marker_position && scene_marker_)
      {
        Eigen::Quaterniond eq(p.rotation());
        scene_marker_->setPose(Ogre::Vector3(ui_->object_x->value(), ui_->object_y->value(), ui_->object_z->value()),
                               Ogre::Quaternion(eq.w(), eq.x(), eq.y(), eq.z()), "");
      }
    }
  }
}

 inline std::string PrettyPrint(const Eigen::Affine3d& transform_to_print, const bool add_delimiters, const std::string& separator)
 {
     UNUSED(add_delimiters);
     UNUSED(separator);
     Eigen::Vector3d vector_to_print = transform_to_print.translation();
     Eigen::Quaterniond quaternion_to_print(transform_to_print.rotation());
     return "Affine3d <x: " + std::to_string(vector_to_print.x()) + " y: " + std::to_string(vector_to_print.y()) + " z: " + std::to_string(vector_to_print.z()) + ">, <x: " + std::to_string(quaternion_to_print.x()) + " y: " + std::to_string(quaternion_to_print.y()) + " z: " + std::to_string(quaternion_to_print.z()) + " w: " + std::to_string(quaternion_to_print.w()) + ">";
 }

void SceneCloudView::setViewerPose(const Eigen::Affine3d& viewer_pose)
{
    Eigen::Vector3d pos_vector = viewer_pose * Eigen::Vector3d(0, 0, 0);
    Eigen::Vector3d look_at_vector = viewer_pose.rotation() * Eigen::Vector3d(0, 0, 0) + pos_vector;
    Eigen::Vector3d up_vector = viewer_pose.rotation() * Eigen::Vector3d(0, -1, 0);
    pcl17::visualization::Camera cam;
    cam.pos[0] = pos_vector[0];
    cam.pos[1] = pos_vector[1];
    cam.pos[2] = pos_vector[2];
    cam.focal[0] = look_at_vector[0];
    cam.focal[1] = look_at_vector[1];
    cam.focal[2] = look_at_vector[2];
    cam.view[0] = up_vector[0];
    cam.view[1] = up_vector[1];
    cam.view[2] = up_vector[2];
    cloud_viewer_->setCameraPosition((double)pos_vector[0], (double)pos_vector[1], (double)pos_vector[2], (double)look_at_vector[0],
                                     (double)look_at_vector[1], (double)look_at_vector[2], (double)up_vector[0], (double)up_vector[1],
                                     (double)up_vector[2]);
    cloud_viewer_->updateCamera();
}

void robot_model::FloatingJointModel::computeJointStateValues(const Eigen::Affine3d& transf, std::vector<double> &joint_values) const
{
  joint_values.resize(7);
  joint_values[0] = transf.translation().x();
  joint_values[1] = transf.translation().y();
  joint_values[2] = transf.translation().z();
  Eigen::Quaterniond q(transf.rotation());
  joint_values[3] = q.x();
  joint_values[4] = q.y();
  joint_values[5] = q.z();
  joint_values[6] = q.w();
}