A humanoid robot can be described by a kinematic tree in a URDF file. I found that the order of the elements in the generalized coordinate vectors of the rigid body tree and the multi-body plant are different. There is a code snippet online showing the way to achieve the mapping.
# q_rbt = X*q_mbp
# rigid_body_tree is an instance of RigidBodyTree()
# multi_body_plant is an instance of MultibodyPlant()
B_rbt = rigid_body_tree.B
B_mbp = multi_body_plant.MakeActuationMatrix()
X = np.dot(B_rbt[6:,:],B_mbp[6:,:].T)
However, RigidBodyTree has been deprecated in the new Drake. Hence, how can I achieve the mapping now? In addition, I am curious about why Drake does not use the same order for the generalized coordinate vectors.
You might like the workflow I used in this littledog example. I have a PR in flight to enable that workflow directly in Drake. You can track the progress on in with this issue.
In general the recommendation, and I would recommend this even if we hadn’t implemented it a particular way in Drake, is that if you have something as complicated as humanoid, don’t try to work with the vectors based on indices. Find a way to access the elements via their names, or in Drake via their joint accessors. Working with the raw vector is very error prone.
I'm trying to add generalized forces on a free-floating system that has been linearized from a MultiBodyPlant. For a fixed base, the generalized forces basically correspond to the joint torques but for a free-floating case, it includes the base torques/forces as well. However, these are not included in the actuation port of the MultiBodyPlant system and hence while providing the actuation port for linearization (or LQR), the base wrenches are not included in the actuation matrix B post-linearization. For my work, I'd like to include the base torques/forces as well in the linearization. For this, I can think of 2 approaches in drake but I'm not sure which would be a better/correct decision architecturally. I plan to use this later for making a TVLQR controller to stabilize trajectories.
Idea 1: Change B matrix in LQR after linearization: Manually edit B matrix after using LinearQuadraticRegulator(system, context, Q, R, plant_actuation_input_pot())
Idea 2: Create a LeafeSystem class (similar to this) with output port connecting to get_applied_generalized_force_input_port() and input port exposed outside diagram (using ExportOutput()) and then linearized this system as a whole (containing the CustomLeafSystem connected to MultiBodyPlant). I assume doing this will add the base actuation to the B matrix of the linearized system.
Actually these two ideas is what I would've done myself. For your case it'd seem that Idea 1 is simpler to implement?
Matrix B maps actuation indexes to generalized velocity indexes. Therefore you have to make sure that you place the necessary "ones" in the right places of B. You can find the free floating body indexes with Body::floating_velocities_start() which returns the start index within the full state x (therefore you'd need to subtract MultibodyPlant::num_positions() for indexing within B). For floating bodies generalized velocities are angular velocity w_WB and translational velocity v_WB, in that order. Therefore the generalized forces corresponds to the torque t_Bo_W and f_Bo_W applied at B about its origin Bo and expressed in the world frame.
I have a LeafSystem in drake, with dynamics \dot{x} = f(x,u) written in DoCalcTimeDerivatives. The fixed points and trim points of this system are not trivial to find. Therefore, I image one would need to write a nonlinear optimization problem to find the fixed points:
find x, u;
s.t. f(x,u)=0
or
find x,u;
min f(x,u)^2
I am wondering, how should I take advantage of the dynamics that I have already written in DoCalcTimeDerivatives of the LeafSystem, and write a non-linear optimization to search over x and u to find the fixed points and trim points in drake? Some existing examples in drake would be greatly appreciated!
It's simple to write for your case (and only slightly harder to write for the general case... it's on my TODO list).
Assuming your plant supports symbolic, then looking at the trajectory optimization will give you a sense for how you might write the constraint:
https://github.com/RobotLocomotion/drake/blob/master/systems/trajectory_optimization/direct_transcription.cc#L212
(the autodiff version is just below):
fwiw, the general case from the old matlab version is here:
https://github.com/RobotLocomotion/drake/blob/last_sha_with_original_matlab/drake/matlab/solvers/FixedPointProgram.m
I know the basics of Reinforcement Learning, but what terms it's necessary to understand to be able read arxiv PPO paper ?
What is the roadmap to learn and use PPO ?
To better understand PPO, it is helpful to look at the main contributions of the paper, which are: (1) the Clipped Surrogate Objective and (2) the use of "multiple epochs of stochastic gradient ascent to perform each policy update".
From the original PPO paper:
We have introduced [PPO], a family of policy optimization methods that use multiple epochs of stochastic gradient ascent to perform each policy update. These methods have the stability and reliability of trust-region [TRPO] methods but are much simpler to implement, requiring only a few lines of code change to a vanilla policy gradient implementation, applicable in more general settings (for example, when using a joint architecture for the policy and value function), and have better overall performance.
1. The Clipped Surrogate Objective
The Clipped Surrogate Objective is a drop-in replacement for the policy gradient objective that is designed to improve training stability by limiting the change you make to your policy at each step.
For vanilla policy gradients (e.g., REINFORCE) --- which you should be familiar with, or familiarize yourself with before you read this --- the objective used to optimize the neural network looks like:
This is the standard formula that you would see in the Sutton book, and other resources, where the A-hat could be the discounted return (as in REINFORCE) or the advantage function (as in GAE) for example. By taking a gradient ascent step on this loss with respect to the network parameters, you will incentivize the actions that led to higher reward.
The vanilla policy gradient method uses the log probability of your action (log π(a | s)) to trace the impact of the actions, but you could imagine using another function to do this. Another such function, introduced in this paper, uses the probability of the action under the current policy (π(a|s)), divided by the probability of the action under your previous policy (π_old(a|s)). This looks a bit similar to importance sampling if you are familiar with that:
This r(θ) will be greater than 1 when the action is more probable for your current policy than it is for your old policy; it will be between 0 and 1 when the action is less probable for your current policy than for your old.
Now to build an objective function with this r(θ), we can simply swap it in for the log π(a|s) term. This is what is done in TRPO:
But what would happen here if your action is much more probable (like 100x more) for your current policy? r(θ) will tend to be really big and lead to taking big gradient steps that might wreck your policy. To deal with this and other issues, TRPO adds several extra bells and whistles (e.g., KL Divergence constraints) to limit the amount the policy can change and help guarantee that it is monotonically improving.
Instead of adding all these extra bells and whistles, what if we could build these stabilizing properties into the objective function? As you might guess, this is what PPO does. It gains the same performance benefits as TRPO and avoids the complications by optimizing this simple (but kind of funny looking) Clipped Surrogate Objective:
The first term (blue) inside the minimization is the same (r(θ)A) term we saw in the TRPO objective. The second term (red) is a version where the (r(θ)) is clipped between (1 - e, 1 + e). (in the paper they state a good value for e is about 0.2, so r can vary between ~(0.8, 1.2)). Then, finally, the minimization of both of these terms is taken (green).
Take your time and look at the equation carefully and make sure you know what all the symbols mean, and mathematically what is happening. Looking at the code may also help; here is the relevant section in both the OpenAI baselines and anyrl-py implementations.
Great.
Next, let's see what effect the L clip function creates. Here is a diagram from the paper that plots the value of the clip objective for when the Advantage is positive and negative:
On the left half of the diagram, where (A > 0), this is where the action had an estimated positive effect on the outcome. On the right half of the diagram, where (A < 0), this is where the action had an estimated negative effect on the outcome.
Notice how on the left half, the r-value gets clipped if it gets too high. This will happen if the action became a lot more probable under the current policy than it was for the old policy. When this happens, we do not want to get greedy and step too far (because this is just a local approximation and sample of our policy, so it will not be accurate if we step too far), and so we clip the objective to prevent it from growing. (This will have the effect in the backward pass of blocking the gradient --- the flat line causing the gradient to be 0).
On the right side of the diagram, where the action had an estimated negative effect on the outcome, we see that the clip activates near 0, where the action under the current policy is unlikely. This clipping region will similarly prevent us from updating too much to make the action much less probable after we already just took a big step to make it less probable.
So we see that both of these clipping regions prevent us from getting too greedy and trying to update too much at once and leaving the region where this sample offers a good estimate.
But why are we letting the r(θ) grow indefinitely on the far right side of the diagram? This seems odd as first, but what would cause r(θ) to grow really large in this case? r(θ) growth in this region will be caused by a gradient step that made our action a lot more probable, and it turning out to make our policy worse. If that was the case, we would want to be able to undo that gradient step. And it just so happens that the L clip function allows this. The function is negative here, so the gradient will tell us to walk the other direction and make the action less probable by an amount proportional to how much we screwed it up. (Note that there is a similar region on the far left side of the diagram, where the action is good and we accidentally made it less probable.)
These "undo" regions explain why we must include the weird minimization term in the objective function. They correspond to the unclipped r(θ)A having a lower value than the clipped version and getting returned by the minimization. This is because they were steps in the wrong direction (e.g., the action was good but we accidentally made it less probable). If we had not included the min in the objective function, these regions would be flat (gradient = 0) and we would be prevented from fixing mistakes.
Here is a diagram summarizing this:
And that is the gist of it. The Clipped Surrogate Objective is just a drop-in replacement you could use in the vanilla policy gradient. The clipping limits the effective change you can make at each step in order to improve stability, and the minimization allows us to fix our mistakes in case we screwed it up. One thing I didn't discuss is what is meant by PPO objective forming a "lower bound" as discussed in the paper. For more on that, I would suggest this part of a lecture the author gave.
2. Multiple epochs for policy updating
Unlike vanilla policy gradient methods, and because of the Clipped Surrogate Objective function, PPO allows you to run multiple epochs of gradient ascent on your samples without causing destructively large policy updates. This allows you to squeeze more out of your data and reduce sample inefficiency.
PPO runs the policy using N parallel actors each collecting data, and then it samples mini-batches of this data to train for K epochs using the Clipped Surrogate Objective function. See full algorithm below (the approximate param values are: K = 3-15, M = 64-4096, T (horizon) = 128-2048):
The parallel actors part was popularized by the A3C paper and has become a fairly standard way for collecting data.
The newish part is that they are able to run K epochs of gradient ascent on the trajectory samples. As they state in the paper, it would be nice to run the vanilla policy gradient optimization for multiple passes over the data so that you could learn more from each sample. However, this generally fails in practice for vanilla methods because they take too big of steps on the local samples and this wrecks the policy. PPO, on the other hand, has the built-in mechanism to prevent too much of an update.
For each iteration, after sampling the environment with π_old (line 3) and when we start running the optimization (line 6), our policy π will be exactly equal to π_old. So at first, none of our updates will be clipped and we are guaranteed to learn something from these examples. However, as we update π using multiple epochs, the objective will start hitting the clipping limits, the gradient will go to 0 for those samples, and the training will gradually stop...until we move on to the next iteration and collect new samples.
....
And that's all for now. If you are interested in gaining a better understanding, I would recommend digging more into the original paper, trying to implement it yourself, or diving into the baselines implementation and playing with the code.
[edit: 2019/01/27]: For a better background and for how PPO relates to other RL algorithms, I would also strongly recommend checking out OpenAI's Spinning Up resources and implementations.
PPO, and including TRPO tries to update the policy conservatively, without affecting its performance adversely between each policy update.
To do this, you need a way to measure how much the policy has changed after each update. This measurement is done by looking at the KL divergence between the updated policy and the old policy.
This becomes a constrained optimization problem, we want change the policy in the direction of maximum performance, following the constraints that the KL divergence between my new policy and old do not exceed some pre defined (or adaptive) threshold.
With TRPO, we compute the KL constraint during update and finds the learning rate for this problem (via Fisher Matrix and conjugate gradient). This is somewhat messy to implement.
With PPO, we simplify the problem by turning the KL divergence from a constraint to a penalty term, similar to for example to L1, L2 weight penalty (to prevent a weights from growing large values). PPO makes additional modifications by removing the need to compute KL divergence all together, by hard clipping the policy ratio (ratio of updated policy with old) to be within a small range around 1.0, where 1.0 means the new policy is same as old.
PPO is a simple algorithm, which falls into policy optimization algorithms class (as opposed to value-based methods such as DQN). If you "know" RL basics (I mean if you have at least read thoughtfully some first chapters of Sutton's book for example), then a first logical step is to get familiar with policy gradient algorithms. You can read this paper or chapter 13 of Sutton's book new edition. Additionally, you may also read this paper on TRPO, which is a previous work from PPO's first author (this paper has numerous notational mistakes; just note). Hope that helps. --Mehdi
I think implementing for a discrete action space such as Cartpole-v1 is easier than for continuous action spaces. But for continuous action spaces, this is the most straight-forward implementation I found in Pytorch as you can clearly see how they get mu and std where as I could not with more renowned implementations such as Openai Baselines and Spinning up or Stable Baselines.
RL-Adventure PPO
These lines from the link above:
class ActorCritic(nn.Module):
def __init__(self, num_inputs, num_outputs, hidden_size, std=0.0):
super(ActorCritic, self).__init__()
self.critic = nn.Sequential(
nn.Linear(num_inputs, hidden_size),
nn.ReLU(),
nn.Linear(hidden_size, 1)
)
self.actor = nn.Sequential(
nn.Linear(num_inputs, hidden_size),
nn.ReLU(),
nn.Linear(hidden_size, num_outputs),
)
self.log_std = nn.Parameter(torch.ones(1, num_outputs) * std)
self.apply(init_weights)
def forward(self, x):
value = self.critic(x)
mu = self.actor(x)
std = self.log_std.exp().expand_as(mu)
dist = Normal(mu, std)
return dist, value
and the clipping:
def ppo_update(ppo_epochs, mini_batch_size, states, actions, log_probs, returns, advantages, clip_param=0.2):
for _ in range(ppo_epochs):
for state, action, old_log_probs, return_, advantage in ppo_iter(mini_batch_size, states, actions, log_probs, returns, advantages):
dist, value = model(state)
entropy = dist.entropy().mean()
new_log_probs = dist.log_prob(action)
ratio = (new_log_probs - old_log_probs).exp()
surr1 = ratio * advantage
surr2 = torch.clamp(ratio, 1.0 - clip_param, 1.0 + clip_param) * advantage
I found the link above the comments on this video on Youtube:
arxiv insights PPO
Specifically, I am talking about programming for this contest: http://www.nodewar.com/about
The contest involves you facing other teams with a swarm of spaceships in a two dimensional world. The ships are constrained by a boundary (if they exit they die), and they must continually avoid moons (who pull the ships in with their gravity). The goal is to kill the opposing queen.
I have attempted to program a few, relatively basic techniques, but I feel as though I am missing something fundamental.
For example, I implemented some of the boids behaviours (http://www.red3d.com/cwr/boids/), but they seemed to lack a... goal, so to speak.
Are there any common techniques (or, preferably, combinations of techniques) for this sort of game?
EDIT
I would just like to open this up again with a bounty since I feel like I'm still missing critical pieces of information. The following is my NodeWar code:
boundary_field = (o, position) ->
distance = (o.game.moon_field - o.lib.vec.len(o.lib.vec.diff(position, o.game.center)))
return distance
moon_field = (o, position) ->
return o.lib.vec.len(o.lib.vec.diff(position, o.moons[0].pos))
ai.step = (o) ->
torque = 0;
thrust = 0;
label = null;
fields = [boundary_field, moon_field]
# Total the potential fields and determine a target.
target = [0, 0]
score = -1000000
step = 1
square = 1
for x in [(-square + o.me.pos[0])..(square + o.me.pos[0])] by step
for y in [(-square + o.me.pos[1])..(square + o.me.pos[1])] by step
position = [x, y]
continue if o.lib.vec.len(position) > o.game.moon_field
value = (fields.map (f) -> f(o, position)).reduce (t, s) -> t + s
target = position if value > score
score = value if value > score
label = target
{ torque, thrust } = o.lib.targeting.simpleTarget(o.me, target)
return { torque, thrust, label }
I may have implemented potential fields incorrectly, however, since all the examples I could find are about discrete movements (whereas NodeWar is continuous and not exact).
The main problem being my A.I. never stays within the game area for more than 10 seconds without flying off-screen or crashing into a moon.
You can easily make the boids algorithm play the game of Nodewar, by just adding additional steering behaviours to your boids and/or modifying the default ones. For example, you would add a steering behaviour to avoid the moon, and a steering behaviour for enemy ships (repulsion or attraction, depending on the position between yours and the enemy ship). The weights of the attraction/repulsion forces should then be tweaked (possibly by genetic algorithms, playing different configurations against each other).
I believe this approach would give you already a relatively strong baseline player, to which you can start adding collaborative strategies etc.
You can achieve the best possible flocking behaviour using control theory. We can start by expressing the state of a ship (a vector of positions and velocities) as variable X. We will use x to represent a small deviation from some state X0. For each node, linearize around the state you want the individual ships to be at:
d/dt(X) = f(X - X0)
where X0 is the state you want the ship to be at. f() can be nonlinear (as in the case of your potential field). Close to this point, the will obey
d/dt(x) = Ax
A is a fixed matrix which you should tweak to achieve stability. Much of the field of control theory tells you how to do this. It's very important to you that A leads to a stable system and that it converges fast. You should now break out MATLAB or GNU Octave. You should also read up on what poles mean in control theory, and Laplace transforms. The poles of the system (eigenvalues of matrix A) will characterize the response of the ship to disturbances of any kind, its stability, and its convergeance speed. It will also tell you whether the ship will move towards X0 or oscillate around X0.
There are several ways of choosing A (You probably don't get full control over A because of the game's limited physics) without having to do much analysis yourself. Two such algorithms are optimal control (You define the cost of controlling the system vs the cost of deviating from X0), and pole placement (You choose where the poles will go).
The state space theory also applies to an entire flock of ships, deviating from the configuration desired by every ship at that point in time. If the system is nonlinear, You might not be able to guarantee that all configurations are stable.
Conclusion:
Use control theory to guarantee individual stability, calculating X0 based on the ships around you, and add a potential field to prevent ships from bumping into eachother and moons.
Disclaimer:
There are several ways of expressing a state space system. This is the simplest but you'll need to express it differently when considering the open loop system (split up A into another 'A' which gives you the physical system and some other matrices which give you the control parameters)