abbegm 0.7.5

externally guided motion for ABB industrial robots
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90

use abbegm::tokio_peer::EgmPeer;
use std::convert::TryInto;
use std::time::Instant;
use structopt::StructOpt;
use structopt::clap::AppSettings;

#[derive(Debug, StructOpt)]
#[structopt(setting = AppSettings::ColoredHelp)]
#[structopt(setting = AppSettings::DeriveDisplayOrder)]
#[structopt(setting = AppSettings::UnifiedHelpMessage)]
struct Options {
	/// Local address to bind to.
	#[structopt(long)]
	#[structopt(value_name = "HOST:PORT")]
	#[structopt(default_value = "[::]:6510")]
	bind: String,

	/// Radius of the cirle, in meters.
	#[structopt(long)]
	#[structopt(value_name = "M")]
	#[structopt(default_value = "0.10")]
	radius: f64,

	/// Speed of the robot, in meters per second.
	#[structopt(long)]
	#[structopt(value_name = "M/S")]
	#[structopt(default_value = "0.10")]
	speed: f64,

	/// Confirm that the robot should perform motion.
	#[structopt(long)]
	confirm_motion: bool,
}

fn rotate_z(angle: f64) -> nalgebra::Rotation3<f64> {
	nalgebra::Rotation3::from_axis_angle(&nalgebra::Vector3::z_axis(), angle)
}

async fn do_main(options: Options) -> Result<(), String> {
	if !options.confirm_motion {
		return Err(String::from("refusing to send motion commands to the robot without --confirm-motion flag"))
	}

	let mut peer = EgmPeer::bind(&options.bind).await
		.map_err(|e| format!("failed to bind to local enpoint {}: {}", options.bind, e))?;

	let local_address = peer.socket().local_addr()
		.map_err(|e| format!("failed to get local socket address: {}", e))?;

	eprintln!("Listening for messages on {}", local_address);

	let (state, _address) = peer.recv_from().await
		.map_err(|e| format!("failed to receive robot state: {}", e))?;

	eprintln!("Received initial robot state.");

	// Retrieve start pose and compute center of circle.
	let start_time = Instant::now();
	let start_pose : nalgebra::Isometry3<f64> = state.feedback_pose().ok_or("state did not contain a pose")?
		.try_into().map_err(|e| format!("failed to convert pose to isometry: {}", e))?;
	let circle_center = start_pose * nalgebra::Translation3::new(-options.radius * 1e3, 0.0, 0.0);

	let mut sequence_number = 0u32;
	let angular_velocity = options.speed / options.radius;

	loop {
		let (state, address) = peer.recv_from().await
			.map_err(|e| format!("failed to receive robot state: {}", e))?;

		let time = state.feedback_time().ok_or("missing feedback.clock in robot message")?;
		println!("Received robot state message from {}:", address);

		// Compute new pose along the circle.
		let elapsed = start_time.elapsed().as_secs_f64();
		let offset  = rotate_z(elapsed * angular_velocity) * nalgebra::Vector3::new(options.radius * 1e3, 0.0, 0.0);
		let target  = circle_center * nalgebra::Translation::from(offset);

		peer.send_to(&abbegm::msg::EgmSensor::pose_target(sequence_number, target, time), &address).await
			.map_err(|e| format!("failed to send message to robot: {}", e))?;
		sequence_number = sequence_number.wrapping_add(1);
	}
}

#[tokio::main]
async fn main() {
	if let Err(e) = do_main(Options::from_args()).await {
		eprintln!("Error: {}", e);
		std::process::exit(1);
	}
}