Fundamentals

Requesting Data

With the connection created in the previous example, you can now request data from the application. For example, the position of the end-effector of the robot can be requested and printed to the console every second. You can test it by adding the following code after the connection code in the previous example:

// Loop endlessly and every second get 'root/ManipulatorControl/manipulatorToolPoseActual'
use std::{thread, time::Duration};

loop {
    match req.get_parameter::<Vec<f64>>("root/ManipulatorControl/manipulatorToolPoseActual") {
        Ok(value) => println!("manipulatorToolPoseActual: {:?}", value),
        Err(e) => eprintln!("Failed to get parameter: {}", e),
    }
    thread::sleep(Duration::from_secs(1));
}

Move the robot around and you should see the end-effector position updating in the console output.

By changing the parameter path in the req.getParameter() function you can request any parameter that is available in the parameter tree of the connected application.

Subscribing to Data

Instead of requesting data in a loop, you can also subscribe to a parameter and receive updates whenever the value changes. This is more efficient for parameters that change frequently. You can subscribe to the same end-effector position parameter as follows:

// Subscribe to parameter changes for 'root/ManipulatorControl/manipulatorToolPoseActual'
let sub_position = sub.subscribe(
    &req,
    "root/ManipulatorControl/manipulatorToolPoseActual",
    "GroupName",
    100,
).map_err(|e| {
    eprintln!("Failed to subscribe to parameter: {}", e);
    e
})?;

// Set the callback function that handles the received data
sub_position.notify(|sub| {
    match sub.read::<(Vec<f64>,)>() {
        Some((time, (v1,))) => {
            println!("Time: {:?} ToolPose: {:?}", time.to_date_time(), v1)
        }
        None => println!("Subscribe empty"),
    }
});

// Keep the application running for 60 seconds to receive updates
std::thread::sleep(std::time::Duration::from_secs(60));

With this code, the function inside sub_position.notify will be called whenever the value of the end-effector position gets updated through the subscription. This does not mean the value has changed necessarily, but that a new value is available. You can move the robot around to see the updates in the console output.

As the notify function runs in a separate thread, you need to keep the main application running (here for 60 seconds) to continue receiving updates. You also do not want to create too many subscriptions with each one parameter, as this will create new threads for each subscription. Instead, you can subscribe to multiple parameters in a single subscription by passing a list of parameter paths to the sub.subscribe() function. As they are grouped together the data will also be synchronized. Here is an example of subscribing to both the end-effector position and the joint positions of the robot in a single subscription:

let sub_position = sub.subscribe(
    &req,
    ["root/ManipulatorControl/manipulatorToolPoseActual", "root/ManipulatorControl/jointPositionsActual"],
    "GroupName",
    100,
).map_err(|e| {
    eprintln!("Failed to subscribe to parameter: {}", e);
    e
})?;

// Set the callback function that handles the received data
sub_position.notify(|sub| {
    match sub.read::<(Vec<f64>,Vec<f64>)>() {
        Some((time, (v1,v2))) => {
            println!("Time: {:?}\n ToolPose: {:?}\n JointPosition: {:?}\n\n", time.to_date_time(), v1, v2);
        }
        None => println!("Subscribe empty"),
    }
});

// Keep the application running for 60 seconds to receive updates
std::thread::sleep(std::time::Duration::from_secs(60));

The client can create multiple subscriptions for different groups, for instance at different data-rates (by setting the “divider” parameter differently for different groups).

The group name is shared between clients and a group is published by the server once to all clients that have subscription to the group with that name. This way the communication can easily scale across large numbers of clients.

If a group name already exists on the server and another client requests a subscription to a set of signals with the same group name, the server adds any requested signals that are not already in the group to the group. The other clients using the same group name will then also receive the additional signals.

Setting parameter values

Now that we can listen to Data from the Motorcortex Application, we can try to send some Data to it. This is the way of interacting with the Motorcortex Application and controlling Robots or Machines.

Firstly, let us start up the robot!

// Set a parameter value (scalar) on the server
req.set_parameter("root/Logic/stateCommand", 2.0)
    .map_err(|e| {
        eprintln!("Failed to set parameter: {}", e);
        e
    })?;
println!("Modified parameter as scalar value");

This code will send a command to the Logic Module of the Motorcortex Application to start up the Robot. The value 2 corresponds to the STARTUP command of the stateCommand parameter.

Using the same technique you can also open and close the gripper of the robot:

// Open and close gripper 5 times
for i in 0..5 {
    req.set_parameter("root/UserParameters/IO/gripper", 0.0)
        .map_err(|e| {
            eprintln!("Failed to set gripper (close) at iteration {}: {}", i + 1, e);
            e
        })?;
    println!("Gripper closed (iteration {})", i + 1);
    std::thread::sleep(std::time::Duration::from_secs(1));

    req.set_parameter("root/UserParameters/IO/gripper", 1.0)
        .map_err(|e| {
            eprintln!("Failed to set gripper (open) at iteration {}: {}", i + 1, e);
            e
        })?;
    println!("Gripper opened (iteration {})", i + 1);
    std::thread::sleep(std::time::Duration::from_secs(1));
}
println!("Finished gripper open/close");

This code will open and close the gripper 5 times with a delay of 1 second in between.

Setting Array Values

Instead of setting single scalar values, you can also set array values. For example, to set the joint positions of the robot, you can use the following code:

// Setting Array values
req.set_parameter("root/Logic/modeCommand", 3.0)
    .map_err(|e| {
        eprintln!("Failed to set modeCommand: {}", e);
        e
    })?;
println!("Set to manual joint mode");

std::thread::sleep(std::time::Duration::from_secs(2));

req.set_parameter(
    "root/ManipulatorControl/hostInJointVelocity",
    vec![0.1, 0.3, 0.0, 0.0, 0.0, 0.3],
)
.map_err(|e| {
    eprintln!("Failed to set joint velocity: {}", e);
    e
})?;
println!("Set joint velocity");

You will notice that the robot turns on manual joint mode and starts moving the joints with the specified velocities. However, it only briefly moves. This is because the command is sent to root/ManipulatorControl/hostInJointVelocity which also has a watchdog running in the Motorcortex Application. If no new command is received within a certain time, the command is reset to zero for safety reasons.