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)
{
}