## I Introduction

Most robots today are programmed to move through the world as if they are afraid of making contact. Perhaps they should be: unexpected collisions while a robot is tracking a trajectory can create a large force at the point of contact, putting at risk both the robot and the environment with which it interacts (Fig. 1). Consequently, a significant amount of effort and care in robot motion planning is spent on avoiding collisions. For example, sampling-based planners need to perform numerous collision checks [lavalle2006planning]; optimization-based planners need to constantly evaluate the signed distance functions and their gradients [ratliff2009chomp]. Moreover, high resolution collision geometries are usually needed to increase the chances of finding a collision-free path, which further increases the computational cost [pan2012fcl].

However, the complete and total avoidance of contacts is a severe limitation, even if we are willing to tolerate the computational cost. First, the effectiveness of collision-free planning is limited by the quality of the geometric models used for collision checks. Unless in structured environments where everything has been perfectly measured, models of the environment need to be reconstructed from range sensor (e.g. depth camera) measurements, which usually have a fair amount of uncertainty and can suffer from occlusion. Moreover, collision-free trajectories can be unnecessarily conservative [mason2018toward]: a task achievable by making some contacts can be deemed infeasible by a collision-free planner.

In this work, we propose a QP controller which, given estimated contact positions and forces in unexpected contacts, tracks the reference trajectory as closely as possible while keeping contact forces below a user-defined upper bound. Compared with similar controllers based on null-space projection [nakamura1987task, siciliano1991general, aghili2005unified, dehio2018modeling, jorda2019contact], the QP formulation shares the same underlying dynamics, but allows for more gentle separation when the robot breaks contact with the environment. The gentle separation happens naturally as a result of bounding the contact forces with inequality constraints, which are not supported by null-space projection.

Instead of the usual second-order dynamics constraints used in robot locomotion [kuindersma2014efficiently, koolen2016design], the proposed QP controller utilizes a quasistatic dynamics model which predicts future equilibrium configurations and contact forces for a stiffness-controlled robot in response to position commands. By assuming bi-lateral, frictionless contacts, the quasistatic dynamics model can be expressed as equality constraints and does not require the estimation of friction coefficients. Moreover, as real-world contacts are uni-lateral and frictional, we also propose measures which both capitalize the simplicity of bi-lateral, frictionless contact models and mitigate the side effects of modeling real-world contacts as such.

## Ii Related Work

### Ii-a Interaction control

As the primary objective of the proposed QP controller is to bound unexpected contact forces while tracking a joint-space or end-effector trajectory, we review existing methods for combined motion and force control, which are also referred to as interaction control in the literature [siciliano2008springer, Chapter 9]

. Interaction control techniques can be broadly classified by whether the interaction force is controlled

directly or indirectly.Direct force control typically splits the task space into two orthogonal subspaces based on the robot’s kinematic constraints: one motion-controlled subspace along the tangents of the kinematic constraints, and one force-controlled subspace along the normals. Desired motion and force trajectories are specified in the motion and force controlled subspaces, and tracked independently using motor torque commands computed from the robot’s second-order model [mason1981compliance, khatib1987unified, raibert1981hybrid].

However, the success of direct force control relies on accurate robot and environment models, which are not easily available in unstructured environments. Moreover, with few exceptions [franka], most industrial robot arms, including the KUKA iiwa, do not have an interface for end-users to directly control motor torques [kuka2019fri]. Last but not least, by directly controlling motor torque, the interaction controller bypasses the robot’s factory motion controller, and thus needs to run at high frequency in order to maintain stability.

On the other hand, a classical example of indirect force control is impedance control [hogan1984impedance], which regulates the robot’s response to external forces to that of a mass-spring-damper system (a mechanical impedance), thereby guaranteeing interaction stability by passivity. When the robot moves slowly, which is often the case in manipulation tasks, impedance control can be simplified to stiffness control [salisbury1980active], which can be interpreted as connecting the robot’s end effector to a user-specified set-point by virtual springs. In the presence of contact, contact force can be controlled by commanding how much the set point penetrates the obstacle.

Compared with direct force control, indirect force control schemes are usually implemented as an outer-loop around the robot’s factory motion controller, and therefore does not bear the responsibility of maintaining stability and can run at a much lower rate.

### Ii-B Quasistatic dynamics models in manipulation

Quasistatic dynamics has been used with great success to simplify the planning and control of simple tasks such as planar pushing [hogan2020feedback], where modeling robots as prescribed motion trajectories is sufficient. However, for multi-contact tasks such as grasping [pang1996complementarity], such simplifications can lead to non-unique contact forces or violation of non-penetration constraints [halm2018quasi, pang2018robust]. By modeling robots as impedances, our recent multibody quasistatic model [pang2021convex] can faithfully reproduce the steady-state behavior of stiffness-controlled robot arms in multi-contact scenarios.

The quasistatic robot dynamics model proposed in this work, which predicts future joint angles in whole-arm contact scenarios, is an extension to classical indirect force control schemes that typically focus solely on the relationship between end effector pose and wrench [salisbury1980active, hogan1984impedance]. The proposed quasistatic dynamics is also a frictionless simplification of the Coulomb-friction-based quasistatic dynamics [pang2021convex], which can be implemented on hardware with minimal contact sensing.

### Ii-C Null-space projection

Null-space projection is a classical and popular technique for executing a hierarchy of tasks defined by equality constraints [nakamura1987task, siciliano1991general, aghili2005unified, dehio2018modeling, jorda2019contact]. The constraints imposed by higher-priority tasks are enforced by projecting the torque needed by low-priority tasks into the null space of the higher-priority tasks. The projections are defined over the ranges and null spaces of the task Jacobians and their weighted pseudo-inverses. It is noteworthy that the projections are generally not orthogonal, unless the weight matrix is identity [dietrich2015overview].

More recently, controllers based on constrained optimizations such as quadratic programs (QP) have gained popularity in both locomotion [koolen2016design, kuindersma2014efficiently] and manipulation [jain2013manipulation]. Compared with null-space projections, QP-based controllers can handle both equality and inequality constraints. In this work, we formulate the problem of trajectory tracking with bounded contact forces as a QP with a novel quasistatic dynamics constraint. We also show that a controller based on null-space projection implicitly enforces the same quasistatic dynamics constraint when the projection is stiffness-consistent [dietrich2015overview].

## Iii Background and Notations

### Iii-a Constrained Inverse Dynamics Control

A popular controller in locomotion and manipulation is based on the following optimization-based formulation [kuindersma2014efficiently, koolen2016design, wang2019impact]:

(1a) | |||

(1b) |

where and are the state and input at the current time step , and is the state at the next time step, . The controller (1) picks an action that minimizes the (usually LQR-style) state and action cost, and , subject to the dynamics constraint (1b).

The most common choice for the dynamics constraint (1b) is the Newton’s Second Law (N2L). For example, the DLR (German Aerospace Center) family of robots, including the KUKA iiwa and FRANKA panda, has the following closed-loop second-order dynamics after gravity compensation[ott2008passivity]:

(2) |

where is the joint angles of the robot, is the Coriolis force, is a diagonal damping matrix, is a diagonal stiffness matrix, is the commanded joint angles, and is the torque by external contacts.

### Iii-B Contact and multibody notations

We consider rigid, point contacts in this work. The number of contacts the robot makes with the environment is denoted by . Each contact point is denoted by . The coordinates of the contact point relative to world frame, expressed in world frame is written as ; contact force at expressed in world frame is represented by .

Position Jacobian of the contact point relative to frame , expressed in frame , is denoted by . It maps the robot’s joint velocity to the velocity of point in world frame :

(3) |

We further define

(4a) | ||||

(4b) | ||||

(4c) |

The -th row of maps to the Cartesian velocity of along in world frame. We assume that is full-rank.

## Iv Quasistatic Dynamics

### Iv-a Dynamics as transitions between equilibria

For robot arms with a joint-level stiffness controller, their steady-state equilibrium condition can be obtained by setting the derivative terms in the second-order dynamics (2) to 0:

(5) |

For a stiffness-controlled robot, we can define its quasi-static dynamics, whose state consists only of the joint angles , and input the commanded joint angles . As shown in Fig. 2, the quasistatic dynamics predicts , the equilibrium configuration at the next time step, from the current equilibrium configuration and the next commanded configuration .

The new equilibrium can be solved for by minimizing the potential energy of the robot, subject to the contact constraints:

(6a) | |||

(6b) |

To derive the contact forces, we start with the Lagrangian of QP (6):

(7) | ||||

where is the Lagrange multipliers of the contact constraint (6b), which can also be interpreted as the contact forces generated by (6b); the dependency of on is dropped for simplicity.

The KKT optimality condition of QP (6) is given by

(8a) | ||||

(8b) |

where (8a) is equivalent to the steady-state force balance condition (5), assuming that is generated by the point contacts, i.e. .

Explicit expressions for and can also be derived from the KKT conditions (8):

(9a) | ||||

(9b) |

### Iv-B Relationship to null-space projection

In this sub-section, we show that when controlling stiffness-controlled robots using null-space projection, lower-priority tasks can be guaranteed to not interfere with higher-priority tasks during transients if the stiffness-consistent projection [dietrich2015overview] is used. We also show that the underlying dynamics model of a controller based on stiffness-consistent projection is the same as the model proposed in Sec. IV-A.

Null-space projection technique revolves around two projections:

(10a) | ||||

(10b) |

where is the pseudo-inverse weighted by a positive-definite . The range and null space of are respectively and , whereas the range and null space of are reversed. Note that such projections can be defined for arbitrary task Jacobians, but we specialize to the contact Jacobian defined in (4c) without loss of generality.

For any choice of and any joint torque , generates contact forces, whereas generates no joint torque in after static equilibrium is reached. Choosing an appropriate , however, can provide additional guarantees during the transient into this steady state [dietrich2015overview]. For instance, the dynamically-consistent pseudo-inverse [featherstone1997load], which uses the robot’s mass matrix for , ensures that

(11) |

Assuming that the dominant effect of during the transient is to generate acceleration, property (11) guarantees that and the generated acceleration lies inside .

To determine the appropriate choice of when the dominant effect of the during transient is to stretch/contract the virtual spring of a stiffness-controlled robot, we start at the instant , immediately after sending the joint angle command at time step . At , the joint torque can be expressed as

(12) |

which can be decomposed as

(13) |

where generates contact forces, and generates a motion that needs to be in .

At time instant , when the equilibrium at time step is reached but has not been commanded, has generated a displacement and been dissipated by damping, but , the generalized force due to contact, remains:

(15) |

Moreover, static equilibrium (5) at dictates that

(16) |

As the motion from to needs to stay in , we need , which implies through (18) that

(19) |

which has the same form as dynamic-consistency defined in (11). Not surprisingly, choosing ensures the motion generated by stays in , and the resulting pseudo-inverse is called stiffness-consistent [dietrich2015overview].

Furthermore, as the contact force is the reaction to , we have , combining this with the definition of in (13) gives

(21) |

It can be shown that (20) and (21) are equivalent to (9b) and (9a), respectively. This equivalence implies that a controller based on stiffness-consistent projections and a controller (1) using QP (6) as its dynamics constraint share the same underlying dynamics model. Therefore, the QP formulation in Sec. V is preferred as it can handle inequality constraints.

## V QP Controller with Quasistatic Dynamics

### V-a Frictionless contacts

To track a reference trajectory as closely as possible while respecting dynamics constraints and upper bounds on contact forces, we can specialize the generic optimization-based controller (1) to the following QP:

(22a) | |||

(22b) | |||

(22c) | |||

(22d) | |||

(22e) |

Here, the dynamics constraint (1b) consists of (22b) and (22c), which are the KKT conditions (8) of the quasistatic dynamics (6). Constraint (22d) places an upper bound on contact forces. The last constraint (22e) bounds how quickly changes.

In the objective (22a), the first term penalizes deviation at from the reference trajectory. The second term, weighted by a small positive scalar , adds regularization without which the objective would become semi-definite. To see why, we re-write by expressing explicitly as a function of using (20):

(23) |

where the second term is a constant, and the first term multiplies by a projection which has a non-zero null space.

In the quasistatic dynamics (6), expressing contact constraints as equality constraints (6b) and contact forces as the constraints’ Lagrange multipliers implies that the contacts are bi-lateral and frictionless. In reality, however, contacts are uni-lateral and frictional.

The bi-lateralness of (6b) is less concerning. As contact sensors are inevitably noisy, only contact forces above a threshold are added to QP (22). In addition, by (9a), the change in contact force is bounded as long as is bounded, and the boundedness of is enforced by (22e). Therefore, as long as the bound is sufficiently small, we do not need to worry about contact forces flipping sign in the middle of a control step.

On the other hand, naively ignoring friction will severely impact the performance of the controller, which motivates the mitigating measures detailed in the next subsection.

### V-B Frictional contacts

It is possible to model friction contact in quasistatic dynamics [pang2021convex], but control through a frictional contact requires estimating the contact normal and the friction coefficient, in addition to estimating contact forces. This requires either more sophisticated whole-arm contact sensors, or making additional assumptions about the environments that make the control-estimation pipeline more brittle.

Therefore, we will retain the simpler frictionless contact model for controlling through a frictional contact, and mitigate the side effects of the wrong contact model by modifying the frictionless QP (22) to

which has the same constraints as (22) but a different objective. In the rest of this section, we will elaborate on the reason for both terms in the objective (24a).

#### V-B1 Tracking

When the reference trajectory leads the robot to make contact with a frictional surface at point (Fig. 3a), the surface normal and the contact force direction can be different. However, the frictionless contact model (22b)-(22c) assumes that is always the same as . Therefore, it is possible for , the commanded velocity of , to have a negative component along but a positive component along , as shown in Fig. 3a. Such a would lead to the robot separating from the obstacle at . When the frictionless QP (22) is constructed again at , no contact force constraints are added but can still lead the robot to contact with a large amount of penetration. Therefore, the robot could re-establish contact with a large contact force at .

Although it is difficult to guarantee that has a negative component along without knowing , undesired contact jitters can be effectively reduced by making as close as possible to . In joint space, this translates to minimizing the distance between and , which can be achieved by replacing the term in (22a) by in (24a).

To further illustrate the advantage of the new objective, we first re-write using the relative quantities defined in Fig. 3b:

(25) |

It is also easy to see from (23) that

(26) |

The first term in the original objective (22a) thus becomes

(27) | ||||

where is the projection into along , as shown in Fig. 3b.

As a result, minimizing encourages to be close to

in the entire vector space. In contrast, when

is used as the cost, only the distance between and the component of along is minimized.#### V-B2 Damping

The goal of this term is to command more conservative robot motions when we are less confident in the correctness of the frictionless contact model. As real-world contacts are almost always frictional, the contact force predicted by the frictionless model and the actual contact force measured by contact sensors are bound to be different. At every time step, this discrepancy can be quantified by

(28) |

where is the contact forces predicted by the frictional QP (24) at time step ; is the measured contact forces at time step ; is a positive constant that weights the force prediction error. The discrepancy is close to when the force prediction error is large, and close to when the error is small.

The weight of the second term of (24a), , is the low-pass-filtered version of the discrepancy :

(29) |

where is the upper bound on and is the forget rate of the low-pass filter. A larger encourages more conservative robot motions by more heavily penalizing the change in from to .

## Vi Experiments

In this section, we demonstrate the advantages of the proposed contact-aware controller through two tasks that involve unexpected contact with the environment, which are shown in Fig. 4. In both experiments, is tracked using the iiwa’s factory impedance controller. The factory controller’s stiffness is set to , from base joint to wrist joint. The proposed contact-aware controller runs at 200Hz. The external contacts are estimated from iiwa’s external torque measurements using the Contact Particle Filter [manuelli2016localizing], which runs at around 100Hz. QPs are constructed using Drake’s MathematicalProgram interface [drake] and solved by GUROBI [gurobi].

In order to reduce sensitivity to measurement noise, only contact forces with norm are considered when constructing the contact Jacobian (4c). We have chosen , and set the upper bound on contact forces in (22d) to be . Ignoring contacts with small contact forces can be justified by the passivity of the robot’s internal controller [albu2007unified], which ensures stability in the presence of external contacts.

### Vi-a Mug placement task (Fig. 4a)

This task is defined by an end-effector pose trajectory , where is the reference for the tool frame , is the orientation of frame w.r.t. the world frame , and is the position of the origin of in world frame. It is straightforward to modify the tracking term in the frictional QP objective (24a) to minimize the pose difference between frame and its reference , as described in [koolen2016design] [tedrake2021manipulation, Chapter 3].

The robot starts with a mug held in the gripper. It then (i) reaches down ( of world frame) by in 4s, (ii) opens the gripper and drops the mug on the cart below the table in 2s, and (iii) moves back up to where it started in 4s. The orientation of the gripper is kept constant throughout the trajectory. As shown in Fig. 4a, the “wrist” (link 6) of the robot collides with the edge of the table as the gripper moves down and up.

#### Vi-A1 Contact force

As shown in Fig. 5, except during the initial impact, our controller is able to keep the contact force norm close to . In contrast, the baseline controller we compare against, which computes by greedily minimizing tracking error without the dynamics and contact force constraints (22b)-(22e), incurs larger contact forces.

#### Vi-A2 Tracking error

As shown in Fig. 6, compared with the baseline, the contact-aware controller produces significantly less tracking error in the presence of external contact.

### Vi-B Mug moving task (Fig. 4b)

This task is defined by a joint-space trajectory , with the goal of moving the mug along a straight line while keeping the mug orientation constant. The 16s trajectory

is an interpolation between joint-space knot points obtained by inverse kinematics. As shown in Fig.

4b, to move the mug to the desired destination, the robot needs to first establish a contact with the top face of the table, and then breaks contact with the side of the table.The baseline we are comparing against is the class of controllers trying to achieve similar goals as ours but uses null-space projection, such as [jorda2019contact]. As null-space projection cannot handle inequality constraints, the contact force upper bound is usually enforced by an equality constraint which sets the contact force to . When commanded to break contact by the reference trajectory, i.e.

(30) |

the contact force constraint needs to be abruptly removed in order to continue to track the reference trajectory [jorda2019contact]. This can lead to large joint velocity during separation, as shown in Fig. 7.

In contrast, the proposed QP controller does not explicitly make the decision to break contact based on (30), instead the break of contact comes naturally as a consequence of solving QP (24) with the inequality constraints on contact forces (22d). As shown in Fig. 7, when the robot separates from the table, the drop in occurs gradually with our QP controller, but abruptly with a null-projection-based controller.

## Vii Conclusions

We have presented a contact-aware controller that reconciles trajectory tracking with safety in unexpected contacts. The proposed controller is formulated as a QP with a quadratic cost on tracking error, a quasi-static model of the robot dynamics as constraints, and upper bounds on contact forces.

The tasks for hardware experiments are designed based on our vision of future motion planners: they are comprised of smooth, simple trajectories defined by only a few knot points. We have shown that the proposed controller is able to keep both the tracking error and contact forces small if the robot makes an accidental contact. In addition, our controller outperforms controllers based on null-space projection when an established contact needs to be broken as the robot follows a reference trajectory.

It is difficult to reliably sense more than one contacts on the arm from only joint torque [pang2021identifying]. Nevertheless, with a more capable contact sensor such as [luo2021learning], we believe the proposed contact-aware controller will greatly reduce robots’ reliance on environment sensing/monitoring and collision-free motion planning.

Comments

There are no comments yet.