//==============================================================================
void EulerJoint::updateDegreeOfFreedomNames()
{
std::vector<std::string> affixes;
switch (getAxisOrder())
{
case AxisOrder::ZYX:
affixes.push_back("_z");
affixes.push_back("_y");
affixes.push_back("_x");
break;
case AxisOrder::XYZ:
affixes.push_back("_x");
affixes.push_back("_y");
affixes.push_back("_z");
break;
default:
dterr << "Unsupported axis order in EulerJoint named '" << Joint::mAspectProperties.mName
<< "' (" << static_cast<int>(getAxisOrder()) << ")\n";
}
if (affixes.size() == 3)
{
for (std::size_t i = 0; i < 3; ++i)
{
if(!mDofs[i]->isNamePreserved())
mDofs[i]->setName(Joint::mAspectProperties.mName + affixes[i], false);
}
}
}
// compute axis transformations from bone axis angles
void Bone::computeLocalAxisTransform()
{
CHANNEL_TYPE* axis_order = getAxisOrder();
Vector3D axis = getAxis();
C = Matrix4x4::identity();
for (short d=0; d<3; d++)
{
Matrix4x4 r;
switch(axis_order[d])
{
case CT_RX:
r = Matrix4x4::rotationPitch(axis.pitch);
C = r * C;
break;
case CT_RY:
r = Matrix4x4::rotationYaw(axis.yaw);
C = r * C;
break;
case CT_RZ:
r = Matrix4x4::rotationRoll(axis.roll);
C = r * C;
break;
default:
break;
}
}
Cinv = C.cheapInverse(true);
}
//==============================================================================
void EulerJoint::updateRelativeJacobianTimeDeriv() const
{
// double q0 = mPositions[0];
const Eigen::Vector3d& positions = getPositionsStatic();
double q1 = positions[1];
double q2 = positions[2];
// double dq0 = mVelocities[0];
const Eigen::Vector3d& velocities = getVelocitiesStatic();
double dq1 = velocities[1];
double dq2 = velocities[2];
// double c0 = cos(q0);
double c1 = cos(q1);
double c2 = cos(q2);
// double s0 = sin(q0);
double s1 = sin(q1);
double s2 = sin(q2);
Eigen::Vector6d dJ0 = Eigen::Vector6d::Zero();
Eigen::Vector6d dJ1 = Eigen::Vector6d::Zero();
Eigen::Vector6d dJ2 = Eigen::Vector6d::Zero();
switch (getAxisOrder())
{
case AxisOrder::XYZ:
{
//------------------------------------------------------------------------
// dS = [ -(dq1*c2*s1) - dq2*c1*s2, dq2*c2, 0
// -(dq2*c1*c2) + dq1*s1*s2, -(dq2*s2), 0
// dq1*c1, 0, 0
// 0, 0, 0
// 0, 0, 0
// 0, 0, 0 ];
//------------------------------------------------------------------------
dJ0 << -(dq1*c2*s1) - dq2*c1*s2, -(dq2*c1*c2) + dq1*s1*s2, dq1*c1,
0, 0, 0;
dJ1 << dq2*c2, -(dq2*s2), 0.0, 0.0, 0.0, 0.0;
dJ2.setZero();
break;
}
case AxisOrder::ZYX:
{
//------------------------------------------------------------------------
// dS = [ -c1*dq1, 0, 0
// c2*c1*dq2-s2*s1*dq1, -s2*dq2, 0
// -s1*c2*dq1-c1*s2*dq2, -c2*dq2, 0
// 0, 0, 0
// 0, 0, 0
// 0, 0, 0 ];
//------------------------------------------------------------------------
dJ0 << -c1*dq1, c2*c1*dq2 - s2*s1*dq1, -s1*c2*dq1 - c1*s2*dq2,
0.0, 0.0, 0.0;
dJ1 << 0.0, -s2*dq2, -c2*dq2,
0.0, 0.0, 0.0;
dJ2.setZero();
break;
}
default:
{
dterr << "Undefined Euler axis order\n";
break;
}
}
mJacobianDeriv.col(0) = math::AdT(Joint::mAspectProperties.mT_ChildBodyToJoint, dJ0);
mJacobianDeriv.col(1) = math::AdT(Joint::mAspectProperties.mT_ChildBodyToJoint, dJ1);
mJacobianDeriv.col(2) = math::AdT(Joint::mAspectProperties.mT_ChildBodyToJoint, dJ2);
assert(!math::isNan(mJacobianDeriv));
}
//==============================================================================
Eigen::Matrix<double, 6, 3> EulerJoint::getRelativeJacobianStatic(
const Eigen::Vector3d& _positions) const
{
Eigen::Matrix<double, 6, 3> J;
// double q0 = _positions[0];
const double q1 = _positions[1];
const double q2 = _positions[2];
// double c0 = cos(q0);
double c1 = cos(q1);
double c2 = cos(q2);
// double s0 = sin(q0);
double s1 = sin(q1);
double s2 = sin(q2);
Eigen::Vector6d J0 = Eigen::Vector6d::Zero();
Eigen::Vector6d J1 = Eigen::Vector6d::Zero();
Eigen::Vector6d J2 = Eigen::Vector6d::Zero();
switch (getAxisOrder())
{
case AxisOrder::XYZ:
{
//------------------------------------------------------------------------
// S = [ c1*c2, s2, 0
// -(c1*s2), c2, 0
// s1, 0, 1
// 0, 0, 0
// 0, 0, 0
// 0, 0, 0 ];
//------------------------------------------------------------------------
J0 << c1*c2, -(c1*s2), s1, 0.0, 0.0, 0.0;
J1 << s2, c2, 0.0, 0.0, 0.0, 0.0;
J2 << 0.0, 0.0, 1.0, 0.0, 0.0, 0.0;
#ifndef NDEBUG
if (std::abs(getPositionsStatic()[1]) == math::constantsd::pi() * 0.5)
std::cout << "Singular configuration in ZYX-euler joint ["
<< Joint::mAspectProperties.mName << "]. ("
<< _positions[0] << ", "
<< _positions[1] << ", "
<< _positions[2] << ")"
<< std::endl;
#endif
break;
}
case AxisOrder::ZYX:
{
//------------------------------------------------------------------------
// S = [ -s1, 0, 1
// s2*c1, c2, 0
// c1*c2, -s2, 0
// 0, 0, 0
// 0, 0, 0
// 0, 0, 0 ];
//------------------------------------------------------------------------
J0 << -s1, s2*c1, c1*c2, 0.0, 0.0, 0.0;
J1 << 0.0, c2, -s2, 0.0, 0.0, 0.0;
J2 << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0;
#ifndef NDEBUG
if (std::abs(_positions[1]) == math::constantsd::pi() * 0.5)
std::cout << "Singular configuration in ZYX-euler joint ["
<< Joint::mAspectProperties.mName << "]. ("
<< _positions[0] << ", "
<< _positions[1] << ", "
<< _positions[2] << ")"
<< std::endl;
#endif
break;
}
default:
{
dterr << "Undefined Euler axis order\n";
break;
}
}
J.col(0) = math::AdT(Joint::mAspectProperties.mT_ChildBodyToJoint, J0);
J.col(1) = math::AdT(Joint::mAspectProperties.mT_ChildBodyToJoint, J1);
J.col(2) = math::AdT(Joint::mAspectProperties.mT_ChildBodyToJoint, J2);
assert(!math::isNan(J));
#ifndef NDEBUG
Eigen::MatrixXd JTJ = J.transpose() * J;
Eigen::FullPivLU<Eigen::MatrixXd> luJTJ(JTJ);
// Eigen::FullPivLU<Eigen::MatrixXd> luS(mS);
double det = luJTJ.determinant();
if (det < 1e-5)
{
std::cout << "ill-conditioned Jacobian in joint [" << Joint::mAspectProperties.mName << "]."
<< " The determinant of the Jacobian is (" << det << ")."
<< std::endl;
std::cout << "rank is (" << luJTJ.rank() << ")." << std::endl;
std::cout << "det is (" << luJTJ.determinant() << ")." << std::endl;
// std::cout << "mS: \n" << mS << std::endl;
}
#endif
return J;
}
//==============================================================================
Eigen::Matrix3d EulerJoint::convertToRotation(const Eigen::Vector3d& _positions)
const
{
return convertToRotation(_positions, getAxisOrder());
}
//==============================================================================
Eigen::Isometry3d EulerJoint::convertToTransform(
const Eigen::Vector3d &_positions) const
{
return convertToTransform(_positions, getAxisOrder());
}