Haptic exploration fits well within the prioritized controller scheme, as some tasks – managing contact forces, avoiding joint limits – can be interpreted as constraints and be given a very high priority, while other tasks such as arm pos- ture are less important. This framework is commonly used for humanoid control, including constraints on contact forces, but not for haptic exploration. Khatib’s operational space framework (Khatib, 1987) allows to express the dynamics of the robot in task coordinates, and the prioritized simultaneous control of sev- eral tasks through cascaded null space projections (Khatib et al., 2004). More recently, this framework was used to control several contacts on different links of a robot arm (Park and Khatib, 2008), but there have not been results showing cases where the robot makes additional unpredicted contacts or looses some of its contacts. In Flacco et al. (2012), commands are automatically scaled down if they violate hard bounds at the joint level (position, velocity or acceleration constraints). This allows to have explicit hard constraints, which was usually not possible in that framework. Another approach towards prioritizing tasks is to formulate the inversion of the Jacobian as a quadratic problem. For instance, the Stack of Tasks approach (SoT) (Mansard et al., 2009) provides an interface to add and remove tasks automatically with a pre-specified hierarchy. Recently, hi- erarchical control schemes based on a sequence of quadratic programs (QP) can also handle inequality constraints for kinematic control (Kanoun et al., 2011) and dynamic control (Saab et al., 2013). Efforts have also been made to solve these problems fast enough for real-time control of humanoid robots with many degrees of freedom (Escande et al., 2014).
In this thesis, we follow the null space approach to prioritizing tasks and introduce a controller based on a modified null space projection matrix that allows to take into account inequality constraints. While this is not as efficient as the latest QP-based methods, it is an alternative approach simpler to implement and closer to the original idea since it only relies on matrix inversion and does not require an otherwise complex solver.
Operational space control
The dynamics of a manipulator in contact describe how the robot moves in response to torques applied at the robot joints and the contacts forces on the links. In this section, these dynamics are detailed in order to describe operational space control, task space and null space control of a robot.
Operational space control of a robot in contact
The equations describing the dynamics of a robot in contact are of the form:
Mq(q)¨q + b(q, ˙q) + g(q) + JcT(q)f = τ (2.1.1) 18
where q, Mq(q), b(q, ˙q), g(q), f and τ are respectively the vector of joint angles, the joint-space inertia matrix, the Coriolis and centrifugal torques, the torques due to gravity, the contact forces and the vector of joint torques. Jc(q) is the contact Jacobian, e.g. considering an operational point x on the robot where a contact occurs, the relationship between the virtual joint velocities ˙q and the
virtual velocity of the operational point, ˙x, is given by the Jacobian matrix at the contact point in this configuration:
˙x = Jc(q) ˙q (2.1.2)
The manipulator dynamics in the operational space are given by pre-multiplying Equation (2.1.1) with J (q)Mq(q)−1. For better readability, we do not specify the dependency on the joint angles vector q and its derivatives from now on:
¨
x− ˙J ˙q + JMq−1(b + g) = J Mq−1(τ− JcTf ) (2.1.3)
Task space and null space for redundant manipulators
The operational space framework for task-level control of redundant manipula- tors decomposes the overall motion behaviour into two components. The first is defined by the task behaviour, specified in terms of forces and moments in the operational space, Ftask. This force is translated into a joint torque based on Equation (2.1.2): τ = JTF
task. This vector is however not completely specified in the case of redundant manipulators. The operational space framework allows to select from a set of task-consistent torque vectors to perform a secondary task. This secondary task is specified by an arbitrary torque vector τsec.
In order to ensure that this secondary torque vector does not affect the task behavior Ftask, the additional torque is projected into the null space N of the task Jacobian J . The torque NTτ
task resulting from the projection on the null- space does not affect the behaviour of the operational point. However, since the rank of Nrobot is N − k (k being the rank of J and Nrobot the number of degrees of freedom of the manipulator), the behaviour of the secondary task is not guaranteed.
The operational space and secondary task are combined to obtain the general expression for the torque-level controller:
τ = JTFtask+ NTτsec (2.1.4)
Practically, the null-space projection matrix N can by obtained with:
N = I− ¯J J (2.1.5) 19
with ¯J the dynamically consistent generalized inverse of J, given by:
¯
J = Mq−1JT(J Mq−1JT)−1 (2.1.6)
The use of this specific inverse ensures that the acceleration ¨x at the end
effector is not affected by the projected torques.
Multiple task behavior
The concept of decomposing the control torques in separate tasks with different priorities can be extended to more than 2 tasks and priorities.
Assume a set of n tasks Ti, where Ti has higher priority than Ti+1. Every task is associated with a torque vector τi, Jacobian Ji and corresponding null space projection matrix Ni. These tasks can be performed simultaneously while ensuring strict hierarchy depending on the task’s priority. The control torques are then given by:
τ = τ1+ N1T(τ2+ N2T(τ3+· · · )) (2.1.7) or τ = τ1+ NT prec(2)τ2+ N T prec(3)τ3+· · · + N T prec(n)τn (2.1.8) with Nprec(i)= N(i−1)N(i−2)· · · N1
This ensures that each task is executed as well as possible in the null space of all the tasks of higher priority. Note that τ1is not projected on the null space of any other task, therefore it is not altered by this projection process. The lower the task priority i, the lower the rank of the corresponding null space Nprec(i), hence the task has less chances to be executed properly. Important tasks such as avoiding joint limits or keeping equilibrium in the case of a humanoid robot should thus have priority 1.