Sequential Monte Carlo controller that integrates physical consistency and motion knowledge

Robots are required to perform complex motions and to adapt to their environments. Programming robots with large degrees of freedom to execute motions is time consuming, and transferring the programmed motions to new environments is difficult. A compact representation of motion primitives and their adaptation to new environments are important problems that need to be solved in order to integrate robots into our daily lives.

The motion primitives are useful representations, in which robots record motions and which they can reuse for motion recognition and motion generation. In imitation learning, a teacher demonstrates to the robots in a simple way, and the demonstrations are encoded into motion primitives based on stochastic models or dynamical systems. The robots can recognize a demonstration in terms of selecting the motion primitive most similar to the demonstration, and generate their own motions from queries of motion primitives. Stochastic frameworks are provided with efficient algorithms to optimize model parameters from demonstrations and to calculate the distances between motion primitives and the demonstration; they are, therefore, widely used to represent motion patterns. In particular, they have been successfully used in motion recognition (Inamura et al. 2004), and have been combined with natural language processing such that robots can relate demonstrations to descriptions in the form of sentences (Takano and Nakamura 2015a, b). In addition, the stochastic frameworks allow robots to execute motions almost the same as those they were trained on Calinon et al. (2006). In these frameworks, demonstrations are expressed kinematically but their dynamics, such as joint torques and contact forces, are not handled. Key challenges in imitation learning using stochastic models are including the dynamics in the representation of the demonstration and synthesizing motions that interact dynamically with the environment. In reinforcement learning, the robots incrementally find the policy of the action given the current state by the manner of trial and error. The dynamical systems, typified by dynamic movement primitives (Ijspeert et al. 2003), create the kinematic control policy, and trial and error learning allows for identifying the dynamics in the motion and improving the control policies (Schaal et al. 2003). The motion primitives are expressed by the stochastic graphical model endowed by the cost function, and the optimal feedback controller is designed according to the inference on the graphical model and identified dynamics model (Ruckert et al. 2013). During the execution of the motions, these models are improved by sampled data, but this improvement requires much time especially for systems with a large degree of freedom. Additionally, the profile of the motion depends on the manually given goal and cost function, and it is not necessarily similar to human demonstrations.

In this paper, we propose a novel approach to representing the kinematics and dynamics in demonstrations in stochastic models, and to controlling a robot’s motion to adapt to the environment. Demonstrations are expressed as joint angles and joint torques, and these demonstrations are encoded into hidden Markov models (HMMs). Physically consistent joint angles and joint torques based on the equation of the motion of the robot that are the most likely to be generated from the HMM are computed. These joint angles and joint torques are found by using a Bayesian sampling method. More specifically, when encoding the joint angles and joint torques in the demonstrations, the importance of each joint angle or joint torque is extracted in the form of its distribution in the HMM. During the robot performing the motion where the joint angles are dominant, the joint angles more similar to the training demonstration and the joint torques less similar are generated such that the robot can perform the motion kinematically similar to the demonstration. When the robot performs the motion where the contact forces are important, the joint torques more similar to the demonstration are generated such that the robot can control the reaction forces to become as close as possible to those during the demonstration. We demonstrate the validity of our approach using a robotic arm with seven joints that draws lines or curves on a wall by contact between its end-effector and the wall.

There have been many studies of imitation learning, including learning by observation, learning from demonstration, and programming by demonstration (Breazeal and Scassellati 2002; Schaal 1999; Mataric 2000).

A classic framework for imitation learning is Behavioral Cloning (BC), where the behaviors of skilled human operators are recorded, and a control policy that maps states to actions is exploited (Bain and Sammut 1996). This policy executes complex tasks by tracing behaviors. BC stores direct mappings between the states and actions without taking into account the physics of the robot. Because it is difficult to apply this mapping to a new environment, BC suffers in environments that are different from those in which the policy was trained.

One popular approach to imitation learning is the use of Dynamic Movement Primitives (DMPs) (Ijspeert et al. 2001, 2003). DMPs represent movements as nonlinear differential equations, and modify the shapes of generated movements so that they reach their goals or have desirable durations. Matsubara et al. (2011) extended DMPs to represent multiple movements in order to improve their scalability, and demonstrated their validity on a dual-arm robot. Prada et al. (2013) proposed DMPs specialized for a robot handing over an object to a human partner. Okada et al. designed the vector field, where the demonstration is a closed curve in polynomial form, and any point is attracted to this closed curve. This attractor field stabilizes the robot motion even if it is disturbed (Okada et al. 2002). These approaches define the motion by differential equations or the polynomial forms of kinematic configurations, and do not deal with the dynamics of the robot. Therefore, the synthesized motion is not guaranteed to follow profiles of joint torques or contact forces that are similar to those of the demonstration.

Neural networks are forms of imitation learning that represent movements as network parameters. Tani and Ito (2003) have developed Recurrent Neural Networks with Parametric Bias (RNNPB) which encode demonstrations into parameters on the bias layer and a set of parameters represents a motion primitive. This framework predicts the movement from the current perception, and this leads to motion generation. This framework has been extended to motions and language. One RNNPB learns motion primitives, and another RNNPB learns sequences of words. These two RNNPB are linked with the bias parameters. This framework enables robots to interpret movements in the forms of language, and to generate movements from language commands (Sugita and Tani 2005; Ogata et al. 2007). However, these models represent the kinematics of the movements, but do not handle the dynamics. Kadone and Nakamura (2005) proposed associative neural networks with non-monotonic activation functions. These functions control the abstraction levels of motion primitives, but motion synthesis using the associative neural networks was not discussed. Neural networks using deep learning have been applied to representing high dimensional sensory-motor data (Noda et al. 2014; Finn et al. 2017; Nair et al. 2017). Robots using these networks can generate motions in response to visual perception, but the neural networks do not represent dynamics, such as contact between the motions and manipulated objects.

Many stochastic approaches to representing motion are well-established. Inamura et al. (2004) used HMMs for representing demonstrations. In their framework, whole body motions are generated according to the distribution of the demonstrations by Monte Carlo sampling. The generated motion is likely to become an average of the demonstrations, and does not dynamically adapt to the environment. Calinon et al. (2006) have extended HMMs to joint angles and joint angular velocities. Their approach can compute the joint angular velocities given the current joint angles. This computation results in kinematically adaptive motions. Takano and Nakamura (2017) have proposed a method to encode human motions into HMMs and to kinematically synthesize human-like motion from the HMMs. In this framework. the HMMs handle only joint angles, and plan a sequence of the joint angles, which are modified to dynamically adapt to the environment by the optimal controller (Takano and Nakamura 2017). Herzog et al. used Parametric Hidden Markov Models (PHMMs) (Wilson and Bobick 1999) that represent motions and relevant objects (Herzog et al. 2008). Several motions are encoded into an HMM as a representative motion primitive, and each motion variant is encoded into an individual HMM. The parameters of the individual HMM are expressed as a linear function of the parameters of the representative HMM and the position of the object. The motion acting on the object can be generated from an individual HMM that is created from the representative HMM and the current position of the object. Another method which represents motions and manipulated objects in a task-parameterized mixture model has been proposed (Calinon et al. 2013). Paraschos et al. (2013) proposes Probabilistic Movement Primitives (ProMP) for representing demonstrations. ProMP can derive the feedback controller to synthesize motions according to distributions over the demonstrations. This approach has been extenteded to segmenting demonstrations and subsequently encoding the segments into ProMP (Lioutikov et al. 2015). All of these stochastic approaches represent only the kinematics of the demonstrations, and do not take into account the dynamics of the demonstrations. So the contact forces are not controlled by the motions synthesized by these approaches.

The contributions of this paper are the extension of HMMs to joint angles and joint torques to capsulate both the kinematics and the dynamics of demonstrations, and a method for generating joint angles and joint torques that satisfy the equation of the motion of the robot and are most likely to be generated from the HMM. The resultant joint angles and joint torques realize robot motion that is both kinematically and dynamically valid, and is similar to the demonstrated motion.

The validity of our approach to generating motions from HMMs was verified by using anthropomorphic arms. An individual arm has seven joints. A trunk and an end effector are connected to the arm. The shoulder, elbow, and wrist consist of three, one, and three joints, respectively. Each joint is equipped with an incremental encoder and a torque sensor. Note that the end effector is not equipped with a force sensor, and so the external force acting on the end effector from the contacting environment cannot be measured. The angles of pitch, roll, and yaw in the shoulder are denoted by \(\theta _1\), \(\theta _2\), and \(\theta _3\), respectively; the pitch in the elbow is denoted by \(\theta _4\); and the yaw, pitch, and roll in the wrist are denoted by \(\theta _5, \theta _6\), and \(\theta _7\), respectively. The corresponding joint torques are denoted by \(\tau _1, \tau _2, \tau _3, \tau _4, \tau _5, \tau _6\), and \(\tau _7\), with the same meaning for the subscripts.

Two robotics arms are connected in a bilateral teleoperation system, as shown in Fig. 3. One robotic arm is referred to as the master robot, and the other is referred to as the slave robot. The master and slave robots mutually communicate their joint angles and joint torques with one another. The slave robot attempts to follow the joint angles of the master robot and to act with the same force as the master robot.Here \({\varvec{\tau }}_c^{(s)}\) is a vector of torques input to the slave robot; \({\varvec{\tau }}^{(m)}\) and \({\varvec{\tau }}^{(s)}\) are joint torques to be measured in the master and slave robots, respectively; and \({\varvec{\theta }}^{(m)}\) and \({\varvec{\theta }}^{(s)}\) are angles of all the joints in the master and slave robots, respectively. \({\varvec{K_p}}\), \({\varvec{K_d}}\), and \({\varvec{K_{\tau }}}\) are coefficient matrices. The master robot is controlled in the same manner as the slave robot.

A human directly moves the master robot such that the slave robot draws a line or a curve on a whiteboard, as shown in Fig. 4. We measured the angles and torques of all the joints in the slave robot during the drawing of a horizontal line, a vertical line, a diagonal line, and a circle. The measurement is conducted at a sampling interval of 1 ms, but measurements at a sampling interval of 100 ms are used as training data for the motion primitive. More specifically, five sequences of the joint angles, \({\varvec{\theta }} = \left( \theta _1, \theta _2, \dots , \theta _7 \right) ^T\), and the joint torques, \({\varvec{\tau }} = \left( \tau _1, \tau _2, \dots , \tau _7 \right) ^T\), for each motion are encoded to its specific HMM. From this, four HMMs are obtained as motion primitives, representing the actions “drawing a horizontal line,” “drawing a vertical line,” “drawing a diagonal line,” and “drawing a circle.” Note that the numbers of nodes and Gaussian distributions on each node in the HMM are set to 30 and 1, respectively,

We tested the HMMs for “drawing a horizontal line” and “drawing a diagonal line” on motion generation without taking into account the dynamics of robot’s motion, as described in Sect. 3.1. Figures 5 and 6 show training and typically generated trajectories of joint angles, along with generated trajectory distributions. Figures 7 and 8 show training and typically generated trajectories of joint torques, along with generated trajectory distributions. The generated trajectories are close to the training trajectories, and the generated trajectory distributions cover the training trajectories well. This implies that the HMMs well represent the trajectories of both joint angles and joint torques.

We tested our approach to generating physically consistent motions from the HMMs. The desired reaction force on the end effector was set to 10 N in magnitude, with force vector \({\varvec{F}}=\left[ -10 \ 0 \ 0 \ 0 \ 0 \ 0 \right] ^T\). As described in the previous section, our approach generates reference angles \({\varvec{\theta }}^{ref}_t\) and torques \({\varvec{\tau }}^{ref}_t\) for all seven joints. The generation of joint angles and joint torques is based on a sampling method using particles, which is time-consuming. Since the computations for each individual particle are independent, the computations are run as parallel processes. In this experiment, computations of 200 particles were implemented across eight parallel processes. In this setting, all processes were completed within 100 ms. We did not use the references of the joint angles but used only the references of the joint torques as the input torques for the robot. However, the motion of the robot was sometimes unstable and the robot failed to perform the drawing motion. We therefore combined the reference joint angles \({\varvec{\theta }}^{ref}_t\) and joint torques \({\varvec{\tau }}^{ref}_t\) to compute the input torques:where \({\varvec{\theta }}_t\) and \(\dot{{\varvec{\theta }}}_t\) are joint angles and joint angular velocities at time t. The reference angular velocities \(\dot{{\varvec{\theta }}}^{ref}_t\) are calculated as \(( {\varvec{\theta }}^{ref}_t - {\varvec{\theta }}_t) / \varDelta t\), where \(\varDelta t\) is the sampling time, which is set to 100 ms. The coefficient matrices \({\varvec{K}}_{p}\) and \({\varvec{K}}_{d}\) take the same values as in Eq. 10, and the diagonal weight matrices \({\varvec{W}}_{\theta }\) and \({\varvec{W}}_{\tau }\) are manually chosen such that \({\varvec{W}}_{\theta }+{\varvec{W}}_{\tau } = {\varvec{I}}\). The joint torques are controlled at a sampling interval of 125 µm.

Figure 9 shows qualitative experimental results where a robot performs the motions “drawing a horizontal line,” “drawing a vertical line,” “drawing a diagonal line,” and “drawing a circle” as generated by the corresponding HMMs. Note that the location of the board was different from that during training. Even in these cases, the end effector moved on the board. The computed joint torques for command are equivalent to the reaction force that shuld come from the board. When the board is distant from the end effector and the reaction force does not exist, the computed joint torques moves the end effector toward the board until it balances with the reaction force as shown on the left panel in Fig. 10. Therefore, the end effector can contact the board.

Figure 11 shows trajectories of the end effector for six trials of motion generation for the robot performing the motion “drawing a horizontal line.” Five trials are generated by using the proposed approach, and one trial is generated without using the equation of motion. More specifically, HMM generates a sequence of joint angles and joint torques based on the Monte Carlo method, and input joint torques are modified according to Eq.11. The position of the end effector is estimated by a forward kinematics computation using the measured joint angles. The proposed approach results in the motions of moving to the whiteboard in straight lines, drawing the line on the board, and moving the initial position, and attains the high repeatability. The motion generation without using the equation of motion leads to wobbling. Figures 12 and 13 show the corresponding profiles of measurements of the joint angles and joint torques. The whiteboard target for drawing lines or a circle was located at a different position from that of during learning, and the desired reaction force” should be ”a different position from that of during learning, and the desired reaction force profile was also different. Comparing Fig. 12 with Fig. 5 and Fig. 13 with Fig. 7 demonstrate that the profiles of the five generated motions are similar to the profiles of the training motions and that the trajectories of the joint angles and joint torques are modulated such that the end effector can move to follow a horizontal line on the board and maintain the desired reaction force. Figures 14 and 15 show the profiles of measurements of the joint angles and joint torques during the robot drawing a diagonal line on the whiteboard. Comparisons of Fig. 14 with Fig. 6 and Fig. 15 with Fig. 8 also show that joint angles and joint torques are varied to be adaptive to the current environment, but it demonstrates that the five motions are controlled with maintenance of the profiles of training motions.These experiments confirm that our proposed approach of generating physically consistent motions from a stochastic model can generate reference joint angles and joint torques similar to the training values and that the generated motion can adapt to slight variations from the training environment.