//============================================================================== void EulerJoint::updateLocalJacobian() { // double q0 = mPositions[0]; double q1 = mPositions[1]; double q2 = mPositions[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 (mAxisOrder) { case AO_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 (fabs(mPositions[1]) == DART_PI * 0.5) std::cout << "Singular configuration in ZYX-euler joint [" << mName << "]. (" << mPositions[0] << ", " << mPositions[1] << ", " << mPositions[2] << ")" << std::endl; #endif break; } case AO_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 (fabs(mPositions[1]) == DART_PI * 0.5) std::cout << "Singular configuration in ZYX-euler joint [" << mName << "]. (" << mPositions[0] << ", " << mPositions[1] << ", " << mPositions[2] << ")" << std::endl; #endif break; } default: { dterr << "Undefined Euler axis order\n"; break; } } mJacobian.col(0) = math::AdT(mT_ChildBodyToJoint, J0); mJacobian.col(1) = math::AdT(mT_ChildBodyToJoint, J1); mJacobian.col(2) = math::AdT(mT_ChildBodyToJoint, J2); assert(!math::isNan(mJacobian)); #ifndef NDEBUG Eigen::MatrixXd JTJ = mJacobian.transpose() * mJacobian; 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 [" << 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 }
//============================================================================== 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; }