use franka::{FrankaResult, Gripper};
use std::time::Duration;
use structopt::StructOpt;
#[derive(StructOpt, Debug)]
#[structopt(name = "grasp_object")]
struct CommandLineArguments {
#[structopt()]
pub gripper_hostname: String,
#[structopt(long)]
pub homing: bool,
#[structopt()]
pub object_width: f64,
}
fn main() -> FrankaResult<()> {
let args: CommandLineArguments = CommandLineArguments::from_args();
let mut gripper = Gripper::new(args.gripper_hostname.as_str())?;
if args.homing {
gripper.homing()?;
}
let state = gripper.read_once()?;
if state.max_width < args.object_width {
eprintln!("Object is too large for the current fingers on the gripper.");
std::process::exit(-1);
}
gripper.grasp(args.object_width, 0.1, 60., None, None)?;
std::thread::sleep(Duration::from_secs(3));
let state = gripper.read_once()?;
if !state.is_grasped {
eprintln!("Object lost");
std::process::exit(-1);
}
println!("Grasped object, will release it now.");
gripper.stop()?;
Ok(())
}