SYSTEMS AND METHODS FOR DETERMINING REACTIONS DUE TO FRICTION OF OBJECTS WITH MULTIPLE CONTACTS

Abstract

In a mechanical system with an object having a set of two or more contacts, methods are disclosed for determining object motion by computing the tangential generalized reaction impulse and force. The methods includes receiving input values that include an initial generalized position and an initial generalized velocity of the object at time t, and computing a generalized force. The methods further includes computing, respectively for the reaction impulse and force: a generalized reaction impulse and force, and a tangential generalized reaction impulse and force. The methods use the tangential generalized reaction impulse and force to, respectively, compute an end-of-interval generalized position and velocity, and a generalized acceleration, which are output for use by an application.

Description

BACKGROUND

Discrete mechanical systems, which may be used for determining motion of objects in space where the number of degrees of freedom of the objects is finite (including degrees of freedom describing deformations), finds applications today in multiple fields including autonomous objects and virtual reality. Systems using discrete mechanical systems to model objects must account for frictional contacts between objects to yield accurate results, for example a robot hand and an object grasped by the robot hand. As needs for real-time computation of rigid-body dynamics grows, there continues to exist a need for faster and more accurate systems and methods for modeling real-world systems using rigid-body dynamics for predicting the motion of objects.

SUMMARY

In a feature, a computer-implemented method, using a computer having one or more processors and memory, determines for an object having a set of two or more contacts and having at time t an initial generalized position q_t and an initial generalized velocity q_t, the object's end-of-interval generalized position q_t+h and end-of-interval generalized velocity q_t+h after an interval h. The method includes: computing a generalized force f over the interval, which is a summary of forces acting on the object other than forces due to the set of contacts, and which is used to define energy over the interval h; computing a normal generalized reaction impulse r_n acting on the object; computing a tangential generalized reaction impulse r_t acting on the object; and using the tangential generalized reaction impulse r_t to compute the end-of-interval generalized position q_t+h and the end-of-interval generalized velocity q_t+h of the object. The method further includes, computing the tangential generalized reaction impulse r_t by applying the following constraints to an objective function minimizing energy over the interval h: (a) associating each contact in the set of contacts with a friction cone in a space of generalized impulses to define a set of friction cones; (b) summing one term from each friction cone in the set of friction cones, minus the normal generalized reaction impulse r_n; and (c) constraining the tangential generalized reaction impulse r_t to be orthogonal to the normal generalized reaction impulse r_n and to have a non-positive inner product with the normal direction of each contact in the set of contacts.

In a feature, a computer-implemented method, using a computer having one or more processors and memory, determines for an object having a set of two or more contacts and having at time t an initial generalized position q_t and an initial generalized velocity q_t, the object's generalized acceleration q_t. The method includes: computing a generalized force f which is a summary of forces acting on the object other than forces due to the set of contacts; computing a normal generalized reaction force R acting on the object; computing a tangential generalized reaction force R_t acting on the object using the computed generalized force f to account for contacts associated with zero relative velocity at time t; and using the tangential generalized reaction force R_t to compute the generalized acceleration q_t of the object. The method further includes computing the tangential generalized reaction force R_t by applying the following constraints to an objective function maximizing power dissipated by friction: (a) associating each contact in the set of contacts with a friction cone in a space of generalized impulses to define a set of friction cones; (b) summing one term from each friction cone in the set of friction cones, minus the normal generalized reaction force R_n; and (c) constraining the tangential generalized reaction force R_t to be orthogonal to the normal generalized reaction force R_n and to have a non-positive inner product with the normal direction of each contact in the set of contacts.

In a feature, a computer-implemented method, executed by one or more processors and memory, for determining for an object having a set of two or more contacts and having at time t an initial generalized position q_t and an initial generalized velocity q_t, the object's end-of-interval generalized position q_t+h and end-of-interval generalized velocity q_t+h after an interval h is described. The method includes: computing a generalized force f over the interval h, the generalized force being a summary of forces acting on the object other than forces due to the set of contacts, and which is used to define energy over the interval h; computing a normal generalized reaction impulse r_n acting on the object; computing a tangential generalized reaction impulse r_t acting on the object; and based on the tangential generalized reaction impulse r_t, computing the end-of-interval generalized position q_t+h and the end-of-interval generalized velocity q_t+h of the object; where the computing the tangential generalized reaction impulse r_t further includes applying the following constraints to an objective function minimizing energy over the interval h: (a) associating each contact in the set of contacts with a friction cone in a space of generalized impulses to define a set of friction cones; (b) summing one term from each friction cone in the set of friction cones, minus the normal generalized reaction impulse r_n; and (c) constraining the tangential generalized reaction impulse r_t to be orthogonal to the normal generalized reaction impulse r_n and to have a non-positive inner product with the normal direction of each contact in the set of contacts.

In further features, the method further includes computing an intermediate generalized position q_m of the object over the interval h by taking a time-step duration that is of length h/2.

In further features, the method further includes the intermediate generalized position q_m is given by a midpoint generalized position at time t_m=t+h/2 by computing:

$q_m\leftarrow q_t+\frac{h}{2}K\left(q_t\right){\stackrel{.}{q}}_t,$

where K is a matrix-valued function.

In further features, the generalized force f over the interval h is given by M_m⁻¹f_m, where M_m is the midpoint mass matrix and f_m is the midpoint force.

In further features, the determining the end-of-interval generalized velocity q_t+h is determined by computing:

${\stackrel{.}{q}}_{t+h}\leftarrow {\stackrel{.}{q}}_t+{\mathrm{hM}}_m^{-1}{f}_m+M_m^{-1}\left(r_n+r_t\right).$

In further features, the determining the end-of-interval generalized velocity q_t+h is determined by computing an end-of-interval free-velocity q₁, given by:

${\stackrel{.}{q}}_f\leftarrow {\stackrel{.}{q}}_t+{\mathrm{hM}}_m^{-1}{f}_m.$

In further features, the objective function corresponding to minimizing energy over the interval h is given by:

${M_m^{-1/2}\left(M_m{\stackrel{.}{q}}_f+r_t\right)}_2.$

In further features, the determining the end-of-interval generalized position q_t+h is determined by computing:

$q_{t+h}\leftarrow q_m+\frac{h}{2}{\stackrel{.}{q}}_{t+h}.$

In further features, the computing the normal generalized reaction impulse r is approximated over the interval h by computing:

$r_n\leftarrow {\sum }_{j\in J_m}\left({\Lambda }_j^{+}+{\Lambda }_j^{-}\right)n_{j,m}^{*},$

where Δ_j⁺ and Δ_j⁻ are Lagrange multipliers and n_j,m^* are vectors.

In further features, the one term from each friction cone is determined in a friction cone by multiplying a tangential contact Jacobian by a tangential cone-position variable w and adding it to the result of multiplying a normal contact Jacobian by a normal cone-position variable γ.

In further features, the end-of-interval generalized position q_t+h and the end-of-interval generalized velocity q_t+h of the object are computed to perform one of system identification, trajectory optimization, learning control policies, and design of mechanical systems.

In further features, the method further includes, based on at least one of the end-of-interval generalized position q_t+h and the end-of-interval generalized velocity q_t+h of the object, at least one of steering a vehicle, accelerating the vehicle, and decelerating the vehicle.

In further features, the method further includes, based on at least one of the end-of-interval generalized position q_t+h and the end-of-interval generalized velocity q_t+h of the object, actuating a component of the object, wherein the object is a robot.

In a feature, a computer-implemented method, executed by one or more processors and memory, for determining for an object having a set of two or more contacts and having at time t an initial generalized position q_t and an initial generalized velocity q_t, the object's generalized acceleration q_t is described and includes: computing a generalized force f which is a summary of forces acting on the object other than forces due to the set of contacts; computing a normal generalized reaction force R acting on the object; computing a tangential generalized reaction force R_t acting on the object using the computed generalized force f to account for contacts associated with zero relative velocity at time t; and using the tangential generalized reaction force R_t to compute the generalized acceleration q_t of the object; where the computing the tangential generalized reaction force R_t further comprises applying the following constraints to an objective function maximizing power dissipated by friction: (a) associating each contact in the set of contacts with a friction cone in a space of generalized impulses to define a set of friction cones; (b) summing one term from each friction cone in the set of friction cones, minus the normal generalized reaction force R_n; and (c) constraining the tangential generalized reaction force R_t to be orthogonal to the normal generalized reaction force R_n and to have a non-positive inner product with the normal direction of each contact in the set of contacts.

In further features, the object is a robot.

In further features, the power dissipated by friction is given by minus the scalar product of the tangential generalized reaction force R_t and the initial generalized velocity q_t.

In further features, the end-of-interval generalized position q_t+h and the end-of-interval generalized velocity q_t+h of the object are computed to perform one of system identification, trajectory optimization, learning control policies, and design of mechanical systems.

In a feature, a computer-implemented method, executed by one or more processors and memory, for determining, in a mechanical system with an object having a set of two or more contacts, a tangential generalized reaction due to friction given a normal generalized reaction is described and includes: associating each contact of the object in the set of contacts with a friction cone and a normal direction in a space of generalized forces or impulses to define a set of friction cones; building a total friction cone as the sum of the set of friction cones and a normal cone as the cone generated by these normal directions; and constraining the sum of the normal generalized reaction and tangential generalized reaction to lie in the total friction cone while the tangential generalized reaction is constrained to be orthogonal to the normal generalized reaction and to lie in a polar cone of the normal cone; outputting the tangential generalized reaction; where each friction cone is a set of vectors that is closed under addition and multiplication by a non-negative scalar; and where the polar cone of a given friction cone is the set of vectors that have a non-positive inner product with all vectors in the given friction cone.

In a feature, the mechanical system includes a robot and one or more objects.

In a feature, a computer-implemented method, executed by one or more processors and memory, for determining, in a mechanical system with an object having a set of two or more contacts, a tangential generalized reaction due to friction given a normal generalized reaction is described and includes: associating each contact of the object in the set of contacts with a friction cone and a normal direction in a space of generalized forces or impulses, the associating defining a set of friction cones; and constraining the tangential generalized reaction to be a sum with one term from each friction cone in the set of friction cones, minus the normal generalized reaction; and outputting the tangential generalized reaction, where the tangential generalized reaction is constrained to be orthogonal to the normal generalized reaction and have a non-positive inner product with each contact's normal direction.

In further features, the mechanical system includes a robot and one or more objects.

In further features, the one term from each friction cone is determined in a friction cone by multiplying a tangential contact Jacobian by a tangential cone-position variable w and adding it to the result of multiplying a normal contact Jacobian by a normal cone-position variable γ.

In a feature, a system for determining, for an object having a set of two or more contacts and having at time t an initial generalized position q_t and an initial generalized velocity q_t, the object's end-of-interval generalized position q_t+h and end-of-interval generalized velocity q_t+h after an interval h is described and includes: one more processors; and memory including code that, when executed by the one or more processors perform to: compute a generalized force f over the interval, which is a summary of forces acting on the system other than forces due to the set of contacts, and which is used to define energy over the interval h; compute a normal generalized reaction impulse r_n acting on the object; compute a tangential generalized reaction impulse r_t acting on the object; and based on the tangential generalized reaction impulse r_t, compute the end-of-interval generalized position q_t+h and the end-of-interval generalized velocity q_t+h of the object, where the computing the tangential generalized reaction impulse r_t further includes applying the following constraints to an objective function minimizing energy over the interval h: (a) associating each contact in the set of contacts with a friction cone in a space of generalized impulses to define a set of friction cones; (b) summing one term from each friction cone in the set of friction cones, minus the normal generalized reaction impulse r_n; and (c) constraining the tangential generalized reaction impulse r_t to be orthogonal to the normal generalized reaction impulse r_n and to have a non-positive inner product with the normal direction of each contact in the set of contacts.

In further features, the memory further includes code for a learning model that, when executed by the one or more processors, performs the following functions including computing an intermediate generalized position q_m of the object over the interval h by taking a time-step duration that is of length h/2.

In further features, the intermediate generalized position q_m is given by a midpoint generalized position at time t_m=t+h/2 by computing:

$q_m\leftarrow q_t+\frac{h}{2}K\left(q_t\right){\stackrel{.}{q}}_t,$

where K is a matrix-valued function.

In further features, the generalized force over the interval his given by M_m⁻¹f_m, where M_m is the midpoint mass matrix and f_m is the midpoint force.

In further features, the determining the end-of-interval generalized velocity q_t+h is determined by computing:

${\stackrel{.}{q}}_{t+h}\leftarrow {\stackrel{.}{q}}_t+{\mathrm{hM}}_m^{-1}{f}_m+M_m^{-1}\left(r_n+r_t\right).$

In further, the determining the end-of-interval generalized velocity q_t+h is determined by computing an end-of-interval free-velocity q f given by:

${\stackrel{.}{q}}_f\leftarrow {\stackrel{.}{q}}_t+{\mathrm{hM}}_m^{-1}{f}_m.$

In further features, the objective function corresponding to minimizing energy over the interval h is given by:

${M_m^{-1/2}\left(M_m{\stackrel{.}{q}}_f+r_t\right)}_2.$

In further features, the determining the end-of-interval generalized position q_t+h is determined by computing

$q_{t+h}\leftarrow q_m+\frac{h}{2}{\stackrel{.}{q}}_{t+h}.$

In further features, the computing the normal generalized reaction impulse r_n is approximated over the interval h by computing:

$r_n\leftarrow {\sum }_{j\in J_m}\left({\Lambda }_j^{+}+{\Lambda }_j^{-}\right)n_{j,m}^{*},$

where Δ_j⁺ and Δ_j⁻ are Lagrange multipliers and n_j,m^* are vectors.

In further features, the one term from each friction cone is determined in a friction cone by multiplying a tangential contact Jacobian by a tangential cone-position variable w and adding it to the result of multiplying a normal contact Jacobian by a normal cone-position variable γ.

In further features, the end-of-interval generalized position q_t+h and the end-of-interval generalized velocity q_t+h of the object is computed to perform one of system identification, trajectory optimization, learning control policies, and design of mechanical systems.

In a feature, a system, for determining for an object having a set of two or more contacts and having at time t an initial generalized position q_t and an initial generalized velocity q_t, the object's generalized acceleration q_t, is described and includes: one more processors; memory including code that, when executed by the one or more processors perform to: compute a generalized force f which is a summary of forces acting on the system other than forces due to the set of contacts; compute a normal generalized reaction force R_n acting on the object; compute a tangential generalized reaction force R_t acting on the object using the computed generalized force f to account for contacts associated with zero relative velocity at time t; and based on the tangential generalized reaction force R_t, compute the generalized acceleration q_t of the object, where the computing the tangential generalized reaction force R_t further includes applying the following constraints to an objective function maximizing power dissipated by friction: (a) associating each contact in the set of contacts with a friction cone in a space of generalized impulses to define a set of friction cones; (b) summing one term from each friction cone in the set of friction cones, minus the normal generalized reaction force R; and (c) constraining the tangential generalized reaction force R_t to be orthogonal to the normal generalized reaction force R_n and to have a non-positive inner product with the normal direction of each contact in the set of contacts.

In further features, the mechanical system is a robot and one or more objects.

In further features, the power dissipated by friction is given by minus the scalar product of the tangential generalized reaction force R_t and the initial generalized velocity q_t.

In further features, the end-of-interval generalized position q_t+h and the end-of-interval generalized velocity q_t+h of the object is computed to perform one of system identification, trajectory optimization, learning control policies, and design of mechanical systems.

Further areas of applicability of the present disclosure will become apparent from the detailed description, the claims and the drawings. The detailed description and specific examples are intended for purposes of illustration only and are not intended to limit the scope of the disclosure.

BRIEF DESCRIPTION OF THE DRAWINGS

The present disclosure will become more fully understood from the detailed description and the accompanying drawings, wherein:

FIG. 1 illustrates an example of a system architecture in which the methods according to the present disclosure may be performed;

FIG. 2 is a flow diagram depicting an example method for determining the generalized position and generalized velocity of the motion of an object;

FIGS. 3A, 3B, 3C, and 3D depict a conceptual representation of a tangential reaction of two individual contacts;

FIG. 4 further depicts the conceptual representation of a tangential reaction shown in FIGS. 3A, 3B, 3C and 3D;

FIG. 5A and FIG. 5B sets forth an implementation in pseudo-code of the example method for determining the motion of objects in FIG. 2;

FIG. 6 is a flow diagram depicting an example method for determining the generalized acceleration of the motion of an object; and

FIG. 7 sets forth a flow diagram overlaying the methods for determining object motion by computing the tangential generalized reaction impulse and tangential generalized reaction force set forth in FIGS. 2 and 6, respectively.

In the drawings, reference numbers may be reused to identify similar and/or identical elements.

DETAILED DESCRIPTION

The systems and methods for determining reactions due to friction in multi-contact systems disclosed hereunder may be implemented within a system 2 architected as illustrated in FIG. 1, which includes one or more servers (server modules) 15 and one or more (e.g., autonomous) objects (modules) 6, which include one or more processors 11 and memory 12, such as autonomous vehicle 6a or autonomous robot 6b, located using for example a positioning system 14 installed on the autonomous object 6. The positioning system 14 may include a geo-positioning system (GPS), cellular positioning system, indoor positioning system (IPS), including beacons, radio frequency identifier (RFID), WiFi and geomagnetic, or a combination thereof. Servers 15 and autonomous objects 6 may communicate over a network 5 (which may be wireless and/or wired) such as the Internet for data exchange. Each server 15 includes one or more processors such as processor 11 and memory 12 such as a hard disk, the processor(s) configured to perform its functions via execution of code.

In one embodiment, the server 15a (with processor 11a and memory 12a) may include a map module 16 in memory 12a containing outdoor and indoor maps, and the server 15b (with processor 11b and memory 12b) may include a trajectory estimation module 17 in memory 12b that computes a trajectory of autonomous objects 6 which accounts for reactions due to friction. In one implementation, the methods of the present disclosure operate on server 15b of system 2. In another implementation, it is noted that the two servers 15a and 15b may be merged/integrated in one server. In yet another implementation, the functionality of the servers 15 may be merged with the autonomous objects 6.

These and other mechanical systems (e.g., autonomous objects 6 in FIG. 1) are described herein in terms of the 3D (three-dimensional) coordinates of predetermined points of its links or joints. The time-derivative of any such coordinate may be referred to as a real-world velocity, and a 3D force acting on any such point may be referred to as a real-world force. Alternatively, the configuration of a mechanical system may be defined using some other parameters, like the angles made by the joints of a robot arm, and such descriptions may be referred to as generalized coordinates and their time derivatives may be referred to as generalized velocities. Further, the forces and torques acting on a mechanism may be summarized using a vector of the same dimension as the mechanism's generalized velocity, which is known as a generalized force; this is done in such a way that the product of a component of the generalized velocity with the corresponding component of the generalized force has the units of power (e.g., energy per unit time). For instance, if a generalized velocity component is an angular velocity, then the corresponding generalized force component is a torque. A real-world coordinate system (or space) for a particle is a possible choice of a generalized coordinate system for the particle.

Further, a real-world description of mechanical systems is about 3D forces acting at specific 3D points in a 3D world (or 2D forces acting at specific 2D points in a 2D world), whereas it is not necessary to state where a generalized force acts. One useful consequence of this observation is that while the real-world description of a rigid body making contact with another body over an area may involve the specification of a pressure field over that contact area, which gives the pressure as a function of position, only a single finite-dimensional vector (the generalized force) is needed to describe everything that needs to be known about that pressure field to predict the motion of the bodies over the area.

2. Determining Object Motion by Computing Reaction Impulse

FIG. 2 is a flow diagram, which may be implemented by one or more modules of the system 2 in FIG. 1 or one or more processors, that depicts an example method for determining the generalized position and velocity of the motion of an object, such as autonomous object 6, that accounts for reactions due to friction in multi-contact systems. In one embodiment, the method is performed by trajectory estimation module 17 shown in FIG. 1.

The method depicted in the flowchart of FIG. 2 employs a time-stepping function, such as that described by Alexandre Charles, Fabien Casenave, and Christoph Glocker, in “A catching-up algorithm for multibody dynamics with impacts and dry friction”, Computer Methods in Applied Mechanics and Engineering 334 (2018): 208-237, which is incorporated herein in its entirety (hereinafter “Charles et al. (2018)”). Unlike the time-stepping function described by Charles et al. (2018), the time-stepping function depicted in FIG. 2 computes the normal reaction using a different yield set (as defined by Charles et al. (2018)) and computes a tangential reaction due to friction.

In addition, the time-stepping function depicted in FIG. 2 may access several associated functions and/or parameters that describe the mechanical system (e.g., a robot and one or more objects that the robot may interact with): these may for instance be given as arguments to the time-stepping function, be member functions of an object passed as argument, or just exist as names in the local scope of the time-stepping function. Specifically, the time-stepping function may use functions determining:

• • (a) the mass matrix M(q) at a given configuration. Algorithms for computing the mass matrix and its inverse may be described in Roy Featherstone in “Rigid Body Dynamics Algorithms” Springer, 2008 (hereinafter “Featherstone et al. (2008)”), and by Justin Carpentier in “Analytical Inverse of the Joint Space Inertia Matrix Rapport LAAS n° 18125, 2018, available on the Internet at hal.laas.fr/hal-01790934v2, which are both incorporated herein in their entireties;
  • (b) the total generalized force or fictitious force f(t, q, q) acting on the system, such as Coriolis, centrifugal, gravitational and external control forces; and
  • (c) the set of indices of active constraints J(q), which corresponds to the set of pairs of points or surfaces that touch or penetrate at a given configuration, or more generally, to the indices of some constraint functions that take a non-positive value at that configuration.

Moreover, for each active constraint j∈j(q), the time-stepping function depicted in FIG. 2 may use one or more functions that determine:

• • (a) the generalized normal direction n(q);
  • (b) Jacobians H_n,j(q) and H_t,j(q) for the normal and tangential positions, respectively, of contact points (situations involving area contacts are described below); and
  • (c) the restitution coefficient e_j and friction coefficient μ_j.

The restitution coefficient e_j and the friction coefficient μ_j may be fixed for a given contact point, but they may reasonably be determined as functions of position and of velocity.

Further, the time-stepping function depicted in FIG. 2 may use linear algebra functions, such as for inverting and taking square roots of matrices; and convex optimization functions, such as for solving quadratic programs and second-order cone programs.

Referring again to the time-stepping function depicted in FIG. 2, the following are received as input by the one or more processors or one or more modules at 102: (a) the current time t; (b) the current generalized position q_t; (c) the current generalized velocity q_t; and (d) a positive number h giving the duration of the time step (or time-step duration). In various implementations, the generalized position and velocity may be represented as arrays of common dimension n, although it may be advantageous to use quaternion representations for rotations, in which the dimension of the position is larger than that of the velocity. In various implementations, the positions and velocities may be represented as multi-dimensional arrays; for example, when using a GPU (Graphics Processing Unit) and training a reinforcement learning system, and processing several positions and velocities is performed in parallel.

At 104 of the time-stepping function depicted in FIG. 2, the one or more processors or one or more modules compute midpoint values. More specifically at 104, in a two-dimensional world, the midpoint generalized position q_m at time t_m=t+h/2 (see Charles et al. (2018)) may be estimated by computing:

$q_m\leftarrow q_t+\frac{h}{2}{\stackrel{.}{q}}_t,$

for the corresponding midpoint mass matrix M_m, midpoint force f_m and midpoint set of active constraints J_m, respectively:

$M_m\leftarrow M\left(q_m\right),f_m\leftarrow f\left(t_m,q_m,{\stackrel{.}{q}}_t\right),J_m\leftarrow J\left(q_m\right).$

More generally, approximate values may be computed for the midpoint generalized position q_m at time t_m=t+h/2 where midpoint values are computed by computing:

${\stackrel{.}{q}}_t{q}_m\leftarrow q_t+\frac{h}{2}K\left(q_t\right){\stackrel{.}{q}}_t,$

where K is a matrix-valued function of generalized position. For instance, if the generalized position q is given by Euler parameters (unit quaternions) then an appropriate choice of matrix function K is given in equation 4.13 of Featherstone et al. (2008).

At 106 of the time-stepping function depicted in FIG. 2, the following are computed by the one or more processors or one or more modules for each active constraint j Elm:

• • (a) the corresponding normal direction and friction cone Jacobians are computed, respectively, as follows:

$n_{j,m}^{*}\leftarrow n_j^{*}\left(q_m\right),H_{n,j,m}\leftarrow H_{n,j}\left(q_m\right),H_{t,j,m}\leftarrow H_{t,j}\left(q_m\right);\mathrm{and}$

• • (b) the corresponding restitution coefficient e₁ and friction coefficient μ_j are computed.

At 108 of the time-stepping function depicted in FIG. 2, the free velocity is computed by the one or more processors or one or more modules using the approximation q_t which is the generalized velocity that would be obtained at the time t+h if there were no active contacts:

${\stackrel{.}{q}}_f\leftarrow {\stackrel{.}{q}}_t+{\mathrm{hM}}_m^{-1}{f}_m.$

At 110 of the time-stepping function depicted in FIG. 2, if the free velocity is admissible in the sense that the inequality n_j,m^*Tq₁, ≥0 holds for all j in J_m, then the normal reaction impulse r to be solved for at 112 will be zero, therefore the method continues at 116 otherwise the method continues at 112.

At 112 of the time-stepping function depicted in FIG. 2, a normal reaction impulse r_n is computed. More specifically at 112, the normal component of the generalized reaction impulse is approximated over the interval [t, t+h), such as using an impact model with the method disclosed by Charles et al. (2018), using the Poisson impact model disclosed by Christoph Glocker, in “On frictionless impact models in rigid-body systems”, Philosophical Transactions of the Royal Society of London. Series A: Mathematical, Physical and Engineering Sciences 359.1789 (2001): 2385-2404, which is incorporated herein in its entirety. In various implementations, other models could be applied such as the Newton, Moreau or LZB impact models as disclosed by Ngoc Son Nguyen and Bernard Brogliato, in “Comparisons of multiple-impact laws for multibody systems: Moreau's law, binary impacts, and the LZB approach”, Advanced Topics in Nonsmooth Dynamics, Transactions of the European Network for Nonsmooth Dynamics, Springer International Publishing AG, pp. 1-45, 2018, 10.1007/978-3-319-75972-2_1 (published on the Internet at hal.inria.fr/hal-019325511), which is incorporated herein in its entirety.

The Poisson impact model applied at 112 includes or consists of a compression phase, where the impulse is sufficient to just stop any penetrating motion as in a fully inelastic collision, and a decompression phase, for which the impulse associated with contact j is at least a factor e₁ larger than the corresponding impulse in the decompression phase. Specifically, for the compression phase the free velocity q f is projected into the space of velocities compatible with the constraints, by solving the quadratic program:

• • minimize

$\frac{1}{2}{\left(u-{\stackrel{.}{q}}_f\right)}^T{M}_m\left(u-{\stackrel{.}{q}}_f\right)$

with respect to μ ∈Rⁿ,

• • subject to n_j,m^*T μ≥0 for all fin J_m.

Let Δ_j⁻ be the Lagrange multiplier associated with constraint j at solution of this quadratic program. The quantity Δ_j⁻n_j,m^* approximates the normal generalized reaction impulse associated with the compression phase for contact j over time interval [t, t+h). For the decompression phase, the following linear complementarity problem is solved for Lagrange multipliers Δ_j⁺ for j in J_m:

$0\le b_j+\sum _{k\in J_m}{G}_{\mathrm{jk}}\left({\Lambda }_k^{+}+{\Lambda }_k^{-}\right)\perp {\Lambda }_j^{+}-e_j{\Lambda }_j^{-}\ge 0$ $\mathrm{where}$ $b_j:=n_{j,m}^{*T}{\stackrel{.}{q}}_f,G_{\mathrm{jk}}:=n_{j,m}^{*T}{M}_m^{-1}{n}_{k,m}^{*}$

for j and k in J_m. Under the reasonable assumptions that the vectors (n_k,m^*: k ∈J_m) are linearly independent and the mass matrix is symmetric positive definite, this linear complementarity problem has a unique solution, which may be determined by solving the quadratic program:

• • minimize

${\sum }_{j\in J_m}\left(b_j+{\sum }_{k\in J_m}{G}_{\mathrm{jk}}\left({\Lambda }_k^{+}+{\Lambda }_k^{-}\right)\right)\left({\Lambda }_j^{+}-e_j{\Lambda }_j^{-}\right)$

with respect to Δ_j⁺∈R for j in J_m,

• • subject to

$b_j+{\sum }_{k\in J_m}{G}_{\mathrm{jk}}\left({\Lambda }_k^{+}+{\Lambda }_k^{-}\right)\ge 0$

and Δ_j⁺−e_jΔ_j⁻≥0 for all j in J_m.Finally, the normal reaction impulse r_n is approximated over interval [t, t+h) by setting

$r_n\leftarrow \sum _{j\in J_m}\left({\Lambda }_j^{+}+{\Lambda }_j^{-}\right)n_{j,m}^{*}.$

At 114 of the time-stepping function depicted in FIG. 2, a tangential reaction impulse is computed by the one or more processors or one or more modules. FIGS. 3A, 3B, 3C, and 3D depict a conceptual representation of a tangential reaction of two individual contacts.

In FIG. 3A, there is represented a rigid rod in a two-dimensional (20) space with a first individual contact 302 and a second individual contact 304. In FIG. 3B there is represented a first friction cone 303 and a second friction cone 305 in a 3D space of generalized reaction forces/impulses with axes r₁, r₂ and r₃, corresponding to individual contacts 302 and 304 shown in FIG. 3A, respectively. Friction cones 303 and 305, which represent the forces that may be applied through frictional contacts, have normal directions 304 and 306, respectively. In FIG. 3C there is represented the sum 308 of the friction cones 303 and 305 in FIG. 3B in the 3D space r₁r₂r₃ of generalized reaction forces/impulses.

In FIG. 3D, there is represented the sum 308 of the friction cones 303 and 305 minus the generalized normal reaction r_n. Further in FIG. 3D, point 0 is the origin of the 3D space r₁r₂r₃ of generalized reaction forces/impulses (i.e., the point of zero tangential reaction). The line 310 in FIG. 3D is the set of generalized reactions that are orthogonal the generalized normal reaction r_n and such that the inner product with the normal directions 304 and 306 (shown in FIG. 3B) are non-positive. Further, line 310 crosses the sum 308 of the friction cones 303 and 305, and passes through point 0. It follows that segment AB of line 310 represents the set of valid tangential reactions.

Referring to FIG. 4 which further depicts the conceptual representation of a tangential reaction shown in FIGS. 3A, 3B, 3C and 3D, the total generalized reaction r 401 (i.e., the sum of the normal generalized reaction r_n and the tangential generalized reaction Ft) lies in the total friction cone F 402, which is the sum of the friction cones of the individual contacts (e.g., friction cones 303 and 305 of individual contacts 302 and 304, respectively, shown in FIGS. 3A and 3B). Typically, generalized reactions occupy a high-dimensional space; the space illustrated in FIG. 3 is portrayed as if it were 30 space for illustrative purposes only because higher-dimensional space may not be readily diagrammed. Furthermore, the 30 space in FIG. 3 does not represent a 30 space of real-world forces used in conventional formulations of Coulomb's law of friction.

With continued reference to FIG. 4, the total friction cone F 402 contains the normal cone N 404 generated by the normal directions of the individual contacts (e.g., normal directions 304 and 306 of friction cones 303 and 305 of individual contacts 302 and 304, respectively, shown in FIGS. 3A and 3B). The normal cone N 404 is represented as a flat “fan”, so the polar cone T 406 to normal cone N 404 is the intersection of half spaces, whose surface includes or consists of two planes denoted by the polar cone T 406. By Moreau's theorem about decomposition into cones, any reaction r can be written as the sum of the projection r_t of r onto polar cone T 406 and the projection r of r onto normal cone N 404, and these projections are orthogonal. (Note the projection and orthogonality here are weighted by the inverse mass matrix; they are not the usual Euclidean projection, unless the mass matrix happens to be a multiple of the identity matrix.) In effect, the tangential reaction r_t 405 may be determined by searching over projections of the total friction cone F 402 onto the polar cone T 406, which are orthogonal to a given normal reaction impulse r_r, 403.

Referring again to 114 in FIG. 2, a tangential reaction impulse r_t over interval [t, t+h) is computed by the one or more processors or one or more modules. Using a friction cone in the space of generalized forces or impulses for each contact j Elm, which is the set F_j of generalized forces or impulses of the form (hereinafter “Equation (1)”):

$H_{t,j,m}{w}_j+H_{n,j,m}{\gamma }_j\mathrm{where}{w_j}_2\le {\mu }_j{\gamma }_j,$

in which a vector w_j is introduced, which is in R² for a point contact in a 30 world, and a scalar γ₁, and in which the inequality says that the pair (w_j, γ_j) lie in a circular cone, also known as a Lorentz cone. The sum of the normal reaction impulse r_n and the tangential reaction impulse r_t are constrained to lie in the sum over these friction cones. Equivalently, the tangential generalized reaction is constrained to be a sum with one term from each friction cone, minus the normal generalized reaction (hereinafter “Constraint (1)”):

$\begin{array}{cc}{r}_t=\left({\sum }_{j\in J_m}{H}_{t,j,m}{w}_j+H_{j,n,m}{\gamma }_j\right)-r_n\mathrm{and}{w_j}_2\le {\mu }_j{\gamma }_j\mathrm{for}j\mathrm{in}{J}_m,& \left(1\right)\end{array}$

where the one term from each friction cone is determined in a friction cone by multiplying a tangential contact Jacobian by a tangential cone-position variable w and adding it to the result of multiplying a normal contact Jacobian by a normal cone-position variable γ (which in an example the one term from each friction cone may be determined using the following Python code: Ht@w[−1]+Hn*gamma[−1] in which w and gamma are lists whose elements are of type cvxpy.Variable).

Moreover, the tangential and normal reactions may be “tangential” and “normal” in an appropriate sense. Regarding friction, the “tangential space” may be the subset of the real-world space vector space R³ that is orthogonal to the real-world direction normal to the contacting surfaces. However, a notion of “tangential” relative to the set of normal generalized reaction vectors may be sought here. As the normal generalized reaction lies in the closed convex cone

$K:=\left\{\sum _{j\in J_m}{\lambda }_j{n}_j^{*}:{\lambda }_j\ge 0\forall j\in J_m\right\},$

which is not in general a vector space (because only non-negative linear combinations of the normal directions are allowed), the “appropriate sense” is defined based on Moreau's Decomposition Theorem (see Jean-Jacques Moreau, “Décomposition orthogonale d'un espace hilbertien selon deux cones mutuellement polaires”, Comptes rendus hebdomadaires des seances de I'Academie des sciences 255 (1962): 238-240, which is incorporated herein in its entirety (hereinafter “Moreau (1962)”), as is now explained. Given closed convex cone C, the polar cone C° includes or consists of all vectors that have a non-positive inner product with all vectors in the closed convex cone C. Given a closed convex cone C in a real Hilbert space H, Moreau's Decomposition Theorem says that any vector in H can be written uniquely as the sum of a vector in cone C and a vector in the polar cone CO, in such a way that these vectors are orthogonal. Thus, the tangential generalized reaction is constrained to be orthogonal to the normal generalized reaction and have a non-positive inner product with each contact's normal direction (so that it lies in the polar cone K°), using the appropriate inner product for generalized reactions, which involves the inverse mass matrix (hereinafter “Constraint (2)”):

$\begin{array}{cc}{r}_n^T{M}_m^{-1}{r}_t=0\mathrm{and}{n}_{j,m}^{*T}{M}_m^{-1}{r}_t\le 0\mathrm{for}j\mathrm{in}{J}_m.& \left(2\right)\end{array}$

The use of Moreau's Decomposition Theorem may have been used to define the notion of “tangential generalized reaction due to friction” without referring to Moreau (1962) in Alexandre Charles, “Dynamique des systemes de so/ides rigides avec impacts et frottement” Dissertation, Aix-Marseille, 2013, which use differs from the case of multiple contacts set forth herein as that use allows the tangential reaction to range only over a slice of the full friction cone given by Constraint (1).

Finally at 114, the one or more processors or one or more modules select from among the tangential reactions compatible with Constraint (1) and Constraint (2). This is done in such a way as to maximize the energy dissipated by the tangential reaction over time interval [t, t+h), or equivalently minimizing the energy at (or just a predetermined period before) the final time t+h. The idea of using a principle of maximum dissipation to select among frictional reactions is described in Jean-Jacques Moreau, “Application of convex analysis to some problems of dry friction”, Trends in applications of pure mathematics to mechanics, Pitman, 1977, which is incorporated herein in its entirety. Advantageously, a unique solution may be found for the tangential generalized reaction by maximizing the energy dissipated over an interval of duration h>0 because this involves the minimization of a strictly convex function. To derive the energy at the final time, the integral of the equation of motion over the time interval can be approximated by

$M_m\left({\stackrel{.}{q}}_{t+h}-{\stackrel{.}{q}}_t\right)=h{f}_m+r_n+r_t.$

Recalling the definition of the free velocity q₁, =q_t+hM_m⁻¹f_m, it follows that

$M_m{\stackrel{.}{q}}_{t+h}=M_m{\stackrel{.}{q}}_f+r_n+r_t.$

Thus, the energy at time t+h may be approximated by

$\frac{1}{2}{\stackrel{.}{q}}_{t+h}^T{M}_m{\stackrel{.}{q}}_{t+h}=\frac{1}{2}{\left(M_m{\stackrel{.}{q}}_f+r_n+r_t\right)}^T{M}_m^{-1.}\left(M_m{\stackrel{.}{q}}_f+r_n+r_t\right).$

When constraint (2) holds, so that r_n and r_t are orthogonal, minimizing this expression with respect to r_t is the same as minimizing

${\left(M_m{\stackrel{.}{q}}_f+r_t\right)}^T{M}_m^{-1}\left(M_m{\stackrel{.}{q}}_f+r_t\right).$

Moreover, minimizing a non-negative expression is the same as minimizing its square root. Therefore, the tangential reaction can be computed by solving the following mathematical program (hereinafter “Equation (3)”):

• • (3) minimize ∥M_m^−1/2(M_mq_f+r_t)∥₂ with respect to r_t, w₁, γ₁ for j in J_m subject to

Constraint (1) and Constraint (2) in which M_m^−1/2 is the matrix square root of M_m⁻¹. This type of program may be referred to as a second-order cone program, and may be solved, for example, as described in the CVXPY library (see for example S. Diamond and S. Boyd, in “CVXPY: A Python-embedded modeling language for convex optimization”, Journal of Machine Learning Research, 2016, and Akshay Agrawal, Robin Verschueren, Steven Diamond, and Stephen Boyd, “A rewriting system for convex optimization problems”, Journal of Control and Decision 5, no. 1 (2018): 42-60, which are incorporated herein in its entirety (hereinafter “Agrawal et al. (2018)”)). To apply these algorithms, Equation (3) may be re-written in canonical form, in which the objective is a linear function. Using results disclosed by M. S. Lobo, L. Vandenberghe, S. Boyd, & H. Lebret, in “Second-order cone programming” Linear algebra and Applications, v. 284, pp. 193-228, 1998, which is incorporated herein in its entirety (hereinafter “Lobo et al. (1998)”, one more scalar variable s and one more cone constraint are introduced (hereinafter “Equation (4)”):

$\begin{array}{cc}{M_m^{-1/2}\left(M_m{\stackrel{.}{q}}_f+r_t\right)}_2\le s,& \left(4\right)\end{array}$

and then one the second-order cone program can be written in canonical form as

(5) minimize s with respect to s, r_t, w₁, γ_j for j in J_m subject to (1), (2) and (4). Equation (5) is thus a summary of a method of finding the tangential reaction impulse r_t.

At 116 of the time-stepping function depicted in FIG. 2, the end-of-step generalized velocity and the end-of-step generalized position at (or just before) time t+h is approximated by

${\stackrel{.}{q}}_{t+h}\leftarrow {\stackrel{.}{q}}_f+M_m^{-1}\left(r_n+r_t\right)\mathrm{and}{q}_{t+h}\leftarrow q_m+\frac{h}{2}{\stackrel{.}{q}}_{t+h}.$

At 118 of the time-stepping function depicted in FIG. 2, the generalized velocity and position computed at 116 are output by the one or more processors or one or more modules, which output may be used in applications involving objects with multiple contacts as described herein. More specifically at 118, the time-stepping function depicted in FIG. 2 returns: (a) the time t+h after the step; (b) an approximation of the generalized position q_t+h at that time; and (c) an approximation of the generalized velocity q_t+h at that time (or more strictly, “just before” that time, noting that there might be an impact at time t+h).

FIG. 5A and FIG. 5B includes an implementation in pseudo-code 502 and 504, respectively, of the example method for determining the motion of objects in FIG. 2. The import CVXPY in 502 used to solve the quadratic programs and second-order cone programs embodiments of which are described above. Also, np.array in 502 may refer to a data structure used to store and manipulate multidimensional arrays, such as disclosed at numpy.org/doctstable/reference/generated/numpy.array.htmi.

In various implementations, an initialization occurs as part of 114 in FIG. 2 as follows (hereinafter “Initialization Process”). Given Equation (1), which provides a set of generalized forces F_j for friction cones for contact index j in the set of active contacts J, the following sequence of steps may be applied to any convex cone F_j to give another convex cone j. In this alternate embodiment, the method set forth in 114 in FIG. 2 may be applied by replacing cones F_j with cones {tilde over (F)}_j with the following three-step process:

• • 1. Define a shifted cross section C_j of convex cone F_j as:

$C_j^{*}:=\left\{f-n_j^{*}:f\in F_j,{\left(f-n_j^{*}\right)}^T{M}^{-1}{n}_j^{*}=0\right\},$

• • where f is a generalized force or impulse and M⁻¹ is the inverse mass matrix, which if the set C/is constructed (i) for Determining Object Motion by Computing Reaction Impulse as set forth in this Section 2, inverse mass matrix M⁻¹ may be set to M_m⁻¹ for use in time-stepping, or (ii) for Determining Object Acceleration by Computing Reaction Force as set forth in Section 6, inverse mass M⁻¹ may be set matrix to M⁻¹(q) where q is the configuration at the time acceleration is to be computed.
  • 2. Find a projection matrix P which maps generalized force f to a projected generalized force P* f that is orthogonal to each of the normal directions n_j for j ∈J, while minimizing the difference:

${\left(f-P^{*}f\right)}^T{M}^{-}\left(f-P^{*}f\right).$

• • Further, let A denote the matrix whose rows are n_j^T for j E j (i.e., it is a k×d matrix if k is the cardinality of j and d is the dimension of n_j), then an explicit expression for the projection matrix P* is:

$P^{*}=I-{A^T\left(A{M}^{-1}{A}^T\right)}^{-1}A{M}^{-1}.$

• • 3. Finally, define the closed convex cone as (hereinafter “Equation (2)”):

${\tilde{F}}_j:=\left\{x\left(P^{*}c+n_j^{*}\right)❘x\ge 0,c\in C_j^{*}\right\}.$

Explicitly, the Initialization Process' alternative friction cones {tilde over (F)}_j may be given by:

${\tilde{F}}_j=\left\{P^{*}{H}_{t,j}w+P^{*}{H}_{n,j}\gamma +x{n}_j^{*}❘x\ge 0,{w}_2\le {\mu }_j\gamma ,{\left(H_{t,j}w+H_{n,j}\gamma -x{n}_j^{*}\right)}^T{M}^{-1}{n}_j^{*}=0\right\}.$

Those skilled in the art will appreciate that friction cones need not have circular or elliptical cross sections. The above three-step process extends naturally when F_j is any convex cone. However, if using other types of cones in the disclosed methods, a more general optimization method may be needed than second-order cone programming.

3. Alternative Embodiments

3.A Formulation of Constraints

In the methods of the present disclosure, the normal reaction r_n is of the form Σ_j∈Jmλ_jn_j for some multipliers λ_j which are non-negative (for instance, λ_j=λ_j⁺+λ_j⁻ for the Poisson impact model described above). For such normal reactions, Constraint (2) above can be rewritten as:

$\begin{array}{cc}\left(n_{j,m}^{*T}{M}_m^{-1}{r}_t=0\mathrm{if}{\lambda }_j>0\mathrm{and}{n}_{j,m}^{*T}{M}_m^{-1}{r}_t\le 0\mathrm{if}{\lambda }_j=0\right)\mathrm{for}j\mathrm{in}{J}_m.& \left(2^{\prime }\right)\end{array}$

Also, various quantities may be multiplied by powers of mass matrix M_m. For instance, the variable v_t=M_m⁻¹r_t, may also be solved for, which may be used to solve the following second-order cone program for variables v_t, w_j, γ₁ for j in J_m:

• • minimize ∥M_m^1/2 (q_f+v_t)∥₂ subject to M_mv_t=(Σ_j∈JmH_t,j,mw_j+H_n,j,mγ_j)−r_n
  • where ∥w_j∥₂≤μ_jγ_j for j in J_m and
  • (n_j,m^*Tv_t=0if λ_j>0 and n_j,m^*Tv_<≤0if λ_j=0) for j in J_m.

3.B Numerical Optimization

In an embodiment, the time-stepping function of the present disclosure is implemented using quadratic programming and second-order cone programming functions from the CVXPY library, but the present application is also applicable to other functions. Mechanical simulators may use non-linear Gauss-Seidel algorithms (see Charles et al. (2018)) and per contact iteration methods (see Hwangbo, J., Lee, J., & Huffer, M., “Per-contact iteration method for solving contact dynamics”, in IEEE Robotics and Automation Letters, 3(2), 895-902, 2018, which is incorporated herein in its entirety), and both may be adapted to the optimization problems encountered herein. Second-order cones may be approximated by linear cones and use methods for linear programming or linear complementarity (see: Stewart, David E. “Rigid-body dynamics with friction and impact.” SIAM Review 42.1 (2000): 3-39; and M. Anitescu and F. Potra, “Formulating dynamic multi-rigid-body contact problems with friction as solvable linear complementarity problems,” Nonlinear Dynamics, vol. 14, no. 3, pp. 231-247, Nov. 1997, which are incorporated herein in its entirety).

Two families of methods for numerical integration of mechanical systems that may undergo impacts are: (i) event-driven or event-tracking methods, which use high-order integration methods in times of smooth motion and accurately approximate the times of non-smooth motion (see Chapter 8 of Acary, Vincent, and Bernard Brogliato, “Numerical methods for nonsmooth dynamical systems: applications in mechanics and electronics”, Springer Science & Business Media, 2008, which is incorporated herein in its entirety (hereinafter “Acary et al. (2008)”)); and (ii) time-stepping or event-capturing methods, which can cope with multiple impacts in a single time step (see Chapter 10 of Acary et al. (2008)), but which may only be accurate to low order, as they do not try to determine event times. Non-smooth generalized-alpha methods may be used (e.g., see Galvez, Javier, Federico J. Cavalieri, Alejandro Cosimo, Olivier Bruls, and Alberto Cardona. “A nonsmooth frictional contact formulation for multibody system dynamics.” International Journal for Numerical Methods in Engineering 121, no. 16 (2020): 3584 3609, which is incorporated herein in its entirety), which split the motion into a (non-smooth) impulsive contribution integrated with first-order accuracy, and a smooth contribution integrated with higher-order accuracy. While the present application provides the example of a time-stepping function, the present application (involving determining tangential reactions by summing friction cones, imposing orthogonality to the generalized normal, constraining to the polar cone for the space of normal reactions, and maximizing dissipation) may also be applied in conjunction with other families of integration method.

3.D Different Friction Models

The methods of the present application include friction models in which the set of friction forces at a contact is given by a cone that contains the generalized normal direction. The friction force may thus be directionally dependent (e.g., see Bullock, James MR, Patrick Drechsler, and Walter Federle, “Comparison of smooth and hairy attachment pads in insects: friction, adhesion and mechanisms for direction-dependence,” Journal of Experimental Biology 211, no. 20 (2008): 3333-3343, which is incorporated herein in its entirety), and it need not be symmetric to 180-degree rotations of the real-world force (in the case of sticking) or velocity (in the case of sliding). Moreover, while the time-stepping function described with reference to FIG. 2 may or may not explicitly model any difference between static and kinetic friction (e.g., see p. 118 of Kane, Thomas R., and David A. Levinson. Dynamics, theory and applications. McGraw Hill, 1985 (hereinafter “Kane et al. (1985)”), which is incorporated herein in its entirety), nor any dependence of kinetic friction on velocity (e.g., see O.M. Braun and M. Peyrard, “Dependence of kinetic friction on velocity: Master equation approach,” Phys. Rev. E 83, 046129, 2011, which is incorporated herein in its entirety), the dependence can be approximated by making the friction coefficient μ_j of a contact be a function of a contact speed function v_j(q, q), which may give the real-world relative speed at a point contact. For example, one might set

${\mu }_j={\mu }_j^k+\left({\mu }_j^s-{\mu }_j^k\right)\exp \left(-{v_j\left(q_m,{\stackrel{.}{q}}_t\right)}^2/k^2\right)$

for a suitable constant k, in the midpoint values step of the time-stepping function, to model static and kinetic friction coefficients μ_j^s and μ_j^k.

3.E Differentiating Methods

Several applications benefit from the possibility of differentiating a friction model with respect to various parameters: (i) in optimal control, it is helpful to differentiate the final state with respect to the initial state and control input; (ii) in design, adaptive control and system identification, it is helpful to differentiate the model with respect to lengths and inertia properties of the links of a mechanism; and (iii) in system identification, it is helpful to also differentiate the model with respect to the inertias, lengths, restitutions and friction parameters of objects or surfaces with which a mechanism is interacting, when both the mechanism and objects are parts of a common physical model.

Provided the associated functions (e.g., mass matrix, force, normal directions) of the model are differentiable with respect to the relevant parameters, the outputs of methods of the present disclosure may be differentiated, such as using the chain rule (i.e., how to differentiate composite functions). One aspect of solving for the tangential generalized reaction is to differentiate the solution of a second-order cone program with respect to parameters of that program. General results to take such a derivative are discussed in, for example, Agrawal et al. (2018), and an implementation of these results is available as part of the Python package diffcp available at github.com/cvxgrp/diffcp. Package diffcp may be accessed via the CVXPY library, provided the optimization problem is formulated in a manner that is compatible with disciplined parameterized programming (DPP, as described at cvxpy.org/tutoriaVadvanced/index.html #disciplined-parametrized-programming). Such a formulation is possible for the second-order cone program (SOCP) at hand.

Examples described herein may use one computational approach of what to do in situations where one or more constraints j are active (in the sense that the contacts touch, so that j ∈j(q)), yet the Lagrange multiplier λ_j vanishes on the time interval. This may not be considered the most physically satisfying approach, because it may allow such contacts to dissipate power, even though no normal reaction is needed to keep them apart. Another approach may be considered more physically satisfying: (i) the set of active constraints may be replaced with a set that are active and have a positive Lagrange multiplier, in the definition of the friction cone; and (ii) the free velocity is replaced by the tangential component of the free velocity. Specifically, (i) and (ii) amount to: in Constraint (1) above, replacing the set J_m by the set (j ∈J_m: λ_j>0); and in Equation (3) and Equation (4) above, replacing the free velocity q f by its tangential projection, which is the unique solution q_ft of minimizing:

• • q_ft^TM_mq_ft with respect to q_ft and the real numbers κ_j for j ∈J_m Subject to

${\stackrel{.}{q}}_{\mathrm{ft}}={\stackrel{.}{q}}_f-{\sum }_{j\in J_m}{\kappa }_j{M}_m^{-1}{n}_{j,m}^{*}.$

4. Applications

4.A System Identification

The present application has many applications/uses. For example, the present application may be used for system identification (i.e., methods for building models of systems based on observed data of the systems). One approach is to use differentiable friction simulations for system identification, such as described in Le Lidec, Quentin, Igor Kalevatykh, Ivan Laptev, Cordelia Schmid, and Justin Carpentier, “Differentiable simulation for physical system identification,” IEEE Robotics and Automation Letters 6, no. 2, pp. 3413-3420, 2021, which is incorporated herein in its entirety (hereinafter “Le Lidec et al. (2021)”). As detailed in Section IV.B of Le Lidec et al. (2021), given one or more trajectories, derived from sensors on a robot and/or inferred from one or more videos, a loss function measures the difference between the predicted and measured trajectory. Estimates of the system parameters to be identified are adjusted (e.g., by a training module) based on minimizing the loss. While Le Lidec et al. (2021) describes using the mean-squared error in the state as a loss, one may beneficially use loss functions that account for dependence between measurement errors and heavy-tailed behavior of such errors. Le Lidec et al. (2021) describes how to attempt to identify a single scalar friction parameter for a box, and indicates that the described approach is also applicable to more complex scenarios involving multiple unknown parameters of walking robots. Le Lidec et al. (2021) make predictions using a time-stepping function, given by an iterative application of equation (5) in Le Lidec et al. (2021), which is based on Coulomb friction.

The time-stepping process used in Le Lidec et al. (2021) can be replaced with a time-stepping function based on the function set forth in FIG. 2 (noting that methods of the present disclosure are also differentiable with respect to system parameters, if the associated model functions and parameters are differentiable). Advantageously unlike the methods set forth in Le Lidec et al. (2021), the methods of the present disclosure are guaranteed to converge, and operate by first solving for normal reactions, then solving for tangential reactions, rather than requiring a costly process that alternates between solving for normal and tangential reactions. In addition, the methods of the present disclosure may be implemented for area contacts in addition to point contacts.

4.B Trajectory Optimization and Model Predictive Control

Concepts of the present application may be used with techniques for trajectory optimization and identification of linear feedback laws, in the presence of frictional contact. Two of such techniques are the two differential dynamic programming methods described in Budhiraja, Rohan, Justin Carpentier, Carlos Mastalli, and Nicolas Mansard, Differential dynamic programming for multi-phase rigid contact dynamics, In IEEE-RAS 18th International Conference on Humanoid Robots (Humanoids), pp. 1-9. 2018 (hereinafter “Budhiraja et al. (2018)”), which is incorporated herein in its entirety. Differential dynamic programming (DDP) designs a sequence of states and control inputs that minimizes the sum over time of a cost function, which is a function of state and control at each time, and which satisfies a dynamic programming equation (such as that in equation 2 of Budhiraja et al. (2018)). Given an initial guess for an optimal trajectory, DDP uses a local quadratic expansion of the state-action cost-to-go function and the state-value function (see equations 5, 6 and 8 of Budhiraja et al. (2018)). The DDP algorithm iteratively improves on the initial guess by a sequence of alternating backward and forward passes. In a backward pass of DDP, the local quadratic expansion is propagated backwards in time and a time-varying linear feedback control is derived by solving quadratic optimization problems (see equation 7 of Budhiraja et al. (2018)). In a forward pass of DDP, specific values of the control input are derived from the feedback law, and the state sequence is updated using a model of the full nonlinear dynamics (see equation 9 of Budhiraja et al. (2018)).

Concepts of the present application may be deployed in an application of DDP by letting the state dynamics function, f described in Budhiraja et al. (2018), be given by the time-stepping function of present application. The function f can provide the next state given the current state and control input, and that it is a twice differentiable function of those inputs (so that equation 6 of Budhiraja et al. (2018) can be applied). In another example, an extended version of DDP may be used in which the cost functions can depend on the normal and tangential reactions (as in equation 14 of Budhiraja et al. (2018)), for instance to model the costs of wear due to friction and damage due to repeated impacts or large normal reactions. To do this output may be the next state with function f and also returned may be the values of the reactions, which is the function g in equation 13 of Budhiraja et al. (2018). Indeed these reactions are readily available as the variables r_n and r_t in the time-stepping function discussed herein. It is necessary to differentiate g, which is possible as described herein. Given such functions, equation 15 of Budhiraja et al. (2018) shows how to calculate the local quadratic expansion of the state-action cost-to-go function, which are then inserted in the algorithm's backward iterations, no change being required to the forward iterations, which only make use of the methods disclosed herein via the dynamics function f.

In model predictive control (MPC), one is given a dynamic model of a process, a cost function over some time horizon and some constraints on the control or system state; and as new state information is received, one MPC optimizes the future control inputs to optimize the cost function over the time horizon from the current state. Thus, MPC can be seen as repeatedly solving a trajectory optimization problem at rates fast enough to execute the results in real time (e.g., see Neunert, Michael, Markus Stauble, Markus Giftthaler, Carmine D. Bellicoso, Jan Carius, Christian Gehring, Marco Huffer, and Jonas Buchli, Whole-body nonlinear model predictive control through contacts for quadrupeds, IEEE Robotics and Automation Letters 3, no. 3 (2018): 1458-1465, which is incorporated herein in its entirety), and the method disclosed hereunder can be deployed for instance, as described herein.

4.0 Reinforcement Learning of Control Policies

Concepts of the present application may be used with reinforcement learning (RL) algorithms to learn policies for the control of mechanical systems with frictional contacts. One way to deploy methods disclosed herein in such an RL system would be to replace the state transition function in the simulator used for RL training, with the time-stepping function depicted in FIG. 2. For instance, one may directly replace the RaiSim simulator used in Lee et al. (see Lee, Joonho, Jemin Hwangbo, Lorenz Wellhausen, Vladlen Koltun, and Marco Huffer, Learning quadrupedal locomotion over challenging terrain, Science Robotics 5, no. 47, 2020, which is incorporated herein in its entirety) to learn policies for control of quadrupeds over challenging terrain, with the time-stepping of the present disclosure; or one may directly replace the PyBullet simulator used in Wu et al. (see Wu, Bohan, Iretiayo Akinola, Jacob Varley, and Peter K. Allen, MAT: Multi-Fingered Adaptive Tactile Grasping via Deep Reinforcement Learning,” In Conference on Robot Learning, pp. 142-161. PMLR, 2020, which is incorporated herein in its entirety) to learn policies for multi-fingered grasping, with the time-stepping described herein. While RL procedures may not assume that the derivatives of the transition function are available, the derivatives of the time-stepping function described herein may be used to accelerate the RL procedure, for instance as described in Parag et al. (see Parag, Amit, Sébastien Kleff, Leo Saci, Nicolas Mansard, and Olivier Stasse. “Value learning from trajectory optimization and Sobolev descent: A step toward reinforcement learning with superlinear convergence properties.” In 2022 International Conference on Robotics and Automation (ICRA), pp. 01-07. IEEE, 2022, which is incorporated herein in its entirety).

Moreover, such RL algorithms may not only be trained with domain randomization (e.g., of friction parameters) to serve to robustly control a mechanism, but might they might also perform some system identification or adaptation, such as to assist future decision-making. For example, a policy might make use of inputs from an adaption module trained by supervised learning using a simulator based the methods described herein, to predict an embedding of the properties of an environment, given a state-action history, for instance as described in Section IV.A of Kumar et al. (see Kumar, Ashish, Zhongyu Li, Jun Zeng, Deepak Pathak, Koushil Sreenath, and Jitendra Malik. “Adapting rapid motor adaptation for bipedal robots.” In 2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 1161-1168. IEEE, 2022, which is incorporated herein in its entirety).

4.D Design of Mechanical Systems

Concepts of the present application may be used in the design of mechanical systems. For instance, Xu et al. (see Xu, Jie, Tao Chen, Lara Zlokapa, Michael Foshey, Wojciech Matusik, Shinjiro Sueda, and Pulkit Agrawal. “An End-to-End Differentiable Framework for Contact-Aware Robot Design.” In Robotics: Science & Systems. 2021, which is incorporated herein in its entirety) propose to simultaneously optimize the physical structure (design) and control inputs of a robot, to minimize a loss function (a process known as co-design or co-optimization). Such applications couple a flexible morphology parameterization with a differentiable simulation of frictional contact, which is based on the ADD simulator (see Geilinger, Moritz, David Hahn, Jonas Zehnder, Moritz Bacher, Bernhard Thomaszewski, and Stelian Coros. “ADD: Analytically differentiable dynamics for multi-body systems with frictional contact.” ACM Transactions on Graphics (TOG) vol. 39, no. 6, pp. 1-15, 2020, which is incorporated herein in its entirety). As methods of the present disclosure may also serve as a differentiable physics simulator, it may be directly used in such co-optimization techniques.

5. Determining Object Acceleration by Computing Reaction Force

On time intervals on which there are no impacts, the present application may be used to estimate the reaction force (as shown in an embodiment in FIG. 6) for a mechanical system rather than the reaction impulse (as shown in an embodiment in FIG. 2), so that higher-than-first-order integrators may be used. This is because such integrators can offer a better speed-accuracy trade-off than first-order methods like time-stepping. One example of a higher-order integrator is the Dormand-Prince pair described by Dormand, J. R.; Prince, P. J. in “A family of embedded Runge-Kutta formulae”, Journal of Computational and Applied Mathematics, volume 6, number 1, pages 19-26, 1980, which is incorporated herein in its entirety.

FIG. 6 is a flow diagram depicting an example method for determining the generalized acceleration of the motion of an object. The flow diagram in FIG. 6, which may be performed by the trajectory estimation module 17 in the system 2 in FIG. 1, determines a tangential generalized reaction force (instead of a reaction impulse as in the flow diagram in FIG. 2). The method in FIG. 6 involves solving a sequence of two second-order cone programs, given at 608 and at 610 in FIG. 6, which are described in further detail below.

At 602 in FIG. 6, the method receives and/or determines: the current time t; the current generalized position (i.e., configuration) q; the current generalized velocity q; the current normal generalized reaction force R_n; and set of active constraints 1(q).

On time intervals with no impacts, the normal reaction force R may be determined by solving a complementary condition between the Lagrange multiplier and the second time derivative of the constraint function, for each constraint such that both the constraint function and its time derivative vanish. Such a condition is given explicitly as the variational inequality in Step 1 of the proof of Proposition 14 disclosed by Patrick Ballard, in “The dynamics of discrete mechanical systems with perfect unilateral constraints,” Archive for Rational Mechanics and Analysis, volume 154, 2000, which is incorporated herein in its entirety.

As in the time-stepping function set forth in FIG. 2, the method in FIG. 6 may at 604 in FIG. 6 access and/or determine several associated functions and/or parameters that describe the mechanical system: the mass matrix (inertial matrix) M(q) at the current configuration; the total generalized force f(t, q, q), other than reaction forces; the generalized normal direction n(q) for each j ∈J(q); as well as the matrix H_t,j(q), vector H_n,j(q) and scalar μ_J describing friction cone F_J for j ∈J(q).

At 606 in FIG. 6, the method determines/computes values for the time derivatives H_n,j (q, q) and H_t,j(q, q) of the vectors H_n,j(q) and matrices H_t,j(q) for j in J(q). While these time derivatives may not be needed for the time-stepping function, they may be directly computed from vectors H_n,j(q) and matrices H_t,j(q) using automatic differentiation or finite differences. As in the time-stepping function set forth in FIG. 2, it is assumed that scalar μ_J is a constant; otherwise, the time derivative μ_j(q, q) would be required if the friction coefficient μ_j(q) were a function of the configuration.

Let M, f, n_j, H_n,j, H_t,j, H_n,j, H_t,j for j in J be the values of these associated functions described above when computed based on the input values t, q_t q.

At 608 in FIG. 6, the first second-order cone program is solved that maximizes the power dissipated by contacts that are currently moving (variable) by:

• • minimizing g^TR_t with respect to R_t, w_j, γ_j for j in J
  • subject to

${\tilde{R}}_t=\left({\sum }_{j\in J}{H}_{j,t}{\tilde{w}}_j+H_{n,j}{\tilde{\gamma }}_j\right)-R_n\mathrm{and}{{\tilde{w}}_j}_2\le {\mu }_j{\tilde{\gamma }}_j\mathrm{for}j\mathrm{in}J,R_n^T{M}^{-1}{\tilde{R}}_t=0\mathrm{and}{n}_j^{*T}{M}^{-1}{\tilde{R}}_t\le 0\mathrm{for}j\mathrm{in}J;$

where tilde accents have been added to the variables to distinguish them from the analogous variables at 610 in FIG. 6. The constraints applied here may be identical to Constraint (1) and Constraint (2) of the time-stepping formulation, but they are applied to reaction forces R_n, and R_t, rather than reaction impulses. This second-order cone program is already in canonical form, so it can be solved directly by libraries such as CVXPY, as discussed above.

At 610 in FIG. 6, the second second-order cone program is solved to resolve possible ambiguity about the tangential generalized reaction force due to contacts which are initially sticking or on the limit between sticking and slipping by:

• • minimizing

$\frac{1}{2}{\left(f+R_t\right)}^T{M}^{-1}\left(f+R_t\right)+{\sum }_{j\in J}\left({\stackrel{.}{H}}_{t,j}{w}_j+{\stackrel{.}{H}}_{n,j}{\gamma }_j\right)\stackrel{.}{q}$

• • given generalized force f, time derivatives H_t,j and H_n,j and generalized velocity q_t and with respect to tangential generalized reaction force R_t, vector w_J and scalar γ_J for j in J subject to the following constraints:

${\begin{array}{cc}{R}_t=\left({\sum }_{j\in J}{H}_{t,j}{w}_j+H_{n,j}{\gamma }_j\right)-R_n\mathrm{and}{w_j}_2\le {\mu }_j{\gamma }_j\mathrm{for}j\mathrm{in}J,& \left(a\right)\\ R_n^T{M}^{-1}{R}_t=0\mathrm{and}{n}_j^{*T}{M}^{-1}{R}_t\le 0\mathrm{for}j\mathrm{in}J,& \left(b\right)\\ {\stackrel{.}{q}}^T{R}_t\le {\stackrel{.}{q}}^T{\tilde{R}}_t;& \left(c\right)\end{array}}^T$

where the constraints (a) and (b) above may be identical to Constraint (1) and Constraint (2), respectively, of the time-stepping formulation as applied to a normal generalized reaction force R_n and a tangential generalized reaction force R. The additional constraint (c) above says that the tangential generalized reaction force R_t should dissipate at least as much power as the reaction force R_t. Since the reaction force R_t solves the first second-order cone program, it already dissipates as much power as possible subject to constraint (a) and constraint (b). Thus, the inequality in the constraint (c) may be replaced by an equality.

The second second-order cone program above can be put in canonical form (e.g., for the CVXPY library) by extending the method described in Lobo et al. (1998). One possible canonical form is:

• • minimize γ+Σ_j∈J(H_t,jw_j+H_n,jγ_j) q with respect to γ, R_t, w_j, γ_j for j in J,
  • subject to

${\left(\begin{array}{c}{M}^{-1/2}\left(f+R_t\right)\\ y\end{array}\right)}_2\le y+1,R_t=\left({\sum }_{j\in J}{H}_{t,j}{w}_j+H_{n,j}{\gamma }_j\right)-R_n\mathrm{and}{w_j}_2\le {\mu }_j{\gamma }_j\mathrm{for}j\mathrm{in}J,R_n^T{M}^{-1}{R}_t=0\mathrm{and}{n}_j^{*T}{M}^{-1}{R}_t\le 0\mathrm{for}j\mathrm{in}J,{\stackrel{.}{q}}^T{R}_t\le {\stackrel{.}{q}}^T{\tilde{R}}_t,$

in which are introduced a scalar variable γ, and a second-order cone constraint on that variable.

At 612 in FIG. 6, the vector R_t is computed that solves the second second-order cone program, which is the value of the tangential generalized reaction force (or an estimate thereof if one of the programs is solved inexactly). Depending on the application, the model may output the value of R. In addition at 612 in FIG. 6 in a simulation function, the generalized acceleration q would also be estimated by applying the inverse mass matrix to the sum of the generalized force and generalized reaction forces as follows:

$\ddot{q}=M^{-1}\left(f+R_n+R_t\right).$

At 614 in FIG. 6, depending on the application, either the value of vector R_t and/or the generalized acceleration q is output.

The method for computing the tangential generalized reaction force described above with reference to FIG. 6 solves two second-order cone programs, whereas the time-stepping function (with reaction impulse) set forth in FIG. 2 may solve only one. To understand this difference, consider the problem of solving for the reaction force as the limit of time-stepping as the interval duration h tends to zero. Now, time-stepping minimizes the end-of-interval energy, and this is equivalent to minimizing the change in squared energy divided by h:

$\Delta :=\frac{E_{\mathrm{after}}^2-E_{\mathrm{before}}^2}{h}.$

Expression Δ can be expanded in powers of h. The terms of 0(1) in such an expansion determine the reaction force for contacts that are currently slipping. In the limit h→0, expression Δ is minimized if these 0(1) terms alone are minimized, as any terms of 0(h) vanish: this is the minimization at 608 in FIG. 6. However, minimizing those terms may not in general give a unique value for the reaction due to contacts that are currently sticking or on the limit between sticking and slipping. Rather, for any h>0, the terms of 0(h) must also be minimized, and this additional minimization is the second program described above with reference to FIG. 6.

In an alternate embodiment, the Initialization Process replacing cones F_j with cones {tilde over (F)}_j described above with respect to the computation of a generalized position and velocity may also be applied in the method of FIG. 6 with respect to the computation of a generalized acceleration.

In yet another embodiment when using cones {tilde over (F)}₁, the Initialization Process referred to with respect to FIG. 2 (computation of a generalized position and velocity) and FIG. 6 (computation of a generalized acceleration) may be streamlined (hereinafter the “Streamlined Embodiment). By way of illustration but not limitation to computation of generalized acceleration, an implementation of the Streamlined Embodiment is performed in two stages with reference to generalized acceleration. Both stages of this implementation of the Streamlined Embodiment use the projection matrix P as defined as part of the Initialization Process, and the transpose of the projection matrix, which is denoted by P=(P*)^T. The second stage also uses the time derivative of P which is denoted by P. As P is only a function of the configuration q_t this derivative only depends on q and q and it may be readily evaluated by automatic differentiation. Also the real numbers λ_j are used for j ∈J which satisfy

$R_n=\sum _{j\in J}{\lambda }_j{n}_j^{*}.$

These numbers exist and are unique if the normal directions {n_j^*}_j∈J are linearly independent.

In the first stage of the Streamlined Embodiment, a cone program is solved corresponding to maximizing the power dissipated at each constraint j in J:

• • minimize (Pq)^TR_j with respect to R_j, w_j, γ_j
  • subject to
  • R_j=H_t,jw_j+H_n,jγ_j−λ_jn_j^* and ∥w_j∥₂≤μ_jγ_j,
  • n_j^*TM⁻¹R_j=0;The tangential reaction resulting from the first stage is then given by

${\tilde{R}}_t=P^{*}\sum _{j\in J}{\tilde{R}}_j.$

In the second stage of the Streamlined Embodiment a second second-order cone program is solved to resolve possible ambiguity about the tangential generalized reaction force due to contacts which are initially sticking or on the limit between sticking and slipping by:

• • minimizing

$\frac{1}{2}{\left(f+P^{*}\stackrel{\_}{R}\right)}^T{M}^{-1}\left(f+P^{*}\stackrel{\_}{R}\right)+{\left({\sum }_{j\in J}{\stackrel{.}{H}}_{t,j}{w}_j+{\stackrel{.}{H}}_{n,j}{\gamma }_j\right)}^{T}P\stackrel{.}{q}+{\stackrel{\_}{R}}^T\stackrel{.}{P}\stackrel{.}{q}$

• • with respect to vector R_t vector w_j and scalar γ_j for j in J, subject to the following constraints:

$\begin{array}{cc}\stackrel{\_}{R}=\left({\sum }_{j\in J}{H}_{t,j}{w}_j+H_{n,j}{\gamma }_j\right)-R_n\mathrm{and}{w_j}_2\le {\mu }_j{\gamma }_j\mathrm{for}j\mathrm{in}J,& \left(a\right)\\ n_j^{*T}{M}^{-1}\left(H_{t,j}{w}_j+H_{n,j}{\gamma }_j-{\lambda }_j{n}_j^{*}\right)=0\mathrm{for}j\mathrm{in}J,& \left(b\right)\\ {\left(P\stackrel{.}{q}\right)}^T\stackrel{\_}{R}\le {\left(P\stackrel{.}{q}\right)}^T{\tilde{R}}_t\mathrm{where}{\tilde{R}}_t\mathrm{is}\mathrm{the}\mathrm{solution}\mathrm{to}\mathrm{the}\mathrm{first}\mathrm{cone}\mathrm{program}.& \left(c\right)\end{array}$

The tangential reaction resulting from the second stage is given by

$R_t=P^{*}\stackrel{\_}{R}.$

Impacts at the start of the Interval. The method for computing the tangential generalized reaction force described above with reference to FIG. 6 may assume that there were no impacts on the time interval at hand. However, it is possible to address both normal and tangential impacts at the start of an interval of integration. One method for doing so is to first apply the time-stepping function, with the duration of the time step set to zero. Specifically, if the state at the start of the interval is t, q, qd and step denotes the time-stepping function shown in FIG. 5A, then the call:

• • tp, qp, qdp, data=step(t, q, qd, model, 0)returns a value qdp for the generalized velocity, for which any normal and tangential impacts have both been resolved. Thus, the state t, q, qdp may be fed directly into the method for computing the tangential generalized reaction force described above with reference to FIG. 6.

6.Overlaying Methods for Determining Object Motion and Acceleration

FIG. 7 sets forth a flow diagram overlaying the methods for determining object motion by computing the tangential generalized reaction impulse and tangential generalized reaction force set forth in FIGS. 2 and 6, respectively, for a mechanical system with an object having a set of two or more contacts. FIG. 7 may be performed by one or more modules or one or more processors, such as the module 17 of FIG. 2.

At 702, input values are received in the case of the reaction impulse (i.e., the embodiment set forth in FIG. 2) and the reaction force (i.e., the embodiment set forth in FIG. 6), which input values include an initial generalized position q_t and an initial generalized velocity q_t of the object at time t.

At 704, a generalized force f_m is computed, which is a summary of all forces acting on the system other than forces due to the contacts, and in the case of the reaction impulse is used to define energy over the interval, and which in the case of the reaction force is used to define an objective for a second-order cone program that accounts for contacts associated with zero relative velocity at time t (e.g., as computed at 610 in FIG. 6). At 706, in the case of the reaction impulse, a normal generalized reaction impulse is computed, and in the case of the reaction force, a normal generalized reaction force is computed.

At 710, a tangential generalized reaction impulse/force (i.e., for each respectively) is computed by applying the following constraints to an objective function, in the case of reaction impulse and force, respectively, that minimizes energy over the interval (where in one embodiment the objective function corresponding to minimizing energy over the interval is given by ∥M_m^−1/2(M_mq_f+r_t)∥₂ (see Equation (3) above)) and that is used to define power dissipated by friction (where in one embodiment the power dissipated by friction is given by minus the scalar product of the tangential generalized reaction force R_t and the initial generalized velocity q_t): (a) associating each contact in the set of contacts with a friction cone in a space of generalized impulses/forces to define a set of friction cones; (b) summing one term from each friction cone in the set of friction cones, minus the normal generalized reaction impulse/force; and (c) constraining the tangential generalized reaction impulse/force to be orthogonal to the normal generalized reaction impulse/force and to have a non-positive inner product with the normal direction of each contact in the set of contacts.

The constraint at 710(c) may be automatically satisfied in the case of the Streamlined Embodiment because the projection matrix P* is defined as the projection into a subspace where the constrains hold with equality. More specifically, constraint at 710(c) may be automatically satisfied depending on properties of friction cones in the set of friction cones, such as when the properties of the friction cones in the set of friction satisfy Equation (2).

At 712, the tangential generalized reaction impulse is used to compute an end-of-interval generalized position and velocity, or the tangential generalized reaction force is used to compute a generalized acceleration, which are output at 714 for use by/for an application/use.

7. Advantages

7.1 Existence of Solution. Unlike existing approaches, there is always a solution to the constraints imposed in the approach disclosed herein. Setting the tangential generalized reaction equal to zero satisfies the constraints of the approach disclosed herein.

7.2 Uniqueness. Other approaches are known to have multiple solutions. In contrast, the approach disclosed herein gives a unique solution for the tangential generalized reaction (when using an embodiment with the maximum dissipation principle, because it involves the minimization of a strictly convex function).

7.3 Energy consistency. Other approaches may predict gains in kinetic energy during frictional impacts, even for a single contact with Newtonian restitution. In contrast, the approach disclosed herein always conserves or dissipates energy during such impacts.

7.4 Non-zero solutions for multiple contacts. Other approaches predict that the tangential generalized reaction is zero, in certain situations involving multiple contacts, such as a ladder in contact with a wall and a floor, in direct contradiction to everyday observations of physical reality. Unlike these approaches, the approach disclosed herein give non-zero results in situations with multiple contacts, in agreement with physical intuition.

7.5 Correspondence to a continuous-time evolution problem. The disclosed approach corresponds to a well-defined continuous-time evolution problem, even at times where there are jumps in the velocity.

8. General

As an example, to avoid contacting an object, to contact an object, or to manipulate (e.g., interact with, move, grasp, etc.) an object, a control module of the vehicle 6a may actuate one or more components of the vehicle 6a to steer the vehicle 6a and/or decelerate or accelerate the vehicle 6a based on one or more parameters determined herein. As another example, to avoid contacting an object, to contact an object, or to manipulate (e.g., interact with, move, grasp, etc.) an object, a control module of the robot 6b may actuate one or more components of the vehicle 6a to steer the robot 6b and/or decelerate or accelerate the robot 6b, or move one or more components (e.g., arms) of the robot 6b based on one or more parameters determined herein.

The foregoing description is merely illustrative in nature and is in no way intended to limit the disclosure, its application, or uses. The broad teachings of the disclosure can be implemented in a variety of forms. Therefore, while this disclosure includes particular examples, the true scope of the disclosure should not be so limited since other modifications will become apparent upon a study of the drawings, the specification, and the following claims. It should be understood that one or more steps within a method may be executed in different order (or concurrently) without altering the principles of the present disclosure. Further, although each of the embodiments is described above as having certain features, any one or more of those features described with respect to any embodiment of the disclosure can be implemented in and/or combined with features of any of the other embodiments, even if that combination is not explicitly described. In other words, the described embodiments are not mutually exclusive, and permutations of one or more embodiments with one another remain within the scope of this disclosure.

Spatial and functional relationships between elements (for example, between modules, circuit elements, semiconductor layers, etc.) are described using various terms, including “connected,” “engaged” “coupled,” “adjacent,” “next to,” “on top of,” “above” “below” and “disposed.” Unless explicitly described as being “direct,” when a relationship between first and second elements is described in the above disclosure, that relationship can be a direct relationship where no other intervening elements are present between the first and second elements, but can also be an indirect relationship where one or more intervening elements are present (either spatially or functionally) between the first and second elements. As used herein, the phrase at least one of A, B, and C should be construed to mean a logical (A OR B OR C), using a non-exclusive logical OR, and should not be construed to mean “at least one of A, at least one of B, and at least one of C.”

In the figures, the direction of an arrow, as indicated by the arrowhead, generally demonstrates the flow of information (such as data or instructions) that is of interest to the illustration. For example, when element A and element B exchange a variety of information but information transmitted from element A to element B is relevant to the illustration, the arrow may point from element A to element B. This unidirectional arrow does not imply that no other information is transmitted from element B to element A. Further, for information sent from element A to element B, element B may send requests for, or receipt acknowledgements of, the information to element A.

In this application, including the definitions below, the term “module” or the term “controller” may be replaced with the term “circuit” The term “module” may refer to, be part of, or include: an Application Specific Integrated Circuit (ASIC); a digital, analog, or mixed analog/digital discrete circuit; a digital, analog, or mixed analog/digital integrated circuit; a combinational logic circuit; a field programmable gate array (FPGA); a processor circuit (shared, dedicated, or group) that executes code; a memory circuit (shared, dedicated, or group) that stores code executed by the processor circuit; other suitable hardware components that provide the described functionality; or a combination of some or all of the above, such as in a system-on-chip.

The module may include one or more interface circuits. In some examples, the interface circuits may include wired or wireless interfaces that are connected to a local area network (LAN), the Internet, a wide area network (WAN), or combinations thereof. The functionality of any given module of the present disclosure may be distributed among multiple modules that are connected via interface circuits. For example, multiple modules may allow load balancing. In a further example, a server (also known as remote, or cloud) module may accomplish some functionality on behalf of a client module.

The term code, as used above, may include software, firmware, and/or microcode, and may refer to programs, routines, functions, classes, data structures, and/or objects. The term shared processor circuit encompasses a single processor circuit that executes some or all code from multiple modules. The term group processor circuit encompasses a processor circuit that, in combination with additional processor circuits, executes some or all code from one or more modules. References to multiple processor circuits encompass multiple processor circuits on discrete dies, multiple processor circuits on a single die, multiple cores of a single processor circuit, multiple threads of a single processor circuit, or a combination of the above. The term shared memory circuit encompasses a single memory circuit that stores some or all code from multiple modules. The term group memory circuit encompasses a memory circuit that, in combination with additional memories, stores some or all code from one or more modules.

The term memory circuit is a subset of the term computer-readable medium. The term computer-readable medium, as used herein, does not encompass transitory electrical or electromagnetic signals propagating through a medium (such as on a carrier wave); the term computer-readable medium may therefore be considered tangible and non-transitory. Non-limiting examples of a non-transitory, tangible computer-readable medium are nonvolatile memory circuits (such as a flash memory circuit, an erasable programmable read-only memory circuit, or a mask read-only memory circuit), volatile memory circuits (such as a static random access memory circuit or a dynamic random access memory circuit), magnetic storage media (such as an analog or digital magnetic tape or a hard disk drive), and optical storage media (such as a CD, a DVD, or a Blu-ray Disc).

The apparatuses and methods described in this application may be partially or fully implemented by a special purpose computer created by configuring a general purpose computer to execute one or more particular functions embodied in computer programs. The functional blocks, flowchart components, and other elements described above serve as software specifications, which can be translated into the computer programs by the routine work of a skilled technician or programmer.

The computer programs include processor-executable instructions that are stored on at least one non-transitory, tangible computer-readable medium. The computer programs may also include or rely on stored data. The computer programs may encompass a basic input/output system (BIOS) that interacts with hardware of the special purpose computer, device drivers that interact with particular devices of the special purpose computer, one or more operating systems, user applications, background services, background applications, etc.

The computer programs may include: (i) descriptive text to be parsed, such as HTML (hypertext markup language), XML (extensible markup language), or JSON (JavaScript Object Notation) (ii) assembly code, (iii) object code generated from source code by a compiler, (iv) source code for execution by an interpreter, (v) source code for compilation and execution by a just-in-time compiler, etc. As examples only, source code may be written using syntax from languages including C, C++, C#, Objective-C, Swift, Haskell, Go, SQL, R_t Lisp, Java®, Fortran, Perl, Pascal, Curl, OCaml, Javascript®, HTML5 (Hypertext Markup Language 5th revision), Ada, ASP (Active Server Pages), PHP (PHP: Hypertext Preprocessor), Scala, Eiffel, Smalltalk, Erlang, Ruby, Flash®, Visual Basic®, Lua, MATLAB, SIMULINK, and Python®.

Claims

1. A computer-implemented method, executed by one or more processors and memory, for determining for an object having a set of two or more contacts and having at time t an initial generalized position qt and an initial generalized velocity qt, the object's end-of-interval generalized position qt+h and end-of-interval generalized velocity qt+h after an interval h, the method comprising: computing a generalized force f over the interval h, the generalized force being a summary of forces acting on the object other than forces due to the set of contacts, and which is used to define energy over the interval h;computing a normal generalized reaction impulse rn acting on the object;computing a tangential generalized reaction impulse rt acting on the object; andbased on the tangential generalized reaction impulse rt, computing the end-of-interval generalized position qt+h and the end-of-interval generalized velocity qt+h of the object;wherein the computing the tangential generalized reaction impulse rt further includes applying the following constraints to an objective function minimizing energy over the interval h: (a) associating each contact in the set of contacts with a friction cone in a space of generalized impulses to define a set of friction cones;(b) summing one term from each friction cone in the set of friction cones, minus the normal generalized reaction impulse rn; and(c) constraining the tangential generalized reaction impulse rt to be orthogonal to the normal generalized reaction impulse rn and to have a non-positive inner product with the normal direction of each contact in the set of contacts.

2. The method of claim 1 further comprising computing an intermediate generalized position qm of the object over the interval h by taking a time-step duration that is of length h/2.

3. The method of claim 2 wherein the intermediate generalized position qm is given by a midpoint generalized position at time tm=t+h/2 by computing:

4. The method of claim 3 wherein the generalized force f over the interval h is given by Mm−1fm, where Mm is the midpoint mass matrix and fm is the midpoint force.

5. The method of claim 4, wherein the determining the end-of-interval generalized velocity qt+h is determined by computing:

6. The method of claim 5, wherein the determining the end-of-interval generalized velocity qt+h is determined by computing an end-of-interval free-velocity q1, given by:

7. The method of claim 6 wherein the objective function corresponding to minimizing energy over the interval h is given by:

8. The method of claim 5, wherein the determining the end-of-interval generalized position qt+h is determined by computing:

9. The method of claim 2, wherein the computing the normal generalized reaction impulse rt, is approximated over the interval h by computing:

10. The method of claim 9 wherein the one term from each friction cone is determined in a friction cone by multiplying a tangential contact Jacobian by a tangential cone-position variable w and adding it to the result of multiplying a normal contact Jacobian by a normal cone-position variable γ.

11. The method of claim 1 wherein the end-of-interval generalized position qt+h and the end-of-interval generalized velocity qt+h of the object are computed to perform one of system identification, trajectory optimization, learning control policies, and design of mechanical systems.

12. The method of claim 1 further comprising, based on at least one of the end-of-interval generalized position qt+h and the end-of-interval generalized velocity qt+h of the object, at least one of steering an autonomous object, accelerating the autonomous object, and decelerating the autonomous object.

13. The method of claim 1 further comprising, based on at least one of the end-of-interval generalized position qt+h and the end-of-interval generalized velocity qt+h of the object, actuating a component of the object, wherein the object is an autonomous object.

14. A computer-implemented method, executed by one or more processors and memory, for determining for an object having a set of two or more contacts and having at time t an initial generalized position qt and an initial generalized velocity qt, the object's generalized acceleration qt, the method comprising: computing a generalized force f which is a summary of forces acting on the object other than forces due to the set of contacts;computing a normal generalized reaction force R acting on the object;computing a tangential generalized reaction force Rt acting on the object using the computed generalized force f to account for contacts associated with zero relative velocity at time t; andusing the tangential generalized reaction force Rt to compute the generalized acceleration qt of the object;wherein the computing the tangential generalized reaction force Rt further comprises applying the following constraints to an objective function maximizing power dissipated by friction: (a) associating each contact in the set of contacts with a friction cone in a space of generalized impulses to define a set of friction cones;(b) summing one term from each friction cone in the set of friction cones, minus the normal generalized reaction force Rn; and(c) constraining the tangential generalized reaction force Rt to be orthogonal to the normal generalized reaction force Rt, and to have a non-positive inner product with the normal direction of each contact in the set of contacts.

15. The method of claim 14 wherein the object is an autonomous object.

16. The method of claim 14 wherein the power dissipated by friction is given by minus the scalar product of the tangential generalized reaction force Rt and the initial generalized velocity qt.

17. The method of claim 14 wherein the end-of-interval generalized position qt+h and the end-of-interval generalized velocity qt+h of the object are computed to perform one of system identification, trajectory optimization, learning control policies, and design of mechanical systems.

18. The method of claim 1 wherein (c) is automatically satisfied when properties of friction cones in the set of friction cones satisfy a preset condition.

19. The method of claim 18 wherein the preset condition when the properties of friction cones in the set of friction cones are automatically satisfied in step (c) is when each friction cone Fj in the set of friction cones satisfies:

20-24. (canceled)

25. A system for determining, for an object having a set of two or more contacts and having at time t an initial generalized position qt and an initial generalized velocity qt, the object's end-of-interval generalized position qt+h and end-of-interval generalized velocity qt+h after an interval h, the system comprising: one or more processors; andmemory including code that, when executed by the one or more processors perform to:compute a generalized force f over the interval, which is a summary of forces acting on the system other than forces due to the set of contacts, and which is used to define energy over the interval h;compute a normal generalized reaction impulse rn acting on the object;compute a tangential generalized reaction impulse rt acting on the object; andbased on the tangential generalized reaction impulse rt, compute the end-of-interval generalized position qt+h and the end-of-interval generalized velocity qt+h of the object,wherein the computing the tangential generalized reaction impulse rt further includes applying the following constraints to an objective function minimizing energy over the interval h: (a) associating each contact in the set of contacts with a friction cone in a space of generalized impulses to define a set of friction cones;(b) summing one term from each friction cone in the set of friction cones, minus the normal generalized reaction impulse rn; and(c) constraining the tangential generalized reaction impulse rt to be orthogonal to the normal generalized reaction impulse rn and to have a non-positive inner product with the normal direction of each contact in the set of contacts.

26. The system of claim 25 wherein the memory further includes code for a learning model that, when executed by the one or more processors, performs the following functions including computing an intermediate generalized position qm of the object over the interval h by taking a time-step duration that is of length h/2.

27. The system of claim 26 wherein the intermediate generalized position qm is given by a midpoint generalized position at time tm=t+h/2 by computing:

28. The system of claim 27 wherein the generalized force f over the interval h is given by Mm−1fm, where Mm is the midpoint mass matrix and fm is the midpoint force.

29. The system of claim 28, wherein the determining the end-of-interval generalized velocity qt+h is determined by computing:

30. The system of claim 29, wherein the determining the end-of-interval generalized velocity qt+h is determined by computing an end-of-interval free-velocity q1, given by:

31. The system of claim 30 wherein the objective function corresponding to minimizing energy over the interval h is given by:

32. The system of claim 29, wherein the determining the end-of-interval generalized position qt+h is determined by computing:

33. The system of claim 26, wherein the computing the normal generalized reaction impulse rt, is approximated over the interval h by computing:

34. The system of claim 33 wherein the one term from each friction cone is determined in a friction cone by multiplying a tangential contact Jacobian by a tangential cone-position variable w and adding it to the result of multiplying a normal contact Jacobian by a normal cone-position variable γ.

35. The system of claim 25 wherein the end-of-interval generalized position qt+h and the end-of-interval generalized velocity qt+h of the object is computed to perform one of system identification, trajectory optimization, learning control policies, and design of mechanical systems.

36. A system, for determining for an object having a set of two or more contacts and having at time t an initial generalized position qt and an initial generalized velocity qt, the object's generalized acceleration qt, the system comprising: one or more processors;memory including code that, when executed by the one or more processors perform to:compute a generalized force f which is a summary of forces acting on the system other than forces due to the set of contacts;compute a normal generalized reaction force Rn acting on the object;compute a tangential generalized reaction force Rt acting on the object using the computed generalized force f to account for contacts associated with zero relative velocity at time t; andbased on the tangential generalized reaction force Rt, compute the generalized acceleration qt of the object,wherein the computing the tangential generalized reaction force Rt further includes applying the following constraints to an objective function maximizing power dissipated by friction: (a) associating each contact in the set of contacts with a friction cone in a space of generalized impulses to define a set of friction cones;(b) summing one term from each friction cone in the set of friction cones, minus the normal generalized reaction force Rn; and(c) constraining the tangential generalized reaction force Rt to be orthogonal to the normal generalized reaction force Rn and to have a non-positive inner product with the normal direction of each contact in the set of contacts.

37. The system of claim 36 wherein the system is a mechanical system with an autonomous object and one or more objects.

38. The system of claim 36 wherein the power dissipated by friction is given by minus the scalar product of the tangential generalized reaction force Rt and the initial generalized velocity qt.

39. The system of claim 36 wherein the end-of-interval generalized position qt+h and the end-of-interval generalized velocity qt+h of the object is computed to perform one of system identification, trajectory optimization, learning control policies, and design of mechanical systems.