There are AddPositionConstraint for PositionConstraint in the InverseKinematics class. Is there similar API for ComPositionConstraint? Thanks!
No we don't have AddComPositionConstraint yet. You can instantiate a ComPositionConstraint, and then add it to InverseKinematics. One example is as follows
InverseKinematics ik(plant, plant_context)
com_position_constraint = ComPositionConstraint(plant, None, plant.world_frame(), plant_context)
# r is the variable for CoM position.
r = ik.prog().NewContinuousVariables(3)
ik.prog().AddConstraint(com_position_constraint, np.concatenate([ik.q(), r]))
# Set the desired Com position as r_des
r_des = np.array([0, 0, 1])
ik.prog().AddBoundingBoxConstraint(r_des, r_des, r)
# Set the initial guess for IK program.
q_init = ...
r_init = ...
ik.prog().SetInitialGuess(ik.q(), q_init)
ik.prog().SetInitialGuess(r, r_init)
result = Solve(ik.prog(), initial_guess)
Related
I'm trying to find a sensible way to move an Mobject along a path defined by the n-length vectors ts,xs,ys,(zs).
The solution I have now is by using ParametricFunction and MoveAlongPath. I can then define a rate function to make sure the timing adds up. This is extremely backwards and not quite reliable in my experience.
I feel like I'm probably missing some builtin function but I can't find it.
# This function takes a path defined by arrays and returns a function
# ts is assumed to be strictly increasing
def manim_curve(ts,xs,ys):
ts,xs,ys = map(np.array,(ts,xs,ys))
# Calculate the total distance traveled over the curve
dist = np.cumsum(np.abs(np.diff(xs+1j*ys,prepend=0)))
# Normalize to a time range of [0,1]
nts = ts / ts[-1]
ndist = dist / dist[-1]
# Create a function that can be passed `ParametricFunction`
def f(t):
n = np.abs(nts-t).argmin() # Find index from t
return (xs[n],ys[n],0)
# Create a rate function for `MoveAlongPath`
def rate(t):
n = np.abs(nts-t).argmin() # Find index from t
return ndist[n]
# Create manim curve
curve = ParametricFunction(function=f)
return curve,rate
# Animation class to move along a discretely defined path
class MoveAlongMeasuredPath(MoveAlongPath):
def __init__(self,object,ts,xs,ys,**kwargs):
ts,xs,ys = map(np.array,(ts,xs,ys))
curve,rate = manim_curve(ts,xs,ys)
super().__init__(object,curve,
run_time = ts[-1],
rate_func = rate,
**kwargs)
The best way to move an Mobject along a path defined by the n-length vectors ts,xs,ys,(zs) is to use the ParametricFunction and MoveAlongPath functions. This will allow you to define a path using the vectors and then move the Mobject along that path. It is a reliable and straightforward way to achieve this.
I dug a little deeper in the code and realized there is a simple solution. The class below is only a slight alteration of the MoveAlongPath class source code:
class MoveAlongTXYZPath(Animation):
def __init__(
self,
mobject: Mobject,
ts:NDArray,
points:NDArray,
is_sorted:bool=False,
suspend_mobject_updating: bool = False,
**kwargs,
) -> None:
assert np.all(ts>=0), "no negative t_values allowed"
assert len(ts)==len(points), "vectors have to be the same length"
# Sort if unsorted in t
if not is_sorted:
ts,points = map(np.array,zip(*sorted([*zip(ts,points)])))
self.points = points
run_time = np.max(ts)
self.alphas = ts/run_time
super().__init__( mobject,
suspend_mobject_updating=suspend_mobject_updating,
run_time=run_time,
rate_func=linear,
**kwargs)
def interpolate_mobject(self, alpha: float) -> None:
index = np.searchsorted(self.alphas,alpha)
point = self.points[index]
self.mobject.move_to(point)
I am trying to use reinforcement learning in julia to teach a car that is constantly being accelerated backwards (but with a positive initial velocity) to apply brakes so that it gets as close to a target distance as possible before moving backwards.
To do this, I am making use of POMDPs.jl and crux.jl which has many solvers (I'm using DQN). I will list what I believe to be the relevant parts of the script first, and then more of it towards the end.
To define the MDP, I set the initial position, velocity, and force from the brakes as a uniform distribution over some values.
#with_kw struct SliderMDP <: MDP{Array{Float32}, Array{Float32}}
x0 = Distributions.Uniform(0., 80.)# Distribution to sample initial position
v0 = Distributions.Uniform(0., 25.) # Distribution to sample initial velocity
d0 = Distributions.Uniform(0., 2.) # Distribution to sample brake force
...
end
My state holds the values of (position, velocity, brake force), and the initial state is given as:
function POMDPs.initialstate(mdp::SliderMDP)
ImplicitDistribution((rng) -> Float32.([rand(rng, mdp.x0), rand(rng, mdp.v0), rand(rng, mdp.d0)]))
end
Then, I set up my DQN solver using crux.jl and called a function to solve for the policy
solver_dqn = DQN(π=Q_network(), S=s, N=30000)
policy_dqn = solve(solver_dqn, mdp)
calling solve() gives me the error MethodError: no method matching logpdf(::Distributions.Categorical{Float64, Vector{Float64}}, ::Nothing). I am quite sure that this comes from the initial state sampling, but I am not sure why or how to fix it. I have only been learning RL from various books and online lectures for a very short time, so any help regarding the error or my the model I set up (or anything else I'm oblivious to) would be appreciated.
More comprehensive code:
Packages:
using POMDPs
using POMDPModelTools
using POMDPPolicies
using POMDPSimulators
using Parameters
using Random
using Crux
using Flux
using Distributions
Rest of it:
#with_kw struct SliderMDP <: MDP{Array{Float32}, Array{Float32}}
x0 = Distributions.Uniform(0., 80.)# Distribution to sample initial position
v0 = Distributions.Uniform(0., 25.) # Distribution to sample initial velocity
d0 = Distributions.Uniform(0., 2.) # Distribution to sample brake force
m::Float64 = 1.
tension::Float64 = 3.
dmax::Float64 = 2.
target::Float64 = 80.
dt::Float64 = .05
γ::Float32 = 1.
actions::Vector{Float64} = [-.1, 0., .1]
end
function POMDPs.gen(env::SliderMDP, s, a, rng::AbstractRNG = Random.GLOBAL_RNG)
x, ẋ, d = s
if x >= env.target
a = .1
end
if d+a >= env.dmax || d+a <= 0
a = 0.
end
force = (d + env.tension) * -1
ẍ = force/env.m
# Simulation
x_ = x + env.dt * ẋ
ẋ_ = ẋ + env.dt * ẍ
d_ = d + a
sp = vcat(x_, ẋ_, d_)
reward = abs(env.target - x) * -1
return (sp=sp, r=reward)
end
function POMDPs.initialstate(mdp::SliderMDP)
ImplicitDistribution((rng) -> Float32.([rand(rng, mdp.x0), rand(rng, mdp.v0), rand(rng, mdp.d0)]))
end
POMDPs.isterminal(mdp::SliderMDP, s) = s[2] <= 0
POMDPs.discount(mdp::SliderMDP) = mdp.γ
mdp = SliderMDP();
s = state_space(mdp); # Using Crux.jl
function Q_network()
layer1 = Dense(3, 64, relu)
layer2 = Dense(64, 64, relu)
layer3 = Dense(64, length(3))
return DiscreteNetwork(Chain(layer1, layer2, layer3), [-.1, 0, .1])
end
solver_dqn = DQN(π=Q_network(), S=s, N=30000) # Using Crux.jl
policy_dqn = solve(solver_dqn, mdp) # Error comes here
Stacktrace:
policy_dqn
MethodError: no method matching logpdf(::Distributions.Categorical{Float64, Vector{Float64}}, ::Nothing)
Closest candidates are:
logpdf(::Distributions.DiscreteNonParametric, !Matched::Real) at C:\Users\name\.julia\packages\Distributions\Xrm9e\src\univariate\discrete\discretenonparametric.jl:106
logpdf(::Distributions.UnivariateDistribution{S} where S<:Distributions.ValueSupport, !Matched::AbstractArray) at deprecated.jl:70
logpdf(!Matched::POMDPPolicies.PlaybackPolicy, ::Any) at C:\Users\name\.julia\packages\POMDPPolicies\wMOK3\src\playback.jl:34
...
logpdf(::Crux.ObjectCategorical, ::Float32)#utils.jl:16
logpdf(::Crux.DistributionPolicy, ::Vector{Float64}, ::Float32)#policies.jl:305
var"#exploration#133"(::Base.Iterators.Pairs{Union{}, Union{}, Tuple{}, NamedTuple{(), Tuple{}}}, ::typeof(Crux.exploration), ::Crux.DistributionPolicy, ::Vector{Float64})#policies.jl:302
exploration#policies.jl:297[inlined]
action(::Crux.DistributionPolicy, ::Vector{Float64})#policies.jl:294
var"#exploration#136"(::Crux.DiscreteNetwork, ::Int64, ::typeof(Crux.exploration), ::Crux.MixedPolicy, ::Vector{Float64})#policies.jl:326
var"#step!#173"(::Bool, ::Int64, ::typeof(Crux.step!), ::Dict{Symbol, Array}, ::Int64, ::Crux.Sampler{Main.workspace#2.SliderMDP, Vector{Float32}, Crux.DiscreteNetwork, Crux.ContinuousSpace{Tuple{Int64}}, Crux.DiscreteSpace})#sampler.jl:55
var"#steps!#174"(::Int64, ::Bool, ::Int64, ::Bool, ::Bool, ::Bool, ::typeof(Crux.steps!), ::Crux.Sampler{Main.workspace#2.SliderMDP, Vector{Float32}, Crux.DiscreteNetwork, Crux.ContinuousSpace{Tuple{Int64}}, Crux.DiscreteSpace})#sampler.jl:108
var"#fillto!#177"(::Int64, ::Bool, ::typeof(Crux.fillto!), ::Crux.ExperienceBuffer{Array}, ::Crux.Sampler{Main.workspace#2.SliderMDP, Vector{Float32}, Crux.DiscreteNetwork, Crux.ContinuousSpace{Tuple{Int64}}, Crux.DiscreteSpace}, ::Int64)#sampler.jl:156
solve(::Crux.OffPolicySolver, ::Main.workspace#2.SliderMDP)#off_policy.jl:86
top-level scope#Local: 1[inlined]
Short answer:
Change your output vector to Float32 i.e. Float32[-.1, 0, .1].
Long answer:
Crux creates a Distribution over your network's output values, and at some point (policies.jl:298) samples a random value from it. It then converts this value to a Float32. Later (utils.jl:15) it does a findfirst to find the index of this value in the original output array (stored as objs within the distribution), but because the original array is still Float64, this fails and returns a nothing. Hence the error.
I believe this (converting the sampled value but not the objs array and/or not using approximate equality check i.e. findfirst(isapprox(x), d.objs)) to be a bug in the package, and would encourage you to raise this as an issue on Github.
I encounter the error message when doing DirectCollocation with collision geometry:
RuntimeError: InputPort::Eval(): required InputPort[0] (geometry_query) of System ::drake/multibody/MultibodyPlant#0000000003550740 (MultibodyPlant<drake::AutoDiffXd>) is not connected
Based on the previous question, I have added both connections as follows, but still not fix the bug.
builder.Connect(plant.get_geometry_poses_output_port(),
scene_graph.get_source_pose_port(plant.get_source_id()))
builder.Connect(scene_graph.get_query_output_port(),
plant.get_geometry_query_input_port())
This is my code skeleton:
builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
# Make and add the model
tmp = make_acrobot_plant(params, finalize, scene_graph)
plant = builder.AddSystem(tmp)
plant.Finalize()
# https://drake.mit.edu/doxygen_cxx/classdrake_1_1multibody_1_1_multibody_plant.html
builder.Connect(plant.get_geometry_poses_output_port(),
scene_graph.get_source_pose_port(plant.get_source_id()))
builder.Connect(scene_graph.get_query_output_port(),
plant.get_geometry_query_input_port())
num_time_samples = 40
dircol = DirectCollocation(
plant,
context,
num_time_samples=num_time_samples,
minimum_timestep=0.01,
maximum_timestep=0.05,
input_port_index=plant.get_actuation_input_port().get_index())
dircol.AddEqualTimeIntervalsConstraints()
u = dircol.input()
dircol.AddRunningCost(Ru * u.dot(u))
dircol.SetInitialTrajectory(traj_init_u, traj_init_x)
result = Solve(dircol)
assert result.is_success()
# Connect
# Simulate u
controls = builder.AddSystem(TrajectorySource(u_trajectory))
builder.Connect(
controls.get_output_port(0), plant.get_actuation_input_port())
'''
# Simulate x
source = builder.AddSystem(TrajectorySource(x_trajectory))
pos_to_pose = builder.AddSystem(
MultibodyPositionToGeometryPose(plant, input_multibody_state=True))
builder.Connect(source.get_output_port(0), pos_to_pose.get_input_port())
builder.Connect(pos_to_pose.get_output_port(),
scene_graph.get_source_pose_port(plant.get_source_id()))
'''
# ...
# Visualization:
builder.Connect(scene_graph.get_pose_bundle_output_port(),
planar_scene_graph_vis.get_input_port(0))
diagram = builder.Build()
You are close, but you have only passed the plant into DirectCollocation. You need to pass the diagram with plant+scenegraph into DirectCollocation. (And you will also need to assume_non_continuous_states_are_fixed=true to ignore the SceneGraph's abstract state).
Here is a part of my tensorflow RNN network code written in jupyter. The whole code runs perfect for the first time, however, running it furthermore produces an error. The code:
x = tf.placeholder(tf.float32)
y = tf.placeholder(tf.float32)
def recurrent_nn_model(x):
x = tf.transpose(x, [1,0,2])
x = tf.reshape(x, [-1, chunk_size])
x = tf.split(x, n_chunks, 0)
lstm_layer = {'hidden_state': tf.zeros([n_batches, lstm_size]),
'current_state': tf.zeros([n_batches, lstm_size])}
layer = {'weights': tf.Variable(tf.random_normal([lstm_size, n_classes])),
'biases': tf.Variable(tf.random_normal([n_classes]))}
lstm = rnn_cell.BasicLSTMCell(lstm_size)
rnn_outputs, rnn_states = rnn.static_rnn(lstm, x, dtype=tf.float32)
output = tf.add(tf.matmul(rnn_outputs[-1], layer['weights']),
layer['biases'])
return output
The error is:
Variable rnn/basic_lstm_cell/kernel already exists, disallowed. Did
you mean to set reuse=True in VarScope? Originally defined at:
If recurrent_nn_model is the whole network, just add this line to reset previously defined graph:
tf.reset_default_graph()
If you're intentionally calling recurrent_nn_model several times and combine these RNNs into one graph, you should use different variable scopes for each one:
with tf.variable_scope('lstm1'):
recurrent_nn_model(x1)
with tf.variable_scope('lstm2'):
recurrent_nn_model(x2)
I am using statsmodels.tsa.arima_model.ARIMA, and I took the square root transform of the endogenous variable before plugging it into the algorithm. The model uses a differencing order of 1:
model = ARIMA(sj_sqrt, order=(2, 1, 0))
After fitting the model and grabbing the predictions, I want to put the predictions back in the original form for comparison with the original data. However, I can't seem to transform them back correctly.
To replicate a simple version of this problem, here is some code:
#original data:
test = pd.Series([1,1,1,50,1,1,1,1,1,1,1,1,40,1,1,2,1,1,1,1,1])
#sqrt transformed data:
test_sqrt = np.sqrt(test)
#sqrt differenced data:
test_sqrt_diff = test_sqrt.diff(periods=1)
#undo differencing:
test_sqrt_2 = cumsum(test_sqrt_diff)
#undo transformations:
test_2 = test_sqrt_2 ** 2
f, axarr = plt.subplots(5, sharex=True, sharey=True)
axarr[0].set_title('original data:')
axarr[0].plot(test)
axarr[1].set_title('sqrt transformed data:')
axarr[1].plot(test_sqrt)
axarr[2].set_title('sqrt differenced data:')
axarr[2].plot(test_sqrt_diff)
axarr[3].set_title('differencing undone with .cumsum():')
axarr[3].plot(test_sqrt_2)
axarr[4].set_title('transformation undone by squaring:')
axarr[4].plot(test_2)
f.set_size_inches(5, 12)
You can see from the graphs that the undifferenced, untransformed data is not quite on the same scale. test[3] returns 50, and test_2[3] returns 36.857864376269056
Solution:
## original
x = np.array([1,1,1,50,1,1,1,1,1,1,1,1,40,1,1,2,1,1,1,1,1])
## sqrt
x_sq = np.sqrt(x)
## diff
d_sq = np.diff(x_sq,n=1)
## Only works when d = 1
def diffinv(d,i):
inv = np.insert(d,0,i)
inv = np.cumsum(inv)
return inv
## inv diff
y_sq = diffinv(d_sq,x_sq[0])
## Check inv diff
(y_sq==x_sq).all()