An Efficient Closed-Form Method for Optimal Hybrid Force-Velocity Control
AAn Efficient Closed-Form Method for Optimal Hybrid Force-VelocityControl
Yifan Hou and Matthew T. Mason
Fellow, IEEE
Abstract — This paper derives a closed-form method forcomputing hybrid force-velocity control. The key idea is tomaximize the kinematic conditioning of the mechanical system,which includes a robot, free objects, a rigid environment andcontact constraints. The method is complete, in that it alwaysproduces an optimal/near optimal solution when a solutionexists. It is efficient, since it is in closed form, avoiding theiterative search of previous work [14]. We test the method on78,000 randomly generated test cases. The method outperformsour previous search-based technique by being from 7 to 40 timesfaster, while consistently producing better solutions in the senseof robustness to kinematic singularity. We also test the methodin several representative manipulation experiments.
I. INTRODUCTIONContact constraints help human manipulation with im-proved precision and dexterity. For example, when cuttinga piece of wood with a band saw, we often slide thewood along a guide rail to position it accurately. HybridForce-Velocity Control (HFVC) naturally suits such tasks.The velocity control (high-stiffness) can drive the systemprecisely, avoiding the need to finely balance the forces.The force control (low-stiffness) can avoid excessive internalforce and maintain desired contacts. A properly designedHFVC could keep both advantages.Under modeling uncertainties, HFVC may become in-feasible, violating constraints and generating huge forces.We describe this phenomenon by treating the robot-object-environment system as a kinematic chain connected bycontact constraints, then use its kinematic conditioning toevaluate the quality of the HFVC. A well conditioned systemcan remain feasible under large modeling errors. Our goalin this work is to compute a HFVC that maximizes thekinematic conditioning of the manipulation system.While kinematic conditioning of a fully-actuated manipu-lator was well studied [1][2][28], kinematic conditioning ofa system with free objects still needs a clear characterization[13]. Our previous work [14] approximated the conditionnumber of the whole system with a polynomial and opti-mized it in a non-convex optimization. Given enough initialguesses and sufficient number of iterations, the algorithmcould find a good solution. However, the trade-off betweencomputation speed and quality of solution is limiting itsapplications, especially in industrial applications where bothspeed and safety guarantee are required. *This work was supported under NSF Grant IIS-1813920 and IIS-1909021.The authors are with the Robotics Institute, Carnegie MellonUniversity, Pittsburgh, PA 15213, USA. [email protected],[email protected]
This paper aims to solve the speed and optimality prob-lem. We make three contributions: first, we provide aprecise characterization of the conditioning of a robot-object-environment system. Second, we present
Optimally-Conditioned Hybrid Servoing (OCHS) , an algorithm thatefficiently computes well-conditioned HFVC. OCHS avoidsnon-convex optimization, which leads to a 7 to 40 timesspeed up comparing with our previous work. Third, we testour algorithm extensively in randomly generated problemsas well as experiments to demonstrate its computation speedand quality consistency.The paper is organized as follows. In the next section wereview the related work. In Section III we introduce the mod-eling for a manipulation problem under contact constraints.Then we introduce the problem formulation in Section IV.Next, we derive the OCHS algorithm in Section V. Sec-tion VI evaluates the algorithm in randomly generated testproblems, while multiple representative experimental resultsare presented in Section VII.II. RELATED WORK
A. Manipulation with Hybrid Force-Velocity Control
The idea of HFVC was originally introduced in [20],which provides a framework for identifying force andvelocity-controlled directions in a task frame given a taskdescription. The framework was then completed and imple-mented in [26]. For the control of a manipulator subject toconstraints, it was common to align the force and velocitycontrol directions with the row and null space of the contactJacobian [33][35]. The approach has industrial applicationsincluding polishing [24] and peg-in-hole assembly [25].However, when the system contains one or more freeobjects with no attachment to any motor, most previouswork was case by case study, such as [8] and [30]. Insome special cases, it is possible to design a HFVC fromsimple heuristics using local contact information, such asin multi-finger grasping [23] and locomotion [4][10]. Ourprevious work [14] proposed the first general algorithm formanipulation under rigid contacts using HFVC.Once we have a HFVC, the final step is to implement iton a robot. HFVC can be implemented by wrapping aroundforce control [11][17][26][27] or position/velocity control[18][19]. The choice depends on the type of robot. Werefer the readers to Whitney [34] and De Schutter [7] forcomparisons of different HFVC implementations. a r X i v : . [ c s . R O ] N ov . Robustness of Manipulation System under HFVC There is plenty of work on the stability of a hybrid force-velocity controlled system under active contact with theenvironment. Lagrange dynamics modeling and Lyapunovstability analysis have been conducted on the whole con-strained robot system [3][9][16][21]. Much attention waspaid to the stability of the engaging/disengaging process[15][22][32], which is particularly difficult to stabilize.Most of these analyses focused on the stability of theinner control loop, i.e. whether the robot action wouldconverge, oscillate, or diverge. However, a stable robot maystill fail a manipulation task for two reasons. The firstis ill-conditioning under modeling errors. The second isunexpected changes of contact modes such as unexpectedslipping or sticking, which may directly cause task failureor lead to crashing. This work assumes the robot has stableHFVC and focuses on handling the above two failure cases.III. MODELINGFirst we introduce the instantaneous modeling of a ma-nipulation system subject to contact constraints. Consider arobot and at least one object in a rigid environment. Therobot, object(s), and the environment has n a , n u , and zerodegree-of-freedoms (DOF), respectively. The total DOF ofthe system is n = n a + n u . The subscript ‘a’ and ‘u’ means‘actuated’ and ‘unactuated’. Denote v = [ v Tu v Ta ] T , f =[ f Tu f Ta ] T ∈ R n as the generalized velocity and force vectors.Note that the unactuated part of f is always zero: f u = 0 .The following assumptions are made: • Motions are quasi-static, i.e . inertia force and Coriolisforce are negligible. • Object, robot and environment are all rigid. • Friction follows Coulomb’s Law.
A. Contact Constraints on Velocity
We consider point contact with clearly defined contactpoint location and contact normal. This is the case for point-to-face contacts. Edge-to-edge, edge-to-face, and face-to-face contacts can be approximated by one or more pointcontacts [5]. Point-to-point and point-to-edge contacts are notconsidered here due to their rare appearance. We considerthree types of contact modes: sticking, sliding, and sepa-ration. Both sticking and sliding contacts impose a linearconstraint in the contact normal direction; a sticking contactalso impose constraints in the contact tangential directions.We consider holonomic constraints imposed by the contacts,which are bilateral constraints on the system configurationthat are also independent of the system velocity. They arelinear constraints on the system velocity: Jv = 0 . (1) J is the contact Jacobian [23]. Equation 1 can also modelany other holonomic constraints, such as the connectionconstraint between two links of a robot joint. B. Goal Description
Users shall provide the control goal, which is an expectedsystem velocity. The goal at a time instant can be written asan affine constraint on the generalized velocity: Gv = b G , (2)which can be derived from a given trajectory by taking first-order derivative. We can use six rows to specify the desiredvelocity of a rigid body in 3D, or only use three rows tospecify a desired rotational velocity.The goal specification 2 must not be redundant. Forexample, to slide an object on a planar surface in 3D, thegoal should have no more than three rows. It should notspecify the object velocity in the contact normal direction,which is already limited to zero by the contact constraint. C. Constraints on force
Denote λ as the vector of contact forces. Using theprinciple of virtual work [31], we can write the contributionof λ to the generalized force space as τ = J (cid:48) T λ . Note the J (cid:48) here is different from the J in 1, because J (cid:48) may havemore rows that corresponds to sliding friction.There are two kinds of force constraints. One is theNewton’s Second Law under quasi-static approximation: J (cid:48) T λ + f + F = 0 . (3)The three terms are contact forces, control actions (internalforces) and external forces, respectively. The external force F ∈ R n may include gravity, disturbance forces, etc .The other force constraint is the condition for stayingin the desired contact mode, we called them the guardconditions [14]. It’s usually a good practice to make theguard condition stricter than necessary to encourage conser-vative actions. We consider guard conditions that are affineconstraints on force variables. Examples are friction coneconstraints and lower/upper bounds on forces. Λ (cid:20) λf (cid:21) ≤ b Λ . (4)Note that 4 has no equality constraints, so we don’t considersliding friction. This is because applying force on the frictioncone is not a robust way to execute a sliding contact [13]. Amore reliable approach requires model information beyondthe scope of this work [12]. D. Hybrid Force-Velocity Control
Consider a HFVC with n av dimensions of velocity controland n af dimension of force control, n av + n af = n a . We usematrix T ∈ R n × n to describe the directions of force/velocitycontrol. T = diag ( I u , R a ) , where I u ∈ R n u × n u is anidentity matrix, R a ∈ R n a × n a is an unitary matrix describingthe control axes. Here we assume R a is orthonormal, sothat the force and velocity controls are reciprocal. Withoutloss of generality, we assume the last n av rows of T are velocity-controlled directions, preceded by n af rows offorce-controlled directions. Denote w = T v, η = T f ∈ R n as the transformed generalized velocity and the transformedeneralized force . We know w = [ w Tu w Taf w Tav ] T , where w u = v u is the unactuated velocity, w af ∈ R n af is thevelocity in the force-controlled directions, w av ∈ R n av is thevelocity control magnitude. Similarly, η = [ η Tu η Taf η Tav ] T ,where η u = f u = 0 is the unactuated force, η af ∈ R n af isthe force control magnitude, η av ∈ R n av is the force in thevelocity-controlled directions.To fully describe a HFVC, we need to solve for n av , n af , R a , w av and η af .IV. THE HYBRID SERVOING PROBLEMComputing the best HFVC for a constrained manipulationproblem is called hybrid servoing [14]. In the following wederive the cost function from kinematic conditioning, thenintroduce the hybrid servoing problem. A. Kinematic Conditioning of Manipulation System
In manipulator kinematic analysis, it is well-known thatthe condition number of the manipulator Jacobian is an indi-cator of the kinematic performance of the system [1][2][28].In a manipulation problem with free objects, we need toconsider two kinematic constraints including 5 and thevelocity control of HFVC: Cv = b c , (5)where C is the last n av rows of T , and b C is simply w av .We need to evaluate the conditioning of the whole kinematicsystem: (cid:20) JC (cid:21) v = (cid:20) b C (cid:21) . (6)The condition number of the coefficient matrix needs to beminimized: min J,C cond( (cid:20) JC (cid:21) ) . (7)Throughout this paper, we use the 2-norm condition number,defined as cond( A ) = (cid:107) A (cid:107) (cid:107) A † (cid:107) = σ max ( A ) σ min ( A ) , (8)which is the ratio between the maximum and minimumsingular values.However, for our system, directly computing the abovecondition number makes little sense for two reasons. First,we only want to evaluate the influence of control C , therest of our coefficient matrix is constant. In fact, J itselfcould already be ill-conditioned if the contact modeling isredundant. To singulate the influence of C , we replace J with an orthogonal basis of its rows, so it represents thesame constraint as 1 but has a condition number of one.Second, the row scaling of C should not affect our criteria,since scaling both sides of 5 does not make a differenceto our control. However, the condition number could bemade arbitrarily large by row/column scaling. This problemis called the artificial ill-condition [29], the typical solution is to pre-normalize each row of our coefficient matrix. Thusour final expression of kinematic conditioning is: min C cond( (cid:20) ˆ J ˆ C (cid:21) ) , (9)where rows of ˆ J form an orthonormal basis of rows in J ; ˆ C is C with each row normalized to unit norm. Figure 1 showsthe condition number value of several planar examples. Whenthe control is collinear with constraints, the condition numbergrows to infinity and a tiny motion can cause huge internalforce. We have been calling this situation crashing in ourprevious work, and introduced a “crashing-avoidance score”in [13] to evaluating it. However, equation 9 is a more precisedescription, we call the cost function the crashing index . V2.41F V3.87F VinfFV7.10F V10.48F VinfF
Fig. 1. 2D examples of HFVC and their corresponding crashing indexes.The robot execute 2D HFVC, with 1D force control and 1D velocity control.
B. Problem Formulation
The task of hybrid servoing is to solve for:1) the dimensions of force-controlled actions and velocity-controlled actions, n af and n av , and2) the directions to do force control and velocity control,described by the matrix T , and3) the magnitude of force/velocity actions: η af and w av ,so as to minimize the crashing index 9 subject to thefollowing constraints: • Any v under the robot action shall satisfy the goalconstraint (2); • Any f under the robot action shall satisfy the guardconditions (4).We use the word ‘any’ because a HFVC usually cannotuniquely determine v and f .V. APPROACHIn this section, we use the notation Null( · ) and Row( · ) todenote a matrix whose rows form an orthonormal basis of thenull space and row space of the argument, respectively. Weuse rows( · ) to denote the number of rows in the argument.efore introducing our algorithm, we need to make someobservations about the nature of the problem. Since thefeasible velocity v under a HFVC may not be unique, theproper statement of the goal constraint is: Gv = b G , ∀ v ∈ { v | Jv = 0 , Cv = b C } , (10)i.e. we need to ensure all possible solutions satisfy the goal.This is an inclusion relationship between the solution sets oftwo linear equations, which is equivalent to:1) The null space of G contains the null space of (cid:20) JC (cid:21) ;2) There exists a common special solution: ∃ v ∗ : Gv ∗ = b G , Jv ∗ = 0 , Cv ∗ = b C .We call them the Goal-Inclusion conditions and will refer tothem repeatedly. Condition one is equivalent to
Null( (cid:20) JC (cid:21) ) ⊆ Null( (cid:20) JG (cid:21) ) , (11)which further implies rank( (cid:20) JC (cid:21) ) ≥ rank( (cid:20) JG (cid:21) ) . (12)Due to the orthogonal complement relation between the rowand null space of a matrix, the null space inclusion 11 canbe reformulated as a reverse row space inclusion: rank (cid:18)(cid:20) JC (cid:21)(cid:19) = rank JCG . (13)Our algorithm involves three steps. First, we derive thecontrol axis directions to satisfy the Goal-Inclusion conditionone while optimizing conditioning. Second, we compute thevelocity control magnitudes to satisfy the Goal-Inclusioncondition two. Finally, we solve for the force control mag-nitudes to satisfy the guard conditions. A. Pick Control Axes to Optimize Conditioning
The information of the control axes ( n av , n af , and T ) iscontained in the velocity control coefficient matrix C (5): C has n av rows; C and its orthogonal complement forms T .First thing we need to know about the velocity control isits dimension. We can compute the instantaneous directionsthat the system can move without conflicting the contact con-straints by computing the null space U of contact Jacobian: U = Null( J ) . (14)The last n a columns of U , i.e. the actuated part, indicate thedirections in which the robot can move freely. It is a linearspace, a basis of which can be computed as: ¯ U = Row( U S a ) , (15)where S a ∈ R n × n is a selection matrix with only oneson the last n a diagonal entries. Any linear combinations ofrows of ¯ U corresponds to a vector in U and is thus a freerobot motion direction. The dimensionality of ¯ U indicatesthe maximum possible dimension of velocity control we canapply freely: n av ≤ rows( ¯ U ) (16) On the other hand, 12 suggests the minimum dimension ofvelocity control required to satisfy the goal 2: n av ≥ rank( (cid:20) JG (cid:21) ) − rank( J ) . (17)Combining 16 and 17, we have a necessary condition for thefeasibility of the problem: rows( ¯ U ) ≥ rank( (cid:20) JG (cid:21) ) − rank( J ) . (18)If equation 18 is not satisfied, the problem has an infeasiblegoal. Otherwise, we can choose the dimension of velocitycontrol within 1617. Sometimes we want more velocitycontrol so as to increase disturbance rejection ability [12];sometimes we want less velocity control to have morecompliance in the system [14]. We solve both situations andleave this choice to the user.If the maximal velocity control is needed, we can simplydo velocity control in all directions in ¯ U : n av = rows( ¯ U ) , (19) C = ¯ U . (20)Then we evaluate equation 13 to check if the problem isfeasible. Otherwise, if the minimal velocity control is desired,we take n av = rank( (cid:20) JG (cid:21) ) − rank( J ) (21)Then the n av rows of velocity controls are linear combina-tions of rows of ¯ U : C = K ¯ U , (22)where K ∈ R n av × rows( ¯ U ) . We compute K using the nullspace form of Goal-Inclusion condition one 11, which im-plies C Null( (cid:20) JG (cid:21) ) = K ¯ U Null( (cid:20) JG (cid:21) ) = 0 . (23)Then K is an orthonormal basis of a null space: K = Null T (cid:18) Null T ( (cid:20) JG (cid:21) ) ¯ U T (cid:19) . (24)The the problem is feasible if K has enough rows: rows( K ) ≥ n av . (25)If this is true, we keep the first n av rows of K and recover C from equation 22. The C obtained this way has orthonormalrows, since it is the product of two orthonormal matrices.After obtaining the velocity-controlled direction C , wecompute the force-controlled direction as its orthogonal com-plement to make the velocity and force controls reciprocal.Denote the last n a columns of C as R C , we can expand itinto a full rank R a : n af = n a − n av . (26) R a = (cid:20) Null( R C ) T R C (cid:21) . (27)hen we have T = diag(I u , R a ) .We summarize the procedure in line 1 to line 13 inalgorithm 1. Note that the method avoids the non-convexoptimization in [14]. B. Solve for Velocity Control Magnitudes
Next, we use Goal-Inclusion condition two to compute b C .Compute a special solution v ∗ from (cid:20) JG (cid:21) v ∗ = (cid:20) b G (cid:21) (28)Such v ∗ must exist, otherwise the goal itself is infeasible.Use it to compute the velocity control magnitude: w av = b C = Cv ∗ (29)This choice of b C satisfies condition two. Algorithm 1:
Optimally-Conditioned Hybrid Servo-ing
Input:
Contact Jacobian J , Goal description G, b G Input:
Guard condition,
Output:
HFVC ( n af , n av , R a , w av , η af ) // Solve for velocity control Compute free motion space U under constraint; Compute free robot motion space ¯ U
15 ; Check necessary feasibility condition 18. ; if Maximal velocity control dimension then Take ¯ U as velocity-controlled directions 1920 ; Check goal feasibility using 13; else Compute the minimal dimension of C from 21 ; Solve for the coefficient matrix K from 24 ; Check goal feasibility using 25 ; Compute the velocity control C from 22; Complete control axes information using 2627. ; Compute the velocity control magnitude 29 using aspecial solution to 28. ; // Solve for Force control Compute the force control magnitude η af by solvingthe QP defined in Section V-C.; C. Solve for Force Control Magnitudes
At this point we already know the dimensionality anddirections of force control. The only remaining unknownvariable in a HFVC is the value of the force control mag-nitude η af . we find a solution by minimizing the magnitudeof force variables: min λ,η λ T λ + η Ta η a (30)subject to the Newton’s Second Law 3 and the guard condi-tions 4, which takes the form of a Quadratic Programming.This is a simplified version of the force magnitude compu-tation in [14] and it is slightly more efficient. D. Discussion
An important advantage of this algorithm over the previouswork [14] is its ability to handle underactuated system.
TABLE IT
YPES OF R ANDOMLY G ENERATED T EST P ROBLEMS
Environment Contacts Hand Contacts
An example is a cubewith one corner stick-ing on the ground andone corner sticking on therobot finger (Figure 2):the robot has no con-trol over the rotation ofthe cube about the linebetween the two contactpoints. Still, a controlproblem on this objectmay still be feasible, e.g. if the goal is to lift the centerof mass of the object. Condition 13 tells us whether this isthe case or not. VI. E
VALUATION
A. Implementation
In this section, we evaluate the performance of the OCHSalgorithm in randomly generated manipulation problems. Weimplement OCHS in Matlab and test it on a desktop with ani7-9700k CPU clocked at 4.7GHz.
B. Test Problems
We consider a rigid object with one to three environmentalcontacts and one to three rigid body fingers. Each rigidbody has three DOFs in planar problems, or six DOFs in3D problems. The settings are listed in Table I, where ‘f’denotes a fixed (sticking) contact point, ‘s’ denotes a slidingcontact point. Each environment contact point can be slidingor sticking; finger contacts are all sticking.We randomly sample 1000 set of contact point locationsand normals for each contact mode setting, making a total of6000 planar and 72,000 3D test problems. The goal constraintis sampled randomly for each problem.The results are summarized in Table II and III, thedifference is that in Table III we give the goal 2 themaximum possible dimension, so all algorithms must selectthe maximum velocity control dimension. OCHS is our algo-rithm with minimal velocity control, OCHS(M) denotes ouralgorithm with maximal velocity control. As a comparison,we also show the performance of the original hybrid servoingalgorithm [14]. HS3 uses three initial guesses and has a goodcomputation speed; HS10 uses ten initial guesses to searchthe problem excessively. Both run each initial guess for 50iterations.
ABLE IIT
EST RESULTS - M IN V ELOCITY C ONTROL D IMENSION
Planar (6 DOF) OCHS OCHS(M) HS3 HS10
EST RESULTS - M AX V ELOCITY C ONTROL D IMENSION
OCHS OCHS(M) HS3 HS10
C. Results
In all tests, OCHS consistently finds solutions with bet-ter crashing indexes than HS3 and HS10, achieves loweraverage crashing index and fewer ill-conditioned solutions.OCHS(M) has a larger crashing index in Table II becauseit applies more dimensions of control. When all algorithmsselects the same dimension of velocity control, OCHS(M)also always achieves better crashing indexes than HS3 andHS10 on every problem.Both OCHS and OCHS(M) are notably faster than HS3and HS10. The velocity part of OCHS shows 7 to 13times speedup comparing with HS3, 20 to 40 times speedupcomparing with HS10. The force part of OCHS has a mildspeed up of 35% due to a simpler problem formulation withless variables. VII. EXPERIMENTS
Fig. 3. The block tilting experiment. The experiment runs 100 times withall successes.
We test the quality of our algorithm in several exper-iments. We implemented hybrid force-velocity control ona position-controlled ABB IRB 120 industrial robot witha wrist-mounted ATI Mini-40 force-torque sensor. In allexperiments, we run OCHS off-line on a given motion plan toobtain a trajectory of HFVC, though the computation speedof OCHS supports feedback control at hundreds of Hz. Inonline execution, HFVC control loop is clocked at 200Hz.The lowest communication rate between the computer andthe robot position controller is 250Hz with 25ms latency.To compare with our previous work, we redo the blocktilting experiment [14] with the same setup, as shown inFigure 3. The block is a wooden cube with length 75mm.We place a 1.5mm rubber sheet on the table to increasefriction. The robot hand is the same rubber ball as before.In our previous work, we perform block tilting 50 times andobtained 47 successes. With OCHS, we obtain a hundredconsecutive successes at 50% higher robot velocity . Theresult demonstrate the ability of our algorithm to consistentlyproduce solutions with good quality. Fig. 4. Two shared grasping tasks executed with OCHS.
OCHS can be used in shared grasping [12] to compute thecontrol axes and velocity control. We use OCHS as a robusttracking controller to execute several shared grasping withmotion plans computed using [6], as shown in Figure 4.VIII. CONCLUSIONIn this work, we provide an algorithm to compute ahybrid force-velocity control for manipulation under contactconstraints. Our algorithm finds the solution that brings thebest kinematic conditioning of the manipulation system. Wedemonstrate that our algorithm reliably and quickly finds thebest solutions among all comparisons in extensive tests andexperiments.The algorithm itself can serve as a robust tracking con-troller to execute a pre-computed motion plan. It is alsoa building block for the more comprehensive contact sta-bility analysis [12]. Due to our algorithm’s computationalefficiency, it also has the potential to be incorporated into aplanning framework for robust manipulation planning.R
EFERENCES[1] Jorge Angeles and Carlos S L´opez-Caj´un. Kinematic isotropy and theconditioning index of serial robotic manipulators.
The InternationalJournal of Robotics Research , 11(6):560–571, 1992. The full 40min video is available at
2] Jorge Angeles, Farzam Ranjbaran, and Rajnikant V Patel. On thedesign of the kinematic structure of seven-axes redundant manipulatorsfor maximum conditioning. In
Proceedings 1992 IEEE InternationalConference on Robotics and Automation , pages 494–495. IEEE Com-puter Society, 1992.[3] Bernard Brogliato, S-I Niculescu, and Pascal Orhant. On the controlof finite-dimensional mechanical systems with unilateral constraints.
IEEE Transactions on Automatic Control , 42(2):200–215, 1997.[4] Thomas Buschmann, Sebastian Lohmeier, and Heinz Ulbrich. Bipedwalking control based on hybrid position/force control. pages 3019–3024, 2009.[5] Nikhil Chavan-Dafle and Alberto Rodriguez. Prehensile pushing: In-hand manipulation with push-primitives. In , pages 6215–6222, 2015.[6] Xianyi Cheng, Eric Huang, Yifan Hou, and Matthew T Mason. Con-tact mode guided sampling-based planningfor quasistatic dexterousmanipulation in 2d.
Under Review , 2020.[7] Joris De Schutter, Herman Bruyninckx, Wen-Hong Zhu, and Mark WSpong. Force control: a bird’s eye view. In
Control Problems inRobotics and Automation , pages 1–17. Springer, 1998.[8] Niels Dehio, Joshua Smith, Dennis Leroy Wigand, Guiyang Xin, Hsiu-Chin Lin, Jochen J Steil, and Michael Mistry. Modeling and controlof multi-arm and multi-leg robots: Compensating for object dynamicsduring grasping. In , pages 294–301. IEEE, 2018.[9] Steven Eppinger and W Seering. Introduction to dynamic modelsfor robot force control.
IEEE Control Systems Magazine , 7(2):48–52,1987.[10] Yasutaka Fujimoto and Atsuo Kawamura. Proposal of biped walkingcontrol based on robust hybrid position/force control. In
Proceed-ings of IEEE International Conference on Robotics and Automation ,volume 3, pages 2724–2730. IEEE, 1996.[11] Neville Hogan. Impedance control: An approach to manipulation: Partii—implementation.
Journal of dynamic systems, measurement, andcontrol , 107(1):8–16, 1985.[12] Yifan Hou, Zhenzhong Jia, and Matthew T Mason. Manipulation withshared grasping.
Robotics: Science and Systems , 2020.[13] Yifan Hou and Matthew T Mason. Criteria for maintaining desiredcontacts for quasi-static systems. In
Proceedings of IEEE/RSJ Interna-tional Conference on Intelligent Robots and Systems . IEEE, November2019.[14] Yifan Hou and Matthew T Mason. Robust execution of contact-rich motion plans by hybrid force-velocity control. In
InternationalConference on Robotics and Automation (ICRA) 2019 . IEEE Roboticsand Automation Society (RAS), May 2019.[15] Hiroshi Ishikawa, Chihiro Sawada, K Kawasa, and Masayuki Takata.Stable compliance control and its implementation for a 6 dof manipu-lator. In
Proceedings, 1989 International Conference on Robotics andAutomation , pages 98–103. IEEE, 1989.[16] H Kazerooni, BJ Waibel, and S Kim. On the stability of robotcompliant motion control: theory and experiments.
Journal of DynamicSystems, Measurement, and Control , 112(3):417–426, 1990.[17] Oussama Khatib. A unified approach for motion and force control ofrobot manipulators: The operational space formulation.
IEEE Journalon Robotics and Automation , 3(1):43–53, 1987.[18] Ant´onio Lopes and Fernando Almeida. A force–impedance controlledindustrial robot using an active robotic auxiliary device.
Robotics andComputer-Integrated Manufacturing , 24(3):299–309, 2008.[19] J. Maples and J. Becker. Experiments in force control of roboticmanipulators. In
Proceedings. 1986 IEEE International Conferenceon Robotics and Automation , volume 3, pages 695–702, Apr 1986.[20] Matthew T Mason. Compliance and force control for computercontrolled manipulators.
IEEE Transactions on Systems, Man, andCybernetics , 11(6):418–432, 1981.[21] N Harris McClamroch and Danwei Wang. Feedback stabilizationand tracking of constrained robots.
IEEE Transactions on AutomaticControl , 33(5):419–426, 1988.[22] James K Mills and David M Lokhorst. Control of robotic manipulatorsduring general task execution: A discontinuous control approach.
TheInternational Journal of Robotics Research , 12(2):146–163, 1993.[23] Richard M Murray, Zexiang Li, and S. Shankar Sastry.
A MathematicalIntroduction to Robotic Manipulation . CRC Press, Inc., USA, 1stedition, 1994. [24] Fusaomi Nagata, Tetsuo Hase, Zenku Haga, Masaaki Omoto, andKeigo Watanabe. Cad/cam-based position/force controller for a moldpolishing robot.
Mechatronics , 17(4-5):207–216, 2007.[25] Hyeonjun Park, Jaeheung Park, Dong-Hyuk Lee, Jae-Han Park, Moon-Hong Baeg, and Ji-Hun Bae. Compliance-based robotic peg-in-holeassembly strategy without force feedback.
IEEE Transactions onIndustrial Electronics , 64(8):6299–6309, 2017.[26] Marc H Raibert and John J Craig. Hybrid position/force controlof manipulators.
Journal of Dynamic Systems, Measurement, andControl , 103(2):126–133, 1981.[27] J Kenneth Salisbury. Active stiffness control of a manipulator in carte-sian coordinates. In
Decision and Control including the Symposiumon Adaptive Processes, 1980 19th IEEE Conference on , volume 19,pages 95–100. IEEE, 1980.[28] J Kenneth Salisbury and John J Craig. Articulated hands: Force controland kinematic issues.
The International journal of Robotics research ,1(1):4–17, 1982.[29] G. W. Stewart. Collinearity and least squares regression.
StatisticalScience , 2(1):68–84, 1987.[30] Masaru Uchiyama and Pierre Dauchez. A symmetric hybrid posi-tion/force control scheme for the coordination of two robots. In
Robotics and Automation, 1988. Proceedings., 1988 IEEE Interna-tional Conference on , pages 350–356. IEEE, 1988.[31] Luigi Villani and Joris De Schutter. Force control. In
Springerhandbook of robotics , pages 161–185. Springer, 2008.[32] Richard Volpe and Pradeep Khosla. A theoretical and experimentalinvestigation of impact control for manipulators.
The InternationalJournal of Robotics Research , 12(4):351–365, 1993.[33] Harry West and Haruhiko Asada. A method for the design of hybridposition/force controllers for manipulators constrained by contact withthe environment. In
Proceedings. 1985 IEEE International Conferenceon Robotics and Automation , volume 2, pages 251–259. IEEE, 1985.[34] Daniel E Whitney. Historical perspective and state of the art in robotforce control.
The International Journal of Robotics Research , 6(1):3–14, 1987.[35] Tsuneo Yoshikawa. Dynamic hybrid position/force control of robotmanipulators–description of hand constraints and calculation of jointdriving force.