1. An Approach to Model-Based Control of Frictionally Constrained Robots
Aaron Greenfield
CFR Talk
Today I will be talking about an approach to model-based . . .
Mention Collaborators in this work are Uluc, Al, Howie.
Biorobotics Lab

2. Talk Outline 2. Planar Dynamics Model - Multi-Rigid-Body
1. Control Under Frictional Contact
2. Planar Dynamics Model
- Multi-Rigid-Body
- Coulomb Friction
3. Dynamic Response Calculation
4. Applications
MOVIE: Real Rhex Flipping
MOVIE: Real Snake Climbing
First we introduce the topic of control under frictional contact by looking at control tasks for two robotic systems, the dynamics model we use for these systems, and the main quantity we would like to infer from the model, what we call the dynamic response function. After this intro, we will delve more deeply into the details of the dynamics model, and then present an algorithm for computing the dynamic response function. Finally we will apply this algorithm to our two desired applications
Of flipping and climbing
(Saranli)
(Borer)
Slide 2 / 26

3. Control Tasks with Frictional Contact
RHex Flipping Task
Snake Climbing Task
So here are two control problems with frictional contacts we would like to consider. In the first one, we have a model of the RHex robot which we would like to flip. The motivation for this is that the RHex robot often runs on rugged terrain and can flip over into the improper orientation, which results in a loss of locomotion ability. The model is subject to frictional contacts between the body and the ground and the legs and the ground and the
Control problem is to choose the hip actuations so that the robot flips over.
In the second task we have a hyperredundant robot which we would like to climb upward. The system is subject to frictional contact at a variety
Of locations and the goal is to choose the joint controls such that the snake climbs. This could be useful in an urban search and rescue scenario, for example.
Our presumption, when choosing controls for these systems is that the specific physics. . .
Therefore our approach will be to utilize a model… and then select the control which acheives the desired behavior.
Presumption: The physics of contact is critical to the robot’s performance
Approach: ● Utilize a model of robot dynamics under contact constraints
● Solve for behavior as a function of control input
Slide 3 / 26

4. Dynamics Model: Multi-Rigid-Bodies and Coulomb Friction
Dynamic Equations: 2nd order ODE relates coordinates, forces
Rigid Body Model: No penetration, Compressive Normal Force
Friction Model: Tangential Force Opposing Slip
If you want to do model based control of dynamic systems with friction, need a model of these dynamics. We select one which models these systems
As multi-rigid-bodies, with frictional contact WITH THE ENVIRONMENT being modeled with coulomb’s law.
Under these assumptions the dynamic equations take the form of a second order ODE between the coordinates describing the robot’s configuration, shown in the figure, and the forces acting on the robot. The forces on the robot include gravity, applied forces, and environmental reaction forces. The reaction forces are determined by our model with the rigid-body model imposing a compressive normal force which prevents penetration, and coulomb friction generating a tangential force which opposes sliding. The magnitude of the tangential force is bounded by coulomb’s law which limits its value to a coefficient of friction times the normal force, with a transition to sliding or sliding implying the tangential force is at a maximum.
The positives of this model are. . .
The negatives of this model are . . .
Add something on impact, discontinuous velocity neg. Put coords in here
Mention have a finite set of coords, and a variety of forces acting on system. Compressive Normal Force prevents penetration, Just mention other Variants, Mention Good Approximation
● Small number of coordinates
● Simple Contact Model (1 parameter)
Advantages
Disadvantages
● No Body is perfectly rigid
● Coefficient of friction can be hard
to determine, non-static
● Solution Ambiguities and
Inconsistencies can exist
Slide 4 / 26

5. Dynamic Response Function
Accelerations
Reaction Forces
Control Inputs
System State
Ambiguity Variables
where
Why compute it?
To select control inputs which achieve
desired instantaneous behavior
How do we compute it?
By solving a series of linear systems of equalities
and inequalities consisting of:
1) Lagrange’s equation )Contact constraints
Why is it hard to compute?
Non-linearity, Solution Ambiguity, Inconsistency,
Inequality Constraints
What is this function?
Relates instantaneous behavior to
controls and ambiguity for a particular
One we have this model, the main quantity we try to infer from it is what we call the dynamic response function G. This function gives us all the instantaneous unknown in the model, which are the accelerations of the robot qDD, and the reaction forces on the robot f, as they depend on our control input tau. Unfortunately, because of the possibility of solution ambiguity mentioned in the previous slide, there may be multiple solutions for a particular control torque, so we also introduce into this function what we call ambiguity variables w, which parametrize the set of solutions possible. Put another way, if we fix a particular torque tau, the uncontrolled ambiguity variables will sweep out the set of model solutions possible at that torque. For example, a table resting on several rigid legs may have an ambiguous load of reaction forces f distribtued on the legs.
(Skip What is function?)
So Why do you want to compute this functon?
TALK About AMBIGUITY VARIABLES-REPRESENT STRUCTURAL FORCES OR FORCES AND ACCELERATIONS
Once we have this model, the main quantity we try to derive from it is what we call the dynamic response function. This function describes all instantaneous unknowns in the model, the acceleration and reaction forces, as a function of the applied control input and some uncontrolled ambiguity variables. (EXPLAIN AMBIGUITY VARIABLES IN BRIEF)
Slide 5 / 26

6. Related Research Single Rigid Bodies
Ambiguities with Rigid Object, Two Walls. (Rajan, Burridge, Schwartz 1987)
Configuration Space Friction Cone. (Erdmann 1994)
Graphical Methods. (Mason 2001)
Multi-Rigid-Bodies:Modeling and Simulation
Early Application of LCP. (Lostedt 1982)
Lagrangian dynamics and Corner Characteristic. (Pfeiffer and Glocker 1996)
3D Case, Existence and Uniqueness Extensions. (Trinkle et al. 1997)
Framework for dynamics with shocks (J.J. Moreau 1988)
Early Application of Time Sweeping. (Monteiro Marques 1993)
Formulation Guarentees Existence. (Anitescu and Potra 1997)
Review of Current Work. (Stewart 2000)
Go Quickly, Otherwise too much info too early
Multi-Rigid-Bodies: Control
Computing Wrench Cones. (Balkcom and Trinkle 2002)
(MPCC) Mathematical Program with Complementarity Constraint. (Anitescu 2000)
Application of MPCC to Multi-Robot Coordination. (Peng, Anitescu, Akella 2003)
Stability, Controllability, of Manipulation Systems. (Prattichizzo and Bicchi 1998)
Open Questions for Control of Complementary Systems. (Brogliato 2003)
Slide 6 / 26

7. Dynamics Equations (Pfeiffer and Glocker) Two coordinate systems
(1) Generalized Coordinates
(2) Contact Coordinates
Related by
Ok, now let’s delve more deeply into the details of the model of the dynamics of these systems we use. We utilize two sets of coordinates . .. This coordinate systems are related by the kinematic relations The dynamic equations on the generalized coordinates can be written using Lagrange’s equations with reaction forces expressed in the contact coordinate frame mapped onto these equations using the Jacobian.
Mention here point contact assumption when mention contact coordinates. Note we could write dynamics on other coordinate set by inverting M and plugging qDD into Jacobian Relation (Maybe Don’t Mention). Primary Use of Second Coordinate System is in mapping of reaction forces and expressing
Constraints on reaction forces as we see in the next slide.
Dynamic Equations on Generalized Coordinates:
Joint Actuations
Reaction Forces
Slide 7 / 26

8. Contact Force Constraints
Key Points on Contact Model
(1) Reaction Forces are NOT an explicit function of state
(2) Reaction Forces ARE constrained by state, acceleration
Normal Force-Acceleration
(Rigid Body)
Tangential Force: Acceleration
(Coulomb Friction)
Contact
Point
--Motion-Force Relationship forces on the horizontal axis
Dynamic Equations are only one part of dynamics model, the other part is our contact model which helps determine the reaction forces. Since the model helps to specify the reaction forces, these are not free variables. However, our contact model does not immediately give us there values as a function of the known states and control, instead the model gives
Us a set of constraints on reaction force. Take the contact point shown in the figure, the model tells us we have constraints between force and motion in both the normal direction shown because of the rigid-body assumption and the tangential shown because of coulomb friction.
First consider the normal direction. If the contact point has no velocity in this direction, then the model gives us a constraint with essentially two branches: In the first, the contact point on the robot is accelerating away from, meaning that the bodies will separate and therefore no compressive normal force is allowed. In the second branch, the normal acceleration is zero, so the bodies are maintaining contact, and a compressive or positive normal force is allowed.
If the contact point has a seperating velocity, then the compressive normal forces is still contrained to be 0, but the acceleration is uinconstrained, and if the contact point has a velocity into the surface, then an impact must occur, requiring an impact model which is not developed in this talk.
Now consider the tangential direction, which we need to consider if the contact point is not seperating. If the contact point has no tangential velocity,
Then we have a three branch constraint between force and motion in this tangential direction. List these. If we have some tangential velocity, then the force will again be constrained with no constraint on the acceleration.
Slide 8 / 26
(Pictures adapted from Pfeiffer,Glocker 1996)

9. Complete Dynamics Model?
Dynamics Model
Desired Solution
AND
Normal Constraints
Tangential Constraints
What exactly to say here
--Constraint Nonlinearity Challenging
Consider Branches Separately
Slide 9 / 26

10. Contact Modes
Contact Modes: Separate (S), Slide Right (R) and Left (L), Fixed (F)
Normal Direction
Tangential Direction
(S)
(R)
(L,R,F)
(F)
(L)
Constraints in Matrix Form
Mode
Equality Constraints
Inequality Constraints S L R F
This can go more quickly then it did when I explained it to Dave. Description of the modes, example of seperating mode, and mention we can encode all these constraints in matrix form
--So if we assume a particular contact mode for each contact point, our contact model consists of a set of linear equalities and inequalities
So our dynamics model gives us these non-linear constraints on the reaction forces. In terms of finding solutions to the model however, it might be attractive to “cut” these constraints to make them into a discrete number of linear constraints. Call each cut of these constraints a contact mode, which has a lot of physical meaning. The four possible contacts modes are {list}.
Thus for each contact point, the assumption of a particular mode for it imposes a set of linear equality and inequality constraints on the dynamic equations on the contact coordinates. If we assume a particular mode for each contact point, and use the Jacobian to map the acceleration constrains on to generations coordinates, then we get a set of linear equality and inequality constraints on the generalized coordiante and reaction forces.
On the picture on the left, whenever the contact point is acceleting away, or has some velocity away, from the surface, the mode is sepearing, the other branch is non-separing. If the mode of a contact is separating, then the linear constraints are than normal force and tangential force are 0, and acceleration is greater than 0. For the non-sepating modes, we look at the tangential motion. ….
So for each contact point
…..
So each contact mode
Another Slide with Linear Algebra of Contact Mode Constraints. For each contact
We turned nonlinear constraints between force and motion into linear equality and inequality constraints on force
And motion by assuming a particular contact mode
Slide 10 / 26

11. Form of Dynamic Response
Contact Mode Specific
Dynamics Model
Contact Mode Solution
AND
Form of Total Solution
Under the assumption of a particular contact mode we have. . .
Iterate over the contact modes and solve each one.
Mention ambiguities inconsistencies
Add something describing solution of single contact mode.
● Linear function from equality constraints
● Polytope domain from inequality constraints
Slide 11 / 26

12. Solving for Response Function
Consider equality constraints only
● Contact Mode Acceleration Constraints
● Contact Mode Force Constraints
● Dynamical Constraints
(Group terms)
Split Slide into two. Just mention would A is not f.r.r.
Solve constraints based on rank- 4 cases
is f.r.r. and f.c.r
is f.r.r. but not f.c.r.
Slide 12 / 26

13. Solving for Response Domain
Now consider inequality constraints
● Contact Mode Acceleration Constraints
● Contact Mode Force Constraints
Substitute to eliminate acceleration, forces
This solutions to this system describe the set of torques and ambiguity variables, for which the functionally dependent qDD and f satisfy all equality and inequality constraints on the system. However, unlike equality systems, it is difficult to determine the characteristics of this solution set just by looking at this system of half-space constraints. In particular, it is not immediately obvious whether the set is empty of not. What we do, is convert this system into what is called a minimal representation by using Linear Programs. What this does is it takes the intial set of half-space contraints, and eliminate those which are not supporting, a.k.a, do not lie on the boundary of the polytope. If all half-spaces are eliminated then we know the set is empty, otherwise it is not empty and we have the smallest representation possible for the polyotpe.
Split into two?? Talk about what a Linear Program, is and does…
Linear Program Used to make this more desciptive, eliminate meaningless half-space constraints, and indicates whether result is non-empty
Reduce inequality constraints
Non-Supporting
Supporting
Use Linear Programs to generate
minimal representation:
Slide 13 / 26

14. Response Domain on Control Input
Description
● Domain of on BOTH
control inputs, ambiguity variables
Description
● Domain of on ONLY
control inputs
The contact mode domain Dc, that we just computed from on the previous page, characterizes the set of ALL solutions in the contact mode, including all torques which produce a solution in the contact mode, and the range of ambiguity w at each torque. The problem with this characterization is that can difficult to determine whether a particular control torque belongs to the domain, which can be important for control purpose.
For example, consider the rigid table resting on two legs, which we would like to keep fixed with respect to the ground. It so happens that this table will have an ambiguity in the reaction forces, and so the domain Dc will have ambiguity variables in it.
If we just want to know the forces we could apply while keeping the table fixed, a simplied domain Tc will express this. This simplied domain is the projection of the original domain Dc on to the control axes and eliminates the information about the range of ambiguity to therefore more clearly express information about the control inputs.
We also compute a SIMPLIFCATION of this domain which we call Tc, which just describes the control torques consistent with the mode without characterizing the range of ambiguity for each torque. For example, if we are characterizing the set of applied forces which keep a rigid table from sliding, it so happening that for each torque
--Mention structural forces or forces and acceletations-MENtion
--Think about this somemore
The contact mode domain Dc, that we just computed from on the previous page, characterizes the set of ALL solutions in the contact mode, including all torques which produce a solution in the contact mode, and the range of ambiguity w at each torque. We also compute a SIMPLIFCATION of this domain which we call Tc. Tc is domain of the contact mode expressed in terms of only control torques, and NOT ambiguity variables. This is a useful characterization, for example, when we want to know the set of controls which keep a table fixed against the floor, but are not as intested in the range of model ambiguity, in this case stuctural forces, at a particular torque.
To derive Tc, we take Dc, and use a polytope projection technique such as Fourier-Motzin. The projeciton of Dc is Tc.
Mention (Structural Forces) (Yes, Tc, simplier, what we control, for example immobilization)
So we just computed the function domain Dc, which describes the domain of the contact mode c by restricting the control torques and ambiguity variables for which the contact mode occurs. However, the ambiguity variables are not something that is controlled, they represent uncertainties in the model response. We would therefore like to characterize the domain of the contact mode response in terms of just the quantities we can control, namely the control inputs. This domain we call Tc and we can compute it by projecting the polytope Dc on to the Torque axes.
So now we have a characterization of set of torques for which the contact occurs, although there may be a range of solutions at this torque as parametrized by w
Fourier-Motzkin is Gaussian Elimination of Inequalities, Dc describes domain of function,Tc domain of tau for which there exists a w
GROUP WITH AMBIGUITY SECTION??
Computation
● Polytope Projection by Fourier-Motzkin.
● Reduce by Linear Program
Slide 14 / 26

15. Mode Enumeration Two pruning techniques
Do we need to repeat this process for all ? Not necessarily.
Two pruning techniques
(1) Contact point velocity: Necessary (2) System Freedoms: Computational
Normal Velocity
Tangential Velocity
Opposite Accelerations
Existence of Solution to:
So the function g, its domain, and simplified domain are all the quantities we compute for a single mode. Do we need to compute these quantities for all possible modes? Not necessarily, we can first prune the number of modes we need to consider in two ways. First, if a contact point has some initial velocity, only certain modes will be possible For example, if the contact has a normal velocity, the point can only separate. If the point has a tangential velocity, the point can only continue to slide in the same direction or separate. Secondly, certain contact modes will be impossible because the system doesn’t have enough degrees of freedom to execute them, for example, the rigid body cannot slide its two contact points in opposite directions.
Normal Vel.
Tangential Vel.
Modes
---- S
S,R
S,L
F,S,L,R
(Graphical Methods. Mason 2001)
Denote Reduced number of Modes:
Slide 15 / 26

16. Algorithm Summary
Goal: Characterize system dynamics as a function of control input
Approach: Break up by contact mode, solve each mode
Algorithm Steps:
(1) Computed Mode Response
(2) Computed Mode Response Domain
(3) Computed Modes we need to consider
Broke down problem into modes, due to linearity within each mode, Move this up after mode enumeration?
Now look at how ambiguity of solution is visible in this function .. .
Slide 16 / 26

17. Solution Ambiguity Ambiguity Definition:
Multiple solutions exist for a particular
Two Ambiguity Types: (Pfeiffer, Glocker 1996)
(1) Between Modes (2) Within Mode
Multiple Domains contain same
Single Function has
Ok, so one of the things we may have to be concerned with when using the dynamics response function to make control decisions is for what torques are there ambiguities of solution.
One more topic to explore before moving on to applications is how ambiguity is represented in the dynamic response function.
Then read slide
Slide 17 / 26

18. Solution Ambiguity: Between Modes
Characterization: Domain Intersection
Unambiguous Set
In this slide we focus on the between mode ambiguities. Recall that for each mode we compute the set Tau_c, which is the set of torques
For which there is a solution consistent with the mode. In the picture we have drawn two such regions, i and j. Whenever this regions overlap, as shown in the gray, there belong solutions to both modes, and thus an ambiguity as to which mode occurs. Consider as an example a ball in a wedge. Intuitively, because of the rigid-body, we cannot determine the compression of the body between the two contact points. … This would be reflected in the sets Tc overlapping for the two modes.
What we can do is compute the set of torques for which the system possess a unique mode by subtracting pairwise intersections
Use Example From Rajan et al?
Example
Fall (SS) or Stick (FF)
(Brogliato)
Slide 18 / 26

19. Solution Ambiguity: Within Mode
Characterization: Response Function
Ambiguity Variable
Examples
The other type of ambiguity of solutions is within a mode, and this type of ambiguity is reflected in the ambiguity variables w. In this case, the behavior of the system is ambiguous even under the assumption of a particular contact mode. In the picture to the right, we see even for a particular control torque, tau1prime, tau2prime, there is still a range of possible ambiguity variables w, with each particualr value corresponding to a different solution. In the exampls, we see a fixed table with unknown tangential reaction forces, a ball
Sliding at all its contact points, with an unknown deceleration due to an unknown torsional load.
Unknown Rotational Deceleration
Unknown Tangential Forces
Slide 19 / 26

20. Application to RHex Flip Task
Task Description
Initial Configuration
Final Configuration
Ok, so with this algorithm in mind let’s look at our first example of Rhex flipping
No body separating to reduce out of plane dynamics
High-level Task Description:
Flip RHex Over
No Body Separation until past vertical (Saranli)
Technical Task Description:
Maximize pitch acceleration
No separating contact mode
Slide 20 / 26

21. RHex Model Details Generalized Coordinates Contact Coordinates
To put the model in the algorithm framework, we start by writing its coordinates in the two different coordinate systems
Other Model Details
Legs Massless
Body Mass Distribution: C.O.M at center, Inertia
Body Friction Toe Friction
Slide 21 / 26

22. Algorithm Outline and Simulation
Input:
Output:
Dynamic Response
Calculate Possible Modes
Compute Response
Compute Domains
Ambiguities
Compute Unambiguous Regions
Optimize
Optimize over
Subject to no body separation
So how do we use the dynamic response function to help generate controls which flip this system? ( 1,2,3 are the algorith, quickly)
Then we optimize alpha double dot over the unambiguous region, which is in this case a linear program.
(Saranli)
Slide 22 / 26

23. Application to Snake Climbing Task
Task Description
Initial Configuration
Final Configuration
Try to move the snake from its initial configuration, with the legs tightly compressed together, into its final configuration where we have kept the bottom of the snake fixed,
But extened the top portion of the snake upward.
Generate a single control for snake.
Try to do it for a range of disturbance forces
High-level Task Description:
Immobilize Lower ‘V’-Brace
Disregard Controls for Remainder-Free
Technical Task Description:
Ensure ‘FFF’ contact mode
Reduce to a Disturbance
Slide 23 / 26

24. Snake Model Details Generalized Coordinates Brace Free
Contact Coordinates
Other Model Details
Single friction coefficient
Point masses at each joint
where
Brace Dynamics
Slide 24 / 26

25. Algorithm Outline Input: Output: Parameterize Disturbance Forces
Calculate disturbance set
Dynamic Response Function
Calculate Possible Modes
Compute Response
Compute Domains
Robust Ambiguities
Compute Unambiguous Region
for all disturbances
(Pure Animation)
Disturbance Forces
Slide 25 / 26

26. Conclusion
● Objective: An approach to model-based control of frictionally
constrained robots
● Dynamics Model: Multi-Rigid-Body with Coulomb Friction
● Model Prediction: Generate the dynamics response function
● Application: RHex flipping and Snake Climbing
Slide 26 / 26

27. END TALK

28. Movie, Rhex Flip
(Pure Animation)
Slide 24 / 25

29. 4 Cases is f.r.r. but not f.c.r. is f.r.r. and f.c.r
is not f.r.r. and f.c.r
is not f.r.r. and not f.c.r.
when
when
otherwise no solution
otherwise no solution