Reinforcement Learning on Variable Impedance Controller for High-Precision Robotic Assembly

Let $\mathcal{F}_{tip}$ be the desired wrench on the end-effector, we can then express the control law in joint space as [36]

where $q$ represents joint angles in generalized coordinates, $M(q)$ is the inertia, $c(q,\dot{q})$ is the Coriolis matrix, $g(q)$ are gravitational forces, $J(q)$ is the Jacobian, $\tau$ is the torque vector applied to manipulators’ joints. In many force control tasks, robots move slowly, hence we ignore acceleration and velocity terms in Eq. 1. For a 7-DOF Sawyer manipulator arm that we consider in this paper, we can also project torques to its non-empty nullspace. Denoting the nullspace torque vector as $\tau_{null}$, joint space control law is:

where $J^{T\dagger}(q)$ is the pseudo-inverse of $J^{T}(q)$. The control law in Eq.2 is appealing and simple, but it would generate undesirable and dangerous motion without enough resistance provided by the environment. In our experimental setup, we do not assume in-contact situations of objects being assembled, there is a relative open free-space that the robot needs to move through; directly applying Eq.2 would result in continuous acceleration. To mitigate this issue, in all our experiments, we wrap a position loop with small gains around the controller in Eq.2:

where $K_{qp}$ and $K_{qd}$ are diagonal gain matrices with small entries, $q$ and $\dot{q}$ are current joint positions and velocities, $q^{\ast}$ and $\dot{q}^{\ast}$ are the desired ones obtained via inverse kinematics from end-effector pose. $\Sigma_{1}$ and $\Sigma_{2}$ are diagonal matrices to weight motion and force portions, respectively. The resulting hybrid controller also achieves adaptive impedance behavior implicitly: $\mathcal{F}_{tip}$ is a time-varying linear-Gaussian controller conditional on robot configuration, which is detailed in Sec. IV-B. This learned piecewise linear controller will ideally yield high-impedance when moving in free-space, and high-admittance whenever in contact; thus implicitly scaling motion-to-force ratio in aforementioned controller. Aforementioned $\mathcal{F}_{tip}$ will be calculated by an RL controller.

The specific model-based reinforcement learning algorithm that we consider here is iterative Linear-Quadratic-Gaussian (iLQG). It is sample efficient and convenient second-order methods are available to solve it quickly [37]. Let $\omega=\{{\mathbf{x}}_{1},{\mathbf{u}}_{1},\,...\,,{\mathbf{x}}_{T},{\mathbf{u}}_{T}\}$ denote a trajectory, such that $p(\omega)=p({\mathbf{x}}_{1})\prod\limits_{t=1}^{T}p({\mathbf{x}}_{t+1}|{\mathbf{x}}_{t},{\mathbf{u}}_{t})p({\mathbf{u}}_{t}|{\mathbf{x}}_{t})$, $\ell(\omega)=\sum_{t=1}^{T}\ell({\mathbf{x}}_{t},{\mathbf{u}}_{t})$ denotes the cost along a single trajectory $\omega$; where ${\mathbf{x}}$ typically consists of joint angles, end-effctor pose and their time derivatives. We wish to minimize this cost; the goal is to minimize the expectation $E_{p(\omega)}[\ell(\omega)]$ over trajectory $\omega$ by iteratively optimizing linear-Gaussian controllers and re-fitting linear-Gaussian dynamics. This algorithm iteratively linearizes the dynamics around the current nominal trajectory, constructs a quadratic approximation of the cost, computes the optimal actions with respect to this approximation of the dynamics and cost by dynamic programming, and forward runs resulting actions to obtain a new nominal trajectory. We adopt a slightly different version of iLQG proposed in [38, 15]. An additional entropy term is added into cost function such that $\tilde{\ell}({\mathbf{x}}_{t},{\mathbf{u}}_{t})=\ell({\mathbf{x}}_{t},{\mathbf{u}}_{t})-\mathcal{H}(p({\mathbf{u}}_{t}|{\mathbf{x}}_{t}))$ to encourage exploration. It can be shown that the optimal control law to this problem is $p({\mathbf{u}}_{t}|{\mathbf{x}}_{t})=\mathcal{N}({\mathbf{K}}_{t}{\mathbf{x}}_{t}+{\mathbf{k}}_{t},\mathbf{C}_{t})$, and $\mathbf{C}_{t}=Q^{-1}_{{\mathbf{u}},{\mathbf{u}}t}$, where $Q$ is cost to go, $\mathbf{K}_{t}=-Q^{-1}_{\mathbf{u,u}t}Q_{\mathbf{u,x}t}$ and $\mathbf{k}_{t}=-Q^{-1}_{\mathbf{u,u}t}Q_{\mathbf{u}t}$, subscripts denote ordered derivatives at time $t$ [38].

Our method could be regarded as generating Pfaffian constraints for each task. At contact, we can formulate holonomic and nonholonomic constraints enforced by the rigid environment as Pfaffian constraints:

where $\mathcal{V}$ is the operational space twist in SE(3) that $\mathcal{V}\in\mathbb{R}^{6}$, $A(q)\in\mathbb{R}^{k\times 6}$, $k$ is the number of natural constraints. In motion control part, we can write down operational space dynamics of the robot as:

where $\Lambda(q)=J^{-T}(q)M(q)J^{-1}(q)$, $\eta(q,\mathcal{V})=J^{-T}(q)c(q,J^{-1}\mathcal{V})-\Lambda(q)\dot{J(q)}J^{-1}(q)\mathcal{V}$. This is essentially the same motion dynamics without $\mathcal{F}_{tip}$ expressed in Eq.1, but calculated in operational space.

By combining Eq.5 and Eq.6, we can express constrained dynamics for this hybrid motion/force controller as

where $\lambda\in\mathbb{R}^{k}$; and in this case, requested wrenches $\mathcal{F}_{tip}$ must lie in the column space of $A^{T}(q)$. In general, constraints $A(q)$ come from the environment the robot is interacting with. Abstracting this type of constraint for each individual manipulation task can be time-consuming and prone to errors. Instead, if we let $\mathcal{F}_{tip}$ be learned by RL through continuous interactions; we can roughly regard this process as iteratively improving $A(q)$ to generate increasingly accurate description of tasks, which is a key ingredient in high-precision settings.

We introduce a novel neural network architecture to process noisy force/torque readings from wrist sensors or other sources, which provide measurements in tool space. The neural network is shown in Fig. 3. Force/torque information is filtered with a low-pass filter (LFP), then concatenated in the second last layer of the neural network. Intuitively, we would like to provide most direct haptics information to the neural network as principle features; also avoiding the neural network to establish unreasonable correspondence between external force/torque readings and robot internal states. Also, the robot needs to move in free space before it is in contact; and the learned policy should be robot-configuration-dependent rather than force-dependent in free space. Since F/T readings are noisy, the learned policy dependent on robot state and coupled F/T generates random motions, if we directly treat F/T as an input to the first layer of the neural network.

For training this neural network, we adopt the mirror descent guided policy search (MDGPS) algorithm in [27]. Denote the neural network parameterized by $\theta$ as $\pi_{\theta}({\mathbf{u}}_{t}|{\mathbf{x}}_{t})$, the goal is to minimize the KL-divergence between linear-Gaussian controller learned via iLQG:

this can be implemented by supervised learning. These guided policy search methods also have some mechanism to enforce agreement between distributions of local policy and global policy by adding an additional KL-divergence cost [27, 26, 4].

We summarize our method in Alg.1.

We evaluate our methods on four assembly tasks, which are shown in Fig.2. We use a Rethink Robotics Sawyer robot. Sawyer offers an interface to query its wrist force/torque measurement, the noise levels (estimated standard deviations) for $F_{x},F_{y}$ are 2.0 N; $F_{z}$ is 0.5 N; $M_{x},M_{y}$ are 0.5 Nm; and $M_{z}$ is 0.1 Nm. Sawyer is commanded via ROS at 20 Hz. During training, we take four roll-outs per iteration. Typically, it takes three iterations to achieve successful behaviors, five iterations for convergence. We define a plane by three points in end-effector space, the cost function is a weighted mixture of the $\ell_{1}$ and $\ell_{2}$ norms of the differences between the current plane and the target plane as specified by the three aforomentioned points.

We compare our method of Sec. IV-B with the following baselines:

• •

  Kinematics Only: For task 1 and task 2, we only specify target poses; for the more difficult task 3 and task 4, we also introduce several way-points. Note that for task 1 and 2, we should get the same result every single time since robot kinematic controller is deterministic as well as these tasks. But for task 3 and 2, the peg and gear can move freely, so it is hard to specify the desired trajectory.

• •

  iLQG with torque control: This is the main baseline for comparison. The control actions from iLQG are directly the seven joint torques. For comparison, we use the same cost function as in our method, i.e., sparsely-defined target end-effector pose, no intermediate way-points are introduced.

• •

  iLQG with torque control, augmented state space: We augment the state space with the F/T vector such that $\tilde{{\mathbf{x}}_{t}}=[{\mathbf{x}}_{t},f_{t}]$, where $f_{t}$ are F/T measurements. We apply direct torque control. The purpose of this is to verify if other formulation other than what we proposed could also actively use this additional information.

• •

  Our Method: We refer to the operational space controller in Section IV with iLQG.

• •

  Our Method with augmented state space: Additional to operational space controller, we augment the state space to $\tilde{{\mathbf{x}}_{t}}=[{\mathbf{x}}_{t},f_{t}]$. This experiment is for verifying if our method can be further improved.

A success is considered if an object is being assembled to a desired pose with defined tolerance. We report success rates for each individual task separately, because we train an individual policy for each task. However, it would be straightforward to report overall success rate by multiplying individual success rates together since policies are trained independently. We execute learned policies after training to calculate the success rates. Table I presents aforementioned success rates for four different tasks.

We interpret these results several fold: (1) kinematics baseline fails consecutively, this confirms the required accuracy and complexity for the gear set; (2) an iLQG with torque control, but without extensive cost shaping fails; the single success we observed is due to Gaussian noise in the controller, which generated some lucky motion to insert, and it is on the easiest task. (3) we did not find reliable improvement by augmenting state space with F/T information. Since F/T signals are not Markovian, fitting a time-correlated dynamics model to them does not produce meaningful information.

We made several interesting observations during the experiments. During task one, the robot moves quickly in free-space to reach in-contact status, then it reduces its speed to slowly probe around, trying to ”feel” the surface; once it has a level of confidence of the hole’s position, it becomes aggressive towards the goal it predicted, resulting in quick motions followed by a large downward force to complete insertion. The most interesting experiment is task 3, where the added uncertainty comes from a rotating peg. The robot first brings the small gear in contact with the peg, while applying a downward force so that small gear would not fall into free-space again; but this amount of downward force also allows room for applying additional rotating torque to the peg and gear aligning them with each other roughly; then the downward force increases to try insertion, if not successful, downward forces decrease but small horizontal force are also observed to fine-tune poses, this procedure iterates until the peg is fully inserted. This kind of behavior roughly aligns with humans’ heuristics when facing such tasks. These behaviors can be found in the supplemental video²²2https://www.youtube.com/watch?v=kqA1tlPaT8E.

Fig.5 shows computed actions (only desired forces in x-directions and y-directions are plotted) for task 2 during one successful insertion. It is interesting to observe how the variance computed by the policy changes over time. Initially, there is a certain level of variance for exploration to search for the target position; once the policy is confident about the goal, the variance reduces dramatically, the robot aggressively moves the object towards the goal; finally during the insertion phase, a certain level of noise is again injected for fine-tuning the gear’s pose to overcome friction. This force-based insertion pattern is automatically discovered through interactions by the algorithm, and matches a human’s intuition on such tasks well. Fig.6 presents F/T measurements during a successful insertion. We can observe peaks both in force and torque data, indicating some critical phases, e.g., contact. This motivates explicit use of F/T measurements, because of its informative nature.

We only train the neural network controller based on a single instance of the local linear-Gaussian controller. Hence, our purpose is not to see if the trained neural network controller can interpolate between multiple local controllers as in original guided policy search methods [4]. Instead, we are interested in examining if the proposed architecture can effectively use F/T information and adapt to environment variations. We compare our proposed neural network against two baselines: one is to directly input F/T information into the first layer of the neural network; second is the iLQG controller that the neural network controller is trained on. We again only consider task 2 in these generalization experiments. We design our experiments as following: we train these three policies towards with the goal fixed, i.e., base does not move; after all policies are trained, we move base to slightly different positions but policies will be kept unchanged; then we count success rates for these variations of different base positions. The intuition is that even base is moved, the proposed neural network controller should still be able to find the hole if F/T information is actively used, since variations in base positions bear same force pattern in terms of peg-hole insertions. The comparison with iLQG baseline is due to the fact that slight variations in the goal position could also result in low cost in LQR. We want to distinguish between this and active F/T information.

We test these three methods in three different settings where the base is moved 1cm, 2cm, 5cm respectively. Success rates are reported in Table II

For neural networks that input F/T data to their first layer, the training was often aborted due to large KL-divergence between iLQG and neural network; for few scenarios we could successfully train a policy, it often generates random, undesirable motions in free-space before insertion; it behaved poorly on three experiments. This validates our hypothesis for not establishing correspondence between external F/T readings and robot internal states.

For the comparison of the proposed neural network controller against iLQG controller, we found that neural network controller produced slightly better results; but at the mean time, the variance of the result was also high. The neural network controller did not outperform iLQG consistently. Getting a more accurate six axes F/T controller might mitigate this issue.