The post Numerically Solving Discrete Systems with Mathematica for Robocode appeared first on Sam Pfrommer.
]]>Robocode works with discrete time steps called ticks. We’ll describe our tank’s state using four variables: the position , the velocity , and the orientation .
The movement physics are described here so I won’t go over them in detail. The control inputs are the acceleration , with a maximum of 1 when accelerating and 2 when decelerating, and the angular velocity , bounded by .
The integration scheme is essentially Implicit Euler with constant step sizes of one tick (however, this has some caveats as we’ll find later). There’s a lot of other weird corner cases that make this problem more difficult than it first appears. My hope is by going through and addressing these issues you can learn why your numerical solution for discrete systems is misbehaving. We’ll start with a simple, naive approach and build up from there.
Let’s first try to formulate the dynamics by ignoring the discrete time steps. For simplicity we’ll also ignore deceleration and assume the tank accelerates from rest in a straight line. This looks like the following:
(* Finish time of simulation *)
tEnd = 10;
dynamics = {
(* Set velocity derivative to target acceleration *)
veqn = v'[t] == ta[t],
(* Set turn rate to target, but clamp between physical limits *)
ϕeqn = ϕ'[t] == Clip[tω[t], {-10 Degree + 3/4 Degree * v[t], 10 Degree – 3/4 Degree * v[t]}],
xeqn = x'[t] == v[t]*Cos[ϕ[t]],
yeqn = y'[t] == v[t]*Sin[ϕ[t]]
};
(* Add dynamics equations and initial conditions *)
soln = NDSolve[{dynamics, x[0]==0, y[0]==0, ϕ[0]==0 Degree,
v[0]==0, tω[0]==0 Degree, ta[0]==1,
(* When velocity reaches maximum, stop acceleration *)
WhenEvent[Evaluate[v[t] == 8], {ta[t]->0}]},
(* Specify desired outputs and integration timeframe *)
{x, y, x’, y’, v, ϕ, ϕ’}, {t, 0, tEnd},
(* Allow target turn / accelerate variables to jump discontinuously *)
DiscreteVariables->{tω, ta}];
(* Plot solution *)
ParametricPlot[Evaluate[{x[t], y[t]} /. soln], {t, 0, tEnd}]
(* Output end position *)
posEnd = Evaluate[{x[t], y[t]} /. soln] /. t->tEnd
At first glance this seems reasonable; since we didn’t turn at all, the tank went straight to the right and ended up traveling 48 pixels in 10 ticks. Let’s compare this to actual measurements from Robocode for maximal acceleration from rest:
Tick #0
Displacement: [0.0, -0.0]
Velocity: [0.0, 0.0], abs: 0.0
Tick #1
Displacement: [0.9999999999999869, -1.73749903353837E-14]
Velocity: [1.0, 2.7755575615628914E-16], abs: 1.0
Tick #2
Displacement: [2.999999999999961, -5.218048215738236E-14]
Velocity: [2.0, 5.551115123125783E-16], abs: 2.0
Tick #3
Displacement: [5.999999999999984, -9.769962616701378E-15]
Velocity: [3.0, 6.661338147750939E-16], abs: 3.0
Tick #4
Displacement: [10.000000000000007, 7.105427357601002E-15]
Velocity: [4.0, 1.1102230246251565E-15], abs: 4.0
Tick #5
Displacement: [15.000000000000004, 1.509903313490213E-14]
Velocity: [5.0, 1.7763568394002505E-15], abs: 5.0
Tick #6
Displacement: [20.999999999999986, 5.329070518200751E-15]
Velocity: [6.0, 1.3322676295501878E-15], abs: 6.0
Tick #7
Displacement: [27.999999999999957, -2.1316282072803006E-14]
Velocity: [7.0, 1.3322676295501878E-15], abs: 7.0
Tick #8
Displacement: [35.99999999999998, 2.8421709430404007E-14]
Velocity: [8.0, 2.220446049250313E-15], abs: 8.0
Tick #9
Displacement: [44.0, 7.815970093361102E-14]
Velocity: [8.0, 2.220446049250313E-15], abs: 8.0
Tick #10
Displacement: [52.00000000000002, 1.3145040611561853E-13]
Velocity: [8.0, 2.220446049250313E-15], abs: 8.0
At tick 10, we see that the tank has actually traveled 52 pixels (ignoring rounding errors). What gives? The key lies in looking at the data for the first tick. After one tick, the Robocode tank has traveled 1 pixel and has a velocity of one pixel/tick. Our Mathematica solution after one tick gives the robot a velocity of one tick per second but only says it’s moved half a pixel. This discrepancy is due to the Robocode’s Implicit Euler integration scheme, where the change in position between two ticks is equal to the velocity at the later tick times the time difference.
Let’s try making NDSolve use Implicit Euler with a fixed step size of one. The first thing to note is that while Mathematica supplies a built-in ExplicitEuler method, we have to supply our own Implicit Euler definition (taken from here). The step size is specified using the “StartingStepSize” argument. Our code then looks like the following:
(* Finish time of simulation *)
tEnd = 10;
dynamics :=
{
veqn = v'[t] == ta[t],
(* Set turn rate to target, but clamp between physical limits *)
ϕeqn = ϕ'[t] == Clip[tω[t], {-10 Degree + 3/4 Degree * v[t], 10 Degree – 3/4 Degree * v[t]}],
xeqn = x'[t] == v[t]*Cos[ϕ[t]],
yeqn = y'[t] == v[t]*Sin[ϕ[t]]}
(* Implicit Euler is not implemented directly, must define ourselves *)
(* Robocode does Implicit Euler for physics *)
ImplicitEuler = {"FixedStep", Method -> {"ImplicitRungeKutta",
"Coefficients" -> "ImplicitRungeKuttaRadauIIACoefficients",
"DifferenceOrder" -> 1, "ImplicitSolver" -> {"FixedPoint",
AccuracyGoal -> MachinePrecision, PrecisionGoal -> MachinePrecision,
"IterationSafetyFactor" -> 1}}};
(* Add dynamics equations and initial conditions (starting velocity must be non-zero,
so I just add a negligible x'[0] to give a direction to move in *)
soln = NDSolve[{dynamics, x[0]==0, y[0]==0, ϕ[0]==0 Degree,
v[0]==0, tω[0]==0 Degree, ta[0]==1,
(* When velocity reaches maximum, stop acceleration *)
WhenEvent[Evaluate[v[t] == 8], {ta[t]->0}]},
(* Specify desired outputs and integration timeframe *)
{x, y, x’, y’, v, ϕ, ϕ’}, {t, 0, tEnd},
(* Allow target turn / accelerate variables to jump discontinuously *)
DiscreteVariables->{tω, ta},
(* Use a ImplicitEuler method with a step size of one tic (matches robocode) *)
StartingStepSize->1, Method->ImplicitEuler];
(* Plot solution *)
ParametricPlot[Evaluate[{x[t], y[t]} /. soln], {t, 0, tEnd}]
(* Output end position *)
posEnd = Evaluate[{x[t], y[t]} /. soln] /. t->tEnd
And we get the following output:
What? 48 again? I thought Implicit Euler integration was supposed to fix that. Closer inspection reveals that the WhenEvent might be the problem:
As you can see, the Implicit Euler integration matches the Robocode physics right up until the WhenEvent occurs at . This might actually be a bug in Mathematica since there’s no reason why the position should only change by four when the surrounding velocities are 7 and 8 pixels per tick. In fact, it seems like the WhenEvent is doing its job just fine (velocity goes up by one each tick and then stays at 8). After some frustrated debugging, I found that setting the WhenEvent’s LocationMethod to “StepEnd” like so:
WhenEvent[Evaluate[v[t] == 8], {ta[t]->0}, "LocationMethod"->"StepEnd"]
Fixes the issue.
Okay, good. It matches. I’m not quite sure why that worked, but I think there might be something funky going on with Mathematica under the hood. Let’s continue and add some turn in there (set ) and compare with Robocode:
Tick #0
Displacement: [0.0, -0.0]
Velocity: [0.0, 0.0], abs: 0.0
Heading: 0.0
Tick #1
Displacement: [0.9848077530122417, 0.17364817766694823]
Velocity: [0.9848077530122081, 0.17364817766693], abs: 1.0
Heading: 0.17453292519943275
Tick #2
Displacement: [2.8729857937978536, 0.8330294681925363]
Velocity: [1.8881780407855688, 0.6593812905255734], abs: 2.0
Heading: 0.33597588100890796
Tick #3
Displacement: [5.52794870618697, 2.229873029167883]
Velocity: [2.654962912389127, 1.3968435609753318], abs: 3.0000000000000004
Heading: 0.4843288674284256
Tick #4
Displacement: [8.78441077961228, 4.552684852011626]
Velocity: [3.25646207342528, 2.3228118228437546], abs: 4.0
Heading: 0.6195918844579857
Tick #5
Displacement: [12.47079746366291, 7.930635890089924]
Velocity: [3.6863866840506243, 3.3779510380782978], abs: 5.000000000000001
Heading: 0.7417649320975892
Tick #6
Displacement: [16.42687235426334, 12.441674734963785]
Velocity: [3.9560748906004157, 4.511038844873862], abs: 5.999999999999999
Heading: 0.8508480103472351
Tick #7
Displacement: [20.516620013725372, 18.12269260871884]
Velocity: [4.089747659462047, 5.681017873755083], abs: 7.000000000000001
Heading: 0.946841119206923
Tick #8
Displacement: [24.636924613005846, 24.980031014335722]
Velocity: [4.120304599280439, 6.857338405616895], abs: 8.0
Heading: 1.0297442586766539
Tick #9
Displacement: [28.268848610922262, 32.10808320784266]
Velocity: [3.6319239979163793, 7.128052193506941], abs: 8.0
Heading: 1.0995574287564271
Tick #10
Displacement: [31.394697638836412, 39.47212203546218]
Velocity: [3.125849027914194, 7.364038827619521], abs: 8.0
Heading: 1.1693705988362004
The end position is way off, but more informative is the fact that after the first tick, the Robocode tank rotated by a full 10 degrees (0.1745 radians) whereas the Mathematica simulation rotated by just 9.25 degrees (0.1614 radians). The Mathematica result is to be expected since in the dynamics we clipped the rotational velocity to be and after one tick with maximum acceleration the velocity should equal one. However, the fact that the Robocode tank rotated a full ten degrees indicated that the rotational bounds were actually computed using velocity from the last tick; this was confirmed by looking at the Robocode source.
Let’s try using a delay differential equation to incorporate last tick’s translational velocity in the angular velocity bounds. Note the “v-1” in the dynamics equations:
(* Finish time of simulation *)
tEnd = 10;
dynamics :=
{
veqn = v'[t] == ta[t],
(* Set turn rate to target, but clamp between physical limits *)
ϕeqn = ϕ'[t] == Clip[tω[t], {-10 Degree + 3/4 Degree * v[t-1], 10 Degree – 3/4 Degree * v[t-1]}],
xeqn = x'[t] == v[t]*Cos[ϕ[t]],
yeqn = y'[t] == v[t]*Sin[ϕ[t]]}
(* Implicit Euler is not implemented directly, must define ourselves *)
(* Robocode does Implicit Euler for physics *)
ImplicitEuler = {"FixedStep", Method -> {"ImplicitRungeKutta",
"Coefficients" -> "ImplicitRungeKuttaRadauIIACoefficients",
"DifferenceOrder" -> 1, "ImplicitSolver" -> {"FixedPoint",
AccuracyGoal -> MachinePrecision, PrecisionGoal -> MachinePrecision,
"IterationSafetyFactor" -> 1}}};
(* Add dynamics equations and initial conditions (starting velocity must be non-zero,
so I just add a negligible x'[0] to give a direction to move in *)
soln = NDSolve[{dynamics, x[0]==0, y[0]==0, ϕ[0]==0 Degree,
v[0]==0, tω[0]==10 Degree, ta[0]==1,
(* When velocity reaches maximum, stop acceleration *)
WhenEvent[Evaluate[v[t] == 8], {ta[t]->0}, "LocationMethod"->"StepEnd"]},
(* Specify desired outputs and integration timeframe *)
{x, y, x’, y’, v, ϕ, ϕ’}, {t, 0, tEnd},
(* Allow target turn / accelerate variables to jump discontinuously *)
DiscreteVariables->{tω, ta},
(* Use a ImplicitEuler method with a step size of one tic (matches robocode) *)
StartingStepSize->1, Method->ImplicitEuler];
(* Plot solution *)
ParametricPlot[Evaluate[{x[t], y[t]} /. soln], {t, 0, tEnd}]
(* Output end position *)
posEnd = Evaluate[{x[t], y[t]} /. soln] /. t->tEnd
Let’s have a look at our new output:
The NDSolve warning is just saying that for tick 0 it doesn’t know what is so it just assumes it’s zero like at (which is fine). We also notice that for is 10 degrees (0.1745 radians). But the solution’s still off… What’s going on at ? The difference difference between the first and second ticks is 9.4366 degrees, not 9.25 degrees like expected. Where did it get that weird number from? The secret lies in looking closer at the individual points being sampled by NDSolve:
Interesting—between and NDSolve seemed to violate the fixed step guarantee and took half-tick steps, causing the descrepancy between Robocode physics and our Mathematica solution. Further playing around shows that this is caused by the delay differential equation in our dynamics. Luckily, we can reformulate the dynamics to be as follows:
dynamics :=
Module[{vdel=Abs[If[t==0, v[t], v[t]-ta[t]]]}, {
(* Set turn rate to target, but clamp between physical limits *)
veqn = v'[t] == ta[t],
ϕeqn = ϕ'[t] == Clip[tω[t], {-10 Degree + 3/4 Degree * vdel, 10 Degree – 3/4 Degree * vdel}],
xeqn = x'[t] == v[t]*Cos[ϕ[t]],
yeqn = y'[t] == v[t]*Sin[ϕ[t]]
}]
Which then does the trick and gets us the expected result:
Although the basic framework is working, there’s still a few corner cases that need to be addressed in order to perfectly match Robocode physics. These include preventing acceleration overshoot (velocity goes from 7.5 to 8.5 and stops there), handling deceleration (two pixels / sec^2), and splitting time between acceleration / deceleration when a tick takes the tank from negative to positive velocity or vice versa. Additionally, there was a weird bug that occured when a WhenEvent fell on the last step of the integration which I fixed by adding a time condition to all the events. I won’t go into the details of finding / fixing the corner cases since it’s just adding a bunch of WhenEvents and corresponding bounds on the initial acceleration. Here’s the finished program, including a range of calculations to show end positions for various and combos:
(* Finish time of simulation *)
tEnd = 10;
dynamics :=
Module[{vdel=Abs[If[t==0, v[t], v[t]-ta[t]]]}, {
veqn = v'[t] == ta[t],
(* Set turn rate to target, but clamp between physical limits *)
ϕeqn = ϕ'[t] == Clip[tω[t], {-10 Degree + 3/4 Degree * vdel, 10 Degree – 3/4 Degree * vdel}],
xeqn = x'[t] == v[t]*Cos[ϕ[t]],
yeqn = y'[t] == v[t]*Sin[ϕ[t]]
}]
(* Implicit Euler is not implemented directly, must define ourselves *)
(* Robocode does Implicit Euler for physics *)
ImplicitEuler = {"FixedStep", Method -> {"ImplicitRungeKutta",
"Coefficients" -> "ImplicitRungeKuttaRadauIIACoefficients",
"DifferenceOrder" -> 1, "ImplicitSolver" -> {"FixedPoint",
AccuracyGoal -> MachinePrecision, PrecisionGoal -> MachinePrecision,
"IterationSafetyFactor" -> 1}}};
(* Solves the forward dynamics for an initial state and a target acceleration / turn rate *)
solveDynamics[initialState_, targetAcceleration_, targetTurnRate_, time_] :=
(
(* Add dynamics equations and initial conditions *)
soln = NDSolve[{dynamics, {x[0], y[0], ϕ[0], v[0]}==initialState,
ta[0]==If[v[0]==0, Clip[targetAcceleration, {-1,1}],
If[v[0]+targetAcceleration>0 && v[0]<0, -v[0]+(1+v[0]/2)*1,
If[v[0]+targetAcceleration<0 && v[0]>0, -v[0]-(1-v[0]/2)*1,
(*Else*) Clip[targetAcceleration, {-8-v[0], 8-v[0]}]]]],
tω[0]==targetTurnRate,
(* When velocity reaches zero, start acceleration *)
WhenEvent[Evaluate[v[t]+ta[t]>0 && t!=time], {ta[t]->-v[t]+(1+v[t]/2) * 1}, "DetectionMethod"->"Sign", "LocationMethod"->"StepEnd"],
WhenEvent[Evaluate[v[t]+ta[t]<0 && t!=time], {ta[t]->-v[t]-(1-v[t]/2) * 1}, "DetectionMethod"->"Sign", "LocationMethod"->"StepEnd"],
WhenEvent[Evaluate[v[t]==0 && t!=time], {ta[t]->Clip[targetAcceleration, {-1,1}]}, "DetectionMethod"->"Sign", "LocationMethod"->"StepEnd"],
(* When velocity reaches maximum, stop acceleration *)
WhenEvent[Evaluate[v[t]+ta[t]>8 && t!=time], {ta[t]->8-v[t]}, "DetectionMethod"->"Sign", "LocationMethod"->"StepEnd"],
WhenEvent[Evaluate[v[t]+ta[t]<-8 && t!=time], {ta[t]->-8-v[t]}, "DetectionMethod"->"Sign", "LocationMethod"->"StepEnd"],
WhenEvent[Evaluate[v[t]*Sign[targetAcceleration]==8 && t!=time], {ta[t]->0}, "DetectionMethod"->"Sign", "LocationMethod"->"StepEnd"]},
(* Specify desired outputs and integration timeframe *)
{x, y, x’, y’, v, ϕ, ϕ’, ta, tω}, {t, 0, time},
(* Allow target turn / accelerate variables to jump discontinuously *)
DiscreteVariables->{tω, ta},
(* Use a ImplicitEuler method with a step size of one tic (matches robocode) *)
StartingStepSize->1, Method->ImplicitEuler];
Flatten[{x[time], y[time], ϕ[time], v[time]} /. soln]
)
(* Plot various end positions starting from rest *)
zeroVelSoln = Flatten[Table[solveDynamics[{0,0,0,0}, a, ω, tEnd], {a, -1, 1, 0.1}, {ω, -10 Degree, 10 Degree, 1 Degree}], 1];
zvxysoln = Part[#, {1,2}]& /@ zeroVelSoln;
ListPlot[zvxysoln, AxesLabel->{Subscript[x, final],Subscript[y, final]}]
(* Plot the extreme positions for a range of initial velocities as a point cloud *)
solnA = Flatten[Table[Join[solveDynamics[{0, 0, 0, vi}, a, ω, tEnd], {vi}], {vi, 8, 8, 16}, {a, -2, 1, 0.1}, {ω, -10 Degree, 10 Degree, 1 Degree}], 2];
solnB = Flatten[Table[Join[solveDynamics[{0, 0, 0, vi}, a, ω, tEnd], {vi}], {vi, -8, -8, 16}, {a, -1, 2, 0.1}, {ω, -10 Degree, 10 Degree, 1 Degree}], 2];
solnC = Flatten[Table[Join[solveDynamics[{0, 0, 0, vi}, a, ω, tEnd], {vi}], {vi, -7, 0, 1}, {a, -1, 2, 3}, {ω, -10 Degree, 10 Degree, 1 Degree}], 2];
solnD = Flatten[Table[Join[solveDynamics[{0, 0, 0, vi}, a, ω, tEnd], {vi}], {vi, -7, 0, 1}, {a, -1, 2, 0.1}, {ω, -10 Degree, 10 Degree, 20 Degree}], 2];
solnE = Flatten[Table[Join[solveDynamics[{0, 0, 0, vi}, a, ω, tEnd], {vi}], {vi, 0, 7, 1}, {a, -2, 1, 3}, {ω, -10 Degree, 10 Degree, 1 Degree}], 2];
solnF = Flatten[Table[Join[solveDynamics[{0, 0, 0, vi}, a, ω, tEnd], {vi}], {vi, 0, 7, 1}, {a, -2, 1, 0.1}, {ω, -10 Degree, 10 Degree, 20 Degree}], 2];
soln = Join[solnA, solnB, solnC, solnD, solnE, solnF];
xysoln = Part[#, {1,2,5}]& /@ soln;
ListPointPlot3D[xysoln, AxesLabel->{Subscript[x, final],Subscript[y, final],Subscript[v, initial]}]
In the second graph, the points in each horizontal slice represent the extreme possible positions of one specific starting velocity (essentially the boundary points of the previous graph). When these are stacked on top of each other over a range of possible initial velocities, this interesting 3D shape is the result.
Here’s a few general remarks about my experience using Mathematica to integrate discrete-time ODEs:
I used Mathematica 11.0.1.0 for this project–later versions may fix some of the problems I encountered. This was also my first time working with Mathematica so don’t take my code as any kind of style guide. Regardless, I hope you can use some of my experiences to debug your own NDSolve issues.
The post Numerically Solving Discrete Systems with Mathematica for Robocode appeared first on Sam Pfrommer.
]]>The post Key Control Strategies Emerge in Spring Loaded Inverted Pendulum Traversal of Slippery Terrain appeared first on Sam Pfrommer.
]]>To start off with, I varied the width of the ice patch as well as the initial state of the SLIP on the patch. The end state constraint was that the SLIP had to finish in a vertical stance (hip over toe) to the right of the patch, so as to hypothetically be able to continue walking. For each scenario, the optimizer ran through a two-stance-phase solution (step directly from the ice patch to the goal) and a three-stance-phase solution (first step backwards, then launch across the patch to the goal), trying to minimize the mechanical work exerted by the torque and leg extension actuators. Over hundreds of randomly initialized trials, the optimizer generally converged to the following five “key” strategies:
One interesting observation is that the solution strategies can be primarily categorized by the movement of the foot during the sliding phase. In addition, previous studies of human SLIP recovery revealed the same walk-over and skate-over strategies discovered by the optimizer [1]. These similarities suggests that on some level humans might be prioritizing energy efficiency when recovering from slipping. I then more closely compared the human foot dynamics data with the optimizer’s. Here are the sliding phase comparison graphs using data from [1] and the simulations:
As you can see, the general arc of the foot position is matched by the optimizer but some of the nuances of the foot velocity profile are lost. Most notably, in the walkover case humans regulate their sliding foot velocity to zero by the end of the stance phase but my optimizer did not. These differences might be attributable to some of the errors listed below.
This work is certainly not perfect and there are a some flaws that may have adversly impacted the results. These include:
I’d like to end with a quick note about how these kinds of optimization exercises can be used to not only improve our understanding of human locomotion but also develop practical robot controllers. If we assume that the robot has some knowledge of its state and an upcoming obstacle, it could be feasible to use that information to select an appropriate traversal strategy from a library of previously-computed solutions. That rough version of the trajectory could then be used to initialize an on-board trajectory optimizer or some other method that ultimately controls the actuators.
[1] T. Bhatt, J. D. Wening, and Y. C. Pai, “Adaptive control of gait stability in reducing slip-related backward loss of balance,” Experimental Brain Research, vol. 170, no. 1, pp. 61–73, 2006.
The post Key Control Strategies Emerge in Spring Loaded Inverted Pendulum Traversal of Slippery Terrain appeared first on Sam Pfrommer.
]]>The post Constructing a Legged Helicopter Landing Gear appeared first on Sam Pfrommer.
]]>One of my biggest projects to date is a legged landing gear that allows helicopters to land on uneven terrain with large touchdown velocity. For a quick overview, please see the following video.
Traditional helicopter landing gears generally consist of two tubular skids, or three wheels for larger helicopters. These designs require a very flat landing surface and a gentle touchdown. For rescue or military situations, these ideal conditions might not always exist—for example, a helicopter might have to land quickly on sloped terrain to dispatch troops while being buffeted by wind.
Enter Georgia Tech’s legged landing gear, which features four retractable legs that use force sensors in each foot to adapt to uneven terrain. As can be seen in the following image, the new gear can handle very uneven terrain. However, there did seem to be some possible areas of improvement.
In the video demonstration, the helicopter was shown being lowered very gently and precisely. This is most likely due to a mechanical limitation of the gear: its inability to absorb landing shock. Each leg consists of two motors and two completely rigid links. Correspondingly, if the helicopter hits the ground with any considerable velocity, the landing force will be transmitted instantaneously without giving the legs any time to retract. This would apply a torque to helicopter body, probably causing a crash.
My improvement over the Georgia Tech design consists of adding a spring in the shank of each leg, absorbing the immediate shock and giving the motors enough time to react. As you will see in the following sections, this introduced numerous mechanical complexities and presented some interesting control problems.
My first idea for the leg consisted of manufacturing my own series elastic actuators from 3D printed parts, using the design described here. After many iterations, it became clear that plastic, 3D printed parts had too much friction and imprecision to make this work. I then moved to including a spring in the shank of each leg, with force transmitted from the foot to the sensor through sliding brass tubes.
The leads of the force sensor were threaded through a slit in the housing. Two rubber frustums on each side of the sensor concentrated force on the sensor’s center, providing more reliable readings. I used hot glue to mate the two halves of the housing and the upper brass tube, and epoxy to connect the lower tube and the plastic foot. Finally, I lubricated the tubes and attached a rubber band between the housing and the foot to hold the shank together.
The thigh of the leg was comparatively simple. The knobs and screw holes on each end line up with the F2 and F3 brackets for the two Dynamixel AX-12As.
All plastic parts were printed using a Lulzbot Mini and HIPS filament on high resolution. One thing I had to watch out for during assembly was that the zero position of the Dynamixel (indicated by the notch in the servo horn) was aligned in the direction of the attached limb.
To read the resistance from each force sensor, I used voltage dividers with 1.7 kΩ resistors. is connected to the analog in pin on the Arbotix-M. To figure out the force in newtons, I took a few data points with known weights and fit a regression.
Once the mechanical design of the leg was finished, I started looking into different control strategies for cushioning landing impact. One that stood out to me in particular was Sliding Mode Control (SMC).
There’s already a lot of good resources discussing sliding mode control so I’ll just touch on some of the more important points here. Let’s say we have a dynamical system of the form:
(1)
Where is a control input and is the state. We then identify a sliding surface , where is called a switching function and represents the “distance” from a certain state to the sliding surface. The sliding surface is designed to achieve some arbitrary task objectives. For example, we might constrain the end effector of a four-bar linkage to move along a line; all states resulting in the end effector lying on the line would have , and all states off the line would have Finally, we pick a discontinuous control law that drives the state trajectory to the sliding surface.
Control laws are classified according to how many time derivatives of they drive to zero. If the controller can enforce , it’s a first order controller, and usually appears explicitly in the first time derivative of (relative degree one). If the controller can enforce , it’s a second order controller, and usually appears explicitly in the second time derivative of (relative degree two). I say usually because there are some exceptions to the relative degree rule—namely, the super twisting SMC, which I’ll be using later.
To understand this, let’s look at a mass sliding along a single axis. We’ll say the state of the system can be described by a single variable, the position . We want to drive the mass to a certain position , so our switching surface would be:
(2)
The core SMC controller would essentially be a bang-bang controller, where and:
(3)
This is a first order sliding mode, since differentiating equation and plugging in gives , indicating a relative degree of one. Theoretically this controller should drive the position to in finite time and maintain it in on that sliding mode perfectly; however, in practical implementations this kind of control strategy results in a phenomenon called chattering where the control input switches very quickly across the surface. This can cause actuator wear and is generally undesirable.
You may be asking yourself why the above controller would not be considered second order, since could clearly be made zero simply by zeroing . This is because of the chattering problem described above; in reality, wouldn’t be enforceable with this control scheme since would actually fluctuate rapidly between +1 and -1, resulting in .
The crux of sliding mode control is picking a control law that quickly drives the state to the sliding surface while avoiding chattering. Many different controllers have been developed to solve this problem, and research is still ongoing in the field. One effective solution that I’ll focus on here is using a high-order sliding mode (HOSM) controller, which can enforce (at least) .
The HOSM controller I used is the super-twisting sliding mode controller. It’s somewhat unique in that it’s second order (can enforce ) but only has relative degree one ( appears explicitly in ). Here are the equations for the super-twisting controller:
(4)
I started testing various simple controllers by dropping one leg with varying heights/springs and examining ground reaction force graphs. My first controller just retracted the leg with full speed starting the instant it read a non-zero ground force (shown right).
First let me apologize for the terrible graph; I originally wasn’t planning on doing a write-up and this was just some old testing data I found lying around. What’s important to notice though are the two humps in the graph, connected by a region of zero force. This meant that after an initial compression phase the leg retracted so quickly that it temporarily lifted off the ground before hitting ground a second time.
Therefore, I approached this sliding mode control problem by picking the very simple sliding surface:
(5)
Where is the length of the shank. In words, the controller tried to keep the compression of the spring constant and avoid force fluctuations. I assumed the control input to be the second derivative of shank length (in reality I controlled hip torque , but via linearization around the horizontal this can be approximated as a vertical acceleration at the knee).
Then I applied equation 4, using 80 for and 20 for (hand tuned). This eliminated the earlier double-hump force profile, leaving just small hump after touchdown that drops down to a constant value. Unfortunately I can’t find a graph of the force profile with the super twisting algorithm though.
The sliding mode controller was just one component of the control scheme for the full four-leg landing gear. The overall control flow is outlined below:
The gear ended up weight 1.2 kilograms, with a maximum leg length of 31 centimeters. I attached the gear to a Gaui X3, which weighed about 1 kilogram.
Here’s a list of all the materials I used in this project:
The post Constructing a Legged Helicopter Landing Gear appeared first on Sam Pfrommer.
]]>The post USGS Terrain Elevation for MATLAB appeared first on Sam Pfrommer.
]]>Part of our solution involved running flood simulations on USGS terrain elevation data. Downloading and processing the data (only available in 1×1 degree cells) was actually fairly tricky, so I decided to clean up and generalize some of my code. The result was this MATLAB add-on, which provides a simple interface for downloading, synthesizing, and rendering terrain for anywhere in the United States with 1/3 arc-second resolution. See the GitHub page to browse through the code.
Here are some images comparing the add-on’s output and the corresponding area on Google Maps. For Jackson Hole the X and Y axes are in meters, while for the Bay Area they’re in longitude and latitude (i.e. the Z axis is not proportional).
The post USGS Terrain Elevation for MATLAB appeared first on Sam Pfrommer.
]]>The post Optimizing Trajectories for the Spring Loaded Inverted Pendulum over Slippery Terrain appeared first on Sam Pfrommer.
]]>This write up expands upon a section of my research paper (which I hope to finish soon) and is meant to be a walkthrough of how to apply numerical trajectory optimization to real-world scenarios.
The Spring Loaded Inverted Pendulum (SLIP) model is often used to approximate human running dynamics. Fundamentally, SLIP hoppers represent the mass of the torso as a point at the hip and connect the hip to the foot via a spring. In other words, the biped becomes a pogo stick, bouncing from foot to foot with each step.
We consider the case where a running planar biped steps on an unforeseen ice patch, surrounded with high-friction ground on each side. The question then becomes this: should the robot attempt to use its momentum to traverse the patch in one step, or first step backwards and then launch itself over the patch to the other side?
In addition to the basic SLIP model, I included a few modifications:
Before constructing the optimization problem, we need to know the dynamics of the system. A “phase” represents a section of a trajectory with continuous state and one set of governing equations. Three different types of phases are possible in this system: standing on ground, slipping on ice, and flying through the air. Since the first two cases are similar, we will first handle them together and then discuss flight phases.
Let (,) be the hip position, be the toe horizontal position, be the spring coefficient, be the damping coefficient, be the SLIP length, be the actuated SLIP length (or neutral spring length), be the hip torque, and be the hip and toe masses, respectively, and be the gravitational constant. The state of the SLIP can be then represented as follows:
And the SLIP length can then be written as:
(1)
Differentiating by the chain rule, we get the following equation for the time derivative of :
(2)
The force exerted by the spring-damper combination can then be expressed as follows:
(3)
While the force exerted by the hip torque on the ground is simply:
(4)
Realizing that the cosine of the angle between the SLIP and the ground is equal to and the sine is equal to , it is possible to express the acceleration of the hip:
(5)
(6)
However, it is also necessary to know the acceleration of the toe on the ice. We first determine the ground reaction force as follows:
(7)
The force of friction on the toe is then given as:
(8)
Where the serves as a smooth approximation of a sign function, whose discontinuities create convergence difficulties for the optimizer.
Finally, the acceleration of the toe can be expressed as follows:
(9)
This concludes the SLIP dynamics during both slippery and high-friction stance phases.
Flight phases are governed by the standard equations for ballistic trajectories and are triggered when the ground reaction force intersects zero. First, we declare a free variable, . The following equations then result:
Where all “to” subscripts represent takeoff states and all “land” subscripts represent landing states.
Within a certain phase, constraints are formulated using direct collocation, which discretizes the states and control inputs at evenly-spaced nodes and enforces the dynamics between nodes using hard equality constraints. The following inequality constraints are also enforced at every node:
There also need to be constraints governing phase transitions. Since the flight dynamics are solvable in closed form, this ultimately amounts to linking the end of one stance phase to the beginning of the next in a way which obeys the flight dynamics. Discretizing the state along the flight path is unnecessary.
As mentioned previously, the flight time parameter here is really critical. I first fooled around with a bunch of different strategies (for example parameterizing a completely separate landing state, with the landing angle and length inferred from the first node of the next phase, and then equality constraining the two). None converged reliably; many thanks to Christian Hubicki for pointing me in the right direction here.
The best method turned out to be evaluating the end state of the ballistic flight trajectory from the take off state and flight time, and then equality constraining that with the first node of the next stance phase. The specific variables that were constrained from the flight phase were , , , , and . The ground reaction force at takeoff and at touchdown are constrained to zero, and is constrained equal to at touchdown.
There are a few more variables and constraints that are important to note. Each stance and flight phase had a time parameter, which was bounded using the lower bound and upper bound arguments to fmincon. Similarly, the max/min actuated leg length second derivative and hip torque were also bounded in the fmincon bounds arguments. The toe x position was bounded to be within the ice patch for the slippery phase and outside for the sticky phase. The initial SLIP state was constrained to a starting position over the ice patch, and the final SLIP x and toe x were constrained to be slightly to the right of the ice patch.
Cold-initializing an energy-optimizing objective with random parameters often results in failure to converge due to the large number of parameters and constraints. Therefore, a three-step optimization was used:
(10)
Where is the number of stance phases, is the number of discretization nodes per stance phase, and is equal to the n_{th} stance phase time divided by .
(11)
Since the absolute value function is inherently nonsmooth—creating optimizer convergence problems—a smooth approximation was employed:
(12)
Where is about .
As mentioned previously, the trajectory optimization problem was transcribed using direct collocation with 10 nodes per stance phase. For linking one node to the next within a stance phase, dynamics were integrated using trapezoidal quadrature. Matlab’s fmincon was used for the optimization, with the SQP algorithm specified. Jacobians were symbolically calculated to accelerate convergence. The whole three-step optimization takes on the order of a few minutes to complete and converges reliably from random initial parameters.
GEN_CONSTRAINTS = true; GEN_COSTS = true; sp = SimParams(['str'], [0,1], [1.1; 0; 1.1-cosd(45); 0; ... sind(45); 0; 1; 0], 1.1); % Options for optimizing the constant cost function cOptions = optimoptions(@fmincon, 'TolFun', 0.00000001, ... 'MaxIterations', 100000, ... 'MaxFunEvals', 50000, ... 'Display', 'iter', 'Algorithm', 'sqp', ... 'StepTolerance', 1e-13, ... 'SpecifyConstraintGradient', true, ... 'SpecifyObjectiveGradient', true, ... 'ConstraintTolerance', 1e-6, ... 'CheckGradients', false, ... 'HonorBounds', true, ... 'FiniteDifferenceType', 'forward'); % Options for optimizing the actual cost function aOptions = optimoptions(@fmincon, 'TolFun', 0.00000001, ... 'MaxIterations', 1000, ... 'MaxFunEvals', 1000000, ... 'Display', 'iter', 'Algorithm', 'sqp', ... 'StepTolerance', 1e-13, ... 'SpecifyConstraintGradient', true, ... 'SpecifyObjectiveGradient', true, ... 'ConstraintTolerance', 1e-6, ... 'CheckGradients', false, ... 'HonorBounds', true, ... 'FiniteDifferenceType', 'forward'); % No linear inequality or equality constraints A = []; B = []; Aeq = []; Beq = []; % Set up the bounds [lb, ub] = bounds(sp); tic numVars = size(sp.phases, 1) * 2 - 1 + sp.gridn * size(sp.phases, 1) * 10; funparams = conj(sym('x', [1 numVars], 'real')'); if GEN_CONSTRAINTS fprintf('Generating constraints...\n'); [c, ceq] = constraints(funparams, sp); cjac = jacobian(c, funparams).'; ceqjac = jacobian(ceq, funparams).'; constraintsFun = matlabFunction(c, ceq, cjac, ceqjac, 'Vars', {funparams}); fprintf('Done generating constraints...\n'); end if GEN_COSTS fprintf('Generating costs...\n'); ccost = constcost(funparams, sp); ccostjac = jacobian(ccost, funparams).'; ccostFun = matlabFunction(ccost, ccostjac, 'Vars', {funparams}); acost = actsqrcost(funparams, sp); acostjac = jacobian(acost, funparams).'; acostFun = matlabFunction(acost, acostjac, 'Vars', {funparams}); awcost = actworkcost(funparams, sp); awcostjac = jacobian(awcost, funparams).'; awcostFun = matlabFunction(awcost, awcostjac, 'Vars', {funparams}); fprintf('Done generating costs...\n'); end flag = -1; tryCount = 0; while flag < 0 && tryCount < 3 fprintf('Generating initial possible trajectory (%d)...\n', tryCount+1); % Generate initial guess x0 = MinMaxCheck(lb, ub, ones(numVars, 1) * rand() * 2); [ci, ceqi, cjaci, ceqjaci] = constraintsFun(x0); while any(imag(ci)) || any(imag(ceqi)) || any(any(imag(cjaci))) || any(any(imag(ceqjaci))) || ... any(isnan(ci)) || any(isnan(ceqi)) || any(any(isnan(cjaci))) || any(any(isnan(ceqjaci))) || ... any(isinf(ci)) || any(isinf(ceqi)) || any(any(isinf(cjaci))) || any(any(isinf(ceqjaci))) fprintf('Regenerating initial guess...\n'); x0 = MinMaxCheck(lb, ub, ones(numVars, 1) * rand()); [ci, ceqi, cjaci, ceqjaci] = constraintsFun(x0); end % Find any feasible trajectory [feasible, ~, flag, ~] = ... fmincon(@(x) call(ccostFun, x, 2),x0,A,B,Aeq,Beq,lb,ub, ... @(x) call(constraintsFun, x, 4), cOptions); tryCount = tryCount + 1; end if flag > 0 fprintf('Optimizing for torque/raddot squared...\n'); % Optimize for torque/raddot squared cost [optimal, ~, ~, ~] = ... fmincon(@(x) call(acostFun, x, 2),feasible,A,B, ... Aeq,Beq,lb,ub,@(x) call(constraintsFun,x,4),aOptions); fprintf('Optimizing for work...\n'); % Optimize for minimum work [optimal, cost, flag, ~] = ... fmincon(@(x) call(awcostFun, x, 2),optimal,A,B, ... Aeq,Beq,lb,ub,@(x) call(constraintsFun,x,4),aOptions); fprintf('Done optimizing\n'); else optimal = []; cost = 0; fprintf('Exited with non-positive flag: %d\n', flag); end fprintf('Finished optimizing in %f seconds\n', toc);
function [ c, ceq ] = constraints( funparams, sp ) % Phase inequality constraints phaseIC = sym('pic', [1, 4*sp.gridn*size(sp.phases, 1)])'; % Phase equality constraints phaseEC = sym('pec', [1, 8*(sp.gridn-1)*size(sp.phases, 1)])'; % Phase transition equality constraints transEC = sym('tec', [1, 8*(size(sp.phases, 1)-1)])'; % Unpack the parameter vector [ stanceT, flightT, xtoe, xtoedot, x, xdot, y, ydot, ... ra, radot, raddot, torque] = unpack(funparams, sp); % Iterate over all the phases for p = 1 : size(sp.phases, 1) phaseStr = sp.phases(p, :); % The index of the first dynamics variable for the current phase ps = (p - 1) * sp.gridn + 1; % Calculate the timestep for that specific phase dt = stanceT(p) / sp.gridn; % Take off state at the end of the last phase if p > 1 toState = stateN; toCompvars = compvarsN; end stateN = [xtoe(ps); xtoedot(ps); x(ps); xdot(ps); ... y(ps); ydot(ps); ra(ps); radot(ps)]; % Link ballistic trajectory from end of last phase to this phase if p > 1 rland = sqrt((x(ps) - xtoe(ps))^2 + y(ps)^2); [xtoedotland, xland, xdotland, yland, ydotland] = ... ballistic(toState, flightT(p-1), sp, phaseStr); % Grf must equal zero at takeoff transEC((p-2)*8+1 : (p-1)*8) = ... [xtoedotland-xtoedot(ps); xland-x(ps); ... xdotland-xdot(ps); yland-y(ps); ydotland-ydot(ps); ... ra(ps) - rland; radot(ps); toCompvars.grf]; end % Offset in the equality parameter vector due to phase pecOffset = 8 * (sp.gridn - 1) * (p - 1); % Offset in the inequality parameter vector due to phase picOffset = 4 * (sp.gridn) * (p - 1); [statedotN, compvarsN] = dynamics(stateN, raddot(ps), ... torque(ps), sp, phaseStr); for i = 1 : sp.gridn - 1 % The state at the beginning of the time interval stateI = stateN; % What the state should be at the end of the time interval stateN = [xtoe(ps+i); xtoedot(ps+i); x(ps+i); xdot(ps+i); ... y(ps+i); ydot(ps+i); ra(ps+i); radot(ps+i)]; % The state derivative at the beginning of the time interval statedotI = statedotN; % Some calculated variables at the beginning of the interval compvarsI = compvarsN; % The state derivative at the end of the time interval [statedotN, compvarsN] = ... dynamics(stateN, raddot(ps+i), torque(ps+i), sp, phaseStr); % The end position of the time interval calculated using quadrature endState = stateI + dt * (statedotI + statedotN) / 2; % Constrain the end state of the current time interval to be % equal to the starting state of the next time interval phaseEC(pecOffset+(i-1)*8+1:pecOffset+i*8) = stateN - endState; % Constrain the length of the leg, grf, and body y pos phaseIC(picOffset+(i-1)*4+1 : picOffset+i*4) = ... [compvarsI.r - sp.maxlen; sp.minlen - compvarsI.r; ... -compvarsI.grf; compvarsI.grf - sp.maxgrf]; end if p == size(sp.phases, 1) % Constrain the length of the leg at the end position % Since it's the end of the last phase, add grf constraint phaseIC(picOffset+(sp.gridn-1)*4+1:picOffset+sp.gridn*4) = ... [compvarsN.r - sp.maxlen; sp.minlen - compvarsN.r; ... -compvarsN.grf; compvarsN.grf - sp.maxgrf]; else % Constrain the length of the leg at the end position % No ground reaction force constraint (this will be handled in % transition equality constraints) phaseIC(picOffset+(sp.gridn-1)*4+1:picOffset+sp.gridn*4) = ... [compvarsN.r - sp.maxlen; sp.minlen - compvarsN.r; -1; -1]; end end c = phaseIC; ceq = [phaseEC; transEC]; initialState = [xtoe(1); xtoedot(1); x(1); xdot(1); ... y(1); ydot(1); ra(1); radot(1)]; % Add first phase start constraints ceq = [ceq; initialState - sp.initialState]; % Add last phase end constraints ceq = [ceq; x(end) - sp.finalProfileX; xtoe(end) - sp.finalProfileX]; end
function [ stanceT, flightT, xtoe, xtoedot, x, xdot, y, ydot, ... ra, radot, raddot, torque] = unpack( funparams, sp ) p = size(sp.phases, 1); cnt = sp.gridn * p; stanceT = funparams(1 : p); flightT = funparams(p + 1 : p * 2 - 1); xtoe = funparams(p * 2 : p * 2 - 1 + cnt); xtoedot = funparams(p * 2 + cnt : p * 2 - 1 + cnt * 2); x = funparams(p * 2 + cnt * 2 : p * 2 - 1 + cnt * 3); xdot = funparams(p * 2 + cnt * 3 : p * 2 - 1 + cnt * 4); y = funparams(p * 2 + cnt * 4 : p * 2 - 1 + cnt * 5); ydot = funparams(p * 2 + cnt * 5 : p * 2 - 1 + cnt * 6); ra = funparams(p * 2 + cnt * 6 : p * 2 - 1 + cnt * 7); radot = funparams(p * 2 + cnt * 7 : p * 2 - 1 + cnt * 8); raddot = funparams(p * 2 + cnt * 8 : p * 2 - 1 + cnt * 9); torque = funparams(p * 2 + cnt * 9 : p * 2 - 1 + cnt * 10); end
function [ statedot, compvars ] = dynamics( state, raddot, torque, sp, curPhase ) stateCell = num2cell(state'); [xtoe, xtoedot, x, xdot, y, ydot, ra, radot] = deal(stateCell{:}); r = sqrt((x - xtoe)^2 + y^2); rdot = ((x-xtoe)*(xdot-xtoedot)+y*ydot)/(r); cphi = (x-xtoe) / r; sphi = y / r; fs = sp.spring * (ra - r) + sp.damp * (radot - rdot); ft = torque / r; fg = sp.masship * sp.gravity; xddot = (1/sp.masship) * (fs*cphi + ft * sphi); yddot = (1/sp.masship) * (fs*sphi + ft * (-cphi) - fg); grf = fs * sphi - ft * cphi + sp.masstoe * sp.gravity; % Check if the phase should be slipping if strcmp(curPhase, 'sli') ff = -sp.friction * tanh(xtoedot * 50) * grf; xtoeddot = (1/sp.masstoe) * (-fs*cphi - ft*sphi + ff); else xtoeddot = 0; end statedot = [xtoedot; xtoeddot; xdot; xddot; ydot; yddot; ... radot; raddot]; compvars.r = r; compvars.grf = grf; end
function [ xtoedotland, xland, xdotland, yland, ydotland ] = ... ballistic( toState, flightTime, sp, curPhase ) stateCell = num2cell(toState'); [~, ~, x, xdot, y, ydot, ~, ~] = deal(stateCell{:}); if strcmp(curPhase, 'sli') xtoedotland = xdot; else xtoedotland = 0; end xland = x + xdot * flightTime; yland = y + ydot * flightTime - 0.5 * sp.gravity * flightTime^2; xdotland = xdot; ydotland = ydot - sp.gravity * flightTime; end
function [ cost ] = constcost( funparams, sp ) cost = sym(1); end
function [ cost ] = actsqrcost( funparams, sp ) % Unpack the vector [stanceT, ~, ~, ~, ~, ~, ~, ~, ~, ~, raddot, torque] = ... unpack(funparams, sp); dts = kron(stanceT./sp.gridn, ones(sp.gridn, 1)); cost = sum((raddot .* dts) .^ 2) + sum((torque .* dts) .^ 2); end
actworkcost.m
function [ cost ] = actworkcost( funparams, sp ) % Unpack the vector [stanceT, ~, xtoe, ~, x, xdot, y, ydot, ra, radot, ~, torque] = ... unpack(funparams, sp); phaseN = size(sp.phases, 1); r = sqrt((x - xtoe).^2 + y.^2); rdot = ((x-xtoe).*(xdot)+y.*ydot)./(r); fs = sp.spring * (ra - r) + sp.damp * (radot - rdot); startRemInd = 1 : sp.gridn : sp.gridn * phaseN; endRemInd = sp.gridn : sp.gridn : sp.gridn * phaseN; [fsA, fsB] = deal(fs); fsA(startRemInd) = []; fsB(endRemInd) = []; fsCombined = (fsA + fsB) .* 0.5; [radotA, radotB] = deal(radot); radotA(startRemInd) = []; radotB(endRemInd) = []; radotCombined = (radotA + radotB) .* 0.5; epsilon = 0.001; workRa = (sqrt((fsCombined.*radotCombined).^2 + epsilon^2) - epsilon); workRa = workRa .* kron(stanceT./sp.gridn, ones(sp.gridn - 1, 1)); angles = atan2(y, x - xtoe); angleShift = [angles(1); angles(1 : end-1)]; angleDeltas = angles - angleShift; angleDeltas(startRemInd) = []; [torqueA,torqueB] = deal(torque); torqueA(startRemInd) = []; torqueB(endRemInd) = []; torqueCombined = (torqueA + torqueB) .* 0.5; workAng = sqrt((torqueCombined.*angleDeltas).^2 + epsilon^2) - epsilon; cost = sum(workRa) + sum(workAng); end
function [ lb, ub ] = bounds(sp) stanceTimeMin = sp.minStanceTime; stanceTimeMax = sp.maxStanceTime; flightTimeMin = sp.minFlightTime; flightTimeMax = sp.maxFlightTime; minXtoedot = -Inf; maxXtoedot = Inf; minX = -Inf; maxX = Inf; minXdot = -Inf; maxXdot = Inf; minY = 0; maxY = Inf; minYdot = -Inf; maxYdot = Inf; minRa = -Inf; maxRa = Inf; minRadot = -Inf; maxRadot = Inf; minRaddot = sp.minraddot; maxRaddot = sp.maxraddot; minTorque = sp.mintorque; maxTorque = sp.maxtorque; minXtoe = []; maxXtoe = []; for p=1:size(sp.phases, 1) phase = sp.phases(p, :); if strcmp(phase, 'sli') minXtoe = [minXtoe; ones(sp.gridn, 1) * sp.slipPatch(1)]; maxXtoe = [maxXtoe; ones(sp.gridn, 1) * sp.slipPatch(2)]; elseif strcmp(phase, 'stl') minXtoe = [minXtoe; ones(sp.gridn, 1) * -Inf]; maxXtoe = [maxXtoe; ones(sp.gridn, 1) * sp.slipPatch(1)]; elseif strcmp(phase, 'str') minXtoe = [minXtoe; ones(sp.gridn, 1) * sp.slipPatch(2)]; maxXtoe = [maxXtoe; ones(sp.gridn, 1) * Inf]; end end stanceTimeVars = ones(size(sp.phases, 1), 1); flightTimeVars = ones(size(sp.phases, 1) - 1, 1); gridVars = ones(sp.gridn * size(sp.phases, 1), 1); lb = [stanceTimeVars * stanceTimeMin; flightTimeVars * flightTimeMin; minXtoe; gridVars * minXtoedot; gridVars * minX; gridVars * minXdot; gridVars * minY; gridVars * minYdot; gridVars * minRa; gridVars * minRadot; gridVars * minRaddot; gridVars * minTorque]; ub = [stanceTimeVars * stanceTimeMax; flightTimeVars * flightTimeMax; maxXtoe; gridVars * maxXtoedot; gridVars * maxX; gridVars * maxXdot; gridVars * maxY; gridVars * maxYdot; gridVars * maxRa; gridVars * maxRadot; gridVars * maxRaddot; gridVars * maxTorque]; end
function [ state ] = randState( slipPatch ) sp = SimParams(); % Generate random initial hip x with 0.3 meter margin from patch edges x = rand() * (slipPatch(2) - 0.6) + 0.3; % Random initial hip x vel must be positive xdot = rand() * 0.5; % Random toe x within 0.2 forward of hip x xtoe = x + rand() * 0.2; % Toe x vel equal to hip x vel xtoedot = xdot; % Y calculated assuming rtouchdown = maxlen y = sqrt(sp.maxlen ^ 2 - (x - xtoe) ^ 2); % Y vel some random downward number ydot = -rand() * 0.4; % Actuated length at max length, and initial actuated length derivative % is zero ra = sp.maxlen; radot = 0; state = [xtoe; xtoedot; x; xdot; y; ydot; ra; radot]; end
function [varargout] = call(fun, x, nout) % Calls the specific function, specifying number of output args varargout = cell(nout, 1); [varargout{:}] = fun(x); end
classdef SimParams < handle properties % Order of phases (sli=slip, stl=stick left side of patch, str = % stick right side of patch phases % The region of slippery terrain slipPatch % The initial state of the SLIP over the ice initialState % The final x position of the toe and the hip finalProfileX gridn = 10; % Number of grid points during stance phase masship = 1; % Mass of body in kilograms masstoe = 0.1; % Mass of the toe in kilograms spring = 20; % Spring coefficient damp = 0.5; % Damping coefficient gravity = 1; % Gravity % Friction coefficient between toe and ground on slippery surfaces friction = 0.05; minStanceTime = 0.1; % Minimum stance phase time maxStanceTime = 2; % Maximum stance phase time minFlightTime = 0; % Minimum flight phase time maxFlightTime = 5; % Maximum flight phase time minraddot = -1; % Minimum second deriv of actuated length maxraddot = 1; % Maximum second deriv of actuated length mintorque = -1; % Minimum torque at hip maxtorque = 1; % Maximum torque at hip minlen = 0.5; % Minimum length of the leg maxlen = 1; % Maximum length of the leg maxgrf = 10; % Maximum ground reaction force end methods function obj = SimParams(phases, slipPatch, initialState, finProfX) if nargin == 0 phases = []; slipPatch = []; initialState = []; finProfX = 0; end obj.phases = phases; obj.slipPatch = slipPatch; obj.initialState = initialState; obj.finalProfileX = finProfX; end function iss = iss(obj) iss = struct(); iss.xtoe = obj.initialState(1); iss.xtoedot = obj.initialState(2); iss.x = obj.initialState(3); iss.xdot = obj.initialState(4); iss.y = obj.initialState(5); iss.ydot = obj.initialState(6); iss.ra = obj.initialState(7); iss.radot = obj.initialState(8); end end end
Visualization of trajectories and other stuff relevant to my research project (more on that later) is on GitHub. Include visualize.m, animate.m, VisParams.m, and rewind.gif to look at the optimized trajectories. My apologies for the slightly messy code. Here’s some images of what the trajectories look like:
Key: dark blue (slide phase), green (first stick phase), red (second stick phase), light blue (flight phase), black (toe position), green highlight (landing), blue highlight (takeoff)—dots equally time-spaced, SLIP in starting state.
The post Optimizing Trajectories for the Spring Loaded Inverted Pendulum over Slippery Terrain appeared first on Sam Pfrommer.
]]>The post Tutorial: Direct Collocation Trajectory Optimization with Matlab appeared first on Sam Pfrommer.
]]>The core difference between shooting methods and simultaneous methods such as direct collocation is that in simultaneous methods the states are discretized along with the control inputs. These state variables are added to the vector of function parameters, and constraints are added at every discretization point to ensure that the dynamics are evolving correctly. For the mathematically minded, the state vector effectively becomes the following:
(Optionally, the time of the simulation can also be included in the state as a variable for the optimizer). Each constraint would be formulated as follows:
Where would depend on . Note that in this formulation the state derivative is linearly interpolated between the two grid points, known as trapezoidal quadrature. Other schemes, such as Hermite-Simpson collocation, are slightly more complex and will be addressed in the next tutorial.
Since fmincon’s equality constraints are given as a vector of values to drive to zero, the end equality constraints would look something like:
Let’s look at an example, using the same double integrator plant from the previous tutorial.
global gridN gridN = 20; tic % Minimize the simulation time time_min = @(x) x(1); % The initial parameter guess; 1 second, gridN positions, gridN velocities, % gridN accelerations x0 = [1; linspace(0,1,gridN)'; linspace(0,1,gridN)'; ones(gridN, 1) * 5]; % No linear inequality or equality constraints A = []; b = []; Aeq = []; Beq = []; % Lower bound the simulation time at zero seconds, and bound the % accelerations between -10 and 30 lb = [0; ones(gridN * 2, 1) * -Inf; ones(gridN, 1) * -10]; ub = [Inf; ones(gridN * 2, 1) * Inf; ones(gridN, 1) * 30]; % Options for fmincon options = optimoptions(@fmincon, 'TolFun', 0.00000001, 'MaxIter', 10000, ... 'MaxFunEvals', 100000, 'Display', 'iter', ... 'DiffMinChange', 0.001, 'Algorithm', 'sqp'); % Solve for the best simulation time + control input optimal = fmincon(time_min, x0, A, b, Aeq, Beq, lb, ub, ... @double_integrator_constraints, options); % Discretize the times sim_time = optimal(1); delta_time = sim_time / gridN; times = 0 : delta_time : sim_time - delta_time; % Get the state + accelerations (control inputs) out of the vector positions = optimal(2 : 1 + gridN); vels = optimal(2 + gridN : 1 + gridN * 2); accs = optimal(2 + gridN * 2 : end); % Make the plots figure(); plot(times, accs); title('Control Input (Acceleration) vs Time'); xlabel('Time (s)'); ylabel('Acceleration (m/s^2)'); figure(); plot(times, vels); title('Velocity vs Time'); xlabel('Time (s)'); ylabel('Velocity (m/s)'); figure(); plot(times, positions); title('Position vs Time'); xlabel('Time (s)'); ylabel('Position (m)'); disp(sprintf('Finished in %f seconds', toc));
function [ c, ceq ] = double_integrator_constraints( x ) global gridN % No nonlinear inequality constraint needed c = []; % Calculate the timestep sim_time = x(1); delta_time = sim_time / gridN; % Get the states / inputs out of the vector positions = x(2 : 1 + gridN); vels = x(2 + gridN : 1 + gridN * 2); accs = x(2 + gridN * 2 : end); % Constrain initial position and velocity to be zero ceq = [positions(1); vels(1)]; for i = 1 : length(positions) - 1 % The state at the beginning of the time interval x_i = [positions(i); vels(i)]; % What the state should be at the start of the next time interval x_n = [positions(i+1); vels(i+1)]; % The time derivative of the state at the beginning of the time % interval xdot_i = [vels(i); accs(i)]; % The time derivative of the state at the end of the time interval xdot_n = [vels(i+1); accs(i+1)]; % The end state of the time interval calculated using quadrature xend = x_i + delta_time * (xdot_i + xdot_n) / 2; % Constrain the end state of the current time interval to be % equal to the starting state of the next time interval ceq = [ceq ; x_n - xend]; end % Constrain end position to 1 and end velocity to 0 ceq = [ceq ; positions(end) - 1; vels(end)]; end
If you run this code, you should see that the optimizer converges to the same bang-bang control it did with single shooting.
Now that you’ve read through the above code and hopefully better understand direct collocation, you may be wondering the advantages and disadvantages are of simultaneous methods over shooting methods. Christian Hubicki, former grad student at Oregon State, succinctly summarizes the difference in his dissertation on optimization for robotics (36-38) [1]:
There is a notable computational advantage to this somewhat non-intuitive formulation. With direct collocation, the relationship between variables and constraints can be expressed entirely with closed-form equations, even if the integral of our dynamical equations cannot (which is frequently the case in legged locomotion). This means that both the values of the objective, constraints, and the gradients with respect to all optimization variables can also be represented in closed form, greatly accelerating the computation time of the optimization. In practice, this computational efficiency and smoothness of the resulting equations has allowed our implementations to solve direct collocation problems with over 10,000 variables and constraints.
One apparent downside of direct collocation is a loss of dynamical accuracy. [Shooting methods] are still free to use variable-time-step time-marching integrators to integrate the dynamics as accurately as possible, but direct collocation often operates with far fewer integration steps and are roughly fixed in size. However, the degree to which direct collocation makes optimization faster often allows for the user to select quite fine resolutions, e.g. hundreds of time points. Further, in this application, the results of these optimizations will often be regulated by a locally stabilizing controllers anyhow, making a small degree of dynamical inaccuracy akin to noise in the system.
As an aside, other sources such as Matthew Kelly’s write-up caution against using variable time step integrators for shooting methods [2].
The post Tutorial: Direct Collocation Trajectory Optimization with Matlab appeared first on Sam Pfrommer.
]]>The post Tutorial: Single Shooting Trajectory Optimization with Matlab appeared first on Sam Pfrommer.
]]>The core of trajectory optimization is constrained function optimization. In this tutorial, I use Matlab’s fmincon—alternatives include SNOPT and IPOPT. In simple terms, fmincon finds the set of parameters that minimize a certain objective function, subject to linear or nonlinear constraints. These constraints can be formulated in four different ways, as documented at the above link: linear equality, linear inequality, nonlinear equality, and nonlinear inequality. Let’s illustrate a possible application of this with a short example (taken from Minoj Srinivasan’s lecture slides).
Say there’s a target at the coordinates [5, 10]. We want to shoot a cannon ball at the target from position [0, 0], minimizing the amount of initial energy required (proportional to ). The optimizer controls three parameters: , , and the flight time. The only constraint is that after flying for the designated time with the given initial conditions the cannon ball must end up close to the target (how close is a parameter to the optimizer). We lower bound the simulation time at zero and give it an initial guess for the appropriate parameters.
% The function to be minimized (E = vx_0^2 + vy_0^2) energy_fun = @(x) (x(2)^2 + x(3)^2); % The initial parameter guess; time = 1; vx_0 = 2, vy_0 = 3 x0 = [1; 2; 3]; % No linear inequality or equality constraints A = []; b = []; Aeq = []; Beq = []; % Lower bound the simulation time at zero, leave initial velocities unbounded lb = [0; -Inf; -Inf]; ub = [Inf; Inf; Inf]; % Solve for the best simulation time + control input optimal = fmincon(energy_fun, x0, A, b, Aeq, Beq, lb, ub, ... @flight_constraint, options); % Unpack the parameters opt_time = optimal(1); opt_vx0 = optimal(2); opt_vy0 = optimal(3); fprintf('Optimal time: %f\n', opt_time); fprintf('Optimal initial velocity: [%f, %f]\n', opt_vx0, opt_vy0);
function [ c, ceq ] = flight_constraint( x ) c = []; % Unpack the parameters time = x(1); vx_0 = x(2); vy_0 = x(3); % Calculate the travel distances x_travel = time * vx_0; y_travel = time * vy_0 - 0.5 * 9.81 * time^2; % The following two constraints must equal zero, meaning that the % target is set at x = 5 and y = 10 ceq = [x_travel - 5; y_travel - 10]; end
In the previous example, the optimizer could only adjust the initial conditions of the cannon ball and the flight time. What if we want to find an optimal trajectory with a control input that varies with time? The trick here is to discretize the control input into n segments and include each segment as a variable in the parameter vector. Let’s consider an example of this strategy applied to the canonical double-integrator (state is position and velocity along line, control input is acceleration). The control input is the second derivative of the position, and the constraints specify that the final position should be one and the final velocity should be zero. Our parameter vector consists of the simulation time followed by one hundred time-discretized accelerations. The optimizer is instructed to find the minimum time solution.
% Minimize the simulation time time_min = @(x) x(1); % The initial parameter guess; 1 second, with a hundred accelerations % all initialized to 5 x0 = [1; ones(100, 1) * 5]; % No linear inequality or equality constraints A = []; b = []; Aeq = []; Beq = []; % Lower bound the simulation time at zero seconds, and bound the % accelerations between -10 and 30 lb = [0; ones(100, 1) * -10]; ub = [Inf; ones(100, 1) * 30]; % Options for fmincon options = optimset('TolFun', 0.00000001, 'MaxIter', 100000, ... 'MaxFunEvals', 100000); % Solve for the best simulation time + control input optimal = fmincon(time_min, x0, A, b, Aeq, Beq, lb, ub, ... @double_integrator_constraints, options); % The simulation time is the first element in the solution sim_time = optimal(1); % The time discretization delta_time = sim_time / (length(optimal) - 1); times = 0 : delta_time : sim_time - delta_time; % The accelerations given by the optimizer accs = optimal(2:end); % The resulting velocities (integrate accelerations with respect to time) vels = cumtrapz(times, accs); % The resulting positions (integrate velocities with respect to time) positions = cumtrapz(times, vels); % Make the plots figure(); plot(times, accs); title('Control Input (Acceleration) vs Time'); xlabel('Time (s)'); ylabel('Acceleration (m/s^2)'); figure(); plot(times, vels); title('Velocity vs Time'); xlabel('Time (s)'); ylabel('Velocity (m/s)'); figure(); plot(times, positions); title('Position vs Time'); xlabel('Time (s)'); ylabel('Position (m)');
function [ c, ceq ] = double_integrator_constraints( x ) % No nonlinear inequality constraint needed c = []; % Discretize the times into a vector sim_time = x(1); delta_time = sim_time / (length(x) - 1); times = 0 : delta_time : sim_time - delta_time; % Get the accelerations out of the rest of the parameter vector accs = x(2:end); % Integrate up the velocities and final position vels = cumtrapz(times, accs); pos = trapz(times, vels); % All elements of this vector must be constrained to equal zero % This means that final pos must equal 1 (target) and the end velocity % must be equal to zero. ceq = [pos - 1; vels(end)]; end
As we can see from the graphs, the optimizer logically found bang-bang control to be optimal. Additionally, note that the fraction of time spent in either max-acceleration or min-acceleration is corresponds to the ratio of the max/min bounds. Intuitively, this provides the fastest trajectory while still satisfying the zero-end-velocity constraint.
The trajectory optimization method detailed above is known as “single shooting.” This means that we simulate the whole trajectory and try to drive the deviation of the endpoint to zero. Multiple shooting is slightly more sophisticated—instead of simulating the whole trajectory at once, the trajectory is broken apart into multiple subintervals. For each subinterval, the starting position is added as a parameter and a constraint is added suchthat the deviation between the end of one subinterval and the beginning of another must end up being zero. This approach enables the optimizer to solve much more complex problems than with single shooting.
I hope this article was helpful for better understanding trajectory optimization! Please keep in mind that this is just a tutorial to get you started with some of the basic concepts, and I highly encourage you to check out the references below:
The post Tutorial: Single Shooting Trajectory Optimization with Matlab appeared first on Sam Pfrommer.
]]>The post 3D Printing Inaccuracies with the Lulzbot Mini appeared first on Sam Pfrommer.
]]>The good news is that these imprecision are fairly consistent, and after some tinkering I figured out a few rules of thumb for sizing dimensions. For the record, I tested these using Gizmo Dorks 2.85mm HIPS filament on high print quality in Cura.
Below are some of the different 3D objects I tried to print and the resulting inaccuracies. These aren’t very precise measurements, but they should be good to about a quarter millimeter. I’ll add on to this list as I keep making parts, and I hope it helps you avoid repeating some of my early mistakes.
For printing cylinders, there were no noticeable differences between commanded and actual diameters as long as the cylinder was reasonably thick.
A good rule of thumb for printing cylindrical holes in solids is make the diameter in your CAD software about 0.6mm larger than what you actually want (e.g. specify 6.6mm to end up with a 6mm hole). I’ve tested this for holes between 5mm and 20mm in diameter.
Another thing to keep in mind is that the diameter is narrower at the printing plate than at the top of the hole. For example, I set my hole diameter to 6.6 millimeters in order to snugly fit a 6mm brass pipe inside; it was a perfect fit at the bottom, but had a small amount of slack at the top. This is most likely because the plastic spreads out a bit more when in contact with the printing plate, resulting in a smaller hole.
When printing a rectangular prism, the actual dimensions are slightly larger than the commanded ones around the centers of the edges, while the corners are rounded off. When I tested this, I wanted to fit the rectangular hole that’s commonly found in the center of most Bioloid frames. My first attempt was printing the exact dimensions of the hole (10mm x 6mm). This resulted in a slight press fit, which is exactly what I wanted; if you don’t want a press fit, you may have to scale down the dimensions.
The post 3D Printing Inaccuracies with the Lulzbot Mini appeared first on Sam Pfrommer.
]]>The post Optimizing Retraction Reflexes Using Reinforcement Learning based on Particle Filtering appeared first on Sam Pfrommer.
]]>Although I won’t be going over all the technical details (especially regarding the passive impedance), I would like to share some of my results in using Reinforcement Learning based on Particle Filtering (Kormushev, Caldwell) for the task of cushioning a landing robot.
The robot model used in this simulation first consisted of a hip body constrained to move in the vertical axis connected to a 312mm link (the thigh) with an unactuated revolute joint. This was in turn connected to a 434mm link (the shank) with an actuated knee joint. Finally, an unactuated revolute joint connected the shank with a foot constrained to move in the same axis as the hip.
The general idea of the active impedance controller is this: first, model what a retraction reflex looks like. In this paper, a knee angle trajectory was generated by smoothing a square retraction reflex. This trajectory was tracked using a PD controller on the difference between the desired and actual knee angle. The parameters for the generated trajectory in the original paper were the time to begin the reflex (relative to the impact time), the time to return to the original pose, and the magnitude of the reflex. In my simulation, I removed the return time parameter, ending each trial with the robot in the fully retracted position. I also added a parameter that controlled the smoothing of the square reflex.
The reinforcement learning algorithm attempts to find the optimal set of parameters (begin time, smooth velocity, reflex magnitude) to optimize the reward function. In my case, the reward function gives high rewards for trials that have the lowest maximum hip acceleration, providing a smoother landing. The reinforcement learning algorithm alternates between exploration (global random sampling of parameter space) and exploitation. The exploitation step simply picks a random particle, with higher rewards corresponding to a higher chance of selection; a new particle is spawned near to the selected one with some exponentially decaying noise added on. As more trials are performed, the chance of exploitation rises.
Although the learning algorithm presented doesn’t include a culling step (removing all but the N best particles every some fixed number of trials), I included them. If you don’t, the algorithm tends to add more and more particles around prior clusters, even if a better policy has been found through exploration. This is because the added weights of the sub-optimal particles outweigh a superior, but isolated particle.
Simulation was performed using CAGE engine (made by my brother and I, not available publicly yet). The engine uses Box2D for physics and OpenGL for rendering. In this simulation, I just wanted to get some experience with the reinforcement learning algorithm and didn’t include passive impedance for simplicity.
The following video shows the first few trials, simulated in real time. The top graph shows the parameter space, with each dot representing an evaluated set of parameters. The colors of the dots are scaled from black (lowest reward) to white (highest reward). The blue circle marks the last added particle. The bottom graph shows the highest reward found at or before a certain trial. As you can see, the algorithm alternates between exploitation and exploration, with a higher emphasis on exploration in the earlier trials.
In the next video, the simulation has already been run for a few thousand trials. The algorithm tries to exploit more heavily now.
Finally, here is the best policy in slow motion (1/5th real time speed). As you can see, the downwards force is relatively constant throughout the landing phase.
The post Optimizing Retraction Reflexes Using Reinforcement Learning based on Particle Filtering appeared first on Sam Pfrommer.
]]>The post Game Engine Design Study #1 – Phage2D appeared first on Sam Pfrommer.
]]>This article will only examine the pros and cons of the overall architecture of Phage2D. It will not present specific implementations of rendering, physics, etc., but will rather discuss the integration of these low-level subsystems into the engine as a whole.
At its core, Phage2D uses the popular entity-component-system, with a few interesting modifications. Since the basic pattern has already been written about widely, I won’t cover it here. If you don’t know what an ECS is, I suggest reading this excellent article first.
Entities, as in the conventional entity-component-system pattern, are essentially “bags” of Components. Each Entity also contains an Aspect, which is a list of all the types of Components contained by the Entity. Multiple Components which extend the same base Component will still be of the same ComponentType. Aspects are very useful since they allow us to express Component dependencies and filter for certain types of Entities. We’ll get more into this later.
A Phage2D Component belongs to one of two basic types: DataComponent or LogicComponent. The third–WrapperComponent–is, in hindsight, a design mistake. I’ll omit it for now since it makes things unnecessarily complicated and doesn’t contribute much to the basic principles of the system.
DataComponents are strictly meant to hold data, either as public member variables or using getters and setters. DataComponents should not have methods which modify their contents, but can return mutated data (provided that the member variables stay untouched).
LogicComponents provide Entity-specific behavior. It is important to differentiate this from system behavior. As the name suggests, Entity-specific behaviors give an Entity its unique logical characteristics (e.g., when up arrow is pressed, Jack should jump). System behavior is common to all applicable Entities in the world, also called an EntitySystem (e.g., all Entities with the DataComponent for physics should fall, collide, and exhibit other physical phenomena). In this case, Jack is exhibiting both Entity-specific and system behavior.
LogicComponents realize their behavior mainly through DataComponents belonging to the same Entity. In order for a LogicComponent to be added to an Entity, the Entity must first already contain the DataComponents necessary for the LogicComponent to do its work. In other words, the Entity’s Aspect must encapsulate the dependency Aspect of the LogicComponent. Let’s have a look at an example:
/** * Stores the Texture of this Entity. */ public class TextureData extends DataComponent { public Texture texture; public TextureData(Texture texture) { this.texture = texture; } }
/** * Stores the Position of this Entity. */ public class PositionData extends DataComponent { public float x; public float y; public PositionData(float x, float y) { this.x = x; this.y = y; } }
Pretty basic stuff, just two basic DataComponents, one containing the position of the Entity, and the other containing a Texture to render. Now how would the rendering LogicComponent look?
/** * Renders a Texture at a certain position. */ public class RenderingLogic extends LogicComponent { private TextureData m_texture; private PositionData m_position; public RenderingLogic() { super(new Aspect(TypeManager.typeOf(TextureData.class), TypeManager.typeOf(PositionData.class))); } public void render(Renderer renderer) { // do rendering stuff } @Override public void loadDependencies() { m_texture = (TextureData) loadDependency(TypeManager.typeOf(TextureData.class)); m_position = (PositionData) loadDependency(TypeManager.typeOf(PositionData.class)); } }
So what does this do? Well, the RenderingLogic Component needs a Texture to render and a position to render at. This requirement is expressed through the argument to the LogicComponent constructor; it takes an Aspect that specifies what DataComponents the Entity must already contain in order to add this LogicComponent. If we try to add a RenderingLogic Component to an Entity lacking a PositionData or TextureData Component, an error will be thrown. Let’s see it all put together:
Entity entity = new Entity(); entity.addComponent(new TextureData(new Texture(/*....*/))); entity.addComponent(new PositionData(1, 2)); entity.addComponent(new RenderingLogic());
This should look pretty familiar to most conventional entity-component-systems. The main conceptual caveat is that the RenderingLogic Component must be added after the TextureData and PositionData Components, as specified by the dependencies.
As an attentive reader, you may have noticed that we never specified what is calling the render function in RenderingLogic. Repeated actions executed over groups of similarly composed Entities (system behavior) are done by AspectActivities, which are commonly referred to as systems in conventional entity-component-systems. In Phage2D, EntitySystems actually hold all the Entities in a given world, and allow AspectActivities to add SystemListeners and respond to the adding and removing of Entities.
Let’s have a look at an AspectActivity that will render all Entities containing the RenderingLogic Component:
/** * Calls RenderingLogic in all Entities with a RenderingLogic Component. */ public class RenderingActivity extends AspectActivity { private List<Entity> m_entities = new ArrayList<Entity>(); public RenderingActivity(EntitySystem system) { super(system, new Aspect(TypeManager.typeOf(RenderingLogic.class))); } @Override public void entityAdded(Entity entity) { m_entities.add(entity); } @Override public void entityRemoved(Entity entity) { m_entities.remove(entity); } public void render(Renderer renderer) { for (Entity entity : m_entities) { RenderingLogic rendering = (RenderingLogic) entity.getComponent(TypeManager.typeOf(RenderingLogic.class)); rendering.render(renderer); } } }
Similarly to the dependencies of LogicComponents, an AspectActivity operates on all Entities that include the Components specified in the Aspect passed to AspectActivity’s constructor. In this basic AspectActivity, we filter for Entities with a RenderingLogic Component, therefore subscribing to entityAdded and entityRemoved method calls on these Entities.
You may be wondering why the RenderingActivity doesn’t just render the Entities directly using the TextureData and PositionData, bypassing the RenderingLogic. This is illustrative of one of the main dilemmas of this design: determining where Entity-specific behavior ends and system behavior begins. In this case, we may want to create different kinds of rendering LogicComponents later that would all extend a common base class (e.g., one rendering text, one videos, etc.). Then, it would be clear that the general system behavior is that all renderable Entities should have the render method called on them, and the Entity-specific part begins when deciding how each unique Entity should render itself. The alternative would be to create a huge RenderingActivity class that switches behaviors based on the Entity it’s rendering (yuk!), or multiple smaller AspectActivities for each type of renderable Entity (e.g., TextRenderingActivity would render all Entities with a TextData Component).
One advantage of the LogicComponent approach would be that it is easier to opt out of rendering even when all the required DataComponents are there. If you had an Entity with a TextData Component, but didn’t want it to render, you could simply not add the TextRenderingLogic Component; in the other case, you would need some kind of a flag in the TextData Component telling the AspectActivity not to render it.
// holds all the Entities in our world EntitySystem system = new EntitySystem(); // makes the RenderingActivity manage rendering in the EntitySystem we created RenderingActivity rendering = new RenderingActivity(system); // create the Entity we want to render Entity entity = new Entity(); entity.addComponent(new TextureData(new Texture(/*....*/))); entity.addComponent(new PositionData(1, 2)); entity.addComponent(new RenderingLogic()); // following two lines will also call entityAdded on the RenderingActivity, since the Entity matches the filter system.addEntity(entity); system.update(); // has separate method to process all addEntity and removeEntity calls, used to avoid concurrency issues while (!/*close condition*/) { rendering.render(/*Renderer argument*/); // sleep until next frame }
Although these examples have of course been very crude, and probably syntactically incorrect at times (since I haven’t actually tested this code…), I hope you understand the general concepts behind Phage2D and will use your discretion in determining what elements to include in you own game engines.
All implemented designs have some lessons that we can take away from it. Below, I attempt to objectively detail some of the benefits and flaws of Phage2D’s architecture. Please note that this list is almost certainly not exhaustive, and if I missed anything please let me know.
Good:
Bad:
Ugly:
Phage2D had its good moments, and even helped me win the “best game” award with FlipFlop at CodeDay NY 2014 (credits to Nick Hartunian for level design). Between my brother and I, the project totaled at over 40,000 lines (fleshed out with physics, multiplayer, and even behavior trees) and still remains one of my biggest accomplishments. In the end, though, I decided to put it down and start developing Big Phage. But that’s a story for another time…
The post Game Engine Design Study #1 – Phage2D appeared first on Sam Pfrommer.
]]>