GoTo – Mastering AttachedFrame¶

no_virtual_ robot Cannot be tested on an emulated robot, requires a real robot.

Goal

In this tutorial, we will see how Pepper can find and follow a human, using AttachedFrames.

Prerequisites

Before stepping in this tutorial, you should:

Be familiar with the Action notion. For further details, see: Running Actions on Pepper.
Know how to use the GoTo action. For further details, see: Understanding GoTo.
Know how to use the HumanAwareness service to find humans. For further details, see: Mastering HumanAwareness & Human.

Let’s start a new project

Start a new project, let’s call it FollowHumanPepper.
Robotify it and make sure it implements the QiSDK & the Robot Life Cycle.

For further details, see: Creating a robot application.

Creating the target frame¶

We want Pepper to follow the human and keep 1 meter between the human and him. To achieve this, we need to build a target frame that moves with the human frame, using an AttachedFrame.

This target frame must be attached to the human frame and the transform between the human frame and the target frame must be a 1 meter translation on the X axis:

To create an AttachedFrame, use the makeAttachedFrame method on the base frame and provide it the transform between the base frame and the frame you want to create. Add the createTargetFrame method to your MainActivity class:

 private fun createTargetFrame(humanToFollow: Human): Frame {
    // Get the human head frame.
    val humanFrame: Frame = humanToFollow.headFrame
    // Create a transform for Pepper to stay at 1 meter in front of the human.
    val transform: Transform = TransformBuilder.create().fromXTranslation(1.0)
    // Create an AttachedFrame that automatically updates with the human frame.
    val attachedFrame: AttachedFrame = humanFrame.makeAttachedFrame(transform)
    // Returns the corresponding Frame.
    return attachedFrame.frame()
}

private Frame createTargetFrame(Human humanToFollow) {
    // Get the human head frame.
    Frame humanFrame = humanToFollow.getHeadFrame();
    // Create a transform for Pepper to stay at 1 meter in front of the human.
    Transform transform = TransformBuilder.create().fromXTranslation(1);
    // Create an AttachedFrame that automatically updates with the human frame.
    AttachedFrame attachedFrame = humanFrame.makeAttachedFrame(transform);
    // Returns the corresponding Frame.
    return attachedFrame.frame();
}

Executing the GoTo action¶

To perform the GoTo action, we will need the following fields in the MainActivity class:

// The QiContext provided by the QiSDK.
private var qiContext: QiContext? = null
// Store the action execution future.
private var goToFuture: Future<Void>? = null

// The QiContext provided by the QiSDK.
private QiContext qiContext;
// Store the action execution future.
private Future<Void> goToFuture;

In the onRobotFocusGained and onRobotFocusLost methods, add the following code:

fun override onRobotFocusGained(qiContext: QiContext) {
    Log.i(TAG, "Focus gained.")
    // Store the provided QiContext.
    this.qiContext = qiContext
}

fun override onRobotFocusLost() {
    Log.i(TAG, "Focus lost.")
    // Remove the QiContext.
    this.qiContext = null
}

@Override
public void onRobotFocusGained(QiContext qiContext) {
    Log.i(TAG, "Focus gained.");
    // Store the provided QiContext.
    this.qiContext = qiContext;
}

@Override
public void onRobotFocusLost() {
    Log.i(TAG, "Focus lost.");
    // Remove the QiContext.
    this.qiContext = null;
}

We can now create a GoTo action from the target frame and run it.

Add a GoTo field in your MainActivity:

// Store the GoTo action.
private var goTo: GoTo? = null

// Store the GoTo action.
private GoTo goTo;

Add the following followHuman method:

private fun followHuman(human: Human) {
    // Create the target frame from the human.
    val targetFrame: Frame = createTargetFrame(human)

    // Create a GoTo action.
    goTo = GoToBuilder.with(qiContext)
            .withFram(targetFrame)
            .build()

    // Execute the GoTo action asynchronously.
    goToFuture = goTo?.async()?.run()
}

private void followHuman(Human human) {
    // Create the target frame from the human.
    Frame targetFrame = createTargetFrame(human);

    // Create a GoTo action.
    goTo = GoToBuilder.with(qiContext)
            .withFrame(targetFrame)
            .build();

    // Execute the GoTo action asynchronously.
    goToFuture = goTo.async().run();
}

Stopping the movement¶

To stop the GoTo action, we use the requestCancellation method on goToFuture:

private void stopMoving() {
    // Cancel the GoTo action asynchronously.
    goToFuture?.requestCancellation()
}

private void stopMoving() {
    // Cancel the GoTo action asynchronously.
    if (goToFuture != null) {
        goToFuture.requestCancellation();
    }
}

Testing the functionality¶

We will implement this functionality using:

a Button to make Pepper follow the closest human,
a Button to stop Pepper’s movement.

Modify your activity_main.xml file with the following code:

<?xml version="1.0" encoding="utf-8"?>
<android.support.constraint.ConstraintLayout xmlns:android="http://schemas.android.com/apk/res/android"
    xmlns:app="http://schemas.android.com/apk/res-auto"
    xmlns:tools="http://schemas.android.com/tools"
    android:layout_width="match_parent"
    android:layout_height="match_parent"
    tools:context=".MainActivity">

    <Button
        android:id="@+id/follow_button"
        android:layout_width="wrap_content"
        android:layout_height="wrap_content"
        android:text="Follow"
        app:layout_constraintLeft_toLeftOf="parent"
        app:layout_constraintRight_toRightOf="parent"
        app:layout_constraintBottom_toTopOf="@+id/stop_button"
        app:layout_constraintTop_toTopOf="parent" />

    <Button
        android:id="@+id/stop_button"
        android:layout_width="wrap_content"
        android:layout_height="wrap_content"
        android:text="Stop"
        app:layout_constraintLeft_toLeftOf="parent"
        app:layout_constraintRight_toRightOf="parent"
        app:layout_constraintBottom_toBottomOf="parent"
        app:layout_constraintTop_toBottomOf="@+id/follow_button" />

</android.support.constraint.ConstraintLayout>

Defining the UI state:

Add the following methods in the MainActivity class.

private fun enterWaitingForOrderState() {
    Log.i(TAG, "Waiting for order...")
    runOnUiThread {
        stop_button.isEnabled = false
        follow_button.isEnabled = true
    }
}

private fun enterMovingState() {
    Log.i(TAG, "Moving...")
    runOnUiThread {
        follow_btton.isEnabled = false
        stop_button.isEnabled= true
    }
}

// Add the the following fields.
private Button followButton;
private Button stopButton;

// Find the button in the view in the onCreate method:
followButton = (Button) findViewById(R.id.follow_button);
stopButton = (Button) findViewById(R.id.stop_button);

// Add the methods.
private void enterWaitingForOrderState() {
    Log.i(TAG, "Waiting for order...");
    runOnUiThread(() -> {
        stopButton.setEnabled(false);
        followButton.setEnabled(true);
    });
}

private void enterMovingState() {
    Log.i(TAG, "Moving...");
    runOnUiThread(() -> {
        followButton.setEnabled(false);
        stopButton.setEnabled(true);
    });
}

Call enterMovingState in the followHuman method, when the GoTo action starts:

// Update UI when the GoTo action starts.
goTo?.addOnStartedListener { this.enterMovingState() }

// Update UI when the GoTo action starts.
goTo.addOnStartedListener(this::enterMovingState);

Do not forget to remove this listener on GoTo in the onRobotFocusLost method:

// Remove on started listeners from the GoTo action.
goTo?.removeAllOnStartedListeners()

// Remove on started listeners from the GoTo action.
if (goTo != null) {
    goTo.removeAllOnStartedListeners();
}

Call enterWaitingForOrderState at the end of the onRobotFocusGained method:

fun override onRobotFocusGained(qiContext: QiContext) {
    Log.i(TAG, "Focus gained.")
    // Store the provided QiContext.
    this.qiContext = qiContext

    enterWaitingForOrderState()
}

@Override
public void onRobotFocusGained(QiContext qiContext) {
    Log.i(TAG, "Focus gained.");
    // Store the provided QiContext.
    this.qiContext = qiContext;

    enterWaitingForOrderState();
}

And in the followHuman method, when the GoTo action finishes:

// Update UI when the GoTo action finishes.
goToFuture?.thenConsume { future ->
    if (future.isSuccess) {
        Log.i(TAG, "Target reached.")
        enterWaitingForOrderState()
    } else if (future.isCancelled) {
        Log.i(TAG, "Movement stopped.")
        enterWaitingForOrderState()
    } else {
        Log.e(TAG, "Movement error.", future.error)
        enterWaitingForOrderState()
    }
}

// Update UI when the GoTo action finishes.
goToFuture.thenConsume(future -> {
    if (future.isSuccess()) {
        Log.i(TAG, "Target reached.");
        enterWaitingForOrderState();
    } else if (future.isCancelled()) {
        Log.i(TAG, "Movement stopped.");
        enterWaitingForOrderState();
    } else {
        Log.e(TAG, "Movement error.", future.getError());
        enterWaitingForOrderState();
    }
});

Finding the closest human:

To find the closest human, we need to compute the distance between a human and the robot. Add the following getDistance method:

private double getDistance(robotFrame: Frame, human: Human) {
    // Get the human head frame.
    val humanFrame: Frame = human.headFrame
    // Retrieve the translation between the robot and the human.
    val translation: Vector3 = humanFrame.computeTransform(robotFrame).transform.translation
    // Get the translation coordinates.
    double x = translation.x
    double y = translation.y
    // Compute and return the distance.
    return sqrt(x*x + y*y)
}

private double getDistance(Frame robotFrame, Human human) {
    // Get the human head frame.
    Frame humanFrame = human.getHeadFrame();
    // Retrieve the translation between the robot and the human.
    Vector3 translation = humanFrame.computeTransform(robotFrame).getTransform().getTranslation();
    // Get the translation coordinates.
    double x = translation.getX();
    double y = translation.getY();
    // Compute and return the distance.
    return Math.sqrt(x*x + y*y);
}

And add this method to get the closest human:

private fun getClosestHuman(humans: List<Human>): Human {
    // Get the robot frame.
    val robotFrame: Frame? = qiContext?.actuation?.robotFrame()

    // Compare humans using the distance.
    return humans.minBy {
        getDistance(robotFrame, it)
    }
}

private Human getClosestHuman(List<Human> humans) {
    // Get the robot frame.
    final Frame robotFrame = qiContext.getActuation().robotFrame();

    // Compare humans using the distance.
    Comparator<Human> comparator = new Comparator<Human>() {
        @Override
        public int compare(Human human1, Human human2) {
            return Double.compare(getDistance(robotFrame, human1), getDistance(robotFrame, human2));
        }
    };

    // Return the closest human.
    return Collections.min(humans, comparator);
}

We need to find the humans around the robot, using the HumanAwareness service. Add the searchHumans method in your MainActivity class:

private fun searchHumans {
    val humanAwareness: HumanAwareness? = qiContext?.humanAwareness
    val humansAroundFuture: Future<List<Human>>? = humanAwareness?.async()?.humansAround
    humansAroundFuture?.andThenConsume { humans ->
        // If humans found, follow the closest one.
        if (humans.isNotEmpty()) {
            Log.i(TAG, "Human found.")
            val humanToFollow: Human = getClosestHuman(humans)
            followHuman(humanToFollow)
        } else {
            Log.i(TAG, "No human.")
            enterWaitingForOrderState()
        }
    }
}

private void searchHumans() {
    HumanAwareness humanAwareness = qiContext.getHumanAwareness();
    Future<List<Human>> humansAroundFuture = humanAwareness.async().getHumansAround();
    humansAroundFuture.andThenConsume(humans -> {
        // If humans found, follow the closest one.
        if (!humans.isEmpty()) {
            Log.i(TAG, "Human found.");
            Human humanToFollow = getClosestHuman(humans);
            followHuman(humanToFollow);
        } else {
            Log.i(TAG, "No human.");
            enterWaitingForOrderState();
        }
    });
}

Defining UI actions:

Add the following code in the onCreate method:

// Search humans on follow button clicked.
follow_button.setOnClickListener {
    if (qiContext != null) {
        follow_button.isEnabled = false
        // Wait 3 seconds before following.
        FutureUtils.wait(3, TimeUnit.SECONDS).andThenConsume { searchHumans() }
    }
}

// Stop moving on stop button clicked.
stop_button.setOnClickListener {
    stop_button.isEnable = false
    Log.i(TAG, "Stopping...")
    stopMoving()
}

// Search humans on follow button clicked.
followButton.setOnClickListener(v -> {
    if (qiContext != null) {
        followButton.setEnabled(false);
        // Wait 3 seconds before following.
        FutureUtils.wait(3, TimeUnit.SECONDS).andThenConsume(ignored -> searchHumans());
    }
});

// Stop moving on stop button clicked.
stopButton.setOnClickListener(v -> {
    stopButton.setEnabled(false);
    Log.i(TAG, "Stopping...");
    stopMoving();
});

Let’s try it¶

The sources for this tutorial are available on GitHub.

Install and run the application.

For further details, see: Running an application.
Choose “Mastering attached frame”.
Make sure the robot’s hatch is closed and that Pepper sees you.
Click on the “Follow” button and move around.

Pepper will start to follow you.
Wait for Pepper to reach you or click on the “Stop” button.

Pepper will stop following you.

You are now able to make Pepper follow a human!