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:
Let’s start a new project
For further details, see: Creating a robot application.
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();
}
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();
}
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();
}
}
We will implement this functionality using:
Button
to make Pepper follow the closest human,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();
});
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!