Behavioral Learning of Dish Rinsing and Scrubbing based on Interruptive Direct Teaching Considering Assistance Rate

We consider learning an autoregressive model that approximates the function $\bm{f}$ as shown below.

Note that $\bm{x}_{1:t}:=(\bm{x}_{1},\bm{x}_{2},\ldots,\bm{x}_{t})$. $\bm{W_{f}}$ denotes the network weight of $\bm{f}$. In this study, $\bm{x}=(\bm{s},\bm{u},p)$, where $\bm{s}$ denotes the state consisting of the robot’s state $\bm{s}_{robot}$ and the object’s state $\bm{s_{obj}}$. $\bm{u}$ is the control input of the robot. $p$ is the amount of need for human assistance, which takes the probabilistic value of $0\leq p\leq 1$. $p=0$ means no human assistance is needed, and $p=1$ means human assistance is needed. The robot executes a geometrically generated manipulation trajectory relative to the object, and gives $p=1$ when the human temporarily corrects the motion during execution, and $p=0$ when the human does not. We use the acquired data $(\bm{s}_{t},\bm{u}_{t},p_{t})$ to learn the function $\bm{f}$ that the network approximates. After training, the control input $\bm{u}_{t}$ is optimized using the learned $\bm{f}$ as shown in the following equation. $Loss$ is the loss function such as mean square error or binary cross entropy error between $\bm{x}_{t+1}^{ref}$ and $\bm{f}(\bm{x}_{1:t}|\bm{W_{f}})$.

As shown in \figrefwash_train the error $L$ is calculated and the weights $\bm{W}$ of model $\bm{f}$ are learned as shown in the following equation.

Here, $p^{actual}$ is an assistance judgment label of $0$ or $1$, where $p^{actual}=1$ means that the human is assisting and $p^{actual}=0$ means that the person is not assisting. For example, $p=1$ is given when a person corrects the trajectory of the robot’s arm while washing dishes, and $p=0$ is given otherwise. $T$ is the length of the manipulation sequence, MSE is the mean square error, and BCE is the Binary Cross Entropy error. $\bm{f}$ is trained by updating $\bm{W_{f}}$ with back propagation of the errors using Adam optimization method Kingma2015 . The batch size is $C^{train}_{batch}=4$ and the number of epochs is $C^{train}_{epoch}=100000$.

The trajectory optimization is performed using the trained model $\bm{f}$ as shown in \figrefwash_opt. The initial values of the run are given as the initial state and the joint angles $(\bm{s},\bm{u})$, and the label $p=0$, meaning the robot does not require human assistance.

We calculate the error between the prediction result of the model and the reference value of each variable.

$p_{const}^{ref}$ is set to all $0$s. In other words, optimization is performed so that human assistance is not required. $\bm{s}_{const}^{ref}$ represents the state of the target object $\bm{s}_{obj}^{ref}$ and the robot itself $\bm{s}_{robot}^{ref}$. $\bm{u}^{ref}$ is optimized to be close to the reference value of the control input according to the dirty points on the dishes. $\delta$ is set to $3$ when $\tau\geq 4$ and $\tau-1$ otherwise. MSE is the mean square error and BCE is the Binary Cross Entropy error.

Using $L_{\tau}$, the control input $\bm{u}_{\tau}$ is optimized with stochastic gradient descent method (SGD) with $C_{iter}^{test}$ iterations shown in Algorithm 2.6 - Algorithm 2.8. Finally, $\bm{u}_{t}$ is executed following Algorithm 1. $C_{epoch}^{test}$ denotes task sequence length.

We use Long Short-Term Memory (LSTM) Hochreiter1997 as our autoregressive model $\bm{f}$. LSTM is capable of learning long-term dependencies. The network input, output and hidden layer are $39$ dimensions.

The preliminary path planning experiment in this study is a game in which an agent moves to a goal while avoiding dynamic obstacles. If the agent follows the initial trajectory, it moves to the goal in a straight line from the start to the goal. If an obstacle approaches the agent while moving randomly along the way and touches the agent, the game fails. Therefore, a human can move the agent before it touches an obstacle. If the agent reaches the goal, the game is successful.

In this game, $\bm{s}$ denotes the global coordinates of the agent and the obstacle. $\bm{u}$ denotes the velocity vector of the agent. We set $p=1$ for cases where a human intervenes to manipulate the agent, and $p=0$ for cases where there is no intervention. A maze environment of size $(128,128)$ is prepared, where point ($0,0$) is the start point and point ($128,128$) is the goal point. For each step, ($\bm{s},\bm{u},p$) are collected and are used as training data only if the game is successful. We collect $C_{dataset}^{train}=150$ sets of these successful games. The model is trained according to \subsecreftrain.

We optimize the trajectory of the agent using the trained model. In the optimization, we define the reference values of each variable according to \subsecrefopt. $\bm{u}^{ref}$ is the reference velocity vector from the current position to goal position direction. $p^{ref}$ is set to $0$, i.e., the optimization is performed not to rely on the human assistance. $\bm{s}^{ref}$ is not set since the coordinates of the agent and the obstacle at the next time is not known in advance. The test sequence is set to no longer than $C_{epoch}^{test}=200$.

The results are shown in \figreftoypro, which shows the trajectory of the agent to reach the goal. The red circle is the agent and the green circle is the obstacle. Note that this is the last moment of the game, and that even if the red and green paths overlap, they don’t collide if they pass at different times. In the four figures in the horizontal row, the random seed of the obstacle behavior is set the same.

As shown in the left side, Without error back-propagation and using $\bm{u}_{t+1}$ predicted from the autoregressive model, the agent did not go around enough and collided with the obstacle. As shown in the second figure from the left in \figreftoypro, when only back propagating the error between $\bm{u}_{t+1}^{pred}$ and $\bm{u}_{t+1}^{ref}$, the agent moved more linearly to the goal, but hit obstacles more easily. As shown in the second figure from the right in \figreftoypro, when only back propagating the error for $p$, the agent moved around the obstacle but sometimes turned too far. As shown in the right side of \figreftoypro, when back propagating the error for both $\bm{u}$ and $p$,
e agent avoided the obstacle while getting closer to the goal.

path shows the statical results of path planning. As to whether the agent has reached the goal, when back propagating the error for $\bm{u}$, the agent reached the goal $100\%$ of the time, because it moved in a way that reduced the error between the goal direction and its own direction of movement. When back propagating the error for $p$, the agent sometimes couldn’t reach goal since it ovely turned around and exceeded the step limit. As to whether the agent has avoided the obstacle, when back propagating the error for $\bm{u}$, The agent sometimes moved smoothly to the goal and did not collide with obstacles, but it often collided without much avoidance. When back propagating the error for $p$, the agent did indeed try to avoid the obstacle, but it became difficult to approach the goal and eventually collided with it. As to the time step from start to goal, when back propagating the error for $\bm{u}$, the agent reached the goal faster. when back propagating the error for $p$, the agent reached the goal slower. From the above, when error back-propagating with respect to both $\bm{u}$ and $p$, the agent reached to the goal in shorter steps while avoiding obstacles.

First, we process and augment the data. For the control input $\bm{u}$, the data are augmented by adding Gaussian noise and are robust to noise. For the estimated joint torque $\bm{s}_{robot}$, we smooth the torque by taking a moving average over three frames. since there is an error due to the fact that it is an estimated value. In addition, when a human assists the robot motion during training, we want to give $\bm{s}_{robot}=0$ because the estimated joint torque of a robot becomes less relevant to object manipulation. Therefore, the filtered value as shown in the following equation is used as the input value of $\bm{s}_{robot}$.

The obtained data are sorted into scrubbing and rinsing intervals. The set of data in each section is learned based on the model described in \subsecreftrain. In other words, the model for the scrubbing operation and the model for the rinsing operation are trained respectively.

The reference of $\bm{s}=(\bm{s}_{robot},\bm{s}_{obj})$ is $\bm{s}^{ref}=(\bm{s}_{robot}^{ref},\bm{s}_{obj}^{ref})$. We don’t set $\bm{s}_{robot}^{ref}$ because the target value of the estimated joint torque is unknown. $\bm{s}_{obj}^{ref}$ is set to the initial state at the time of operation, because the target object is not expected to move significantly by the operation. $\bm{u}^{ref}$ is the joint angle solved by inverse kinematics at the tip of the gripper relative to the target point. The target point is the lower part of the faucet in the rinsing operation and the dirty point in the scrubbing operation. The right figure of \figrefdirt shows the dirty point extraction. The points with a large color difference are extracted by using RGB Canny Edge Detection of OpenCV. The distribution of the points is clustered inside the edge of the object, and the center of gravity of the point group in the largest cluster is used as the dirty target point. If $p^{predict}\geq 0.8$, we assume that the robot is unable to return to the original position by itself, and we abort the operation, place the dishes in the sink, and start over from the initial pose. The initial values for the inference are the data of the dish and the robot immediately after grasping and lifting the dishes. The optimization step is set to $C_{epoch}^{test}=5$. The sequence length is set to $C_{epoch}^{test}=80$ steps for rinsing and $C_{epoch}^{test}=150$ steps for scrubbing.

Looking back at \subsecrefpath_opt, without back-propagating $u$, the agent sometimes got stuck in the place of the local optimal solution. On the other hand, with back-propagating $u$, the agent was more like to proceed toward the goal.

Looking back at \subsecrefwash_opt, without back-propagating the error of $s$, the robot shook the dish overly when rinsing. On the other hand, with back-propagating it, the robot was more likely to swing dish stably. Without back-propagating the error of $u$, the robot couldn’t reach a dirty point. On the other hand, with back-propagating it, the robot end-effector moved toward the dirt.

Considering the results of \subsecrefpath_opt and \subsecrefwash_opt, without backpropagation, the robot executed the action associated with the trained action, which seemed to be a teacher action but it couldn’t completely reproduce the action, rather it was ambiguous action mixing the several teaching actions. On the other hand, with backpropagation, the robot modified the motion according to the reference. This enabled the robot not only to imitate the teacher but at the same time to modify the action to complete the task. As a results, the robot can perform dextrous manipulation such as scrubbing moderately and spreading water on the dish.

Efforts to generate more dextrous motion is a future challenge. For example, we use PR2 robot which does not have torque sensor around the motor in the arm. If we can control the end effectors more precisely using a torque-controlled robot and we may be able to achieve precise movements. In this study, however, a human was able to teach the robot while it was running because the robot had soft arms. Our method is unique because it is difficult to apply it in a torque-controlled robot.

Looking back at \subsecrefpath_opt, without back-propagating assistance rate, the agent is more likely to collide with the obstacle. On the other hands, with back-propagating assistance rate, the agent is more likely to avoid the obstacle.

Looking back at \subsecrefwash_opt, without back-propagating assistance rate, the robot shook the plate and splashing the water away. In the scrubbing phase, the robot operated unexpected movement. On the other hands, with back-propagating the assistance rate, the robot is more likely to tilt dish carefully when rinsing dish and scrub it with less human assistance. There is a time when the robot sliped and dropped dish even when improving motion with back pagation. In such cases, a human may be able to anticipate danger by observing an increase in the assistance rate.