Struct OPWKinematics

pub struct OPWKinematics { /* private fields */ }

Implementations

impl OPWKinematics

pub fn new(parameters: Parameters) -> Self

Creates a new OPWKinematics instance with the given parameters.

Examples found in repository

examples/frame.rs (line 11)

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 13)

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 8)

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/jacobian.rs (line 34)

fn main() {
    let robot = OPWKinematics::new_with_constraints(
        Parameters::irb2400_10(), Constraints::new(
            [-0.1, 0.0, 0.0, 0.0, -PI, -PI],
            [PI, PI, 2.0 * PI, PI, PI, PI],
            BY_CONSTRAINS,
        ));

    let joints: Joints = [0.0, 0.1, 0.2, 0.3, 0.0, 0.5]; // Joints are alias of [f64; 6]
    let jakobian = rs_opw_kinematics::jacobian::Jacobian::new(&robot, &joints, 1E-6);
    let desired_velocity_isometry =
        Isometry3::new(Vector3::new(0.0, 1.0, 0.0),
                       Vector3::new(0.0, 0.0, 1.0));
    let joint_velocities = jakobian.velocities(&desired_velocity_isometry);
    println!("Computed joint velocities: {:?}", joint_velocities.unwrap());

    let desired_force_torque =
        Isometry3::new(Vector3::new(0.0, 0.0, 0.0),
                       Vector3::new(0.0, 0.0, 1.234));

    let joint_torques = jakobian.torques(&desired_force_torque);
    println!("Computed joint torques: {:?}", joint_torques);

    // 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_complete = rs_opw_kinematics::tool::Tool {
        robot: Arc::new(robot_with_base),
        tool: tool_translation,
    };

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

examples/paralellogram.rs (line 40)

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.
}

pub fn new_with_constraints( parameters: Parameters, constraints: Constraints, ) -> Self

Create a new instance that takes also Constraints. If constraints are set, all solutions returned by this solver are constraint compliant.

Examples found in repository

examples/constraints.rs (lines 9-14)

fn main() {
    let robot = OPWKinematics::new_with_constraints(
        Parameters::irb2400_10(), Constraints::new(
            [-0.1, 0.0, 0.0, 0.0, -PI, -PI],
            [PI, PI, 2.0 * PI, PI, PI, PI],
            BY_PREV,
        ));

    let joints: Joints = [0.0, 0.1, 0.2, 0.3, 0.0, 0.5]; // Joints are alias of [f64; 6]
    let when_continuing_from_j6_0: [f64; 6] = [0.0, 0.11, 0.22, 0.8, 0.1, 0.0];

    let pose: Pose = robot.forward(&joints); // Pose is alias of nalgebra::Isometry3<f64>    

    println!("If we do not have the previous pose yet, we can now ask to prever the pose \
    closer to the center of constraints.");
    let solutions = robot.inverse_continuing(&pose, &CONSTRAINT_CENTERED);
    dump_solutions(&solutions);

    println!("With constraints, sorted by proximity to the previous pose");
    let solutions = robot.inverse_continuing(&pose, &when_continuing_from_j6_0);
    dump_solutions(&solutions);

    let robot = OPWKinematics::new_with_constraints(
        Parameters::irb2400_10(), Constraints::new(
            [-0.1, 0.0, 0.0, 0.0, -PI, -PI],
            [PI, PI, 2.0 * PI, PI, PI, PI],
            BY_CONSTRAINS,
        ));
    println!("With constraints, sorted by proximity to the center of constraints");
    let solutions = robot.inverse_continuing(&pose, &when_continuing_from_j6_0);
    dump_solutions(&solutions);
}

examples/jacobian.rs (lines 11-16)

fn main() {
    let robot = OPWKinematics::new_with_constraints(
        Parameters::irb2400_10(), Constraints::new(
            [-0.1, 0.0, 0.0, 0.0, -PI, -PI],
            [PI, PI, 2.0 * PI, PI, PI, PI],
            BY_CONSTRAINS,
        ));

    let joints: Joints = [0.0, 0.1, 0.2, 0.3, 0.0, 0.5]; // Joints are alias of [f64; 6]
    let jakobian = rs_opw_kinematics::jacobian::Jacobian::new(&robot, &joints, 1E-6);
    let desired_velocity_isometry =
        Isometry3::new(Vector3::new(0.0, 1.0, 0.0),
                       Vector3::new(0.0, 0.0, 1.0));
    let joint_velocities = jakobian.velocities(&desired_velocity_isometry);
    println!("Computed joint velocities: {:?}", joint_velocities.unwrap());

    let desired_force_torque =
        Isometry3::new(Vector3::new(0.0, 0.0, 0.0),
                       Vector3::new(0.0, 0.0, 1.234));

    let joint_torques = jakobian.torques(&desired_force_torque);
    println!("Computed joint torques: {:?}", joint_torques);

    // 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_complete = rs_opw_kinematics::tool::Tool {
        robot: Arc::new(robot_with_base),
        tool: tool_translation,
    };

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

Trait Implementations

impl Clone for OPWKinematics

fn clone(&self) -> OPWKinematics

Returns a copy of the value.

fn clone_from(&mut self, source: &Self)

Performs copy-assignment from source.

impl Debug for OPWKinematics

fn fmt(&self, f: &mut Formatter<'_>) -> Result

Formats the value using the given formatter.

impl Kinematics for OPWKinematics

fn inverse(&self, pose: &Pose) -> Solutions

Return the solution that is constraint compliant anv values are valid (no NaNs, etc) but otherwise not sorted. If this is 5 degree of freedom robot only, the 6 joint is set to 0.0 The rotation of pose in this case is only approximate.

fn inverse_continuing(&self, pose: &Pose, prev: &Joints) -> Solutions

Find inverse kinematics (joint position) for this pose This function handles the singularity J5 = 0 by keeping the previous values the values J4 and J6 from the previous solution Use CONSTRAINT_CENTERED as previous if there is no previous position but we prefer to be as close to the center of constraints (or zeroes if not set) as possible.

fn forward(&self, joints: &Joints) -> Pose

Find forward kinematics (pose from joint positions). For 5 DOF robot, the rotation of the joint 6 should normally be 0.0 but some other value can be given, meaning the tool is mounted with fixed rotation offset.

fn forward_with_joint_poses(&self, joints: &Joints) -> [Pose; 6]

Computes the forward kinematics for a 6-DOF robotic arm and returns an array of poses representing the position and orientation of each joint, including the final end-effector.

fn inverse_5dof(&self, pose: &Pose, j6: f64) -> Solutions

Calculates the inverse kinematics for a robot while ignoring the rotation around joint 6. The position of the tool center point remains precise, but the rotation is approximate (rotation around the tool axis is ignored). The return value for joint 6 is set according to the provided parameter. This method is significantly faster

fn inverse_continuing_5dof(&self, pose: &Pose, prev: &Joints) -> Solutions

Calculates the inverse kinematics for a robot while ignoring the rotation around joint 6. The position of the tool center point remains precise, but the rotation is approximate (rotation around the tool axis is ignored). The return value for joint 6 is set based on the previous joint values. This method is significantly faster

fn kinematic_singularity(&self, joints: &Joints) -> Option<Singularity>

Detect the singularity. Returns either A type singlularity or None if no singularity detected.

fn constraints(&self) -> &Option<Constraints>

Returns constraints under what the solver is operating. Constraints are remembered here and can be used for generating random joint angles needed by RRT, or say providing limits of sliders in GUI.

impl Copy for OPWKinematics