C++ Vector3d::coeff方法代码示例

Eigen::Matrix4d create_translation3d(const Eigen::Vector3d center)
{
	double dx(center.coeff(0)),dy(center.coeff(1)),dz(center.coeff(2));
	Eigen::Matrix4d trans;
	trans << 1,0,0,dx,
		    0,1,0,dy,
			0,0,1,dz,
			0,0,0,1;
	return trans;
}

Quaternion::Quaternion(const Eigen::Vector3d& axis, double angle)
{
  if(axis.norm() == 0.0)
  {
    throw Exception("Quaternion::Quaternion", "Norm of axis is zero.");
  }

  double inv_norm = 1.0 / axis.norm();
  double sin_val = sin(0.5 * angle);

  x_ = axis.coeff(0) * inv_norm * sin_val;
  y_ = axis.coeff(1) * inv_norm * sin_val;
  z_ = axis.coeff(2) * inv_norm * sin_val;
  w_ = cos(0.5 * angle);
}

    void rpyToQuaternion(const Eigen::Vector3d& rpy, Eigen::Quaternion<double>& q)
    {
      double a = rpy.coeff(0);
      double b = rpy.coeff(1);
      double g = rpy.coeff(2);

      double sin_b_half = sin(0.5 * b);
      double cos_b_half = cos(0.5 * b);
      double diff_a_g_half = 0.5 * (a - g);
      double sum_a_g_half = 0.5 * (a + g);

      q.x() = sin_b_half * cos(diff_a_g_half);
      q.y() = sin_b_half * sin(diff_a_g_half);
      q.z() = cos_b_half * sin(sum_a_g_half);
      q.w() = cos_b_half * cos(sum_a_g_half);
    }

    void rpyToRotationMatrix(const Eigen::Vector3d& rpy, Eigen::Matrix3d& mat)
    {
      double sin_a = sin(rpy.coeff(2));
      double cos_a = cos(rpy.coeff(2));
      double sin_b = sin(rpy.coeff(1));
      double cos_b = cos(rpy.coeff(1));
      double sin_g = sin(rpy.coeff(0));
      double cos_g = cos(rpy.coeff(0));

      mat.coeffRef(0, 0) = cos_a * cos_b;
      mat.coeffRef(0, 1) = cos_a * sin_b * sin_g - sin_a * cos_g;
      mat.coeffRef(0, 2) = cos_a * sin_b * cos_g + sin_a * sin_g;
      mat.coeffRef(1, 0) = sin_a * cos_b;
      mat.coeffRef(1, 1) = sin_a * sin_b * sin_g + cos_a * cos_g;
      mat.coeffRef(1, 2) = sin_a * sin_b * cos_g - cos_a * sin_g;
      mat.coeffRef(2, 0) = -sin_b;
      mat.coeffRef(2, 1) = cos_b * sin_g;
      mat.coeffRef(2, 2) = cos_b * cos_g;
    }

Eigen::Matrix4d create_rotation3d_line_angle(const Eigen::Vector3d& center, Eigen::Vector3d v, double theta)
{
	//% normalize vector
	v.normalize();

	//% compute projection matrix P and anti-projection matrix
	Eigen::Matrix3d P = v * v.transpose();
	Eigen::Matrix3d Q;
	Q << 0, -v.coeff(2), v.coeff(1),
		v.coeff(2), 0, -v.coeff(0),
		-v.coeff(1), v.coeff(0), 0;
	Eigen::Matrix3d I = Eigen::Matrix3d::Identity();

	//% compute vectorial part of the transform
	Eigen::Matrix4d mat = Eigen::Matrix4d::Identity();
	mat.block(0, 0, 3, 3) = P + (I - P)*cos(theta) + Q*sin(theta);

	//% add translation coefficient
	mat = recenter_transform3d(mat, center);
	
	return mat;
}

void Manipulator::computeJacobian(int32_t idx, Eigen::MatrixXd& J)
{
  J = Eigen::MatrixXd::Zero(6, dof_);

  if(idx < dof_) // Not required to consider end-effector
  {
    for(uint32_t i = 0; i <= idx; ++i)
    {
      if(link_[i]->ep) // joint_type is prismatic
      {
        J.block(0, i, 3, 1) = T_abs_[i].block(0, 0, 3, 3) * link_[i]->tf->axis();
        J.block(3, i, 3, 1) = T_abs_[i].block(0, 0, 3, 3) * Eigen::Vector3d::Zero();
      }
      else // joint_type is revolute
      {
        Eigen::Matrix4d Tib;
        math::calculateInverseTransformationMatrix(T_abs_[i], Tib);
        Eigen::Matrix4d Cin = Tib * C_abs_[idx];
        Eigen::Vector3d P = Cin.block(0, 3, 3, 1);

        J.block(0, i, 3, 1) = T_abs_[i].block(0, 0, 3, 3) * link_[i]->tf->axis().cross(P);
        J.block(3, i, 3, 1) = T_abs_[i].block(0, 0, 3, 3) * link_[i]->tf->axis();
      }
    }
  }
  else // Required to consider the offset of end-effector
  {
    --idx;
    for(uint32_t i = 0; i <= idx; ++i)
    {
      if(link_[i]->ep) // joint_type is prismatic
      {
        J.block(0, i, 3, 1) = T_abs_[i].block(0, 0, 3, 3) * link_[i]->tf->axis();
        J.block(3, i, 3, 1) = T_abs_[i].block(0, 0, 3, 3) * Eigen::Vector3d::Zero();
      }
      else // joint_type is revolute
      {
        Eigen::Matrix4d Tib;
        math::calculateInverseTransformationMatrix(T_abs_[i], Tib);
        Eigen::Matrix4d Cin = Tib * C_abs_[idx];
        Eigen::Vector3d P = Cin.block(0, 3, 3, 1);

        J.block(0, i, 3, 1) = T_abs_[i].block(0, 0, 3, 3) * link_[i]->tf->axis().cross(P);
        J.block(3, i, 3, 1) = T_abs_[i].block(0, 0, 3, 3) * link_[i]->tf->axis();
      }
    }

    Eigen::MatrixXd J_Pne = Eigen::MatrixXd::Identity(6, 6);
    Eigen::Vector3d Pne;
    if(C_abs_.size() - 1 - 1 >= 0.0)
    {
      Pne = T_abs_[T_abs_.size() - 1].block(0, 3, 3, 1) - C_abs_[C_abs_.size() - 1 - 1].block(0, 3, 3, 1);
    }
    else
    {
      std::stringstream msg;
      msg << "C_abs_.size() <= 1" << std::endl
          << "Manipulator doesn't have enough links." << std::endl;
      throw ahl_utils::Exception("Manipulator::computeJacobian", msg.str());
    }

    Eigen::Matrix3d Pne_cross;
    Pne_cross <<           0.0,  Pne.coeff(2), -Pne.coeff(1),
                 -Pne.coeff(2),           0.0,  Pne.coeff(0),
                  Pne.coeff(1), -Pne.coeff(0),           0.0;
    J_Pne.block(0, 3, 3, 3) = Pne_cross;
    J = J_Pne * J;
  }
}

Quaternion::Quaternion(const Eigen::Vector3d& pos)
  : x_(pos.coeff(0)), y_(pos.coeff(1)), z_(pos.coeff(2)), w_(0.0)
{
}