Unable to converge on 2 dof setup

[tmitchel2]

Hi,

Im investigating your library with the aim of building a kinematic model for a 5 axis cnc machine. Im starting off very basic with 1 then 2, etc dof. My 1 dof test seems to work but my 2 dof test does not converge.

Could you shed some light on what I'm doing wrong?

I've included a bare bones reproduction of the code...Apologies as this is more of a question than an issue.

fn test_kinematics() {
    // Works...
    test_kinematics_1_dof();

    // Errors...
    //    thread 'main' panicked at 'called `Result::unwrap()` on an `Err`
    //    value: NotConvergedError { num_tried: 10, position_diff:
    //    Matrix { data: [0.0009765625, 0.0009765625, 0.0] },
    //    rotation_diff: Matrix { data: [0.0, 0.0, 0.0] } }'
    test_kinematics_2_dof();
}

fn test_kinematics_1_dof() {
    let machine_frame = k::Node::new(
        k::NodeBuilder::new()
            .name("machine_frame")
            .translation(na::Translation3::new(0.0, 0.0, 0.0))
            .joint_type(k::JointType::Fixed)
            .finalize(),
    );

    let x_axis_linear = k::Node::new(
        k::NodeBuilder::new()
            .name("x_axis_linear")
            .translation(na::Translation3::new(0.0, 0.0, 0.0))
            .joint_type(k::JointType::Linear {
                axis: na::Vector3::x_axis(),
            })
            .finalize(),
    );

    x_axis_linear.set_parent(&machine_frame);
    let chain = k::Chain::from_root(machine_frame);

    // Initial position
    let origin_positions_vec = vec![0.0];
    chain.set_joint_positions(&origin_positions_vec).unwrap();
    println!("initial positions = {:?}", chain.joint_positions());
    // Forward kinematics
    let target_positions_vec = vec![1.0];
    let workpiece = chain.find("x_axis_linear").unwrap();
    chain.update_transforms();
    let mut target = x_axis_linear.world_transform().unwrap();
    println!("initial target pos = {:?}", target.translation);
    println!("move x: {:?}", target_positions_vec[0]);
    target.translation.vector.x += target_positions_vec[0];

    // Inverse kinematics
    let solver = k::JacobianIKSolver::default();
    let arm = k::SerialChain::from_end(workpiece);
    let mut constraints = k::Constraints::default();
    constraints.rotation_x = false;
    constraints.rotation_y = false;
    constraints.rotation_z = false;
    // constraints.position_x = false;
    constraints.position_y = false;
    constraints.position_z = false;
    solver
        .solve_with_constraints(&arm, &target, &constraints)
        .unwrap();
    println!("solved positions = {:?}", chain.joint_positions());
    chain.update_transforms();
    let solved_pose = workpiece.world_transform().unwrap();
    println!("solved target positions = {:?}", solved_pose.translation);
}

fn test_kinematics_2_dof() {
    let machine_frame = k::Node::new(
        k::NodeBuilder::new()
            .name("machine_frame")
            .translation(na::Translation3::new(0.0, 0.0, 0.0))
            .joint_type(k::JointType::Fixed)
            .finalize(),
    );

    let x_axis_linear = k::Node::new(
        k::NodeBuilder::new()
            .name("x_axis_linear")
            .translation(na::Translation3::new(0.0, 0.0, 0.0))
            .joint_type(k::JointType::Linear {
                axis: na::Vector3::x_axis(),
            })
            .finalize(),
    );

    let y_axis_linear = k::Node::new(
        k::NodeBuilder::new()
            .name("y_axis_linear")
            .translation(na::Translation3::new(0.0, 0.0, 0.0))
            .joint_type(k::JointType::Linear {
                axis: na::Vector3::y_axis(),
            })
            .finalize(),
    );

    x_axis_linear.set_parent(&machine_frame);
    y_axis_linear.set_parent(&x_axis_linear);
    let chain = k::Chain::from_root(machine_frame);

    // Initial position
    let origin_positions_vec = vec![0.0, 0.0];
    chain.set_joint_positions(&origin_positions_vec).unwrap();
    println!("initial positions = {:?}", chain.joint_positions());
    // Forward kinematics
    let target_positions_vec = vec![1.0, 1.0];
    let workpiece = chain.find("y_axis_linear").unwrap();
    chain.update_transforms();
    let mut target = y_axis_linear.world_transform().unwrap();
    println!("initial target pos = {:?}", target.translation);
    println!("move x: {:?}", target_positions_vec[0]);
    println!("move y: {:?}", target_positions_vec[1]);
    target.translation.vector.x += target_positions_vec[0];
    target.translation.vector.y += target_positions_vec[1];

    // Inverse kinematics
    let solver = k::JacobianIKSolver::default();
    let arm = k::SerialChain::from_end(workpiece);
    let mut constraints = k::Constraints::default();
    constraints.rotation_x = false;
    constraints.rotation_y = false;
    constraints.rotation_z = false;
    // constraints.position_x = false;
    // constraints.position_y = false;
    constraints.position_z = false;
    solver
        .solve_with_constraints(&arm, &target, &constraints)
        .unwrap();
    println!("solved angles = {:?}", chain.joint_positions());
    chain.update_transforms();
    let solved_pose = workpiece.world_transform().unwrap();
    println!("solved target pos = {:?}", solved_pose.translation);
}