I Introduction
Planning locomotion trajectories for humanoid robots requires considering highdimensional multibody systems instantiating contacts with the surrounding environment. Given their intrinsic underactuation, these robots have to exploit the interaction with the environment and their ability to change “shape” in order to move.
During the DARPA Robotics Challenge [12], it became popular to approach the locomotion problem with a hierarchical control architecture. In our previous works [5, 26] we explored this concept by adopting a three layer control architecture. The outer layer, i.e. the trajectory optimization for footstep planning, is in charge of generating walking trajectories starting from highlevel commands, such as those coming from a joystick. The output of this layer is served to the receding horizon controller (RHC), also referred to as Model Predictive Control (MPC) [18]. Its aim is to generate centroidal [24] quantities obtainable by the robot without incurring into an undesired fall state. Compared to the first, this second layer adopts more complex models with a shorter prediction horizon. The last stage, the wholebody quadratic programming control is in charge of stabilizing the planned trajectories exploiting the full robot model with a suitable Quadratic Programming formulation.
In this paper, we merge the first two layers, generating locomotion trajectories adopting the full kinematics of the robot and the centroidal dynamics. The approach follows what presented by authors of [7]. Nevertheless, no prior knowledge is injected on the system to generate walking trajectories, but the wholebody motions result from a particular choice of cost function.
When planning locomotion trajectories, the definition of contacts plays a central role. Several strategies are available in literature, here roughly summarized in four categories.
Fixed contact sequence, timing and location. A common approach consists of assuming to know in advance where the contacts will be established and in which instant [13, 16, 25, 8, 6]. Such choice simplifies the planning problem, leading to a lower computational effort. However, they need to rely on external contact planners.
Predefined contact sequence. During locomotion, it can be assumed to know in advance the contact sequence. As an example, for a biped robot, it can be assumed that a contact with the left foot will be followed by another one with the right foot. In other words, the phases are predefined while the remaining quantities (like positions and timings) are an output of the planner [30, 4, 3]. By specifying a different set of equations depending on the contact state, the hybrid nature arising from the establishment of contacts is easily modeled. The time spent by each phase can be turned into an optimization variable. Nevertheless, in case several point contacts, the definition of the various phases could become intractable.
Mixed Integer Programming. Instead of receiving the contact sequence as input, it is possible to use integer variables to determine when a particular contact has to be considered active or not [9, 1]. This approach requires Mixed Integer Programming tools. While providing enhanced modeling capabilities, the exploitation of integer variables strongly affects the computational performances, especially in case several contacts are available. In addition, the availability of specialized solvers is limited.
Complementarityfree. Authors of [28, 10] presented an approach which allows simulating multibody systems subject to contacts, without enforcing complementarity conditions directly. Equivalently accurate results are obtained by maximizing the rate of energy dissipation. Such approach can be used to generate complex movements [21, 27].
In this paper, we present a planner where neither contact sequences, locations or timings are fixed a priori. Additionally, we adopt a complementarityfree approach. Through a tailored parametrization of contacts, we impose complementarity conditions indirectly. Since the full robot kinematics is used, planned footsteps are within the robot workspace.
Ia Notation

The transpose operator is denoted by .

is a fixed inertial frame with respect to (w.r.t.) which the robot’s absolute pose is measured. Its axis points against gravity, while the direction points forward.

Given a function of time the dot notation denotes the time derivative, i.e. . Higher order derivatives are denoted with a corresponding amount of dots.

denotes a zero matrix.

The weighted L2norm of a vector is denoted by , where is a weight matrix.

and denote the rotation and transformation matrices which transform a vector expressed in the frame into one expressed in .

returns the direction normal to the walking plane given the argument’s and coordinates.

returns two perpendicular directions normal to . The composition of and , , defines the rotation matrix .

The function defines the distance between and the walking surface.

, and denote the canonical basis vectors of .

is the relative velocity between frame and , whose coordinates are defined in frame .

is a function casting the argument into the corresponding diagonal function.
Ii System Modeling
Iia Contact Interface Modeling
When performing a step, the foot can impact the ground in a not flat configuration, reducing the amount of contact wrenches obtainable from the ground. At the same time, toeoff motions can be used to increase the workspace available during double support phases [15]. Given these reasons, all the various contact configurations should be taken into account when planning step motions.
In order to reduce the complexity, it is possible to consider the foot as composed by a set of points, for example four points located at the corners of the foot [7, 3]. Thanks to this choice, the several contact configurations can be modeled independently, depending on the number of points in contact.
A pure force can be applied on each contact point. In case of four points, twelve variables define a six dimensional quantity, i.e. the resulting contact wrench acting on the foot. This is a drawback that will be addressed later in Sec. IIID.
IiB Contacts Force Constraints
Define as the th contact point location in an inertial frame , and as the force exerted on that point. Such force is expressed on a frame located in and with orientation parallel to . Since it results from the interaction of the foot with the ground, it is subject to constraints. Being a reaction force, its normal component with respect to the walking ground is supposed to be nonnegative. In particular, . Additionally, in order to avoid slippage, friction constraints should be satisfied:
(1) 
where is the static friction coefficient.
IiC Contact Parametrization
The contact force applied to the th contact point is supposed to be notnull only if the point is in contact with the walking surface. This condition could be represented by the following equality:
(2) 
Such constraint can be difficult to be tackled in an optimization framework. This is due to the fact that the feasible set is only constituted by two lines, namely and , which are intersecting in the origin. In this point, the constraint Jacobian gets singular, thus violating the linear independence constraint qualification (LICQ), on which most offtheshelf solvers rely upon [2].
In order to avoid the complications related to Eq. (2), we adopt a simple parametrization. In particular, we assume to have full control over the derivative of both contact point positions and forces:
(3a)  
(4a) 
where are control inputs. Then, we can impose Eq. (2) dynamically through the following constraints:
if h(_i p) = 0,  (5a)  
if h(_i p) ≠0.  (6a) 
When the point is in contact, is free to take any value in with defining control bounds (Eq. (5a)). On the other hand, if the contact point is not on the walking surface, the control input makes the contact force decreasing exponentially (Eq. (6a)). Defining as a binary function such that
(7) 
it is possible to write Eq. (5a) as a set of two inequalities:
(8a)  
(9a) 
Even if would require the adoption of integer variables, it is possible to use a continuous approximation, , namely the hyperbolic secant:
(10) 
where is a userdefined scaling factor. Since only when , Eq. (8a) satisfies the LICQ condition.
Given Eq. (1), it is enough to apply Eq. (8a) only to the force component normal to the ground: if it decreases to zero, also planar force components have to vanish.
Since contact points are not supposed to penetrate the walking ground, we can impose .
None of the constraints defined above could prevent the contact points to move on the walking plane when in contact. In fact, even if friction constraints defined in Eq. (1) are satisfied, the contact points are still free to move on the contact surface. Force and position variables are (almost) independent at this stage. It is possible to prevent planar motions when in contact by limiting the effect of the control input along the planar components:
(11) 
where is a userdefined scaling factor. Eq. (11) multiplies the control input along the planar direction to zero when is null and, at the same time, it will reduce the velocity when the contact point is approaching the ground. It is possible to rewrite Eq. (11) as
(12) 
where the function is defined as:
(13) 
Note that, from now on, is assumed to be defined in coordinates. Thus, the normal component of the velocity is directly affected by . Also, it is necessary to bound this control input, , to properly exploit the effect of the hyperbolic tangent. Note that Eq. (11) allows avoiding the use of complementarity conditions along planar directions.
IiD Contact Point Position Consistency
While each contact point is supposed to be independent from the control point of view, they all need to remain on the same surface and maintain a constant relative distance, since they belong to the same rigid body. At the same time, we want them to be within the workspace reachable by the robot legs. We can achieve both the objectives by enforcing the following constraint on each of the contact points:
(14) 
where is the (fixed) position of the contact point within the foot surface, expressed in foot coordinates. Here, the transformation matrix would depend on the base position , the base quaternion and the joints configuration , with being the number of joints. As a consequence, the full kinematics of the robot is taken into consideration and the following dynamic equations have to be considered:
(15a)  
(16a)  
(17a) 
Here , and are considered control inputs defining the base linear velocity, the quaternion derivative and the joints velocity, respectively. More specifically, is the linear part of the lefttrivialized (i.e. measured in body coordinates) base velocity.
IiE Momentum Dynamics
In Sec. IIA, we consider the contact points as if they have the possibility of exerting a force with the environment. We can describe the effect of these contact forces through the momentum, or centroidal, dynamics. This choice is supported by the fact that the momenutm dynamics depends only on the contact forces, their location and on the center of mass (CoM) position, . Define as the robot total momentum, with where and are respectively the linear and angular momentum. This quantity is expressed in a frame oriented as the inertial frame , with the origin placed on the CoM position. Such frame is called or simply . The momentum dynamics has the following form:
(18a)  
(19a) 
with the total mass of the robot, . We also need to make sure that the integrated CoM corresponds to the one obtained via the joint variables. This is done through an additional constraint:
(20) 
where is the function mapping base pose and joint positions to the CoM position. While this constraint defines a link between the linear momentum and the joint variables, the same would not hold for the angular part. To this end, we can exploit the Centroidal Momentum Matrix [24] (). In fact, the robot angular momentum can be defined as a function of the base and joints velocity:
(21) 
where . Here, the base angular velocity can be substituted with the quaternion derivative through the map [14, Section 1.5.4], such that
Some additional constraints can be considered:
(22a)  
(23a) 
Eq (22a) avoids solutions which would bring the CoM position too close or below the ground. Eq. (23a) provides an upper and lower bound to the angular momentum. These constraints avoid trajectories that would cause excessive motions or let the robot falling.
IiF Feet Minimum Lateral Distance
While taking steps, we need to make sure that the robot legs do not collide with each other. Self collision constraints are usually hard to be considered and may slow down consistently the determination of a solution. A simpler solution consists in avoiding the left leg to be on the right of the other leg. Consequently, cross steps are forbidden. Let us consider a frame attached to the right foot with the positive direction pointing toward left. In this case, it is sufficient to impose the component of the (i.e. the relative position of the left foot expressed in the right foot frame) to be greater than a given quantity, i.e. .
Iii Tasks
We present the set of tasks used to plan a walking trajectory. While constraints define the model and the control limitations, the tasks embed the planning objectives.
Iiia Contact point centroid position task
In order to make the robot moving toward a desired position, we minimize the L2 norm of the error between a point attached to the robot and its desired position in an absolute frame. In particular, we select the centroid of the contact points as target, thus avoiding to specify a desired placement for each foot:
(24) 
where is the number of contact points in a single foot. Thus, we have and is a userdefined reference value.
IiiB CoM linear velocity task
While walking, we want the robot to keep a constant forward motion. In fact, since the positions of the feet are not scripted, it may be possible to plan two consecutive steps with the same foot. By requiring a constant forward velocity, such phenomena can be avoided. This task is defined as:
(25) 
with a desired CoM velocity. The matrix selects and weights the different directions separately.
IiiC Frame orientation task
While moving, we want a robot frame to be oriented in a specific orientation . In particular, we weight the distance of the rotation matrix from the identity. Having to express this task in vector form, we convert into a quaternion (through the function quat which implements the Rodriguez formula) and weight its difference from the identity quaternion . Namely:
(26) 
This corresponds to a simplified version of the quaternion difference metric listed in [17], under the assumption for quat to always return a quaternion with positive real value.
IiiD Force regularization task
While considering each single contact force in a foot as independent, they still belong to a single body part. Thus, we prescribe the contact forces in a foot to be as similar as possible, refraining from using partial contacts if not strictly necessary. This can be obtained through the following:
(27) 
IiiE Joint regularization task
The joint configuration
is part of the optimization variables. In order to prevent the planner from providing solutions with huge joint variations, we introduce a regularization task for joint variables:
(28) 
with the desired joint configurations and a weight matrix. The minimum of this cost is when , with . When this equality holds, joint values converge exponentially to their reference . Hence, joint velocities and joint positions are regularized at the same time.
IiiF Swing height task
When performing a step, the swing foot clearance usually ensures some level of robustness with respect to ground asperity. Nevertheless, since the soil profile is supposed to be known in advance, a solution satisfying all the equations described in Sec. II may require the swing foot to be raised just few millimeters from the ground. In order to specify a desired swing height, we impose the following cost:
(29) 
It penalizes the distance between the component of each contact point position from a desired height when the corresponding planar velocity is not null. Trivially, this cost has two minima: when the planar velocity is zero (thus the point is not moving) or when the height of the point is equal to the desired one.
Iv Optimal Control Problem
Given the set of equations listed in Sec. II and the tasks described in Sec. III it is possible to define an optimal control problem, whose complete formulation is presented below. Here, the vector contains the set of weights defining the relative “importance” of each task.
Iva Problem definition
subject to : 
Dynamical Constraints
∀ contact point  (30a)  
∀ contact point  (31a)  
(32a)  
(33a)  
(34a)  
(35a)  
(36a) 
Equality Constraints
(37a)  
(38a)  
(39a)  
(40a) 
Inequality Constraints
(41a)  
(42a)  
(43a)  
(44a)  
(45a)  
(46a)  
(47a)  
(48a)  
(49a) 
Here, the state variables are those derived in time, all the others. More specifically:
(50) 
where the symbol represents the repetition of variables for each contact point. The optimal control problem is solved using a Direct Multiple Shooting method [2]. The system dynamics, defined in Eq. (30a), is discretized adopting an implicit trapezoidal method with a fixed integration step. The corresponding optimization problem is solved thanks to Ipopt [29].
The walking trajectories are generated using the Receding Horizon Principle [19], adopting a fixed prediction window.
IvB Considerations
The optimal control problem described in Sec. IVA is built such that (almost) no constraint is task specific. As a consequence, it is particularly important to define the cost function carefully since the solution will be a tradeoff between all the various tasks. On the other hand, the detailed model of the system allows achieving walking motions without specifying a desired CoM trajectory or by fixing the angular momentum to zero. Nevertheless, due to the limited time horizon, it is better to prevent the solver from finding solutions which would bring the robot to unfeasible states in future planner iterations. To this end, Eq. (22a) and Eq. (23a) have been added, using reasonably large bounds.
Another possible effect resulting from the application of the Receding Horizon principle, is the emergence of “procrastination” phenomena. Due to the moving horizon, the solver may continuously delay in actuating motions, since the task keeps being shifted in time. A simple fix to this phenomena is to increase the weights with time, such that it is more convenient to reach a goal position earlier.
Finally, given that the problem under consideration is nonconvex, the optimizer will find a local minimum. This may result in a suboptimal solution for the given tasks.
During the first iteration, the solver is initialized by simply translating the whole robot in the desired position. In successive iterations, the solver is warmstarted with the solution previously computed.
V Results
The optimal control problem described in Sec. IV is used to generate a walking motion. The integration step is set to , while the prediction horizon is . After each iteration of the MPC controller, the previously computed state is used as a feedback. Regarding the scaling factors, we use and . They appeared to be reasonable values for having meaningful simulations of contacts. In addition, these values are robot and ground independent, since they depend only on the position of a contact point with respect to the ground.
The trajectories have been generated using the iCub humanoid [22] robot model on a 7 generation Intel^{®} Core i7@2.8GHz laptop. We assume the ground to be flat, while we control 23 of the robot joints. For each foot, we consider four contact points located at the vertexes of the rectangle enclosing the robot foot. Concerning the references, the desired position for the centroid of the contact points is moved along the walking direction every time the robot performs a step. A simple state machine, where the reference is moved as soon as a step is completed, is enough to generate a continuous walking pattern. The speed is modulated by prescribing a fixed desired CoM forward velocity equal to .
Figure 5 shows some snapshots of the first generated step. Also, it can be observed the effect of the contact parametrization described in Sec. IIC from Fig. 6. The normal force decreases to zero as soon as the foot starts leaving the ground, and then it grows again at touchdown. It is possible to recognize the different walking phases, even though they are not planned a priori.
Figure 7 presents the planned CoM position. Here, it is possible to notice that position grows at a constant rate. This is a direct consequence of the task on the CoM velocity. Figure 8 shows the planned angular momentum, which is not fixed to zero. Although it is limited to , such limit is never reached. Similarly, the bound on the CoM height, , is set to half of the initial robot height, but such constraint is never activated.
It is worth stressing that none of the tasks described above define how and when to raise the foot. By prescribing a reference for the centroid of the contact points and by preventing the motion on the contact surface, swing motions are planned automatically. Nevertheless, this advantage comes with a cost. It is difficult to define a desired swing time and, more importantly, the relative importance of each task, i.e. the values of , must be chosen carefully. During experiments, we adopted an incremental approach. We added the tasks one by one, starting from and then we gradually refine the walking motion by tuning a cost at a time.
Vi Conclusions
This paper presents a planner capable of generating walking trajectories using a minimal set of references. It considers the centroidal momentum of the robot and its full kinematics to plan dynamically consistent step motions. The modelling of contacts makes use of a novel parametrization approach, allowing to model the interface between the robot and the ground with a set of continuous equations. Currently, this model does not consider impacts nor contact sliding. The consideration of slipturn motions [20] is left as future work.
The results have to be considered as an initial validation of the generated trajectories when the iCub model is adopted. In particular, it is shown that walking trajectories can emerge by specifying a moving reference for the centroid of the contact points and the desired CoM velocity only.
The planner considers relatively large timesteps. This enables the insertion of another control loop at higher frequency, whose goal is to stabilize the planned trajectories. As a future work, we will consider connecting this planner to the wholebody controller presented in [5].
The main bottleneck is represented by the computational time. A single planner iteration may take from slightly less than a second to more than a minute. This prevents an online implementation on the real robot. On the other hand, the continuous formulation of the optimal control problem allows the application of techniques, like those presented in [23, 11]
, which do not rely on the transcription to a nonlinear programming problem.
References
 [1] (2018) Simultaneous contact, gait, and motion planning for robust multilegged locomotion via mixedinteger convex optimization. IEEE Robotics and Automation Letters 3 (3), pp. 2531–2538. Cited by: §I.

[2]
(2010)
Practical methods for optimal control and estimation using nonlinear programming
. Vol. 19, Siam. Cited by: §IIC, §IVA.  [3] (2017) When to make a step? tackling the timing problem in multicontact locomotion by toppmpc. In 2017 IEEERAS 17th International Conference on Humanoid Robotics (Humanoids), pp. 522–528. Cited by: §I, §IIA.
 [4] (2016) A versatile and efficient pattern generator for generalized legged locomotion. In Robotics and Automation (ICRA), 2016 IEEE International Conference on, pp. 3555–3561. Cited by: §I.
 [5] (2018) A control architecture with online predictive planning for position and torque controlled walking of humanoid robots. In 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 1–9. Cited by: §I, §VI.
 [6] (2017) A receding horizon push recovery strategy for balancing the icub humanoid robot. In International Conference on Robotics in AlpeAdria Danube Region, pp. 297–305. Cited by: §I.
 [7] Wholebody motion planning with centroidal dynamics and full kinematics. In 2014 IEEERAS International Conference on Humanoid Robots, pp. 295–302. Cited by: §I, §IIA.
 [8] (2010) Featurebased locomotion controllers. ACM Transactions on Graphics (TOG) 29 (4), pp. 1–10. Cited by: §I.
 [9] Footstep planning on uneven terrain with mixedinteger convex optimization. In Humanoid Robots (Humanoids), 2014 14th IEEERAS International Conference on, Cited by: §I.
 [10] (2010) Modeling contact friction and joint friction in dynamic robotic simulation using the principle of maximum dissipation. In Algorithmic foundations of robotics IX, pp. 249–266. Cited by: §I.
 [11] (2017) Realtime motion planning of legged robots: a model predictive control approach. In 2017 IEEERAS 17th International Conference on Humanoid Robotics (Humanoids), pp. 577–584. Cited by: §VI.
 [12] (2015) Optimizationbased full body control for the darpa robotics challenge. Journal of Field Robotics 32 (2), pp. 293–312. Cited by: §I.
 [13] (2018) CROC: convex resolution of centroidal dynamics trajectories to provide a feasibility criterion for the multi contact planning problem. In 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Cited by: §I.
 [14] (2008) Quaternions and dynamics. Available at https://arxiv.org/pdf/0811.2889.pdf. Cited by: §IIE.
 [15] (2018) Straightleg walking through underconstrained wholebody control. In 2018 IEEE International Conference on Robotics and Automation (ICRA), pp. 1–5. Cited by: §IIA.
 [16] (2015) Trajectory generation for multicontact momentum control. In Humanoid Robots (Humanoids), 2015 IEEERAS 15th International Conference on, pp. 874–880. Cited by: §I.
 [17] (2009) Metrics for 3d rotations: comparison and analysis. Journal of Mathematical Imaging and Vision 35 (2), pp. 155–164. Cited by: §IIIC.
 [18] (2000) Constrained model predictive control: stability and optimality. Automatica 36 (6), pp. 789 – 814. Note: External Links: ISSN 00051098, Document Cited by: §I.
 [19] (1990) Receding horizon control of nonlinear systems. IEEE Transactions on automatic control 35 (7), pp. 814–824. Cited by: §IVA.
 [20] (2013) Slipturn for biped robots. IEEE Transactions on Robotics 29 (4), pp. 875–887. Cited by: §VI.
 [21] (2012) Discovery of complex behaviors through contactinvariant optimization. ACM Transactions on Graphics (TOG) 31 (4), pp. 43. Cited by: §I.
 [22] (2017) ICub: the notyetfinished story of building a robot child. Science Robotics 2 (13). External Links: Document Cited by: §V.
 [23] (2018) Wholebody nonlinear model predictive control through contacts for quadrupeds. IEEE Robotics and Automation Letters 3 (3), pp. 1458–1465. Cited by: §VI.
 [24] (2008) Centroidal momentum matrix of a humanoid robot: structure and properties. Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on, pp. 653 – 659. Cited by: §I, §IIE.
 [25] (2016) Optimization and stabilization of trajectories for constrained dynamical systems. In 2016 IEEE International Conference on Robotics and Automation (ICRA), pp. 1366–1373. Cited by: §I.
 [26] (2018) A benchmarking of dcm based architectures for position and velocity controlled walking of humanoid robots. In 2018 IEEERAS 18th International Conference on Humanoid Robots (Humanoids), pp. 1–9. Cited by: §I.
 [27] (2012) Synthesis and stabilization of complex behaviors through online trajectory optimization. In 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 4906–4913. Cited by: §I.
 [28] (2011) A convex, smooth and invertible contact model for trajectory optimization. In 2011 IEEE International Conference on Robotics and Automation, pp. 1071–1076. Cited by: §I.
 [29] (2006) On the implementation of an interiorpoint filter linesearch algorithm for largescale nonlinear programming. Mathematical programming 106 (1), pp. 25–57. Cited by: §IVA.
 [30] (201807) Gait and trajectory optimization for legged systems through phasebased endeffector parameterization. IEEE Robotics and Automation Letters (RAL) 3, pp. 1560–1567. External Links: Document Cited by: §I.
Comments
There are no comments yet.