US 11,883,962 B2
Object manipulation with collision avoidance using complementarity constraints
Arvind Raghunathan, Cambridge, MA (US); Devesh Jha, Cambridge, MA (US); and Diego Romeres, Cambridge, MA (US)
Assigned to Mitsubishi Electric Research Laboratories, Inc., Cambridge, MA (US)
Filed by Mitsubishi Electric Research Laboratories, Inc., Cambridge, MA (US)
Filed on May 28, 2021, as Appl. No. 17/333,530.
Prior Publication US 2022/0379478 A1, Dec. 1, 2022
Int. Cl. B25J 9/16 (2006.01)
CPC B25J 9/1676 (2013.01) [B25J 9/163 (2013.01); B25J 9/1666 (2013.01); G05B 2219/40476 (2013.01); G05B 2219/40517 (2013.01)] 13 Claims
OG exemplary drawing
 
1. A controller for controlling a motion of an object performing a task for changing a state of the object from a start state to an end state while avoiding collision of the object with an obstacle, comprising: at least one processor; and a memory having instructions stored thereon that, when executed by the at least one processor, cause the controller to:
obtain data indicative of dynamics of the object and the obstacle, and geometry of the object and the obstacle, wherein the geometry of the object includes a convex shape enclosing an actual shape of the object and the geometry of the obstacle includes a convex shape enclosing an actual shape of the obstacle, wherein each of the convex shapes of the object and the obstacle is a polytope defined by a set of vertices, wherein points within the polytope of the object are defined as weighted combinations of the vertices of the polytope of the object, and wherein points within the polytope of the obstacle are defined as weighted combinations of the vertices of the polytope of the obstacle;
solve an optimization problem of the dynamics of the object producing an optimal trajectory for performing the task subject to constraints on a solution of first-order stationary conditions modeling a minimum distance between the convex shape of the object and the convex shape of the obstacle using complementarity constraints, wherein the complementarity constraints are imposed on the weights for the vertices of the polytopes of the object and the obstacle to enforce the minimum distance between the object and the obstacle; and
control the motion of the object according to the optimal trajectory.
 
10. A method for controlling a motion of an object performing a task changing a state of the object from a start state to an end state while avoiding collision of the object with an obstacle, wherein the method uses a processor coupled with stored instructions implementing the method, wherein the instructions, when executed by the processor carry out steps of the method, comprising:
obtaining data indicative of dynamics of the object and the obstacle, and geometry of the object and the obstacle, wherein the geometry of the object includes a convex shape enclosing an actual shape of the object and the geometry of the obstacle includes a convex shape enclosing an actual shape of the obstacle, wherein each of the convex shapes of the object and the obstacle is a convex hull defined by a set of inequalities defining points within the convex hull of the object and a set of inequalities defining points within the convex hull of the obstacle;
solving an optimization problem of the dynamics of the object producing an optimal trajectory for performing the task subject to constraints on a solution of first-order stationary conditions modeling a minimum distance between the convex shape of the object and the convex shape of the obstacle using complementarity constraints, wherein the complementarity constraints are imposed on the sets of inequalities of the object and the obstacle to enforce the minimum distance between the object and the obstacle; and
controlling the motion of the object according to the optimal trajectory.