Function dump_joints

pub fn dump_joints(joints: &Joints)

Print joint values, converting radianst to degrees.

Examples found in repository

examples/path_planning_rrt.rs (line 119)

fn print_summary(planning_result: &Result<Vec<[f64; 6]>, String>) {
    match planning_result {
        Ok(path) => {
            println!("Steps:");
            for step in path {
                dump_joints(&step);
            }
        }
        Err(error_message) => {
            println!("Error: {}", error_message);
        }
    }
}

examples/frame.rs (line 24)

fn main() {
    let robot = OPWKinematics::new(Parameters::irb2400_10());
    // Shift not too much to have values close to previous
    let frame_transform = Frame::translation(
        Point3::new(0.0, 0.0, 0.0), 
        Point3::new(0.011, 0.022, 0.033));

    let framed = Frame {
        robot: Arc::new(robot),
        frame: frame_transform,
    };
    let joints_no_frame: [f64; 6] = [0.0, 0.11, 0.22, 0.3, 0.1, 0.5]; // without frame

    println!("No frame transform:");
    dump_joints(&joints_no_frame);

    println!("Possible joint values after the frame transform:");
    let (solutions, _transformed_pose) = framed.forward_transformed(
        &joints_no_frame, &joints_no_frame);
    dump_solutions(&solutions);

    let framed = robot.forward(&solutions[0]).translation;
    let unframed = robot.forward(&joints_no_frame).translation;

    println!("Distance between framed and not framed pose {:.3} {:.3} {:.3}",
             framed.x - unframed.x, framed.y - unframed.y, framed.z - unframed.z);
}

examples/tool_and_base.rs (line 10)

fn main() {
    let joints: Joints = [0.0, 0.1, 0.2, 0.3, 0.4, 0.5]; // Joints are alias of [f64; 6]
    dump_joints(&joints);

    // Robot with the tool, standing on a base:
    let robot_alone = OPWKinematics::new(Parameters::staubli_tx2_160l());

    // 1 meter high pedestal
    let pedestal = 0.5;
    let base_translation = Isometry3::from_parts(
        Translation3::new(0.0, 0.0, pedestal).into(),
        UnitQuaternion::identity(),
    );

    let robot_with_base = rs_opw_kinematics::tool::Base {
        robot: Arc::new(robot_alone),
        base: base_translation,
    };

    // Tool extends 1 meter in the Z direction, envisioning something like sword
    let sword = 1.0;
    let tool_translation = Isometry3::from_parts(
        Translation3::new(0.0, 0.0, sword).into(),
        UnitQuaternion::identity(),
    );

    // Create the Tool instance with the transformation
    let robot_on_base_with_tool = rs_opw_kinematics::tool::Tool {
        robot: Arc::new(robot_with_base),
        tool: tool_translation,
    };

    let tcp_pose: Pose = robot_on_base_with_tool.forward(&joints);
    println!("The sword tip is at: {:?}", tcp_pose);

    // robot_complete implements Kinematics so have the usual inverse kinematics methods available.    
    let inverse = robot_on_base_with_tool.inverse_continuing(&tcp_pose, &joints);
    dump_solutions(&inverse);

}

examples/basic.rs (line 11)

fn main() {
    let robot = OPWKinematics::new(Parameters::irb2400_10());
    let joints: Joints = [0.0, 0.1, 0.2, 0.3, 0.0, 0.5]; // Joints are alias of [f64; 6]
    println!("\nInitial joints with singularity J5 = 0: ");
    dump_joints(&joints);

    println!("\nSolutions (original angle set is lacking due singularity there: ");
    let pose: Pose = robot.forward(&joints); // Pose is alias of nalgebra::Isometry3<f64>

    let solutions = robot.inverse(&pose); // Solutions is alias of Vec<Joints>
    dump_solutions(&solutions);

    println!("\nSolutions assuming we continue from somewhere close. The 'lost solution' returns");
    let when_continuing_from: [f64; 6] = [0.0, 0.11, 0.22, 0.3, 0.1, 0.5];
    let solutions = robot.inverse_continuing(&pose, &when_continuing_from);
    dump_solutions(&solutions);

    println!("\nSame pose, all J4+J6 rotation assumed to be previously concentrated on J4 only");
    let when_continuing_from_j6_0: [f64; 6] = [0.0, 0.11, 0.22, 0.8, 0.1, 0.0];
    let solutions = robot.inverse_continuing(&pose, &when_continuing_from_j6_0);
    dump_solutions(&solutions);

    println!("\nIf we do not have the previous position, we can assume we want J4, J6 close to 0.0 \
    The solution appears and the needed rotation is now equally distributed between J4 and J6.");
    let solutions = robot.inverse_continuing(&pose, &JOINTS_AT_ZERO);
    dump_solutions(&solutions);

    println!("\n5 DOF, J6 at fixed angle 77 degrees");
    let solutions5dof = robot.inverse_5dof(&pose, 77.0_f64.to_radians());
    dump_solutions(&solutions5dof);
    println!("The XYZ location of TCP is still as in the original pose x = {:.3}, y = {:.3}, z = {:.3}:",
             pose.translation.x, pose.translation.y, pose.translation.z);
    for solution in &solutions {
        let translation = robot.forward(solution).translation;
        println!("Translation: x = {:.3}, y = {:.3}, z = {:.3}", translation.x, translation.y, translation.z);
    }
}

examples/paralellogram.rs (line 56)

fn main() {
    // Robot without parallelogram
    let robot_no_parallelogram = Arc::new(OPWKinematics::new(Parameters::irb2400_10()));

    // Robot with parallelogram
    let robot_with_parallelogram = Parallelogram {
        robot: Arc::new(OPWKinematics::new(Parameters::irb2400_10())),
        driven: J2,
        coupled: J3,
        scaling: 1.0
    };

    // Initial joint positions in degrees
    let joints_degrees: [f64; 6] = [0.0, 5.7, 11.5, 17.2, 0.0, 28.6]; // Values in degrees
    // Convert joint positions from degrees to radians
    let joints: Joints = joints_degrees.map(f64::to_radians); // Joints are alias of [f64; 6]

    println!("\nInitial joints: ");
    dump_joints(&joints);

    // Forward kinematics for both robots
    let pose_no_parallelogram: Pose = robot_no_parallelogram.forward(&joints);
    let pose_with_parallelogram: Pose = robot_with_parallelogram.forward(&joints);

    // Get initial orientation in degrees for both robots
    let initial_orientation_no_parallelogram = euler_angles_in_degrees(&pose_no_parallelogram);
    let initial_orientation_with_parallelogram = euler_angles_in_degrees(&pose_with_parallelogram);

    // Apply change to joint 2 (this will show the difference in behavior between the two robots)
    let mut modified_joints = joints;
    modified_joints[1] += 10_f64.to_radians();
    println!("\nModified joints (joint 2 increased by 10 degrees): ");
    dump_joints(&modified_joints);

    // Forward kinematics after modifying joints for both robots
    let modified_pose_no_parallelogram: Pose = robot_no_parallelogram.forward(&modified_joints);
    let modified_pose_with_parallelogram: Pose = robot_with_parallelogram.forward(&modified_joints);

    // Get modified orientation in degrees for both robots
    let modified_orientation_no_parallelogram = euler_angles_in_degrees(&modified_pose_no_parallelogram);
    let modified_orientation_with_parallelogram = euler_angles_in_degrees(&modified_pose_with_parallelogram);

    // Print orientation changes for both robots
    println!("\nOrientation changes after joint change:");
    print_orientation_change(
        initial_orientation_no_parallelogram,
        modified_orientation_no_parallelogram,
        "Robot without parallelogram"
    );
    print_orientation_change(
        initial_orientation_with_parallelogram,
        modified_orientation_with_parallelogram,
        "Robot with parallelogram"
    );

    // Calculate and print travel distances
    let travel_distance_no_parallelogram = calculate_travel_distance(&pose_no_parallelogram, &modified_pose_no_parallelogram);
    let travel_distance_with_parallelogram = calculate_travel_distance(&pose_with_parallelogram, &modified_pose_with_parallelogram);

    println!("\nTravel distances after joint change:");
    println!(
        "Robot without parallelogram: travel distance = {:.6}",
        travel_distance_no_parallelogram
    );
    println!(
        "Robot with parallelogram: travel distance = {:.6}",
        travel_distance_with_parallelogram
    );

    // The result should show that the robot without a parallelogram experiences a larger change in orientation.
    // The robot with parallelogram has much less orientation change, but still travels a reasonable distance.
}