Problems occured when solving the inverse kinematics of a robot with less than 6 DOFs

[DofixInGithub]

Hi!

I run "roslaunch relaxed_ik_ros1 demo.launch" but changed the urdf to a 5 DoFs robot, and I modified the line_tracing.py program so that the planned trajectory remains in the original position：

 for i in range(4):
            for j in range(num_points):
                poses = self.copy_poses(self.starting_ee_poses)
                # for k in range(self.robot.num_chain):
                    # poses[k].position.z = z[k]
                    # poses[k].position.y = y[k]
                    # z[k] += dz[i]
                    # y[k] += dy[i]
                trajectory.append(poses)
        print(trajectory)
        return trajectory

But the inverse kinematics solution seems to have failed with the error message: "No valid solution found! Returning previous solution: [0.2, -0.2, 0.2, -0.2, 0.1]. End effector position goals: [[[-0.23089286732848902, 1.2196985135056702, 0.02319401335631154]]]".

Furthermore, I changed the rotation and Quat weight to 0 (or a small, non-zero number) and re-compile the rust code because I thought that the robot with less than 6 degrees of freedom might not be able to control the rotation precisely, but it doesn't help:

impl ObjectiveMaster {
    pub fn standard_ik(num_chains: usize) -> Self {
        let mut objectives: Vec<Box<dyn ObjectiveTrait + Send>> = Vec::new();
        let mut weight_priors: Vec<f64> = Vec::new();
        for i in 0..num_chains {
            objectives.push(Box::new(MatchEEPosGoals::new(i)));
            weight_priors.push(1.0);
            objectives.push(Box::new(MatchEEQuatGoals::new(i)));
            weight_priors.push(0.0); 
        }
        Self{objectives, num_chains, weight_priors, lite: true, finite_diff_grad: true}
    }

    pub fn relaxed_ik(chain_lengths: &[usize]) -> Self {
        let mut objectives: Vec<Box<dyn ObjectiveTrait + Send>> = Vec::new();
        let mut weight_priors: Vec<f64> = Vec::new();
        let num_chains = chain_lengths.len();
        let mut num_dofs = 0;
        for i in 0..num_chains {
            objectives.push(Box::new(MatchEEPosiDoF::new(i, 0)));
            weight_priors.push(50.0);
            objectives.push(Box::new(MatchEEPosiDoF::new(i, 1)));
            weight_priors.push(50.0);
            objectives.push(Box::new(MatchEEPosiDoF::new(i, 2)));
            weight_priors.push(50.0);
            objectives.push(Box::new(MatchEERotaDoF::new(i, 0)));
            weight_priors.push(0.0);
            objectives.push(Box::new(MatchEERotaDoF::new(i, 1)));
            weight_priors.push(0.0);
            objectives.push(Box::new(MatchEERotaDoF::new(i, 2)));
            weight_priors.push(0.0);
            // objectives.push(Box::new(EnvCollision::new(i)));
            // weight_priors.push(1.0);
            num_dofs += chain_lengths[i];
        }

Please let me know if I did anything wrong or need to fix something to let it work!
Thank you!

[yepw]

Hi @DofixInGithub , thank you for your interest in our tool. The error message is raised because the optimization solver fails to find a solution. The most common reason is that the input end-effector pose is unreachable to the robot. But here you are giving its starting pose, so it should be reachable. I can take a further look if you can send me the urdf and the setting (.yaml) files of your 5 DoF robot. Either posting it here or sending it to [email protected] is fine.

[DofixInGithub]

@yepw Thanks for your reply!
I have sent you an email which contains my package and an overview of the modifications I've made to the code, details on the execution process, and error messages encountered during runtime.
Thank you for your time!

[yepw]

Thanks for sending the code. The issue was caused by the manipulability objective. For a 5 DoF robot, the manipulability index is always 0. Due to numerical instability, it sometimes becomes a negative value, which leads to a NaN when trying to compute the sqrt(). I've submitted a new commit. Could you pull the latest relaxed_ik_core and check if it resolves the problem?

[DofixInGithub]

Thanks for your reply! I have pulled the latest relaxed_ik_core and it works very well so far, Thank you very much for your help! Best wishes! At 2024-08-21 21:56:47, "Yeping Wang" ***@***.***> wrote: Thanks for sending the code. The issue was caused by the manipulability objective. For a 5 DoF robot, the manipulability index is always 0. Due to numerical instability, it sometimes becomes a negative value, which leads to a NaN when trying to compute the sqrt(). I've submitted a new commit. Could you pull the latest relaxed_ik_core and check if it resolves the problem? — Reply to this email directly, view it on GitHub, or unsubscribe. You are receiving this because you were mentioned.Message ID: ***@***.***>