Refactor to use fr3 and panda

[marcbone]

Huge Pr to allow the use FR3 and Panda. They are separate structs and can be abstracted over the RobotWrapper type.

Example

use std::f64::consts::PI;
use std::time::Duration;

use clap::Parser;

use franka::{Fr3, FrankaResult, JointPositions, MotionFinished, Panda, Robot, RobotState};

/// An example showing how to generate a joint position motion.
///
/// WARNING: Before executing this example, make sure there is enough space in front of the robot.
#[derive(Parser, Debug)]
#[clap(author, version, name = "generate_joint_position_motion")]
struct CommandLineArguments {
    /// IP-Address or hostname of the robot
    pub franka_ip: String,
    /// Use this option to run the example on a Panda
    #[clap(short, long, action)]
    pub panda: bool,
}

fn main() -> FrankaResult<()> {
    let args = CommandLineArguments::parse();
    match args.panda {
        true => {
            let robot = Panda::new(args.franka_ip.as_str(), None, None)?;
            generate_motion(robot)
        }
        false => {
            let robot = Fr3::new(args.franka_ip.as_str(), None, None)?;
            generate_motion(robot)
        }
    }
}

fn generate_motion<R: Robot>(mut robot: R) -> FrankaResult<()> {
    robot.set_default_behavior()?;
    println!("WARNING: This example will move the robot! Please make sure to have the user stop button at hand!");
    println!("Press Enter to continue...");
    std::io::stdin().read_line(&mut String::new()).unwrap();

    // Set additional parameters always before the control loop, NEVER in the control loop!
    // Set collision behavior.
    robot.set_collision_behavior(
        [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0],
        [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0],
        [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0],
        [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0],
        [20.0, 20.0, 20.0, 25.0, 25.0, 25.0],
        [20.0, 20.0, 20.0, 25.0, 25.0, 25.0],
        [20.0, 20.0, 20.0, 25.0, 25.0, 25.0],
        [20.0, 20.0, 20.0, 25.0, 25.0, 25.0],
    )?;

    let q_goal = [0., -PI / 4., 0., -3. * PI / 4., 0., PI / 2., PI / 4.];
    robot.joint_motion(0.5, &q_goal)?;
    println!("Finished moving to initial joint configuration.");
    let mut initial_position = JointPositions::new([0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0]);
    let mut time = 0.;
    let callback = |state: &RobotState, time_step: &Duration| -> JointPositions {
        time += time_step.as_secs_f64();
        if time == 0. {
            initial_position.q = state.q_d;
        }
        let mut out = JointPositions::new(initial_position.q);
        let delta_angle = PI / 8. * (1. - f64::cos(PI / 2.5 * time));
        out.q[3] += delta_angle;
        out.q[4] += delta_angle;
        out.q[6] += delta_angle;
        if time >= 5.0 {
            println!("Finished motion, shutting down example");
            return out.motion_finished();
        }
        out
    };
    robot.control_joint_positions(callback, None, None, None)
}

The examples are already working and the usage for end-users is as I intended. However, there are still things to do:

• Refactor to clear up the mess
• Fix missing documentation
• Fix broken intra-doc links
• Properly document how to use the new version of library
• Decide on version number
• Test every example on real robot
• Come up with a solution for the different defaults for the rate-limiter
• Adapt README.md
• Create Changelog
• Add code coverage
• Find out why gripper::tests::gripper_read_once is failing on CI

[codecov]

Welcome to Codecov 🎉

Once merged to your default branch, Codecov will compare your coverage reports and display the results in this comment.

Thanks for integrating Codecov - We've got you covered ☂️