The success of robots in real-world scenarios is still largely dependent on their ability to interact with the environment and with other agents, possibly with a degree of intelligence. Traditionally, robots interact with inanimate objects and take unilateral actions to achieve specific objectives. Today’s robots, however, are more frequently asked to collaborate with each other to achieve complex tasks, or to interact with humans either to assist them or to augment their capabilities [koeppe2005robot]. Collaborative scenarios have thus become a priority for both the industry and the scientific community [robotics2020]. For successful and efficient completion of future scenarios, it is crucial to implement control strategies that coordinate the interactions of several agents. This paper introduces control strategies that enable energetically efficient robot-robot interaction for collaborative lifting of payloads, here referred to as ergonomic collaborative lifting.
In industrial environments, robots can be used for heavy payloads lifting and carrying activities, relieving human operators from repeated and excessive efforts [ajoudani2018progress, vysocky2016human]. As a consequence, robots can contribute to make workplaces more ergonomic by reducing the risk factor associated with work-induced musculoskeletal disorders [wmds]. Our objective is to understand how humanoid robotic platforms can help towards this direction, in particular, by endowing them with collaborative lifting capabilities, e.g. Fig. 1. In fact, humans have shown specialized efficient payload sharing strategies [reed2006], and specific control frameworks have been proposed for human-robot collaboration scenarios [ikeura2002, griffin2005, lawitzky2010, Magrini2015]. The existing strategies, however, mainly focus on modelling and stabilizing each system separately, and the advantages arising from joint actions are seldom explored. Hence, centralized approaches for agent-agent interaction can lead to unprecedented applications of robotic systems [yan2009control].
Concerning humanoid robot platforms, state-of-the-art whole-body controllers often focus on maintaining robot stability by suppressing external disturbances. Particular attention has been paid to momentum-based strategies that have been applied successfully to different platforms [stephens2010dynamic, righetti2011inverse, herzog2014balancing, nava2016stability, kanemoto2018compact]. The main idea behind those strategies consists in controlling the robot momentum through full body joint torques, which are computed via inverse dynamics from desired feet contact forces. For the particular case of multi-contact motion, the contact forces at the hands can also be considered [audren2014model, kamioka2015dynamic]. These control strategies enable the robot to handle unexpected contacts while being compliant with respect to external interactions [hyon2007full, pucci2016, ko2018]. A payload lifting task, however, requires the control of interaction forces as well, which should not be treated as disturbances. Additionally, in presence of a second collaborative agent, its actions need to be taken into account as they contribute to the achievement of the task. Whole-body control strategies enabling real humanoid robots to lift a payload either autonomously [harada2005, arisumi2008], or by collaborating with a human partner [evrard2009, sheng2014, Agravante2019] have been proposed. However, control strategies for efficient humanoid robot-robot collaborative payload sharing and co-manipulation are still missing in the current literature.
This paper presents a novel momentum-based whole-body shared control strategy for robot-robot collaboration. The controller achieves balancing of the robots and the control of a payload position-and-orientation during lifting task execution. Additionally, we discuss robot ergonomics in terms of efficient energy consumption, and two concurrent ergonomic metrics are proposed, here referred to as force ergonomics and postural ergonomics. The proposed approach extends and encompasses the partner-aware approach proposed in [tirupachuri2019, tirupachuri2019-des], where the partner actions are exploited in an unidirectional way. Differently from previous works, we propose a bidirectional optimization of the agents action thanks to a centralized control, and coupled system dynamics modelling. Thanks to modern technologies, in fact, it is possible to solve and share online full body dynamics among the agents [latella2019simultaneous, lorenzini2020online]. Feasibility and performances of the proposed control approach are verified using two iCub humanoid robots [Natale2017].
The paper is organized as follows: Sec. II introduces the notation and modelling. Sec. III presents robot ergonomics, used in the proposed shared control strategy presented in Sec. IV. Sec. V presents experimental results, and the validation of the proposed approach is discussed. Sec. VI concludes the article with conclusions and perspectives.
denotes an inertial frame of reference.
is the canonical vector, consisting of all zeros but the-th component that is equal to one.
Constant is the norm of the gravitational acceleration.
denotes the identity matrix of size.
is the the position of the origin of the frame with respect to the frame .
represents the rotation matrix of the frames with respect to .
is the angular velocity of the frame with respect to , expressed in .
The operator denotes skew-symmetric operation of a matrix, such that given , it is defined as .
The operator denotes skew-symmetric vector operation, such that given two vectors , it is defined as .
The vee operator denotes the inverse of skew-symmetric vector operator. Given a matrix and a vector , it is defined as .
denotes a wrench 6D vector transform, as defined in [traversaro2016multibody], such that
The operator indicates vector squared norm. Given , it is defined as .
A humanoid robot can be modelled as a multi-body mechanical system composed of rigid bodies, called links, connected by joints
with one degree of freedom (DoF) each. The system is thus assumed to be floating base, i.e. none of the links has an a priori constantposition-and-orientation – hereafter referred to as pose – with respect to the inertial frame . Hence, a specific frame, attached to a link of the system, is referred to as the base frame, and denoted by . The model configuration is characterized by the pose of the base frame along with the joint positions. The configuration space lies on the Lie group . An element of the configuration space is defined as the triplet where and denote the position and the orientation of the base frame respectively, and is the joints configuration representing the topology of the mechanical system. The position and orientation of a frame attached to the model can be obtained via geometrical forward kinematic map from the model configuration. The model velocity is characterized by the linear and angular velocity of the base frame along with the joint velocities. The velocity space lies on the set . An element of the configuration velocity space is defined as where denotes the linear and angular velocity of the base frame, and denotes the joint velocities. The velocity of a frame attached to the model is denoted by with the linear and the angular velocity components respectively. The mapping between the frame velocity and the system velocity is obtained via the Jacobian , i.e. . The Jacobian is composed of the linear part and the angular part that maps the linear and the angular velocities, i.e. and .
Ii-C System Dynamics during Physical Interaction
The model of each agent is obtained by applying the Euler-Poincarè formalism [Marsden2010]. Agent dynamics is thus described by a set of differential equations complemented with holonomic constraints characterising the contacts:
where is the mass matrix, the term takes into account of Coriolis and gravity forces, is a selector matrix, is a vector representing the robot’s joint torques, represents the wrenches acting on contact links of the robot, and is the contact Jacobian. Now, consider the case of two agents physically interacting, as shown in Fig. 2. The contacts are not limited to those with the environment, but agent-agent contacts should also be considered. Denoting the terms related to each agent using subscripts, and denoting composite matrices with font, the coupled system dynamics is described by the following equations [tirupachuri2019]:
where is a coupling matrix taking into account both the constraints of the contacts with the environment () and those of the agent-agent contact points (), and is a vector containing all the interaction wrenches (exchanged with the environment and between the two agents) taking into account the action-reaction property for internal forces () and reflecting the ordering in the constraints matrix . Analogously, modelling agents and objects as rigid body, any number of them can be included in the set of equations. Defining properly the composite matrices , , , , and , any system of multi-rigid bodies with those characteristics can be written compactly as:
Iii Force and Postural Ergonomics
Ergonomics is the scientific discipline concerned with the analysis of human interactions with other elements of the environment, and the design of theory and methods that optimise human well-being and overall system performance [dul2012strategy]. When humans interact with robots, optimal ergonomic interactions shall minimize metrics that consider both the human and the robot (here called agents). In this case, energy expenditure fits well both agents. Human energy expenditure is in fact used to evaluate ergonomics in workplaces [battini2016ergonomics], and it was considered in many studies aiming at ergonomic human-robot interactions [kim2018, marin2018, derspaa2020]. Robot energy expenditure is directly related to joint torques, which depend on contact forces and joint posture configuration – see Eq. (1). Hence, when choosing joint torques as metric, ergonomy optimization can be decomposed into two sub-problems here referred to as force and postural ergonomy.
To provide the reader with a better comprehension of the differences between these two metrics, consider the hyperstatic structure shown in Fig. 3, which exemplifies more complex human-robot scenarios – e.g. that of Fig. 2. This structure is statically indeterminate, so given an equilibrium configuration, e.g. that of Fig. 2(a), joint torques are not uniquely determined [matheson1959hyperstatic]. It follows that, despite deliberately high torques, the equilibrium can be maintained when redundant torques compensate each other, resulting in high internal and reaction forces. In this sense, torque minimization can be attempted through force optimization. So, given a system configuration (e.g. a specific set of joint angles), force ergonomics aims at minimizing the joint torques at that configuration, eventually through the contact forces. Now, consider the configuration in Fig. 2(b). In this case, the equilibrium is maintained in absence of internal joint torques, showing that an optimized posture can further minimize joint torques. So, postural ergonomics exploits the system configuration (e.g. the robot joint angles) at the equilibrium to further optimize the overall joint torques.
Note that postural ergonomics does not in general imply force ergonomics. For instance, consider again Fig. 2(b): if the torques and have equal intensity, then the system keeps the equilibrium with unnecessary energy expenditure. The shared control architecture presented next aims at solving both force and postural ergonomics for a robot-robot interaction scenario. The detailed formulation is given in Secs. IV-C and IV-D. Let us remark that the force and torque ergonomics metrics can be used also to characterise the energy expenditure of a human being interacting with a robot.
Iv Shared Control Architecture
The proposed architecture for robot-robot collaborative payload manipulation is shown in Fig. 4. A shared controller solves the force optimization online taking into account force ergonomics, system balancing, and payload pose control. The optimized contact forces are then used together with a postural task to compute joints torque via inverse dynamics, and send references to the robots. The reference trajectories of the postural task are computed offline using a second optimization framework that tackles postural ergonomics.
Iv-a System Balancing
When considering an underactuated floating-base system, the system balancing is a key-factor for the successful achievement of desired task. As mentioned in Sec. I, momentum-based balancing control strategies can ensure a stable robot behaviour against external disturbances. In a multi-agent scenario, a momentum-based controller can be implemented for each -th robot independently defining
where is the robot mass, is a vector containing all the wrenches applied to the robot, () is the reference trajectory for the robot momentum, is a symmetric positive definite matrix, and is the center of mass (CoM) of the robot. Eq. (4a) describes the dynamics of the robot momentum, while Eq. (5a) defines the desired dynamics with proportional compensation. Note that takes into account both the wrenches exchanged with the environment and among the agents . Independent controllers, however, do not exploit the presence of a partner agent. In order to achieve an optimized solution for the entire system, we add an extra task for controlling the total momentum of the multi-agent system. The dynamics of total momentum depends on external wrenches only and it is described as,
where is the total mass of the system, and the external wrenches vector. Similarly to Eq. (5a), given a desired trajectory for the total momentum (), we define desired total momentum starred dynamics. Same task priority is assigned to robots and total momentum control. Using the representation introduced in Sec. II-B, the momentum control tasks are written compactly as:
Iv-B Payload Pose Control
The payload is a rigid body object assumed to be attached to the agents via contact constraints. Given a desired trajectory for the payload position () and orientation (
), according to feedback linearization control of rigid body pose[olfati2001nonlinear], the desired body velocity is chosen as
where and are positive definite diagonal matrices. Given , the internal wrenches (also referred to as grasp forces) have to satisfy the rigid body dynamics equation
where is the payload mass matrix, its mass, and is the grasp matrix that transforms wrenches into payload CoM, assuming rigid contacts model [bicchi2001robotic]. The desired internal wrench solution , obtained from Eq. (10), depends the grasp matrix and on the associated inequality constraints, e.g. frictional contacts model [han2000grasp], and can be not unique if . This condition is often verified in presence of multiple contacts. The null-space component of the solution represents the squeeze wrench, i.e. the internal wrench that causes no motion of the object.
Iv-C Force Optimization
Systems balancing and payload pose control are achieved by regulating the external contact wrenches , and internal wrenches , according to Eqs. (7a) and (10). The solution for those equations can be redundant, therefore, wrenches can be chosen according to force ergonomy optimization. As discussed in Sec. III, ergonomics is achieved via joint torque minimization. Combining the system dynamics description in Eq. (3), the control laws in Eqs. (7a) and (10), and the ergonomy optimization, we have designed an optimization-based shared controller. In the language of optimization it is formulated as,
where and define an additional constraint that ensure the contact and grasping wrenches belong to the associated friction cones, and and are the sub-matrices of and relative to joint dynamics only. The stack-of-task optimization in (18a) can be solved in two steps: optimization (18a) is solved as minimum-norm solution obtaining a linear expression for , and optimal wrenches are computed via Quadratic Programming. The free variable is used to ensure the stability of the zero dynamics by means of the so called postural task that, given a desired joint configuration , is computed independently for each robot as [nava2016stability]
The gain is a positive definite matrix used to distributes the effort among the joints and the agents. Assuming matrix is block diagonal, if the effort is equally distributed among the agents, otherwise, the effort distribution is asymmetric and the ergonomy of agent with higher gain is favoured at the expense of the other agent. Once the optimum value is determined, the robot torques are obtained by evaluating .
Iv-D Postural Optimization
The optimization-based controller presented in Sec. IV-C stabilizes the behaviour of the robots during the execution of the task. However, the generation of reference trajectories for momentum tasks , object pose (,), and robots configuration , has not been discussed. The idea is to compute the reference trajectories according to the ergonomy optimization principle of joint torques minimization. Given the geometric properties of the systems, all the above reference quantities can be retrieved from agents configuration, i.e. , , and . Hence, the search space is reduced to the robots configuration only, and we express the problem as the posture ergonomics optimization discussed in Sec. III. The optimization problem is formulated as,
where and describe the joint limits for the -th robot. As for the shared controller, the optimizer takes into account the presence of the payload trough the coupled dynamics described in Eq. (3), and the gain regulates the distribution of the effort. Differently from the optimization (18a), however, the problem is non-linear. It follows that the optimization is computationally expensive, and it might be preferred to solve it offline. The choice of finding the posture at the steady-state, i.e. , reduces the complexity of the problem, and well approximates the behaviour for slow motions. Given the steady-state optimal configuration, the transition trajectories are generated online as minimum-jerk trajectories, ensuring human-like motion [pattacini2010experimental]. Alternatively, computing dynamic optimization for the whole trajectory is possible, but implies higher computational cost. Fig. 5 presents the initial and optimized configuration for the iCub robot. In the optimized configuration, it can be noticed that singular configurations are preferred, and the effort is better distributed among the joints.
V Experimental Results
The proposed control infrastructure has been tested using two iCub humanoid robots [Natale2017], endowed with 23 degrees of freedom. According to the robot capability, a payload of 3 Kgs is used, and ad hoc mechanical connectors are assembled in order to strength the robot manipulation capability and ensuring rigid coupling with the payload. The robots are required to perform a vertical weight lifting task. Despite belonging to the same major iCub version, the two robots present minor differences in hardware and low-level motor control configuration, therefore, we do not expect to observe perfect symmetric behaviour. The robots communicate with a central server using YARP middleware [fitzpatrick2008towards] trough a Local Area Network. The central server receives robots state information and runs the shared controller at 100 Hz solving the stack-of-task optimization and sending the target joint torques for the robots low-level torque control loops which run at 1 kHz on each robot. The experimental setup is shown in Fig. 1, all the experiments start from an initial configuration where the payload lies on a support before being lifted by the robots. The non-linear posture optimization problem is solved offline using interior-point method [waltz2006interior]. Fig. 6 shows typical results obtained during collaborative lifting experiments in term of trajectories tracking. The controller appears to be capable of performing balancing while controlling the payload pose. The desired vertical displacement of the payload is tracked for the entire duration of the experiments, and it rotates few degrees from the ground plane. Good tracking is also achieved for the total center of mass trajectory.
Different motion sequences have been tested in order to evaluate the ergonomy optimization framework through joint torques measurement. In particular, the two sequences presented in Fig. 7 are well explicative, and demonstrate the effects of ergonomics optimization. In both cases the robots start from the same initial configuration (A), before moving among three possible configurations: (B) is a non-optimized configuration, (C) is the optimized ergonomic configuration obtained from (24a) with , and (D) is an asymmetric configuration computed from (24a) using unequal gains such that . When moving from the non-optimized to the optimized posture, the torques vector norm decreases for both the robots showing the effect of ergonomic optimization in achieving a more efficient configuration. Considering the transient time, in the transition (A)-(C) the torques norm is lower compared to transition (A)-(B), despite the optimization is solved considering the steady-state only, but it appears to have an higher percentage overshoot. Analysing the asymmetric configuration (D), the torques norm shows that the framework succeeds in overloading Robot 2 to accommodate Robot 1 ergonomics.
In this paper, a shared control framework for multiple humanoid robots collaboration is proposed. Thanks to a centralized controller, the joint action of the robots is controlled to guarantee the balancing of the system and the control of the payload during a lifting task. Moreover, efficient collaboration is ensured by taking into account the ergonomics requirements of the robots. The contact forces and the robot posture are optimized in order to distribute the effort among the agents minimizing energy consumption. Preliminary tests with the iCub humanoid robot verify the soundness of the proposed framework, but further experiments are required in order to have a statistical validation.
As future work we plan to extend the architecture to a more complex experimental scenarios involving different type of grasping and agents locomotion. Another interesting extension can be the application of the proposed algorithm for human-robot collaboration with the objective of optimizing human ergonomics through robot assistive actions. Finally, the precise definition of the force and posture ergonomics principles introduced in Section III will be the subject of a forthcoming publication.
This work is supported by Honda R&D Co., Ltd and by EU An.Dy Project that received funding from the European Union’s Horizon research and innovation programme under grant agreement No. . The content of this publication is the sole responsibility of the authors. The European Commission or its services cannot be held responsible for any use that may be made of the information it contains.