Robotic Contact Juggling J

Home , Contact juggling, Object manipulation

Robotic Contact Juggling J. Zachary Woodruff, (Member, IEEE), and Kevin M. Lynch, (Fellow, IEEE)

Abstract—We define “robotic contact juggling” to be the purposeful control of the motion of a three-dimensional smooth Start object as it rolls freely on a motion-controlled robot manipulator, or “hand.” While specific examples of robotic contact juggling have been studied before, in this paper we provide the first general formulation and solution method for the case of an arbitrary smooth object in single-point rolling contact on an arbitrary smooth hand. Our formulation splits the problem into four subproblems: (1) deriving the second-order rolling End kinematics; (2) deriving the three-dimensional rolling dynamics; t = 0 t = tf (3) planning rolling motions that satisfy the rolling dynamics; and (4) feedback stabilization of planned rolling trajectories. The Fig. 1. Example of a contact juggling skill known as “the butterfly.” A smooth theoretical results are demonstrated in simulation and experiment object is initially at rest in the palm of the hand, and the motion of the hand using feedback from a high-speed vision system. causes the object to roll to the back of the hand.

I.INTRODUCTION simulation and experiment using feedback from a high-speed ONTACT juggling is a form of object manipulation vision system. C where the juggler controls the motion of an object, often a crystal ball, as it rolls on the juggler’s arms, hands, torso, or even shaved head. The manipulation is nonprehensile (no A. Background form- or force-closure grasp) and dynamic, i.e., momentum plays a crucial role. An example is shown in Figure 1. This is When a three-dimensional rigid body (the object) is in a variation of a contact juggling skill called “the butterfly,” and single-point contact with another rigid body (the hand), the robotic implementations of the butterfly have been described configuration of the object relative to the hand is five dimen- in [1]–[3]. The object (typically a ball) is initially at rest on sional: the six degrees of freedom of the object subject to the the palm, and the goal state is rest on the back of the hand. single constraint that the distance to the hand is zero. This The hand is accelerated to cause the object to roll up and over five-dimensional configuration space can be parameterized by the fingers to the other side of the hand. two coordinates uo = (uo, vo) describing the contact location We define “robotic contact juggling” to be the purposeful on the surface of the object, two coordinates uh = (uh, vh) control of the motion of a three-dimensional smooth object describing the contact location on the surface of the hand, as it rolls freely on a motion-controlled robot manipulator, and one coordinate ψ describing the angle of “spin” between or “hand.” Specific examples of robotic contact juggling have frames fixed to each body at the contact point. Collectively been studied before, such as the butterfly example mentioned the contact configuration is written q = (uo, vo, uh, vh, ψ) above and specific geometries such as a sphere rolling on (Figure 2). a motion-controlled flat plate. This paper extends previous Rolling contact is maintained when there is no relative linear work by providing the first general formulation and solu- velocity at the contact vrel = (vx, vy, vz) = 0 (i.e. no slipping arXiv:2102.10421v1 [cs.RO] 20 Feb 2021 tion method for the case of an arbitrary smooth object in or separation). For rolling bodies modeled with a point contact, single-point rolling contact on an arbitrary smooth hand. no torques are transmitted through the contact, and relative Our formulation splits the problem into four subproblems: spin about the contact normal is allowed. We refer to this (1) deriving the second-order rolling kinematics; (2) deriving as “rolling.” For rolling bodies modeled with a soft contact, the three-dimensional rolling dynamics; (3) planning rolling torques can be transmitted and no relative spin about the motions that satisfy the rolling dynamics and achieve the contact normal is allowed (ωrel,z = ωz = 0). We refer to desired goal state; and (4) feedback stabilization of planned this as “pure rolling” (or “soft rolling”). rolling trajectories. The theoretical results are demonstrated in

This work supported in part by the NSF Graduate Research Fellowship B. Paper Outline Program under Grant DGE-1324585, and in part by the NSF under Grant IIS-1527921. As mentioned above, our approach to contact juggling J. Z. Woodruff ([email protected], corresponding author) and K. M. Lynch ([email protected]) are with the Center for divides the problem into four subproblems: (1) deriving the Robotics and Biosystems (CRB), Northwestern University, Evanston, IL second-order rolling kinematics; (2) deriving the rolling dy- 60208 USA. K. M. Lynch is also afﬁliated with the Northwestern Institute on namics; (3) planning rolling motions that satisfy the dynamics; Complex Systems, Evanston, IL 60201 USA. Open source code that accompanies this paper can be found at: https: and (4) feedback stabilization of rolling trajectories. An outline //github.com/zackwoodruff/rolling dynamics of each subproblem is given below. 2

1) Rolling Kinematics: First-order kinematics models the vh uo evolution of contact coordinates q between two rigid bodies increasing increasing when the relative contact velocities are directly controlled. y ch The second-order kinematics is a generalization of the first- y order model where the relative accelerations at the contact co nc c h,ph are controlled. The second-order kinematics is used in our o o { } h { } n { } derivation of the rolling dynamics to describe the evolution of v c o,po ch u o { } x h the contact coordinates during rolling motions. In Section IV increasing ch increasing we describe the second-order kinematics, which include the ψ x work of Sarkar. et al. [4], our corrections to that work [5], co and our new expression for the acceleration constraints that Object Hand enforce pure rolling. 2) Rolling Dynamics: Given the acceleration of the hand Fig. 2. The object and hand are in contact at the origin of frames {co} and and the state of the hand and the object, the rolling dynamic {ch}, but they are shown separated for clarity. Two coincident contact frames {pi} and {ci} for i ∈ [o, h] are given for each body at the contact, where equations calculate the acceleration of the object and the {pi} is fixed to the object and {ci} is fixed in the inertial frame {s}. The contact wrench between the hand and the object. In Section V surfaces of the object and hand are orthogonally parameterized by (uo, vo) and (u , v ), respectively. At the point of contact, the x - and y -axes of we combine the rolling kinematics with the Newton-Euler h h ci ci the coordinate frames ({ci},{pi}) are in the direction of increasing ui (and dynamic equations to derive an expression for the rolling constant vi) and increasing vi (and constant ui), respectively, and the contact dynamics. This formulation enforces rolling contact and calcu- normal nci is in the direction xci ×yci . Rotating frame {ch} by ψ about the n -axis of frame {c } aligns the x -axis of frame {c } and the x -axis lates the required contact wrench, which then can be checked co o ch h co of frame {c }. to see if it satisfies normal force constraints (no adhesion) and o friction limits. We validate the simulation of rolling dynamics using the analytical solutions of a sphere rolling on a spinning a generalization of the first-order model where the relative plate. accelerations at the contact are specified. 3) Rolling Motion Planning: In Section VI we use direct Montana derives the first-order contact kinematics for two collocation methods and the rolling dynamics equations to plan 3D objects in contact [6]. His method models the full five- dynamic rolling motions for an object rolling on a manipulator. dimensional configuration space, but it is not easily gener- Given an initial state, we find a set of manipulator controls that alized to second-order kinematics. Harada et al. define the brings the system to the goal state. concept of neighborhood equilibrium to perform quasistatic 4) Feedback Control: In Section VII we demonstrate the regrasps of an object rolling on a flat manipulator using use of a Linear Quadratic Regulator (LQR) feedback controller Montana’s kinematics equations [7]. First- and second-order to stabilize a nominal rolling trajectory. contact equations were derived by Sarkar et al. in [4] and The motion planning and feedback control are validated republished in later works [8], [9]. Errors in the published experimentally in Section VIII. equations for second-order contact kinematics in [4], [8], [9] A video that accompanies this paper can be found in the sup- were corrected in our recent work [5]. Another of our recent plemental media or at: youtu.be/QT55 Q1ePfg. Open-source works covers motion planning and feedback control for first- MATLAB code to derive and simulate open-loop rolling can order-kinematic rolling between two surfaces [10]. be found here: github.com/zackwoodruff/rolling dynamics. Each of [4]–[10] assumes an orthogonal parameterization, as shown in Figure 2. Recent work by Xiao and Ding derives the C. Statement of Contributions second-order kinematics equations for non-orthogonal surface parameterizations [11]. This paper provides the first formulation of the rolling dynamics of a smooth rigid body rolling in point contact on a second, motion-controlled, smooth rigid body. We show that B. Dynamic Rolling the equations can be used for simulation or in optimization- based motion planning considering constraints on contact The evolution of dynamic rolling systems is governed by normal forces and friction limits. By linearizing the dynamics, forces and torques at the contact. We review planar and spatial we can derive feedback controllers to stabilize the motion rolling for two- and three-dimensional models. Planar rolling plans. Finally, we validate motion planning and feedback is holonomic whereas spatial rolling is nonholonomic, but both control for contact juggling in simulation and experiment. present challenging problems in motion planning and control. 1) Planar Rolling: The butterfly example shown in Figure 1 II.RELATED WORK was first introduced by Lynch et al. [1]. Cefalo et al. [2] demonstrate energy-based control of the butterfly robot to A. Kinematic Rolling move a ball to an unstable equilibrium and stabilize it using The rolling kinematics equations describe the evolution of a linear approximation of the system. Surov et al. [3] plan the contact coordinates during rolling motions. First-order and stabilize rolling motions for the butterfly robot using kinematics addresses the rolling problem where the relative virtual holonomic constraint-based planning and transverse contact velocities are given. The second-order kinematics is linearization-based orbital stabilization. 3

Taylor and Rodriguez perform shape optimization of ma- is the first to formulate the rolling dynamics of a rigid body nipulators and motion planning for dynamic planar manipula- rolling on a six-DoF motion-controlled manipulator for general tion tasks [12]. Ryu and Lynch derive a feedback controller manipulator and object shapes. that enables a planar, disk-shaped manipulator to balance a disk while tracking trajectories [13]. Erumalla et al. perform III.NOTATION throwing, catching, and balancing for an experimental setup For general variables, matrices and vectors are bold upper- of a disk on a disk-shaped manipulator [14]. Lippiello et al. case (e.g., R) and bold lowercase (e.g., r) letters, respectively. develop a control framework for nonprehensile planar rolling Scalars are non-bold italic (e.g., u), and coordinate frames dynamic manipulation and validate it experimentally for a disk are expressed as lowercase letters in curly brackets (e.g., on a rotating disk [15]. Serra et al. [16] apply a passivity-based s ). Notations for variables and operators (mappings between approach to the same problem. spaces){ } are shown in Table I, and selected operator expressions 2) General Spatial Rolling: General methods exist for sim- are given in Appendix A. ulating rigid bodies in contact, some of which explicitly handle The space frame (i.e., inertial reference) is defined as s , rolling contacts. Anitescu et al. develop a general method for and the hand frame is define as h . The object frame { o} contact dynamic simulations [17]. It uses a complementary for- is located at its center of mass and{ aligned} with the principal{ } mulation that allows simulation of multiple rigid bodies, and axes. Four frames with coincident origins, pi and ci (i uses the first-order-rolling equations from Montana [6] to solve o, h ) are defined at the current object-hand{ } contact,{ } where∈ for contact constraints. Liu and Wang develop a time-stepping { } pi is fixed relative to the body frame i and ci is fixed method for rigid-body dynamics that specifically addresses in{ }s (see Figure 2). { } { } rolling contacts [18]. Duindam et al. model the kinematics Double{ } subscripts indicate a comparison between two and dynamics of compliant contact between bodies moving in frames expressed in the frame of the first subscript. For Euclidean space [19]. There are many software packages for example, R gives the rotation matrix relating frame o performing dynamic simulations [20], and a subset of those so { } relative to frame s expressed in s , and sh gives the can explicitly handle rolling constraints. Because each object twist of the hand relative{ } to the space{ frame} sV expressed in state is represented as an independent six-dof rigid body, these s . In deriving the kinematics and dynamics{ } equations, we methods allow for non-zero contact distances (separation or {often} compare the relative velocities between different frames penetration) and are not specifically focused on modeling the expressed in a third frame which we denote with a preceding rolling interaction between objects. superscript. For example, ch gives the twist of frame 3) Prehensile Spatial Rolling: phpo There are many works that p relative to p expressedV in the c frame and is address prehensile motion planning for a ball (sphere) that is { o} { h} { h} equivalent to [AdT ]( sp sp ), where [AdT ] is the in contact with a stationary plate and actuated by a second, chs V o − V h ab 6 6 adjoint map matrix associated with Tab SE(3), the opposing, plate [21]–[25]. Sarkar et al. expand the second- transformation× matrix representing the frame ∈b relative to order kinematics equations to generate a dynamic model for i { } frame a . “Body twists” are defined as si, and represent an object caged between two surfaces [8]. They use feedback the twists{ } of the body relative to the spaceV frame, expressed linearization to control dynamic rolling motions for two planes in its own coordinate frame i . Variables must be expressed in contact with a sphere. in the same frame to compare,{ } and we resolve the kinematics 4) Nonprehensile Spatial Rolling: Jia and Erdmann derive and dynamics expressions in the contact frame of the hand dynamic equations and show the observability of an object ch . with an orthogonal parameterization on a flat plate equipped { The} surface of each body is represented by an orthogonal with a contact position sensor [26]. Choudhury and Lynch 3 parameterization: Fi : ui R :(ui, vi) (xi, yi, zi), where stabilize the orientation of a ball rolling in an ellipsoidal the coordinates (x , y , z→) are expressed7→ in the i frame. dish actuated along a single degree of freedom [27]. Both i i i { } We assume that Fi is continuous up to the third derivative these works assume no rotational motion of the hand which (class C3), so that the local contact geometry (contact frames simplifies the modeling and control problem. associated with the first derivative of Fi, curvature associated Gahleitner models a sphere with three rotation inputs bal- with the second derivative, and derivative of the curvature ancing another sphere, designs a stabilizing controller, and associated with the third derivative) are uniquely defined. validates the results experimentally [28]. Additionally there Standard expressions for the local geometry of smooth bodies are numerous papers on control of an object in nonprehensile are given in Appendix B-A. contact with a plate that control the contact location on the hand but do not consider the orientation of the object [29], [30]. To our knowledge, only one paper studies dynamic, non- IV. ROLLING KINEMATICS prehensile planning for a ball on a plate while considering the full position and pose of the ball [31]. Works by Milne (part A. First-Order Rolling Kinematics III.XV of [32]) and Weltner [33] derive analytical solutions The contact configuration of two bodies in rolling contact for a sphere rolling on a rotating plate with a constant angular can be parameterized by q = (uo, vo, uh, vh, ψ) (see Figure 2). velocity, and we use these solutions to validate our rolling First-order kinematics models the evolution of contact coor- simulation in Section V. dinates between two rigid bodies when the relative contact As mentioned in Section I-C, to our knowledge this paper velocities are known. The relative twist at the contact in 4

T TABLE I relative linear accelerations arel = [ax ay az] . The second- NOTATION order kinematics from [4] can be expressed in matrix form Variable Description Dimensions as: {·} Coordinate frame - q¨ = K2(q, ωrel) + K3(q) ˙ rel, (3) r Position vector r = (x, y, z) 3 × 1 V Φ xyz Euler angles Φ = (θ, β, γ) 3 × 1 where the terms K2(q, ωrel) and K3(q) are given in Ap- R Rotation matrix R ∈ SO(3) 3 × 3 pendix B-C. T Transformation matrix T ∈ (3) × SE 4 4 This general form allows relative sliding at the contact. To ω Rotational velocity ω = (ωx, ωy, ωz) 3 × 1 v Linear velocity v = (vx, vy, vz) 3 × 1 maintain rolling, ˙ rel must lie in a three-dimensional subspace V Twist V = (ω, v) 6 × 1 satisfying V α Rotational accel α =ω ˙ = (αx, αy, αz) 3 × 1 a Linear accel a = v˙ = (ax, ay, az) 3 × 1 T ch arel = aroll = [ax ay az]roll = ωrel vcoo, (4) V˙ Change in twist V˙ = (α, a) 6 × 1 − × q Contact coords q = (uo, vo, uh, vh, ψ) 5 × 1 as derived in Eq. (60) of [4]. To maintain pure rolling, ˙ rel must G Spatial inertia matrix 6 × 6 V F Wrench F = (τx, τy, τz, fx, fy, fz) 6 × 1 lie in a two-dimensional subspace additionally satisfying the s Dynamic rolling states 22 × 1 constraint αz = αz,pr, which is different from the result found h ˙ ξ State and control pair (s, Vsh) 28 × 1 in [4], and is derived in Appendix B-C1. For the case of pure Operator Description (expressions in Appendix A) Mapping rolling ωz = 0, and the relative x and y linear acceleration T 3 constraints in Eq. (4) simplify to [a a ] = 0. [ · ] Vector to skew-symmetric form R 7→ so(3) x y roll 6×6 [AdT] Adjoint map associated with T SE(3) 7→ R An expression for the body accelerations of the object [ad ] V 6 7→ 6×6 o h V Lie bracket matrix form of R R ˙ so as a function of the hand accelerations ˙ sh and the F(u) Surface parameterization (u, v) 7→ (x, y, z) V V relative accelerations ˙ rel is used in the dynamics derivation in Section V. TakingV the derivative of Eq. (2) in frame

ch T ch (following the derivative rule for expressions in different ch is given by p p = rel = [ωx ωy ωz vx vy vz] . { } { } V h o V frames from Appendix A-B) gives: To maintain contact, the non-separation constraint vz = 0 o h must be satisfied. Enforcing the constraints vx = vy = 0 ˙ so =[AdT ] ˙ sh + [AdT ] ˙ rel V oh V och V (5) ensures rolling without linear slip in the contact tangent plane h + K4(q, ωrel, ωsh), (rolling). Further, enforcing the constraint ωz = 0 ensures h no relative spinning about the contact normal (pure rolling). where K4(q, ωrel, ωsh) contains the velocity-product terms We refer to these as the first-order rolling and first-order and is given in Appendix B-C2. pure-rolling constraints respectively. The first-order rolling kinematics (vx = vy = vz = 0) from [4] can be expressed V. ROLLING DYNAMICS in matrix form as: A diagram of the object and hand in rolling contact is shown in Figure 3. The full six-dimensional orientation and position q˙ = K1(q)ωrel, (1) of each body i [o, h] is expressed by the transformation c T ∈ where ω = h ω = [ω ω ω ] is the relative rotational matrix Tsi SE(3) that consists of the rotation matrix rel phpo x y z ∈ velocity at the contact expressed in c . The matrix K (q), Rsi SO(3) and vector rsi = (xsi, ysi, zsi) that give the h 1 ∈ given in Appendix B-B, maps the relative{ } rotational velocity orientation and position of i relative to the space frame { } at the contact to the change in contact coordinates q˙ . The s . The orientation of each body can be minimally repre- { } dimension of valid contact velocities for rolling and pure sented by the roll-pitch-yaw Euler angles Φsi = (θi, βi, γi). rolling are three and two respectively, so the five-dimensional These map to a rotation matrix by combining rotations about q˙ is subject to constraints given at the end of Appendix B-B. the x-, y-, and z-axes of the space frame: Rsi(Φsi) = An expression for the body twist of the object given the Rot(zs, γi)Rot(ys, βi)Rot(xs, θi), where Rot(zs, γi) is the ro- h body twist of the hand and the relative twist at the tation matrix representing a rotation of angle γi about the Vsh contact rel is used in the dynamics derivation in Section V. axis zs. The six-dimensional velocity vector for each body V i i i The equation is defined as: is represented by the body twist si = ( ωsi, vsi) expressed in the i frame for i [o, h]. V o h { } ∈ so = [AdT ] sh + [AdT ] rel, (2) The combined configurations of the object and hand are V oh V och V 12-dimensional, subject to one constraint that the distance ch where rel = . V Vphpo between the two bodies is zero. The velocities of the object and hand are 12-dimensional, with three velocity constraints for rolling (v = v = v = 0), and four velocity con- B. Second-Order Rolling Kinematics x y z straints for pure rolling (vx = vy = vz = ωz = 0). A The general form of the second-order kinematics gives q¨ minimal representation of the state of the system therefore ˙ ch ˙ as a function of the current state and rel = phpo = requires 20 states for rolling, or 19 states for pure rolling. T V V [αx αy αz ax ay az] (the derivative of the relative twist at The configuration of the hand can be minimally represented the contact expressed in ch ). This expression includes the by the pair (Φsh, rsh), and therefore Tso, the configuration { } T relative rotational accelerations αrel = [αx αy αz] and the of the object, is fully specified by the hand configuration 5

Hand

nco zh

xc h Object o { } yh h rhp xh h yc z o xch co, po o vx { } or ωy c , p opo ωx { h h} vy zs s yo ych ωz g rsh o vz xo { } n s y ch { } s xs

Fig. 3. Rolling rigid bodies in space. The contact cordinate frames {co, po} and {ch, ph} are shown by solid and dotted coordinate axes respectively. The box shows a zoomed view of the frames at the contact and the relative rotational and linear velocity directions.

(Φsh, rsh) and the contact configuration variables q. The body be expressed as: h twist of the hand is represented by sh, and the body twist o T o o o ˙ o o V o so = [ad Vso ] o so + go + contact, (6) of the object so is fully specified by Eq. (2) using the G V G V F F V h o state of the hand (Φ , r , ), the contact configuration where o is the body inertia matrix of the object and g sh sh Vsh G F o q, and the relative rolling velocity ωrel. We represent the is the gravitational wrench on the object. The contact wrench relative rolling velocity as the five-dimensional vector q˙ from o is given by o = [Ad ]Tch , where contact contact Tcho contact chF F T F ch Eq. (1) instead of the three-dimensional ωrel. This adds two contact = [0 0 0 fx fy fz] for rolling and contact = F T F variables to the state representation, but allows the evolution [0 0 τz fx fy fz] for pure rolling. Eq. (5) can be substituted of the contact velocities to be integrated using Eq. (3). We into Eq. (6) to obtain: therefore define the state of the dynamic rolling system as ˙ −1 Tch h 22 [AdToc ] rel o [AdTc o ] contact = s = (Φsh, rsh, q, sh, q˙ ) R with two constraints on q˙ h V − G h F −1 T o o V ∈ o for rolling and three constraints on q˙ for pure rolling, given o ([ad Vso ] o so + go ) (7) G G V F in Appendix B-B. h h ˙ K4(q, ωrel, ωsh) [AdToh ] sh, We assume that the hand is directly controlled by − − V ˙ ch acceleration inputs h ˙ . The model parameters include the where the only unknowns are in rel and contact. We Vsh V F contact friction model (rolling vs. pure rolling) and friction substitute the second-order-rolling constraints arel = aroll in Eq. (4) for rolling, and the additional constraint α = α in coefficient(s), the surface parameterizations Fi(ui), and the z z,pr Eq. (43) for pure rolling. Rearranging gives us the following spatial inertia matrix of the object, o = blockdiag(Jo, moI3), G forms for rolling and pure rolling, respectively: where Jo is the rotational inertia matrix for the object, mo is the mass of the object, and I is the 3 3 identity matrix. T h 3 K5(s)[αx αy αz fx fy fz] = K6(s) [AdT ] ˙ sh, (8) We then solve for dynamic equations describing× the motion − oh V K (s)[α α τ f f f ]T = K (s) [Ad ]h ˙ . of the object as follows: 5pr x y z x y z 6pr Toh sh − V (9)

Given: The expressions for K5(s), K6(s), K5pr (s), K6pr (s) are omit- 1) Model parameters Fi(ui), o, and friction parameters ted for brevity, but are the result of straightforward linear G 2) State s = (Φ , r , q, h , q˙ ) algebra on equation (7). An example derivation can be found sh sh Vsh 3) Acceleration of the hand h ˙ = (hα , ha ) in the supplemental code. For the rolling assumption, Eq. (8) Vsh sh sh find: can be solved to find the relative rotational acceleration at the T 1) Relative rotational accelerations at the contact αrel contact αrel = [αx αy αz] . For the pure-rolling assumption, ch T 2) Contact wrench contact Eq. (9) can be solved for [α α ] and combined with the F x y pure-rolling constraint on αz from Eq. (43) to construct αrel. The equations enforce the rolling constraints, so the contact The contact wrenches for rolling and pure rolling can be wrench must be examined to see if rolling motion satisfies extracted from Eq. (8) and Eq. (9), respectively: unidirectional contact constraints and friction limits. ch T contact,roll = [0 0 0 f f f ] , (10) F x y z ch T contact,pr = [0 0 τ f f f ] . (11) A. Rolling Dynamics Derivation F z x y z o The six body accelerations of the object ˙ so are governed For the rolling solution to be valid, the contact wrench must by the Newton-Euler equations (see Ch. 8.2V of [34]) and can satisfy f 0 (nonnegative normal force) and (f , f ) z ≥ k x y k ≤ 6

µsfz, where µs is the coefficient of static friction. For pure g sin θ yˆ yˆ rolling, the contact wrench must also satisfy τ µspinf , k zk ≤ z where µspin is a torque friction coefficient at the contact. These constraints can be checked in optimization-based motion planning. xˆ xˆ B. Simulating the Rolling Dynamics The state of the dynamic rolling system is defined as ωplate h 22 s = (Φsh, rsh, q, sh, q˙ ) R . The state evolution of the hand is directly controlledV by∈ the change in body twist h ˙ . sh Fig. 4. The x- and y-axes shown are fixed in the inertial frame and aligned The state of the object is represented by the state of the hand,V with the plate. The plate spins about the z-axis at an angular velocity ωplate. the contact coordinates q, and coordinate velocities q˙ . The A sphere is initially in contact at the origin and rolling in the −y direction contact coordinate accelerations q¨ are needed to integrate the without slipping. (a) The plane of the plate is in the x-y plane of the inertial frame and gravity acts in the −z direction. The contact point of the sphere contact coordinates over time. An expression for q¨ is given by follows a circular orbit. (b) The plate is tilted by angle θ about the x-axis the second-order kinematics in Eq. (3), which takes the relative of the inertial frame, so gravity has a component g sin θ in the −y direction. The contact point motion is the sum of the circular orbit and a constant drift rotational accelerations αrel as an input. The expression for in the +x direction. αrel is found by solving Eq. (8) and Eq. (9) for rolling and pure rolling respectively. For both rolling and pure rolling the h dynamics equations can be rearranged into the control-affine plate at rhch (0), where ch is the contact frame on the plate. { } h form The ball has an initial linear velocity vho and no rotational s˙ = K (s) + K (s)h ˙ , (12) velocity. From [33], the ball follows a circular trajectory with 7 8 Vsh the following properties: where the expressions for K7(s) and K8(s) are large symbolic expressions that are omitted for brevity (an example derivation 2 ωc = ωplate, (13) can be found in the supplemental code). The evolution of the 7 h h h T state s can be simulated using a numerical integrator such as rhc = rhch (0) vho [0 0 1/ωc] , (14) − × MATLAB’s ode45. h h ρc = rhch (0) rhc , (15) In implementation one can avoid large, symbolic matrix in- || − || h versions needed to solve for Eq. (12) by numerically evaluating where rhc and ρc are the center and radius of the circle trajectory, respectively, and ωc is the angular velocity of the K5 and K6 from Eq. (8) for rolling or K5 and K6 from pr pr contact point about the center of the circle. Eq. (9) for pure rolling at each time step, solving for αrel, and then solving Eq. (3) numerically for q¨. The state s, controls For this validation, we choose the following parameters h and initial conditions for a ball in contact at the origin ˙ sh, and q¨ can then be combined into the vector s˙ from Eq.V (12). that is initially rolling without slipping in the y direction: h − T ωplate = 7 rad/s, ρo = 0.2 m, rhch (0) = [0 0 0] m, h T vho = [0 0.2 0] m/s. Evaluating at the initial conditions C. Example: Ball on a Rotating Plate − h T gives ωc = 2 rad/s, rhc = [0.1 0 0] m, and ρc = 0.1 m. To validate our dynamic equations (Eq. (12)) we consider a We now simulate the rolling sphere using the solid, homogeneous sphere rolling without slipping on a plate dynamic rolling method derived in Section V-A. spinning at a constant speed about an axis perpendicular to the The surface of the ball is parameterized by the 3 plate. The plate may be perpendicular to gravity (horizontal) sphere equation Fo : uo R :(uo, vo) → 7→ or inclined. This is a well-studied problem (see [33] and Part (ρo sin(uo) cos(vo), ρo sin(uo) sin(vo), ρo cos(uo)), where the III.XV of [32]) with analytical solutions for the motion of the “latitude” uo satisfies 0 < uo < π. The plate is parameterized 3 sphere. For a horizontal plate, the contact point of the ball rolls as a plane Fh : uh R :(uh, vh) (uh, vh, 0). in a circular orbit on the plate (Figure 4(a)), and if the plate The spatial inertia matrix→ for the sphere7→ is given by is inclined in gravity, the motion is the circular orbit plus a = blockdiag(J , m I ), where J = 2 m ρ2I , Go o o 3 o 5 o o 3 constant drift in a direction perpendicular to the gravitational mo = 0.1 kg, and I3 is the 3 3 identity matrix. The friction component in the plane of the plate (Figure 4(b)). The circle model is rolling (relative spin× at the contact allowed), and the radius and center point are determined analytically from the static friction µs is large enough so slip does not occur. initial conditions of the ball. Simulations of the dynamic The state of the sphere-on-plane system can be repre- rolling equations from Section V-B are consistent with the sented by the state vector s = (Φ , r , q, h , q˙ ), where sh sh Vsh analytical solutions, as demonstrated in the following two Φsh(0) = (0, 0, 0), rsh(0) = (0, 0, 0), q(0) = (π/2, 0, 0, 0, 0), h examples. MATLAB scripts to simulate these two examples sh(0) = (0, 0, 7, 0, 0, 0), ωrel(0) = (1, 0, 7) gives q˙ = are included in the open-source code. (0V, 1, 0, 0.2, 7) from Eq. (1), and the control− input is given h − − 1) Horizontal Rotating Plate: Consider a horizontal plate as ˙ sh(t) = (0, 0, 0, 0, 0, 0). coincident with the origin of the inertial frame with a constant TheV first- and second-order kinematic equations for the h rotational velocity about its z-axis, ωsh,z = ωplate (Fig- sphere-on-plane system are found using the expressions in Ap- ure 4(a)). A ball with radius ρo is initially in contact with the pendix B. The state of the system is then simulated using the 7

(a) Horizontal

Fig. 6. Comparison of the horizontal rolling trajectory using our method (solid blue) and using the Bullet physics simulator (dashed red) with the same initial conditions and simulated for 10 s. Our method matches the circular analytical trajectory of radius 0.1 m centered at (0.1,0). The Bullet simulation has the same initial conditions but drifts from of the circular trajectory.

(b) Tilted Fig. 5. Visualizations of the sphere-on-plane rolling trajectories for a plane h with a constant rotational body velocity ωsh,z = ωplate = 7 rad/s (axis units in meters). The paths are shown by the black, dotted lines for (a) a horizontal plane and (b) a plane tilted by 0.01 rad about the x-axis of the inertial frame (see Figure 4(b)). The spheres move in a counter-clockwise motion along the trajectories with a period of π seconds, and (b) drifts in the x direction at a velocity given by Eq. (16). These results are consistent with the analytical solutions shown in Figure 4 and derived in [33]. kinematics equations and the rolling dynamics in Eq. (12). The simulated rolling trajectory matches the analytical solution by Fig. 7. Dynamic rolling example of an ellipsoid rolling in an ellipsoidal dish. An animation with a pure-rolling friction contact is included in the tracing a circular trajectory on the plane of radius ρc = 0.1 m, h T supplemental video. centered at rhc = [0.1 0 0] m, and in tf = π (ωc = 2 rad/s). A visualization of the trajectory is shown in Figure 5(a), and an animation is included in the supplemental video. We tested the We simulate the system for 10 seconds using the same numerical accuracy of our method by simulating the system for initial conditions and controls for the horizontal case except 120 seconds with the variable step-size integrator ode45. The except with a tilt of 0.01 rad about the x-axis of the inertial divergence of the radius from the analytical circular trajectory frame Φsh(0) = (0.01, 0, 0) rad. The sphere follows a circular − was less than 5 10 6%, and the average integrator step size motion of period π seconds combined with a constant drift × was 0.01 seconds. velocity of 0.035 m/s which is consistent with Eq. (16). A To compare the accuracy of our approach to a commonly visualization of the rolling trajectory is shown in Figure 5(b), used physics simulator we implemented the rolling example and an animation is included in the supplemental video. using Bullet Physics C++ version 2.89, and found comparable results except that the object diverged from the circular tra- D. 3D Rolling Example jectory over time as shown in Figure 6. Even with a step size of 0.001 seconds this method had much larger errors, and the Our dynamics formulation applies to arbitrary smooth ge- divergence of the radius from the analytical circular trajectory ometries of the object and hand, unlike previous work re- was 34% after only 10 seconds. stricted to speciﬁc geometries (e.g., a sphere on a plane) or 2) Tilted Rotating Plate: The motion of a ball on a tilted the rolling contact approximations built into standard physics plate with a constant rotational velocity is a cycloidal orbit engines (e.g., Bullet). As one example, Figure 7 illustrates an ellipsoid rolling in an ellipsoidal shaped bowl. An animation with the same rotational velocity ωc from the previous section but with an additional uniform linear drift velocity perpendic- with a pure-rolling friction contact is included in the supple- ular to the force of gravity given by: mental video, and the derivation is included in the open-source code. 5 g vdrift = sin θ, (16) 2 ωplate VI.CONTACT JUGGLING MOTION PLANNING h where g is gravity, ωplate = ωsh,z is the rotational velocity of In this section the rolling dynamics equations are used to the plate about its z-axis and θ is the tilt of the plate creating a plan rolling motions that bring the hand and object from gravitational component in the y direction (see Figure 4(b)). an initial state s to a goal state s . This builds on − start goal 8

our previous work in [10] where we demonstrate an iterative TABLE II planning method for kinematic rolling between smooth objects TRAJECTORY OPTIMIZATION PARAMETERS FOR BALL-ON-PLATE (the relative rolling velocities are directly controlled). The EXAMPLE two primary differences in this implementation are: (1) we Description Value replace the kinematic equations of motion with the higher- Trajectory time tf 2 s Initial # of segments N 50 (∆t = 0.04 s) dimensional dynamic-rolling equations; (2) we incorporate Goal error tolerance η 0.1 nonlinear constraints to enforce positive normal forces and Max iDC iterations 4 5 bounded frictional wrenches. Max fmincon func evals/iter 10 Control limits ||αx|| ≤ 50, ||αy|| ≤ 50 An “admissible” trajectory is defined as a set of states and Constraint integration method trapezoidal h controls ξ(t) = (s(t), ˙ sh(t)), from t = 0 to the final time Initial guess stationary V P1 (terminal state weight) blkdiag(I , 10I , 1000I , 10, I , 10I )1000 t = tf , that satisfies the rolling dynamics equation (12) and 3 2 2 3 5 Q (tracking weight) blkdiag(I3, 10I2, 1000I2, 10, I3, 10I5)/100 the contact wrench limits. A “valid” trajectory is defined as R (control weight) diag(0.001, 0.001) an admissible trajectory that also satisfies serror(tf ) < η, where η is the tolerance on the final state error and serror(tf ) = s(tf ) sgoal , where corresponds to generated by time-scaling a geometric planning method from || − || || · || a weighted norm that puts state errors in common units. [25], and then tracked using a feedback controller. An example (Throughout the rest of this paper, we use the Euclidean from the paper is reproduced in Figure 8; The stationary initial norm.) The motion planning problem can be stated as: and goal configurations for the sphere on the plate are shown in Figure 8(a) and (b), respectively. Visualizations of the contact Given: The rolling dynamics equations and contact wrench locations on the hand for the geometric solution are shown by expressions from Section V, the states (sstart, sgoal), and the the dashed black lines in Figure 8(c) and (d). rolling time tf , Our method for trajectory planning utilizes the iterative h ˙ find: a valid rolling trajectory ξ(t) = (s(t), sh(t)) for direct collocation method detailed in Appendix C. To match V t [0, tf ] that brings the system from s(0) = sstart to the method above we constrain the hand to have two rota- ∈ s(tf ) = sgoal. tional degrees of freedom. We simplify the dynamics (12) by h h substituting the constant values rsh = vsh = ash = 0, and We plan rolling motions by adapting the iterative direct the constraint on the hand accelerations expressed in the world s collocation (iDC) nonlinear optimization method described frame αsh,z = 0. The number of states is therefore decreased in our recent work [10]. The optimization first solves for a from 22 to 16. trajectory history that is represented coarsely, using a small We initialize the planner with a stationary trajectory where number of state and control segments. The solved-for con- the ball stays in place and no controls are applied. The iDC trols are then simulated by a more accurate, higher-order algorithm was then run using the parameters given in Table II. numerical integration method than the integrator implicit in The weight Q was reduced to zero after the first iteration the constraints in the nonlinear optimization. If the simulated because the inadmissible straight-line trajectory is no longer trajectory satisfies the error tolerances, the problem is solved. needed to bias the trajectory towards a solution. If not, the previous solution is used as an initial guess, the A valid trajectory was found after three iterations of iDC number of state and control segments is increased, and the iterative refinement: 50 time segments (planning time 171 s), optimization is called again. This is repeated until a valid 100 time segments (346 s), and 200 time segments (946 s), for trajectory is found, the maximum number of iDC iterations a total planning time of 24 minutes running on an i7-4700MQ is reached, or the optimization converges to an invalid point. CPU @ 2.40 GHz with 16 GB of RAM. A visualization The details are included in Appendix C. and plot of the optimized trajectory are shown in Figure 8 We focus on plans for cases where the object and hand are (c) and (d) respectively, and an animation is included in the stationary at sstart. We refer to plans where sgoal is also sta- supplemental video. In comparison to the trajectory planner tionary as “stationary-to-stationary” motions. We refer to plans of [31], the optimized plan found by IDC results in a much where sgoal is moving as “stationary-to-rolling” motions, with shorter path length on the hand. An advantage of the planner the special case “stationary-to-freeflight” when the object and in [31] is that it returns a plan in negligble time, but the hand separate (fz = 0, az,rel > az,roll) at the final time tf . The approach applies only to a ball on a plane, whereas the iDC h ˙ planner can make use of all six hand accelerations sh(t), approach with our dynamics formulation applies to general V h or subsets such as control of the rotational accelerations αsh smooth bodies for the hand and object. h only or control of the linear accelerations ash only.

A. Planning for a Ball on Plate Reconﬁguration VII.FEEDBACK CONTROLFOR DYNAMIC ROLLING Recent work [31] addresses planning and control for dy- We apply a linear quadratic regulator (LQR) to stabilize namic, nonprehensile repositioning and reorientation for a ball the linearized dynamics along a known trajectory [10]. LQR on a horizontal plate (the z-axis is aligned with gravity). The computes a time-varying gain matrix KLQR(t) that optimally ball is initially at rest on the plate, and a goal position and reduces the total cost for small perturbations about the nominal orientation for the ball is given. The controls are the two rota- trajectory. LQR requires a cost function, and we use the one tions about the x- and y-axes of the plate. The desired path is given in Eq. (47), where sdes(t) is set to the nominal trajectory 9

π/2 rad

0.033 m

(a) Start (b) Goal

(c) (d) Fig. 8. Initial and goal states for reorienting a sphere on a plate are shown in (a) and (b) respectively. The goal state is 0.033 m from the start state in the −y direction, with the object rotated π/2 rad about the x-axis. A visualization of the sphere rolling trajectory from the geometric plan from [31] is shown by the black dashed lines in (c) and (d), and the optimized plan from the iterative direct collection method is shown by the solid blue lines. The start position is shown by the “×,” and the goal position is shown by the “◦.” An animation of the optimized solution can be seen in the supplemental media.

we are tracking snom(t). We solve the matrix Riccati equation A. Feedback Control Example to find the time varying feedback control matrix KLQR(t) (see Section 2.3 of [35]). Consider the optimized open-loop rolling trajectory shown in Fig. 8 (c) and (d). The feedback controller in Eq. (19) P˙ (t) = P(t)Ae (t) + Ae (t)TP(t) − − provides robustness to (1) initial state error, (2) perturbations −1 T along the trajectory, and (3) error due to planning rolling P(t)Be (t)RLQRBe (t) P(t) + QLQR, (17) motions using a coarse approximation of the dynamics. In this P(t ) = P f 1,LQR example we demonstrate how the feedback controller can be −1 T KLQR(t) = RLQRBe (t) P(t). used to decrease error from the the third source, the coarse approximation of the dynamics. The time-varying matrices Ae (t) and Be (t) come from lin- As discussed in Section VI, the optimization first solves earizing about the nominal trajectory snom(t), and P1,LQR, for a trajectory history that is represented coarsely, using a QLQR, and RLQR are the controller gain matrices weighting the goal-state error, desired trajectory deviation, and control small number of state and control segments. The solved-for controls are then simulated by a more accurate, higher-order cost respectively. The time-varying gain matrix KLQR(t) from Eq. (17) can be expressed as the function: numerical integration method than the integrator implicit in the constraints in the nonlinear optimization. If the final state KLQR(t) = (s (t), P1,LQR, QLQR, RLQR). (18) K nom error in the fine trajectory is too large (i.e., greater than the The matrix KLQR(t) is then used in the feedback control law goal error tolerance η), we perform another iteration of the h h motion plan with additional state and control segments. The ˙ (s, t) = ˙ (t) KLQR(t)(s(t) s (t)) (19) Vsh,fbk Vsh,nom − − nom goal error tolerance for this motion plan was given by η = 0.1 to stabilize the nominal trajectory. The linearized dynamics from Table II. To further decrease this error, we stabilize the are controllable about almost all trajectories (i.e., generic tra- planned trajectory with a feedback controller. jectories). Specially chosen trajectories can be uncontrollable, We generate the time-varying gain matrix KLQR(t) by plug- such as those with symmetry properties as demonstrated for ging the planned trajectory and gain matrices into Eq. (18), kinematic rolling in [10]. and then stabilize the trajectory using the feedback control 10

high-speed camera

geffective g g g effective ⊥ Harmonic Drive DC motors

object

manipulator

Side View Top View

Fig. 11. Diagram of the experimental setup

Fig. 9. Comparison of trajectory error over time for open-loop and closed-loop trajectories. The open-loop trajectory uses the coarse set of controls found by A. Experimental Setup the trajectory optimization method which leads to error during the simulation with the finer integration method. Closed-loop control drives the final state to The experimental setup consists of a 3-DOF robot arm the desired final state. The feedback gains are tuned to eliminate error at the that moves in a plane parallel to the surface of an inclined terminal state, which is why some error is not eliminated in the middle of the trajectory. (The two spikes occur where the ball rapidly changes direction, air table. A diagram of the experimental setup is shown in because the coarse dynamics are more sensitive to integration errors at these Figure 11. Each link is actuated by a brushed DC motor with points.) harmonic drive gearing and current controlled using Junus motor amplifiers. The 1000 Hz motion controller runs on a PC104 embedded computer running the QNX real-time operating system. Vision feedback is given by a 250 Hz IR Optitrack camera and reflective markers attached to the object. The manipulator is a flat plate of width 0.375 m, and the object is an ellipse of mass 0.0553 kg, and major and minor axes given by 0.0754 m and 0.0504 m, respectively. Experiments (a) are conducted at 40% full gravity by inclining the table at 24 degrees with respect to horizontal.

u = 0 o uo = π o { } B. 2D Rolling Model

uh = 0 We model the system in full 3D, but restrict object and hand π motions to the -plane so all the motion is planar as shown in zs u = 0.1 xz (uo, uh)=( , 0.1) h h 2 − { } Figure 10(b). All rotations occur about the y-axis, and the full h 22 s x system state s = (Φ , r , q, , q˙ ) simplifies to { } s sh sh sh R (b) h V h ∈ h s2D = (βsh, xsh, zsh, uo, uh, ωsh,y, vsh,x, vsh,z, u˙ o, u˙ h) 10 and all other states are zero. The controls are also∈ Fig. 10. The full three-dimensional model used for planning rolling motions R is shown in (a) and the 2D projection is shown in (b) along with the body limited to rotational accelerations about the y-axis, and linear frames and some uo and uh values. accelerations in the xz-plane. Knowledge of the system states is necessary to run feedback controllers based on our rolling dynamics equations. h h h law from Eq. (19). The open-loop simulation using the higher- The six hand states (βsh, xsh, zsh, ωsh,y, vsh,x, vsh,z) are order numerical integration method (MATLAB’s ode45) gives estimated using robot encoders, and the object states are us the state trajectory sfine(t). The closed-loop simulation estimated using the IR camera that tracks reflective markers gives us the state trajectory sfbk(t). Plots of the distance errors on the object. We use the object and hand states to estimate between the output trajectories and the nominal trajectory the contact coordinates and velocities on the object and hand snom(t) are shown in Figure 9. (uo, uh, u˙ o, u˙ h). We analytically solve for the closest points on the object and plate using equations defining the ellipse and a line. VIII.EXPERIMENTS

This section outlines a series of experiments in planning C. Rolling Damping Controller and feedback control for rolling manipulation tasks. The The ﬁrst experiment demonstrates a damping controller experiments are in two dimensions, but the model is a full that stabilizes an ellipse in rolling contact with a plate three-dimensional model as shown in Figure 10. in a neighborhood of the stable equilibrium s2D = 11

found that simple arm trajectories, parameterized by only a few parameters, suffice to achieve most planar throwing goals. To search for optimal parameters in these low-dimensional trajectory spaces, we adopted a shooting method for trajectory optimization rather than iDC. Our throwing and catching example is inspired by pole throwing, catching, and balancing by quadcopters [36]. The idea is to throw the pole with nonzero angular velocity so that, after a high-friction, inelastic impact with a stationary horizontal surface, the pole has just enough kinetic energy to rotate to the upright position at zero velocity, at which point it can be stabilized in the vertical position. To plan a throw, catch, and balance of an ellipse, we adapted Fig. 12. A plot showing the object angle in the world frame during the flip- Eq. (42) from [36] to derive a pre-impact state of the ellipse up motion. It includes open and closed-loop results, with +/- two standard deviations of the 12 trials shown by the shaded region surrounding the lines. that, after impact with a stationary horizontal hand, would The object overshot the desired balance state for all of the open-loop trials. rotate the ellipse to the vertical position at a desired location. All closed-loop trials successfully flipped up the object to the balance position We then used shooting to plan a rolling carry by the hand and kept it balanced until shutting off at seven seconds. Snapshots from one trial are shown in Figure 13. from an initial rest state to a release state of the ellipse that would carry the ellipse by ballistic free flight to the planned pre-impact state. (0, 0, 0, π/2, 0, 0, 0, 0, 0, 0). Perturbations of the object cause it For the hand’s trajectory, we chose a trapezoidal accelera- to wobble back and forth until dissipation brings it back to rest. tion profile for the hand’s three degrees of freedom, specified We generate the gain matrix KLQR by plugging the constant by the duration of the carry tthrow, the x acceleration of the nominal trajectory s2D(t) = (0, 0, 0, π/2, 0, 0, 0, 0, 0, 0) and h hand in the body frame ash,x, and the rotational acceleration gain matrices into Eq. (18), and stabilize the object using the h of the hand in the body frame αsh,y. Note that the hand feedback control law from Eq. (19). When the controller is can still accelerate in the z direction of the s frame if turned off, the object has large rotational motions in response { } βsh = 0, π . At the release state, the hand accelerates away to perturbations, but the object barely rotates when the con- from6 the{ object} and moves to its catching position. Because the troller enabled. The experiment is shown in the supplemental duration of the throw is short, we performed the throw open- video. loop. Robustness to small errors in execution comes from the LQR balance controller that turns on after the ellipse impacts D. Flip Up to Balance the hand. The second experiment is a rolling version of the classic Snapshots from a throw, catch, and balance are shown in inverted pendulum swing-up to balance problem. We use iDC Figure 14, and a trial is shown in the supplemental video. to plan a flip-up motion and construct an LQR feedback controller to stabilize the trajectory. We also construct a second LQR feedback controller that only stabilizes the goal IX.CONCLUSIONS equilibrium state, and we switch to that controller when the This paper demonstrates modeling, motion planning, and state is in a neighborhood of the goal state. feedback control for robotic contact juggling. We first derived All of the initial conditions are zero except that uo = π/2 the rolling dynamics equations and friction constraints, then and uh = cellipse/4 where cellipse is the circumference of the validated the results in simulation against known analytical − ellipse, and the goal states are all zero except uo = π. The goal solutions. We used direct collocation methods and the rolling state is at a singularity of the surface parameterization, so we dynamics equations to plan robotic contact juggling motions, switch to a different coordinate chart to derive the balancing and we demonstrated the use of an LQR feedback controller controller. to stabilize planned trajectories in both simulation and exper- The contact location and velocity on the object for the iment. planned, open-loop, and closed-loop trajectories are shown in Figure 12. The open-loop execution of the planned trajectories consistently overshot the goal and the object rolled off the A. Future Work edge of the manipulator. The closed-loop execution was able There are many possible directions for future work. to stabilize the trajectory and successfully balance the object 1) Surface Parameterization: This paper requires orthogo- in 12/12 trials. Snapshots from a successful closed-loop trial nal parameterizations of surfaces, and while any smooth sur- are shown in Figure 13, and a video of the 12 closed-loop face can be locally represented this way, deriving orthogonal trials can be seen in the supplemental video. parameterizations can be cumbersome. We have used non- orthogonal parameterizations for simulating the second-order E. Throw and Catch kinematics by defining local orthogonal frames at each point We now demonstrate a motion planner for throwing and on the surface, but the dynamics derivation in Section V would catching an object in rolling contact with a manipulator. We need to be modified to relax the orthogonality assumption. 12

Fig. 13. Demonstration of the closed-loop ﬂip up to balance with LQR stabilization about the trajectory with snapshots taken every 0.5 seconds.

Fig. 14. Snapshots from an experimental rolling throw that ﬂips the object, moves to a catch position where an inelastic, high-friction impact would result in a post-impact velocity that brings the object upright (see [36]), and then balances it about the unstable equilibrium.

Future research could process an object defined by a point- hybrid manipulation planning frameworks, including sliding cloud, automatically generate an atlas of coordinate charts that contacts, such as that described in [37]. cover the surface, and plan contact juggling with stabilizing feedback controllers through multiple coordinate charts. APPENDIX A BACKGROUND 2) Feedback Control: Our approach to feedback control relies on estimating the contact parameters on the object A. Operator Definitions and hand. In this paper, we rely on vision to estimate the The skew-symmetric matrix form of a vector ω = 3 contacts, but tactile sensing could be employed in addition, (ωx, ωy, ωz) R is given by ∈ particularly when coupled with contact state observers [26].   0 ωz ωy We showed one example of feedback control but there are − [ω] =  ωz 0 ωx so(3). (20) many areas for extending feedback control for rolling objects. − ∈ ωy ωx 0 Our method utilizes a linearized LQR controller to stabilize − 3 planned trajectories and balance states which results in a sim- For a rotation matrix R SO(3), a vector r R , and a transformation matrix T∈ SE(3) defined by ∈(R, r), the ple feedback law given by Eq. (19) that can easily run at high ∈ speeds (1000 Hz in our implementation). This method requires operator [AdT] is the adjoint map knowledge of the contact coordinates which are difficult to R 0 [Ad ] = . (21) estimate for general, 3D rolling motions. Hardware such as T [r]RR manipulators equipped with contact location sensing could For a twist = (ω, v) 6, the operator [ad ] is defined as help address this, especially if combined with an observability R V V ∈ model such as the one developed in [26]. Other feedback [ω] 0 [adV ] = , (22) methods such as energy-based feedback controllers could be [v][ω] used to stabilize trajectories as well, and may avoid the need where the Lie bracket of 1 and 2 is [adV1 ] 2. for estimating specific contact states [2]. V V V 3) Hybrid Rolling, Sliding, and Free Flight Dynamics: B. Derivatives of Expressions in Multiple Frames This paper focuses on rolling and pure rolling, with simple The formula for the derivative in frame ph of an expres- examples of transitions to free flight. Such transitions result sion represented in frame o is { } in hybrid dynamics. The methods developed in this paper { } for rolling manipulation can be incorporated in more general ph d/dt(ov ) = oa + oω ov . (23) so so pho × so 13

−1 APPENDIX B entries (n, l) of the metric tensor inverse (Gi) . This gives 2 2 KINEMATICS EXPRESSIONS Γ11,i and Γ12,i as A. Local Geometry of Smooth Bodies T T 2 ∂xci 12 ∂xci 22 Γ11,i = xci gi + yci gi , Below are some standard expressions for the geometry ∂ui ∂ui (30) of a surface that are used to define the first- and second- T T 2 ∂xci 12 ∂xci 22 order kinematics in the following sections. References and Γ12,i = xci gi + yci gi . ∂vi ∂vi derivations of these expressions can be found in [4]. The surface of each rigid body is represented by an orthog- B. First-Order Kinematics onal parameterization: : u 3 :(u , v ) (x , y , z ) Fi i R i i i i i This form of the first-order kinematics was initially derived for i [o, h], where the coordinates→ (x , y , z ) 7→are expressed i i i in [4]. We reproduce it in matrix form with rolling constraints in the∈ i frame. It is assumed that is continuous up Fi (v = v = v = 0) in Eq. (1) as to the third{ } derivative (class C3), so that the local contact x y z geometry (contact frames associated with the first derivative q˙ = K1(q)ωrel, of , curvature associated with the second derivative, and Fi with ω = ch ω = [ω ω ω ]T and K (q) defined as: derivative of the curvature associated with the third derivative) rel phpo x y z 1   are uniquely defined. K1o(q) 02×1

The natural bases at a point on a body are given as xci = K1(q) =  K1h(q) 02×1 , ∂ /∂u and y = ∂ /∂v . We also assume that coordinate σoΓoK1o(q) + σhΓhK1h(q) 1 Fi i ci Fi i − (31) charts are orthogonal (xc yc = 0), and note that xc and p −1 −1 i i i K1o(q) = ( Go) Rψ(He o + Hh) E1, are not necessarily unit· vectors. The unit normal is given yci p −1 −1 as n = (x y )/ x y . K1h(q) = ( Gh) (He o + Hh) E1, ci ci × ci || ci × ci || The normalized Gauss frame at a point ui on body i is where G is the metric tensor of body i [o, h] from Eq. (25), i ∈ defined as the coordinate frame ci with origin at Fi(ui) the 2 2 rotation matrix Rψ and E1 are defined as and coordinate axes given by { } × cos(ψ) sin(ψ) 0 1 Rψ = − , E1 = − , xci yci sin(ψ) cos(ψ) 1 0 Rici = , , nci , (24) − − xci yci H is a 2 2 matrix that gives the curvature of the surface || || || || i × from Eq. (27), He o is defined as He o = RψHoRψ, the scalar where Rici expresses the Gauss frame in the object or hand p σi is defined as σi = g22,i/g11,i where g11,i and g22,i are frame i . The metric tensor Gi is a 2 2 positive-definite matrix{ defined} as × the diagonal entries of the metric tensor Gi, and Γi is a 1 2 matrix of the Christoffel symbols of the second kind from× xci xci xci yci Eq. (28). Gi = · · . (25) yci xci yci yci We use a five-dimensional representation q˙ for the relative · · rolling velocity at the contact, but these are subject to two The coefficients g reference the indices of matrix G , and jk,i i constraints for rolling and pure rolling: Gi is diagonal (g12,i = g21,i = 0) when the coordinate chart −1 −1 Fi is orthogonal. The 2 2 matrix Li is the second fundamental K1o(q) u˙ o = K1h(q)) u˙ h. (32) form given by the expression× Pure rolling is subject to the constraints above as well as the 2 2 " ∂ Fi ∂ Fi # no-spin constraint ωz = 0. Eq. (31) gives us: 2 nci nci ∂ui ∂ui∂vi Li = 2 · 2 · . (26) ∂ Fi ∂ Fi ωx nci 2 nci ˙ ∂vi∂ui ∂vi ψ = [σoΓoK1o(q) + σhΓhK1h(q)] . (33) · · ωy Hi combines the metric tensor Gi with the second fundamental form Li and is given by, C. Second-Order Kinematics p p Second-order contact equations were derived by Sarkar et −1 −1 (27) Hi = ( Gi) Li( Gi) . al. in [4] and published again in later works [8], [9]. Errors in the published equations for second-order contact kinematics The 1 2 array Γi is given by the expression × in [4], [8], [9] were corrected in our recent work [5]. We first 2 2 Γi = Γ11,i Γ12,i , (28) define some additional higher-order local contact geometry expressions used in the second-order kinematics first defined l where Γjk,i are Christoffel symbols of the second kind given in [4], and then provide the corrected second-order-kinematics by equations in a newly derived matrix form. 2 T The first order kinematics includes expressions for X ∂(χi)j Γi (1 2) l nl (29) × Γjk,i = (χi)ngi , and Li (2 2). We now give four additional expressions for ∂(ui)k × n=1 Γ¯ (2 3), L¯ (1 3), Γ¯ (1 3), and L¯ (2 3): i × i × i × i × where (χ ) is the jth vector in the list χ = (x , y ), (u ) 1 1 1 i j i ci ci i k ¯ Γ11,i 2Γ12,i Γ22,i is the kth variable in the list u = (u , v ), and gnl are the Γi = 2 2 2 , (34) i i i i Γ11,i 2Γ12,i Γ22,i 14

L¯ i = L11,i 2L12,i L22,i , (35) velocity terms are given by the matrix K2(q, ωrel) defined as: K2a K2(q, ωrel) = , l K2b where Γjk,i is the Christoffel symbol of the second kind −1 defined in Eq. (29), and Ljk,i refers to the entry (j, k) of Rψ√Go √Gh ¯ K2a = − matrix Li in Eq. (26). The final two expressions for Γi (1 3) RψE1Ho√Go E1Hh√Gh ¯ × − and Li (2 3) are given as: R √G Γ¯ √G Γ¯ × − ψ o o w + h h w R E (√G )−1L¯ o E (√G )−1L¯ h ψ 1 o o − 1 h h ¯ Γ¯ = 2ωzE1Rψ√Go 02×2 u˙ o i + − T ˙  ∂Γ2  ωzRψHo√Go ψHh√Gh u˙ h (Γ2 Γ1 )Γ2 + 11,i − − 21,i 11,i 11,i ∂ui (36)    − ∂Γ2 ∂Γ2  02×1 (Γ2 Γ1 )Γ2 + (Γ2 Γ1 )Γ2 + 12,i + 11,i  ,  21,i 11,i 12,i 22,i 12,i 11,i ∂ui ∂vi  − − ∂Γ2  ωy  , (Γ2 Γ1 )Γ2 + 12,i − σoΓou˙ o 22,i 12,i 12,i ∂vi ω − − x T ωy p −1 K2b = RψE1( Go) Lou˙ o − ωx − ¯ ¯ ¯ L¯ i = + σoΓowo + σhΓhwh + [σoΓo σhΓh]K2a. T (39)  ∂L11,i   Γ1 L 11,i 11,i ∂ui  1 1 − ∂L12,i ∂L11,i  The matrix that operates on the acceleration terms of the Γ L12,i + Γ L11,i   11,i 12,i ∂ui ∂vi    −L −  second-order kinematics is given by K (q):  Γ1 L ∂ 12,i . (37) 3 12,i 12,i ∂vi  − T  2 ∂L21,i   I4×4 04×1  Γ L21,i  (40)  21,i ∂ui  K3(q) = K3a(q), 2 2 − ∂L22,i ∂L21,i [σoΓo σhΓh] 1 Γ L22,i + Γ L21,i   −  21,i 22,i ∂ui ∂vi   2 −∂L22,i − Γ L22,i where 22,i − ∂vi  −1  Rψ√Go √Gh − 04×1 K3a(q) =  R E H √G E H √G  E2, The second-order kinematics expression can therefore be ex- ψ 1 o o − 1 h h pressed as: 01×4 1 0 0 0 1 0 0 − −1 0 0 0 0 1 0 uö Rψ√Go √Gh  −  = − E2 = 1 0 0 0 0 0 . u¨ R E H √G E H √G   h ψ 1 o o − 1 h h 0 1 0 0 0 0 R √G Γ¯ √G Γ¯ − ψ o o w + h h w 0 0 1 0 0 0 R E (√G )−1L¯ o E (√G )−1L¯ h ψ 1 o o − 1 h h 1) Second-Order Rolling and Pure Rolling Constraints: 2ω E R √G 0 z 1 ψ o 2×2 u˙ o The relative linear accelerations a = ch a are constrained + − ˙ rel phpo ωzRψHo√Go ψHh√Gh u˙ h by the second-order rolling constraints a = a . These were  − −     (38) rel roll 02×1 02×1 ax derived in Eq. (60) of [4]. The general version is given in  ωy  +  αx   ay  , Eq. (4) and the full form is reproduced below: − σoΓou˙ o − ωx αy 02×1     −T ax ωzE1 − T p ωy p −1 a = a = R G u˙ . (41) ψ¨ = RψE1( Go) Lou˙ o αz roll  y  ωy  ψ o o − ωx − a − z roll ωx ¯ ¯ − + σo(Γouö + Γ¯owo) + σh(Γhu¨h + Γ¯hwh), The relative rotational acceleration αz is constrained by the second-order-pure-rolling constraints αz = αz,pr. This ch T ˙ ch ˙ was given in [4] as αz,pr = 0. We found this to be valid for where ωrel = ωphpo = [ωx ωy ωz] , rel = phpo = T V V simple geometries such as sphere-on-sphere, sphere-on-plane, [αx αy αz ax ay az] , and wi comprises the velocity product 2 2 T and ellipsoid-on-plane, but for more complex geometries such terms [u ˙ i u˙ iv˙i v˙i ] . as ellipsoid-on-ellipsoid and sphere-on-ellipsoid, αz,pr = 0 The second-order kinematics expression in Eq. (38) can be did not enforce the no-spin constraint. expressed in the form of Eq. (3) To derive the rolling constraint we set the derivative of the expression for ωz from the first-order kinematics equal to zero: q¨ = K2(q, ωrel) + K3(q) ˙ rel, V d d ω = (σ Γ u˙ + σ Γ u˙ ψ˙), dt z dt o o o h h h − (42) which separates the velocity and acceleration components. The = 0. 15

From Eq. (42) and the second-order kinematics in Eq. (39) we where P1, Q, and R, penalize goal-state error, desired tra- solved for an expresion of the form: jectory deviation, and control cost respectively, and sdes(t) is a desired trajectory. The path sdes(t) is chosen as the linear αx αz,pr = d1(q, ωrel) + d2(q) . (43) interpolation from sstart to sgoal, which penalizes motions αy that do not move s towards the goal. Note that sdes(t) is not For the ellipsoid-on-ellipsoid, and ellipsoid-on-sphere models admissible in general (i.e., the states and controls do not satisfy we tested, Eq. (43) simpliﬁed to: the rolling dynamics equations).

ch T The collocation method divides the trajectory ξ(t) into N α pr = (ωrel ω ) n , (44) z, × oco h segments, and the N +1 nodes at the ends of each segment are where nh is the unit contact normal of ch . This expression called collocation points. Each collocation point is expressed is equivalent to the following term from{ the} ψ¨ expression in as ξ (t) = (s(t ), h ˙ (t )) for k [0,...,N]. For systems k k Vsh k ∈ the second-order kinematics in Eq (38): with m state variables and n control variables there are a total T of (N + 1)(m + n) collocation points. The dynamics between ωy p −1 each pair of sequential collocation points are enforced by the αz,pr = RψE1( Go) Lou˙ o. (45) − ωx − following condition: 2) Relative Acceleration Expression: Eq. (5) gives an ex- 1 h ˙ h ˙ pression for the body acceleration of the object given the body sk+1 sk = ∆tk( (sk+1, shk+1) + (sk, shk)), acceleration of the hand and the relative acceleration at the − 2 F V F V contact, and is reproduced below: k [0,...,N 1], ∈ − (48) o h ˙ =[Ad ] ˙ + [Ad ] ˙ where ∆tk = (tk+1 tk) indicates the interval duration and so Toh sh Toch rel V V V (s, h ˙ ) = K (s−) + K (s)h ˙ is the rolling dynamics + K (q, ω , hω ). sh 7 8 sh 4 rel sh FfunctionV from Eq. (12). Equation (48)V is unique to the choice of The velocity product terms K4 are given by: trapezoidal collocation, and other integration methods require a different constraint [38]. h 03×1 K4(q, ωrel, ωsh) =[AdToh ] h h h The optimal control problem can be represented as the [ ωsh]([ ωsh] rhph ) o following nonlinear programming problem: [ωrel](Rcho ωso) [AdToc ] (46) − h 03×1 N X h ˙ 03×1 arg min (s(tf )) + (s(tk), sh(tk))∆tk , h ˙ M L V o o o s(tk), Vsh(tk) i=0 − [ ωso]([ ωso] ropo ) h ˙ h ˙ o such that (s(t0): s(tN ); sh(t0): sh(tN−1)) = 0, where ωso comes from Eq (2), and the [AdT], r, and R H V V h ˙ h ˙ expressions can be derived from the contact configuration q. (s(t0): s(tN ); sh(t0): sh(tN−1)) 0, I V V ≤(49) where ( ) enforces the rolling dynamics in Eq. (48), and APPENDIX C H · gives the equality constraints s(0) = sstart (and optionally ITERATIVE DIRECT COLLOCATION s(tf ) = sgoal which can be relaxed by replacing with a We first describe the details of the direct collocation method, high weighting matrix on the goal state P1). The expression and then outline our iterative version. Direct collocation is a ( ) includes the contact wrench inequality constraints on I · h ˙ h ˙ h ˙ method for trajectory optimization that optimizes an objec- Eq. (10), the controls limits ( shmin sh shmax), h ˙ V ≤ V ≤ V tive function (ξ(t)) = (s(t), sh(t)) using polynomial and enforces any constraints on the configurations (e.g., due to J J V spline approximations of the continuous states and controls. singularities in the coordinate chart). Equation (49) is a finite- We chose to use trapezoidal collocation where the control dimensional nonlinear optimization problem, and a solution h ˙ trajectory sh(t) is represented by piecewise-linear splines, ξ (t) can be found using nonlinear optimizers such as V iDC the state trajectory s(t) is represented by quadratic splines, SNOPT, IPOPT, or MATLAB’s fmincon. and the trapezoidal rule is used for integration. Higher-order The integration error can be determined by comparing the representations such as Hermite-Simpson collocation can also trajectory siDC(t) from the direct collocation method with the be used but with increased computational cost [38]. We define trajectory sfine(t), where sfine(t) is obtained by integrating the objective function (s(t), h ˙ (t)) as the sum of the J Vsh the initial state over the interval t = [0, tf ] using Eq. (12), terminal cost and the running cost and omit the dependence h ˙ the piecewise-linear output controls shiDC(t), and a higher- on t for clarity: order integrator with small time stepsV (e.g. dt 0.001). With ≤ Z tf fewer segments N, the integration error is larger, but there h ˙ h ˙ (s, sh) = (s(tf )) + (s, sh)dt, are fewer constraints for the nonlinear solver. This means J V M 0 L V 1 that the optimizer is more likely to find a solution, and (s(t )) = (s(t ) s )TP (s(t ) s ), with less computational cost. The choice of N is therefore M f 2 f − goal 1 f − goal a trade-off between computational cost/optimizer convergence h ˙ 1 T 1 h ˙ T h ˙ (s, sh) = (s sdes) Q(s sdes) + sh R sh, and integration error. We implemented the iterative direct L V 2 − − 2 V V (47) collocation (iDC) method to address this. 16

We first run the nonlinear optimization method using MAT- [13] J.-C. Ryu and K. M. Lynch, “Contact juggling of a disk with a disk- LAB’s fmincon for a small number of segments (e.g. shaped manipulator,” IEEE Access, vol. 6, pp. 60 286–60 293, 2018. [14] S. R. Erumalla, S. Pasupuleti, and J.-C. Ryu, “Throwing, catching, N = 25) to find a trajectory ξiDC(t). The recalculated path and balancing of a disk with a disk-shaped end effector on a two-link sfine(t) is found using smaller integration timesteps and a manipulator,” Journal of Mechanisms and Robotics, vol. 10, no. 5, 2018. higher-order integrator (ode45), and the planner is terminated [15] V. Lippiello, F. Ruggiero, and B. Siciliano, “The effect of shapes in input-state linearization for stabilization of nonprehensile planar rolling if the goal-state tolerance of the fine trajectory is satisfied dynamic manipulation,” IEEE Robotics and Automation Letters, vol. 1, (serror(tf ) < η). If the goal-state error is too large, the no. 1, pp. 492–499, 2016. previous output trajectory serves as the initial trajectory guess [16] D. Serra, F. Ruggiero, A. Donaire, L. R. Buonocore, V. Lippiello, and B. Siciliano, “Control of nonprehensile planar rolling manipulation: for the next iteration with twice as many segments (N 2N) → A passivity-based approach,” IEEE Transactions on Robotics, vol. 35, (N could also be increased by a fixed value ∆N between each no. 2, pp. 317–329, 2019. iteration to add an additional tuning parameter for the planning [17] M. Anitescu, J. F. Cremer, and F. A. Potra, “Formulating three- dimensional contact dynamics problems,” Journal of Structural Mechan- method). This is repeated until a valid trajectory ξsol(t) is ics, vol. 24, no. 4, pp. 405–437, 1996. found, the maximum number of iDC iterations is reached, or [18] T. Liu and M. Y. Wang, “Computation of three-dimensional rigid- the optimization converges to an invalid point. body dynamics with multiple unilateral contacts using time-stepping and gauss-seidel methods,” IEEE Transactions on Automation Science and In our tests, an initial optimization with a fine control dis- Engineering, vol. 2, no. 1, pp. 19–31, 2005. cretization often takes an unnecessarily long time to converge [19] V. Duindam and S. Stramigioli, “Modeling the kinematics and dynamics or even fails to converge to a feasible solution. The coarse of compliant contact,” in 2003 IEEE International Conference on Robotics and Automation, vol. 3. IEEE, 2003, pp. 4029–4034. initial guess followed by successive refinement yields higher- [20] S. Ivaldi, J. Peters, V. Padois, and F. Nori, “Tools for simulating quality solutions faster and more consistently. The iterative humanoid robot dynamics: a survey based on user feedback,” in IEEE- refinement process acts as a form of regularization. RAS International Conference on Humanoid Robots. IEEE, 2014, pp. 842–849. Additional methods can improve the planning success such [21] V. Jurdjevic, “The geometry of the plate-ball problem,” Archive for as ignoring the friction inequality constraints for the initial it- Rational Mechanics and Analysis, vol. 124, no. 4, pp. 305–328, 1993. eration(s), and decreasing/zeroing the weight Q that penalizes [22] D. Hristu-Varsakelis, “The dynamics of a forced sphere-plate mechanical system,” IEEE Transactions on Automatic Control, vol. 46, no. 5, pp. trajectories that do not track a straight line to the goal state. 678–686, 2001. [23] H. Date, M. Sampei, M. Ishikawa, and M. Koga, “Simultaneous control ACKNOWLEDGMENT of position and orientation for ball-plate manipulation problem based on time-state control form,” IEEE transactions on robotics and automation, We would like to thank Paul Umbanhowar and Shufeng Ren vol. 20, no. 3, pp. 465–480, 2004. for helpful discussions while developing this work. [24] G. Oriolo and M. Vendittelli, “A framework for the stabilization of general nonholonomic systems with an application to the plate-ball REFERENCES mechanism,” IEEE Transactions on Robotics, vol. 21, no. 2, pp. 162– 175, 2005. [1] K. M. Lynch, N. Shiroma, H. Arai, and K. Tanie, “The roles of [25] A. Becker and T. Bretl, “Approximate steering of a plate-ball system shape and motion in dynamic manipulation: the butterfly example,” under bounded model perturbation using ensemble control,” in 2012 in Proceedings. 1998 IEEE International Conference on Robotics and IEEE/RSJ International Conference on Intelligent Robots and Systems. Automation, vol. 3, 1998, pp. 1958–1963 vol.3. IEEE, 2012, pp. 5353–5359. [2] M. Cefalo, L. Lanari, and G. Oriolo, “Energy-based control of the [26] Y.-B. Jia and M. Erdmann, “Local observability of rolling,” in Workshop butterfly robot,” IFAC Proceedings Volumes, vol. 39, no. 15, 2006. on Algorithmic Foundation of Robotics, 1998, pp. 251–263. [3] M. Surov, A. Shiriaev, L. Freidovich, S. Gusev, and L. Paramonov, “Case [27] P. Choudhury and K. M. Lynch, “Rolling manipulation with a single study in non-prehensile manipulation: planning and orbital stabilization control,” The International Journal of Robotics Research, vol. 21, no. of one-directional rollings for the “butterfly” robot,” in 2015 IEEE 5-6, pp. 475–487, 2002. International Conference on Robotics and Automation (ICRA). IEEE, [28] R. Gahleitner, “Ball on ball: Modeling and control of a novel experiment 2015, pp. 1484–1489. set-up,” IFAC-PapersOnLine, vol. 48, no. 1, pp. 796–801, 2015. [4] N. Sarkar, V. Kumar, and X. Yun, “Velocity and acceleration analysis [29] K.-K. Lee, G. Batz, and D. Wollherr, “Basketball robot: Ball-on-plate of contact between three-dimensional rigid bodies,” Journal of Applied with pure haptic information,” in 2008 IEEE International Conference Mechanics, vol. 63, no. 4, pp. 974–984, 1996. on Robotics and Automation. IEEE, 2008, pp. 2410–2415. [5] J. Z. Woodruff and K. M. Lynch, “Second-order contact kinematics [30]G.B atz,¨ A. Yaqub, H. Wu, K. Kuhnlenz,¨ D. Wollherr, and M. Buss, between three-dimensional rigid bodies,” Journal of Applied Mechanics, “Dynamic manipulation: Nonprehensile ball catching,” in 18th Mediter- vol. 86, no. 8, May 2019. ranean Conference on Control and Automation, MED’10. IEEE, 2010, [6] D. J. Montana, “The kinematics of contact and grasp,” The International pp. 365–370. Journal of Robotics Research, vol. 7, no. 3, pp. 17–32, 1988. [31] D. Serra, J. Ferguson, F. Ruggiero, A. Siniscalco, A. Petit, V. Lippiello, [7] K. Harada, T. Kawashima, and M. Kaneko, “Rolling based manipulation and B. Siciliano, “On the experiments about the nonprehensile recon- under neighborhood equilibrium,” The International Journal of Robotics figuration of a rolling sphere on a plate,” in 2018 26th Mediterranean Research, vol. 21, no. 5-6, pp. 463–474, 2002. Conference on Control and Automation (MED). IEEE, 2018, pp. 13–20. [8] N. Sarkar, X. Yun, and V. Kumar, “Dynamic control of 3-D rolling [32] E. A. Milne, Vectorial mechanics. Methuen & Company, 1957. contacts in two-arm manipulation,” IEEE Transactions on Robotics and [33] K. Weltner, “Stable circular orbits of freely moving balls on rotating Automation, vol. 13, no. 3, pp. 364–376, Jun. 1997. discs,” American Journal of Physics, vol. 47, no. 11, pp. 984–986, 1979. [9] N. Sarkar, Xiaoping Yun, and V. Kumar, “Control of contact interactions [34] K. M. Lynch and F. C. Park, Modern robotics: mechanics, planning, with acatastatic nonholonomic constraints,” The International Journal of and control. Cambridge University Press, 2017. Robotics Research, vol. 16, no. 3, pp. 357–374, Jun. 1997. [35] B. D. Anderson and J. B. Moore, Optimal control: linear quadratic [10] J. Z. Woodruff, S. Ren, and K. M. Lynch, “Motion planning and methods. Courier Corporation, 2007. feedback control of rolling bodies,” IEEE Access, vol. 8, pp. 31 780– [36] D. Brescianini, M. Hehn, and R. D’Andrea, “Quadrocopter pole acrobat- 31 791, 2020. ics,” in 2013 IEEE/RSJ International Conference on Intelligent Robots [11] M. Xiao and Y. Ding, “Contact kinematics between three-dimensional and Systems. IEEE, 2013, pp. 3472–3479. rigid bodies with general surface parameterization,” Journal of Mecha- [37] J. Z. Woodruff and K. M. Lynch, “Planning and control for dynamic, nisms and Robotics, vol. 13, no. 2, 2021. nonprehensile, and hybrid manipulation tasks,” in IEEE International [12] O. Taylor and A. Rodriguez, “Optimal shape and motion planning for Conference on Robotics and Automation. IEEE, 2017, pp. 4066–4073. dynamic planar manipulation,” Autonomous Robots, vol. 43, no. 2, pp. [38] M. Kelly, “An introduction to trajectory optimization: How to do your 327–344, 2019. own direct collocation,” SIAM Review, vol. 59, no. 4, pp. 849–904, 2017.