Trajectory Generation from Motion Capture for a Planar Biped Robot in Swing Phase

This paper proposes human motion capture to generate movements for the right leg in swing phase of a biped robot restricted to the sagittal plane. Such movements are defined by time functions representing the desired angular positions for the joints involved. Motion capture performed with a Microsoft Kinect TM camera and from the data obtained joint trajectories were generated to control the robot’s right leg in swing phase. The proposed control law is a hybrid strategy; the first strategy is based on a computed torque control to track reference trajectories, and the second strategy is based on time scaling control ensuring the robot’s balance. This work is a preliminary study to generate humanoid robot trajectories from motion capture.


Introduction
Nature is a source of inspiration for robotics. The design of bipedal robots is inspired from the functional mobility of the human body, nevertheless, the number of degrees of freedom (DoF), range of motion and speed of an actual biped robot are much more limited compared to humans. The choice of the number of DoF for each articulation is important. The approach consists in analyzing the structure of the robot from three main planes: the sagittal, frontal, and transversal planes. The movement of walking mainly takes place in the sagittal plane; all bipeds have the largest number of important articulations in this plane [1].
Modeling, monitoring, and understanding of human motion based on motion capture is a field of research resurgent during the past decade, with the emergence of applications in sports science, medicine, biomechanics, animation (online games), monitoring and security [2]. Development of these requests is anchored on progress in artificial vision and biomechanics. Although these research areas are often treated separately, the analysis of human movement methodologies requires the integration of computer vision and modeling of the human body as a mechanical system that improves the robustness of this approach [3].
Motion capture is the process of recording a live motion event and translating it into usable mathematical terms by tracking a number of key points in space over time and combining them to obtain a single three-dimensional (3D) representation of the performance; [4]. The subject captured could be anything that exists in the real world and has motion; the key points are the areas that best represent the movement of the subject's different moving parts. These points should be pivot points or connections between rigid parts of the subject. For a human, for example, some of the key points are the joints that act as pivot points and links for the bones. The location of each of these points is identified by one or more sensors, markers, or potentiometers placed on the subject and that serve, in one way or another, as conduits of information to the primary collection device.
Many motion capture systems are available in the market that can be used to acquire the motion characteristics of humans with a relatively high degree of accuracy, for example: PeakMotus (Vicon) TM SkillSpector TM and DartFish TM , among others. These include inertial, optical, and markerless motion capture systems.
Currently, the most common methods for proper 3D capture of human movement require a laboratory environment and setting of markers, sensors accessories body segments. At present, technical progress that has enabled the study of human movement is the measurement of skeletal motion with tags or sensors placed on the skin. The movement of the markers is typically used to derive the underlying relative motion between two adjacent segments to define the motion of a joint accurately. Care of skin movement relative to the underlying bone is the main factor limiting the application of some sensors; [5], [6], [7].
Markerless motion capture systems like Microsoft's Kinect TM range camera present alternative new approaches to motion capture technology; [8]. The Kinect TM is a light-coded range camera capable of estimating the 3D geometry of the acquired scene at 30 frames per second (fps) with a Depth Sensor with (640 × 480) spatial resolution. Besides its light-coded range camera, the Kinect TM also has a color video camera VGA and an ing.cienc., vol. 11, no. 22, pp. 25-47, julio-diciembre. 2015.
array of microphones. The Kinect TM has readily been adopted for several other purposes such as robotics; [9], human skeleton tracking [10], and 3D scene reconstruction [11].
Motion capture can be used to allow robots to imitate human motion. Although the motion imitation is merely mapping human motion on to humanoid robots, which have a similar appearance, it is not a trivial problem [12]. The main difficulties to overcome in developing human motion for bipedal robots are the anthropomorphic differences between humans and robots, the physical limits of the robot's actuators, robot balance, [13] and collision avoidance. A control scheme is used to follow the desired trajectories. Two main classes of methods can be used to compute the reference trajectories: either they can be obtained by capturing human motions or computer generated [14]. In this paper, the reference trajectories were obtained from captured human motion data and a hybrid control law strategy allowed a geometrical tracking of these paths.
The hybrid control strategy comprises in a Computed Torque Control (CTC) and time scaling control; this control strategy switches according to the desired Zero Moment Point (ZMP). The approach ensured the balance of the biped robot within safety limits. The control law with time-scaling is defined in such a way that only the geometric evolution of the biped robot is controlled, not the temporal evolution. The concept of control with time-scaling was introduced by Hollerbach [15] to track the reference trajectories in a manipulator robot. It has also been implemented to control mechatronics systems, [16]. In [17], Chevallereau applied the time scaling control of the underactuated biped robot without feet and actuated ankles. This work sought to define the sequence of joint movements that allow a biped robot to make the swing phase of the right leg similar to that of humans by using motion capture data, the proposed control law will be validated in a planar biped robot with feet and actuated ankles.
The rest of the paper is structured as follows: section 2 shows the dynamic model of the robot; section 5 calculates the control law, followed by its results in section 6 and concluding the paper in section 7.

Dynamic model of the robot
The dynamical model was derived from the biped robot Hidroïd, [18] which include the dynamic parameters to calculate the inertia matrix and vector of forces. The Lagrangian representation of the robot's dynamic model is the following: In the previous model q ∈ IR 9 is the vector of generalized coordinates. This vector contains three types of coordinates: (i) (x 0 , y 0 ) denoting the Cartesian position of the reference frame < x 0 , y 0 > in the reference frame < x g , y g >, (ii) q 0 denoting the orientation of the reference frame < x 0 , y 0 > with respect to the reference frame < x g , y g >, (iii) (q 1 , . . . , q 6 ) denoting the six joint positions depicted in Figure 1.
Vectorsq ∈ IR 9 andq ∈ IR 9 are respectively the first and second derivatives of q with respect to time. A ∈ IR 9×9 is the robot's inertia matrix. H ∈ IR 9 is the vector of centrifugal, gravitational, and Coriolis forces. Matrix B ∈ IR 9×6 contains only ones and zeros. The first three rows of B are zero, indicating that Γ has no direct influence on the acceleration of the first three generalized coordinates. The following 6 rows of B form an identity matrix, indicating that Γ i (i = 1, . . . , 6) directly affectsq i . Γ ∈ IR 6 is the vector of torques applied to the joints of the robot. This leads to B fulfilling the following property: J r ∈ IR 3×9 is the jacobian matrix relating the linear and angular velocities of the robot's right foot in the reference frame < x g , y g > with the vectoṙ In (4), ( gṗ rx , gṗ ry ) are the linear velocities of the reference frame < x 3 , y 3 > with respect to < x g , y g >, whereas that,θ r is the angular velocity of the reference frame < x 3 , y 3 > with respect to < x g , y g >. The matrix J l ∈ IR 3×9 is used to express the linear and angular velocities of the robot's left foot to the ground, ( Figure 2).
In (5), ( gṗ lx , gṗ ly ) are the linear velocities of the reference frame < x 6 , y 6 > with respect to < x g , y g >, whereas that,θ l is the angular velocity of the reference frame < x 6 , y 6 > with respect to < x g , y g >. The unknowns of the previous model areq ∈ IR 9 , F r ∈ IR 3 and F l ∈ IR 3 . That is; there are 9 equations and 15 variables. The required additional equations can be obtained by using complementarity conditions related to normal and tangential foot reaction forces [19]. However, as it was demonstrated by several authors complementarity conditions can be written as an implicit algebraic equation [20], [21]: The unknowns in (6) are F r and F l . In summary, the robot's dynamic behaviour is described by the differential equation (1) subject to the algebraic constraint (6) Model (1) allows to represent robot's dynamic behavior in all phases of a gait pattern: single support on left foot, double support and single support on the right foot. In the following, (7) will be called the simulation model, and it will be used to validate the proposed control strategy. To design of such strategy is also necessary another model called synthesis control model. This latter model depends on the phase of the gait pattern. In most cases three different models are used, one of them for each phase of the gait pattern. As the proposed control strategy for motion imitation will be tested when robot is resting on the left foot and right limb is trying to track a joint reference motion obtained by a Kinect system, only the synthesis control model for single support phase on the left foot will be required.
The six joint angle trajectories, denoted as q r i (i = 1, . . . , 6), are estimated from N + 1 Cartesian positions measured by a motion capture system based on Microsoft's Kinect TM camera. The trajectory for the joint i is represented by using a set of N third order polynomials called cubic splines,

Control for the single support phase on the left foot
When the robot is resting on the left foot, the model (1) can be rewritten as: The unknowns of the previous model areq ∈ IR 9 and F l ∈ IR 3 . That is; there are 9 equations and 12 variables. For the purpose of the synthesis of the control system only, the previous model will be considered as subject to immobility constraints on the left foot immobility In order to involve the unknowns of the problem in the three equations above, these are derived twice with respect to time. By deriving once, we obtain: By deriving once again:

Ingeniería y Ciencia
By combining equations (9) and (12) a model relatingq and F l as a function of q,q and Γ is obtained rewriting the constraint (12) in the form: With q u being the vector containing the undriven coordinates and q a the driven coordinate vector Matrix J lu (q) ∈ IR 3×3 is composed by the first three columns of J l (q) and J la (q) ∈ IR 3×6 by the last six columns. By solvingq u of (14) we obtained: Equation indicates that the contact between the left foot and the ground leads to the acceleration of the coordinates not being directly driven by the acceleration of the coordinates actuated. In order to lighten the mathematical notation, the following variables are defined: In the following the arguments of A, M, H and N will be omitted. Substitutingq = Mq a + N in the dynamic model (9) Equation (18) is multiplied by M T and by simplifying we obtain Ifq a is replaced by the desired accelerationq d a and it is calculated as follows, equation (19) becomes a classical computed torque control [22] q r a ∈ IR 6 ,q r a ∈ IR 6 ,q r a ∈ IR 6 being the joint reference motion obtained by using the motion capture system. By replacingq d a in (19) we obtain the following equations which allow to calculate the joint torques required to the joint coordinates follow theirs respective reference values

Hybrid control
Control law (21) ensures the convergence of the joint variables q i (t),q i (t) andq i (t) (i = 1 . . . 6) towards the reference motion q r i (t),q r i (t) andq r i (t) described in section 3. However, tracking of these variables does not prevent the robot may fall. To prevent falling, a technique called time-scaling, initially developed for robot manipulators [15], was used applied in [17] to control of the Zero Moment Point [13] (ZMP) of a bipedal robot. This technique, unfortunately, requires to know the desired position of the ZMP, which cannot be obtained by our Kinect-based motion capture system. This reason has motivated us to propose a new control strategy using a rule based on the following variables: (i) the current position of the robot's ZMP, denoted p x and (ii) the length of the foot, denoted l f : If p x , satisfies the condition |p x | ≤ 0.45 l f , then the control law (21) is used. Otherwise set the desired position of the ZMP, denoted p r x = 0.45 sign(p x ) l f , and apply time-scaling control.
The upper limit of the inequality in the paragraph above ensures that the ZMP never reaches the edges of the foot. In this way, the ZMP belongs in the interval [−0.45 l f , 0.45 l f ] and the proposed control law allows |34 Ingeniería y Ciencia track a desired reference motion and maintain robot's equilibrium without requiring to know the ZMP of the performer (a human being).
To apply time-scaling control, the reference trajectory (8) must to be parametrized as a function of a variable s(t) called virtual time Given s(t), a unique trajectory is defined. Any trajectory defined by (22) corresponds to the same path in the joint space as the reference trajectory, but the evolution of the robot with respect to time may differ. If instead of q r (t),q r (t) andq r (t), the variables defined by (22) are used to calculatë q d a , (21) becomes With s = t,ṡ = 1 ands = 0, the control law (23) is a classical CTC, otherwise, it is a time scaling control. The procedure to finds is explained in the next section.

Dynamic of zero moment point.
The concept of Zero Moment Point (ZMP) was first introduced by Vukobratović [13], is used for the control of humanoid robots. The ZMP specifies the point where the reaction forces generated by the foot contact with the ground produce no moment in the plane of contact, it is assumed that the surface of the foot is flat and has a coefficient of friction sufficient to prevent slippage. The zero moment point (ZMP) can be calculated as follows: P x being the zero moment point, m z the angular momentum on axis z and f y the reaction force of the ground exerted on foot. In an equivalent way, if p r x is the desired position for the ZMP, such value have to satisfy

D. Bravo and C. Rengifo
When (28) is combined with (26), the resulting system has four equations and four unknowns  The question is: ¿how to select p r x if the ZMP of the performer is not known?

Summary of the hybrid strategy
If |p x | ≤ 0.45 l f , then Γ is calculated by using (21) ands is set to zero. Otherwise, p r x = 0.45 sign(p x ) l f , use (29) to computes and (23) to obtain Γ.

Results and discussion
We can represent human anatomy as a sequence of rigid bodies (links) connected by joints; [3]. Kinect TM allows optical tracking of a human skeleton and gives the Cartesian coordinates (x, y, z) of 20 joint points. In our case, we are interested in joint coordinates of the lower limbs in the sagittal plane. So, it was necessary to develop an application to transform Cartesian coordinates (x, y, z) into joint coordinates (q 1 , . . . , q 6 ) by using inverse kinematics. The experiment was designed to capture motion data of the right leg, consisting of moving the right leg in swing phase while the left leg remains still. The reference trajectories for swing phase of the right leg were obtained through a Microsoft's Kinect TM camera at 30 fps. In this experiment the user is in front of Kinect, the rotation matrix that relates the referent of Kinect with the body 0 right leg, is writes: The sensor position and the reference axes shown in Figure 3. The reference trajectories of the joint positions are described as a function of time q r (t).
ing.cienc., vol. 11, no. 22, pp. 25-47, julio-diciembre. 2015. The results shown in Figure 4 are relative to measurements of kinematic variables for the movement of a walking male of a height of 1.65m. The sinusoidal feature of the movement of the hip articulation q 1 is easy to identify. The shapes of the trajectory for the hip q 1 and right knee q 2 are similar to those reported in the literature for the swing phase [23], [24]. The trajectory q 3 represents the angular motion of the heel in a sagittal plane, the rapid variation is by poor detection of anatomical landmarks during the toe-off and ground contact phases of movement, which is likely due to the inability of the machine learning algorithm to accurately identify landmarks in this position when compared to regular standing positions, and the failure to identify multiple significant land marks on the foot such as the metatarsophalangeal and calcaneus, which would allow for more precise detection of gait events, [25]. Figure 5 shows a geometrical tracking, rather than temporal tracking of the reference trajectory.

|38
Ingeniería y Ciencia [Degrees] Tracking Joint Q1   Figure 4 shows the joint trajectories for lower limb data taken from human motion capture in the swing phase at a frequency of 30 fps and trajectories filtered with f c = fs 2 . Cubic spline interpolation was used between each of the joint positions to ensure the continuity of the paths. The primary articulations associated with human locomotion are those of the hips, the knees, the ankles and the metatarsal articulations. Hip movement combined with pelvis rotation enables humans to lengthen theirsstep, [1]. During a walking cycle, the movement of the hip in the sagittal plane is essentially sinusoidal. Thus, the thigh moves from back to front and vice versa. The articulation of the knee allows for flexion and extension movements of the leg during locomotion. As for the ankle, flexion movements occur when the heel is re-grounded. There is a second flexion during the balancing phase.
Due to the robot cannot execute the motion with all constraints satisfied, then the robot moves slower in this region of the curve, like Figure 6, where the slope of the curve that represents the virtual time is always less than one.  To implement the control law (23), velocity, and joint acceleration measurements are necessary. Because the velocity and acceleration are not directly measurable on the paths obtained by Kinect, these are calculated |40 Ingeniería y Ciencia from measurements of joint position. To estimate the velocity and acceleration, first the interpolation between each joint position is performed by using third order polynomials. Then, the resulting sequence of polynomials is derived for the first time to estimate the speed and the second time to calculate acceleration. The joint velocity of the right leg in swing phase is shown in Figure 7. For these reasons, values of desired joint coordinates q r (t),q r (t) andq r (t) were calculated offline. Unfortunately, Kinect is sensitive to infrared light, this generates a high level of noise in the measurements, despite being filtered, [26]. However, studies have been conducted to improve its accuracy by implementing a Kalman filter in human gait studies, [27]. Although this approach was not used in the development of this work. The controller switches between two control laws to achieve a stable result. When the constraint |p x | ≤ 0.45 l f is met, the control law is a classic CTC, wheres = 0. Here, the controller is tuned for a settling time of 0.1 seconds and damping factor of 0.707. On the other hand, the time scaling control is used to ensure the stability of the robot and fulfill the constraint |p x | ≤ 0.45 l f . Then, the controller is tuned for a settling time of 0.01 seconds and damping factor of 1. The simulation of the whole system was done in the MATLAB Simulink c programming environment.
For the classic CTC control, the temporal evolution of the ZMP is shown in Figure 8. Although the robot makes tracking reference trajectories, the ZMP leaves the limits of the support polygon, rendering the system unstable. This case may be dangerous. We propose conducting time scaling control when this situation occurs. In Figure 9, at time t = 0 second, the ZMP trajectory reaches the upper limit of the support polygon. Then, time scaling control is activated to ensure the ZMP remains within the support polygon for the motion and provide the robot's stability. Vector Γ ∈ IR 6 for each one of joints (q 1 , q 2 , . . . , q 6 ) is a plot. Figure 10 shows the input torques for classic CTC control. The values of Γ 1 , Γ 2 , Γ 3 are of the joints of left leg, whereas Γ 4 , Γ 5 , Γ 6 are of the joints of right leg. The values are big, in average 200 [N t · m] making the control law impracticable. But, for the hybrid control the average is 40 [N t · m] decreasing in a relationship (1 : 5), see Figure  11. This law control is practicable, considering that the total mass of the robot for the simulation is 45. 75    The constraint |p x | ≤ 0.45 l f is only satisfied by using the hybrid control. Figure 12 shows that the tangential force is within the limits of the cone of friction for friction coefficient µ = 0.75. Therefore, the robot could not slide and fall.

Conclusion
The trajectory generation from motion capture for a planar biped robot in swing phase is presented. The robot modeled is a planar biped with only six actuators and underactuated during the single support phases. The reference trajectory to be tracked using the proposed hybrid control strategy was calculated off-line via human motion capture. The simulations show that the hybrid control strategy of joint trajectories ensures only geometrical tracking of the reference trajectory, besides the robot's stability.
The comparison between the control techniques, classic CTC and time scaling control, show that although both methods allow tracking reference trajectories, only the hybrid control ensures robot stability, but not tracking of time references. This is an open problem, to be solved: tracking reference trajectories in space and time to provide robot stability.
In our future work, we will experimentally validate the hybrid control strategy with the humanoid robot Bioloid TM by means of trajectories obtained from human motion capture and analyze stability for trajectory generation of a biped robot from human motion capture.