how to use ik

[QAbot-zh]

hello, I have implemented the forward kinematics of Franka as follows. When I use the obtained pose matrix to calculate the inverse kinematics, I always get a " nan " result. Can you provide some suggestions?

Eigen::Matrix4d computeDHTransform(double d, double a, double alpha, double theta) {
    Eigen::Matrix4d transform;
    transform << std::cos(theta), -std::sin(theta), 0, a,
            std::sin(theta) * std::cos(alpha), std::cos(theta) * std::cos(alpha), -std::sin(alpha), -d *  std::sin(alpha),                                                                
            std::sin(theta) * std::sin(alpha), std::cos(theta) * std::sin(alpha), std::cos(alpha), d * std::cos(alpha),
            0, 0, 0, 1;
    return transform;
}
// forward kinematics w.r.t. End Effector Frame (using Franka Hand data)
Eigen::Matrix4d franka_FK_EE_eigen(const std::array<double, 7> jointAngles) {
    const double d1 = 0.3330;
    const double d3 = 0.3160;
    const double d5 = 0.3840;
    const double d7e = 0.2104;
    const double a4 = 0.0825;
    const double a7 = 0.0880;

    Eigen::Matrix<double, 9, 4> dhParams; 
    dhParams <<   0,      0.333,            0,          0,
                            0,            0, -M_PI / 2,          0,
                            0,     0.316,   M_PI / 2,          0,
                    0.0825,           0,   M_PI / 2,          0,
                  -0.0825,     0.384, -M_PI / 2,          0,
                            0,           0,   M_PI / 2,           0,
                     0.088,           0,   M_PI / 2,           0,
                            0,    0.107,              0,           0,
                            0,  0.1034,              0,  M_PI_4;

    Eigen::Matrix4d transformation = Eigen::Matrix4d::Identity();
    double d, a, alpha, theta;
    for (int i = 0; i < 9; ++i) {
        a = dhParams(i, 0);
        d = dhParams(i, 1);
        alpha = dhParams(i, 2);
        theta = i < 7 ? jointAngles[i] : dhParams(i, 3);

        Eigen::Matrix4d dhTransform = computeDHTransform(d, a, alpha, theta);
        transformation *= dhTransform;
    }
    return transformation;
}

[JaeHyung-Kim]

Oh sorry for the late reply.

In my case, I only used this analytical IK to solve the initial configuration to match the end-effector 6D pose. Since we have redundant parameter q7, I put multiple candidates of q7 and chose the IK with the most stable configuration (far from joint limit, no NaNs, etc.) and used it.

If you are still having problems, please let me know, and let's figure it out together!