A Robot Teleoperation Framework for Human Motion Transfer
Transferring human motion to a mobile robotic manipulator and ensuring safe physical human-robot interaction are crucial steps towards automating complex manipulation tasks in human-shared environments. In this work we present a robot whole-body teleoperation framework for human motion transfer. We propose a general solution to the correspondence problem: a mapping that defines an equivalence between the robot and observed human posture. For achieving real-time teleoperation and effective redundancy resolution, we make use of the whole-body paradigm with an adequate task hierarchy, and present a differential drive control algorithm to the wheeled robot base. To ensure safe physical human-robot interaction, we propose a variable admittance controller that stably adapts the dynamics of the end-effector to switch between stiff and compliant behaviors. We validate our approach through several experiments using the TIAGo robot. Results show effective real-time imitation and dynamic behavior adaptation. This could be an easy way for a non-expert to teach a rough manipulation skill to an assistive robot.
Service robots may assist people at home in the future. However, robotic systems still face several challenges in unstructured human-shared environments. One of the main challenges is to achieve human-like manipulation skills [Torras2016]. Another big concern is to ensure a compliant and safe behavior when physically interacting with a human in assistive or cooperative tasks. Semi-autonomous robots, where a remote human operator can take control, are a viable short-term solution, taking advantage of their complementary capabilities. In the long-term, robotic systems might be fully autonomous and able to learn dynamic manipulation capabilities from human demonstrations.
Imitation is an intuitive teleoperation approach due to the similarities in embodiment between humans and service robots. A fundamental problem is to create an appropriate mapping between the actions afforded to achieve corresponding states by the model and imitator agents [Alissandrakis2006]. This problem is known in literature as the correspondence problem. It implies determining what and how to imitate, which is not trivial. Imitation is also a powerful learning tool [Argall2009][Chernova2014][Hussein2017]. Rather than analytically decompose and manually program a desired behavior, a controller can be derived from observations of human performance. The classical approach is kinesthetic teaching. The human teacher holds the robot along the trajectories to be followed to accomplish a specific task, while the robot does gravity compensation [Rozo2016]. The robot can learn an approximate joint trajectory model. This approach avoids the correspondence problem, however, holding the robot constrains the taught motions.
Determining the human-robot correspondence, usually involves solving the inverse kinematics problem since human motion is generally represented in Cartesian space. The classical formulation only constrains the end-effector pose; which is not sufficient for posture imitation since only involves 6 degrees of freedom (DOF). In [Asfour2003] the authors propose to exploit the redundancy of a robot arm for the generation of human-like motions. Although their solution is accurate and has low computational cost, they did not consider the elbow position, which is an important constraint for imitating arm posture. In [Mohammad2013] the authors present an analytic solution for a humanoid upper body. However, they assume a specific arm kinematic structure and only the imitation of a static pose is considered. Another solution is direct joint motion retargeting [Penco2018]. The operator posture is directly mapped to each of the robot DOF. This allows to skip inverse kinematics and redundancy resolution. The main disadvantage is that the solution is strongly robot-dependent.
For a robot operating in a human-shared environment, it is of critical importance to ensure safety and compliance. Admittance control [Villani2008] is a suitable approach for physical human-robot interaction. The main drawback is that there is always a compromise between position accuracy and compliance. Variable admittance control has already been proposed to adjust robot behavior, however, research efforts have been mainly focused on cases where the robot motion is only driven by the force exerted by a human [Keemink2017][Ferraguti2019]. This is not the case for a teleoperated robot, where a reference position should also be considered.
The aim of this work is to push the state-of-the-art towards easily teaching robots complex manipulation skills. We propose a general whole-body teleoperation by imitation interface that ensures compliance during physical human-robot interaction. Moreover, our objective is to lay the foundations of a system able to transfer human motion to a mobile manipulator. This may allow robots to learn dynamic manipulation from human demonstrations in an intuitive way. The paper is organized as follows: in section II we discuss the main aspects of the proposed system; in section III we present the conducted experiments to validate our approach; finally in section IV, we summarize the main conclusions.
Achieving accurate, real-time robotic imitation of human motion while ensuring safe physical human-robot interaction, involves several steps. The first one is adequately capture human motion [Beth2003][Endres2012]. Then, what and how to imitate the human motion must be answered. This implies not only to determine a correspondence between human posture and the robot configuration, but also an effective management of the robot DOF [Tunstel2013]. Finally, compliance and an accurate position control should be adequately balanced since they involve different dynamics [Kronander2015]. In this section we present the proposed methods to address these issues.
Ii-a Motion Capture
Motion capture is a way to digitally record human movements. Data is mapped on a digital model in 3D [Nakazawa2018]. Inertial motion capture, compared to camera-based systems, does not rely on any external infrastructure allowing it to be used anywhere [Schepers2018]. We will use the Xsens MVN motion capture suit. It uses 17 body-attached inertial measurement units (IMUs) to obtain a body configuration and provide a real-time estimation of the human posture. The suit is supplied with MVN Studio software that processes raw sensor data and estimates body segment position and orientation. It is capable of sending real-time motion capture data of 23 body segments using the UDP/IP communication protocol [Stanton2012].
Ii-B Correspondence Problem
The correspondence problem can be stated as: given an observed behaviour of the model, which from a given starting state evolves through a sequence of sub-goals in states, the robot must find and execute a sequence of actions using its own (possibly dissimilar) embodiment, which from a corresponding starting state, leads through corresponding subgoals to corresponding states [Alissandrakis2006]. This accounts that the model and imitator may not share the same morphology or affordances. Posture imitation involves solving the correspondence problem. This requires adequate considerations regarding the differences in the kinematic chains and joint limits. It can be divided into three subproblems:
Observation: Measure the person state
Equivalence: Establish a relation between the observed state and the robot desired behavior .
Imitation: Determine the robot configuration that allows to achieve the goal state .
where is the mapping from to and , , and refer to the person state, observational, goal and robot configuration spaces respectively.
Formally, the problem is finding , defined as:
where is the composition operator and the inverse. Based on this definition, the solution depends on the motion capture system, as it conditions the observational space. Using a system like the one presented in section II-A, , where is the number of observed person segments and an -dimensional array of three-dimensional Euclidean groups. Each element of the array is a homogeneous transformation from reference to :
where and are the rotational and translational components, respectively.
Then, and . Ultimately, the problem is finding a mapping between Cartesian and robot configuration spaces. This is a problem widely studied in robotics. Currently, specially in humanoid robotics research, where robots have a high number of DOF, frameworks for defining in such a way that the pose of robot links can be constrained in Cartesian space are being developed, such as Whole-Body Control [Sentis2018]. Therefore, in order to make our proposed solution as general as possible, it seems convenient to define a correspondence function such as . This can be summarized in the following scheme:
We will consider that the pose is equivalent if the relative position and orientation of the person’s right (or left) wrist, elbow, chest, and the projection of the pelvis onto the floor with respect to an arbitrary fixed reference frame are as close as possible to those equivalent links of the robot up to an scaling factor in the Cartesian space. Given , , and where , , , , and stand for the person arbitrary origin, virtual footprint, torso, shoulder, elbow and wrist reference frames respectively and a sample equivalent person-robot pose. We propose that the correspondent robot pose is fully-determined by , , and , where , , , , , are the equivalent robot links (figure 2). Their rotational components are defined as:
where is the equivalent sample pose, and (also and ) are arbitrary equivalent person and robot links respectively. The translational components are defined as:
where is the robot’s base to torso height when the torso is fully extended, and are the lengths of the robot’s equivalent shoulder to elbow and elbow to wrist segment respectively. Therefore, a complete definiton of is provided.
Ii-C Whole Body Control
Whole-Body Control (WBC) has been proposed as a promising research direction when using robots with many DOF and several simultaneous objectives. The redundant DOF can be conveniently exploited to meet the multiple tasks constraints [RAS-WBC2019]. Given a set of control actions targeting an individual task , which defines a desired motion in Cartesian space, a generic definition of a WBC is [Sentis2018]:
where is the robot configuration, is the Moore-Penrose pseudo-inverse of the task Jacobian which is defined by . A task can represent, for example, the end-effector pose or the available joint range.
A hierarchical ordering among tasks can be defined. Let be the null-space projector of the augmented Jacobian . Then, the joint velocity can be determined with the following relationship [Siciliano1991]:
This allows the task to be executed with lower priority respect the previous task, not interfering with the higher priority tasks. If is singular, the task cannot be satisfied. However, the subsequent tasks are not affected since the dimension of the null-space of is not decreased.
For whole-body imitation, the robot needs to achieve multiple varying goals in Cartesian space simultaneously. This makes WBC a suitable control framework, since they can be defined as a set of tasks with an adequate hierarchy. The main advantage over other inverse kinematic solvers is that the WBC can find online solutions automatically preventing self-collisions and ensuring joint limits. We are using PAL robotics implementation for the upper-body, which is based on the Stack of Tasks [Mansard2009]. Taking into consideration the equivalence human-robot relations presented in the previous section, we propose the following task hierarchy:
Joint limit avoidance
Torso position control
End-effector pose admittance control
Elbow pose control
The first two tasks should always be active with the higher priority for safety reasons. The torso task is of higher priority because, by constraining the torso, the arm DOF are not affected, but the opposite is not true. Defining the end-effector task with higher priority we ensure a correct end-effector goal tracking, which is of critical importance for manipulation tasks. The use of admittance control for this particular task in discussed in section E. Then, with the elbow task we ensure the arm posture imitation. In the particular case that the robot arm has human-like affordances, which is the case of the TIAGo robot, a correct imitation can be achieved with the presented hierarchy. With the WBC we focus on redundancy resolution, finding the optimal configuration to accomplish the high-level task.
Ii-D Differential Drive Base Control
Differential drive base is a mechanism used in many mobile robots, such as TIAGo or Roomba [Morin2008]. It usually consists on two drive wheels mounted on a common axis [Topiwala2016]. Linear and angular velocity are the control commands. [Norr2017]. Let be the coordinates that define the base pose. Let and be the instantaneous linear and angular velocity commands respectively. The kinematic model is:
with the non-holonomic constraint , which does not allow movements in the wheels’ axis direction.
Using the notation presented in section B, the robot footprint pose should coincide with the person’s, plus an arbitrary constant offset for a successful imitation. It is an inverse kinematics problem i.e., find the velocity commands that allow the robot to reach a given pose. Many motion planning frameworks have already been proposed for this. However, they are not suitable for cases where the goal is constantly changing, which is the case of human walking. They usually involve complex calculations that cannot be solved faster than the goal changing rate, which makes the robot remain in a planning state. We propose algorithm 1, which does not involve complex calculations. When initialized, it assumes the person and the robot footprint frames are coincident in an arbitrary fixed reference frame. Then the relative transform between the person and the robot footprint is determined at each time step. When the robot is further than a certain margin to the reference, angular velocity commands orientate the robot towards the goal position. If the robot position is close enough, angular velocity commands align the robot with the goal orientation. Additionally, the possibility of moving backwards is also considered.
Ii-E Variable Admittance Control
Admittance control [Hogan1984] is a method where, by measuring the interaction forces, the set-point to a low-level motion controller is changed through a virtual spring-mass-damper model dynamics to achieve some preferred interaction responsive behavior [Keemink2018]. In simple cases, the parameters of such a system can be identified in advance and kept fixed. However, when interaction forces are subject to uncertainties, the desired response can be adaptively regulated [Peternel2016]. Variable admittance control allows to change the dynamics in a continuous manner during the task. When imitating the human posture in real-time, an accurate pose control is desirable, so an stiff behavior is preferable. On the other hand, when physically interacting with a human, a compliant (i.e. low stiffness) behavior is of vital importance to ensure safety [Ott2015][Talignani2017]. The virtual end-effector dynamics are:
where inertia , damping and stiffness determine the virtual dynamics of the robot, , when subjected to an external force . and are the position controller reference when using pure position or admittance control respectively.
If , and are constant, the system will be asymptotically stable for any symmetric positive definite choice of the matrices. However, in this work we will assume that remains constant while and vary in time. It can be proved (see [Kronander2016]) that for a constant, symmetric, positive definite , and , continuously differentiable, the system is globally asymptotically stable if there exists a such that, :
is negative semidefinite
is negative definite
We will assume that , and are diagonal matrices. Therefore, the system can be uncoupled in six independent scalar systems. To condense, we focus on the translational DOF. However, for the rotational components, the deduction is analogous:
As design criteria, we will ensure a constant damping ratio . Thus, the damping is chosen as . By substituting on the second stability condition, it yields the following upper bound for the stiffness derivative:
In order to switch the robot role between follower (low stiffness, ) and leader (high stiffness, ) [Jarrasse2014][Gomes2018], we propose a continuously differentiable scalar role factor and the following varying stiffness profile:
Role adaptation can be derived from the interaction force feedback. Experience of varying stiffness control suggests that continuous and smooth variations show no destabilization tendencies. We propose the following role factor profile:
where and are design parameters and is a proposed interaction factor that varies according to the interaction force feedback. Note that higher values of give a faster transition between roles while determines the value of at which the transition starts. We propose the following interaction factor dynamics:
where is the force threshold to consider physical human-robot interaction, and and are design parameters. Note that the values of and will modulate the transition speed when switching from leader to follower and from follower to leader roles respectively. As a design guideline, for safety reasons it is important to achieve a fast stiff to compliant transition, but that is not the case for the opposite. Thus, high values are desirable but values should be kept relatively smaller in absolute value.
From the first stability condition, to have the least conservative constraints, since the damping profile is bounded, . Given that all the varying parameters are bounded, we can determine an upper bound of the stiffness profile derivative, and a lower bound for the second stability condition. Thus, a sufficient stability condition is:
Tuning the parameters empirically, we have assigned , , , , , , and for all six end-effector DOF (the units of the rotational components’ stiffness are ). By direct substitution, the sufficient stability condition holds. For filtering the noise in the interaction force feedback signal we implemented a moving average filter [Smith1997] of 25 samples. An overview of the proposed variable admittance controller can be seen in figure 4.
Iii Experimental Results
In order to analyze the effectiveness of the proposed approach, we carried out three different experiments using the TIAGo robot, with 10 DOF excluding the head, and the Xsens motion capture system. The objective of the first experiment was to show the upper-body motion similarity when using the proposed solution for the correspondence problem and the WBC with the presented task hierarchy. In the second experiment we tested the mobile base imitation using the proposed algorithm for differential drive control. For the third experiment we evaluated the performance of the role adaptation mechanism and that the stability condition derived analytically is sufficient to ensure a stable behavior.
Iii-a Upper-body Teleoperation
The robot was teleoperated in real-time while the human operator described an spiral trajectory with the hand. To evaluate the motion similarity, we compared the trajectory described by the robot’s end-effector and the evolution of the angle formed by the robot elbow-wrist and elbow-shoulder segments with the reference trajectories. Results are shown in figure 3. The obtained mean absolute error for the end-effector position is of and of for the elbow opening angle. As it can be seen, the robot is able to describe an spiral with the end-effector accurately while imitating the arm posture with its 7 DOF, proving a successful redundancy resolution. It can also be seen, from inspecting the results, that although a real-time teleoperation is achieved, the commanded motion is of an average speed of . During the experiments, we observed that due to the robot’s joint speed limits and own inertia, the operator movements should be limited to low speed motions.
Iii-B Base Teleoperation
The operator described a path with a series of turns while the robot moved in parallel. The obtained results are shown in figure 6. The mean absolute error in position is of and of in orientation. It can be observed that the robot motion is very similar to the reference trajectory. We are able to imitate the operator pose during the walking motion through velocity commands with the proposed algorithm. It should be remarked that the non-holonomic constraint does not apply to human walking motion. Therefore, in order to achieve a successful imitation the operator trajectory should not include side steps. However, when the non-holonomic constraint is not satisfied in the operator movement, for a sufficient large time, the base position and orientation always converge to the reference if it remains static.
Iii-C Role Adaptation
A reference trajectory was commanded to the robot, while executing the motion, the end-effector is grasped by a person, displacing it from its goal trajectory. Then, it is released. The experiment results are shown in figure 5. The results show how, when the grasping occurs, the interaction factor starts to increase, while the stiffness rapidly decreases to switch the robot behavior from stiff to compliant. This allows to easily move the end-effector away from its commanded trajectory. When it is released, the interaction factor starts to decrease while the stiffness starts to restore its initial value and the robot end-effector position converges to the original trajectory. Note that when the stiffness is at its minimum value the difference between the stability bound and the stiffness profile derivative reaches its minimum value. Nevertheless, stability is fulfilled during the whole realization. No oscillations or unstable behavior were observed.
Iv Discussion and Conclusions
In this paper we have presented a robot teleoperation by imitation framework for human motion transfer. Imitation, on one hand, offers many advantages, not only because it is intuitive, but also because it allows to transfer human-like manipulation capabilities to the robot. On the other hand, it involves solving the correspondence problem. We present a general solution, that first, defines the equivalence between an arbitrary human body posture and the correspondent robot posture as a goal pose for a series of links in Cartesian space. Then, we propose the use of WBC to find the robot configuration that achieves the defined goals. By defining an adequate task hierarchy, we can achieve an effective upper-body redundancy resolution. Furthermore, we present an algorithm that allows the robot differential drive base to imitate the human walking motion. Finally, when a robot is operating in human-shared environments it is important to ensure safe human-robot interaction. However, achieving a compliant behavior and accurate position control are opposite objectives. We propose a variable admittance controller that allows continuous adaptation of the end effector dynamics when physically interacting with a human by means of scalar role and interaction factors. For the proposed controller, we have derived analytically an state-independent and sufficient condition for ensuring stability.
Experimental results show that an effective whole-body imitation can be achieved in real-time. Also, that the robot successfully adapts its role when physical interaction with a person occurs. The main limitations observed during the experiments for achieving real-time fast imitation, are due to the robot’s joint speed limits and inertia. Regarding the walking motion imitation, for differential-drive bases, it is limited because of the non-holonomic constraint.
This work is aimed to set the foundations of a robotic platform that is able to transfer human body motion to a mobile manipulator. This could be an easy way for a non-expert to teach a rough manipulation skill to a service or assistive robot. Afterwards, the robot could autonomously practice and improve the skill (e.g., its accuracy) through reinforcement learning. Future research will be conducted towards achieving human-like manipulation skills. The main challenge will be to generalize and adapt the learned motion when dealing with uncertainty. Imitation and variable admittance control may be a first step towards robots performing complex manipulation tasks in human-shared environments.