I'm a student teaching myself Drake, specifically pydrake with Dr. Russ Tedrake's excellent Underactuated Robotics course. I am trying to write a combined energy shaping and lqr controller for keeping a cartpole system balanced upright. I based the diagram on the cartpole example found in Chapter 3 of Underactuated Robotics [http://underactuated.mit.edu/acrobot.html], and the SwingUpAndBalanceController on Chapter 2: [http://underactuated.mit.edu/pend.html].
I have found that due to my use of the cart_pole.sdf model I have to create an abstract input port due receive FramePoseVector from the cart_pole.get_output_port(0). From there I know that I have to create a control signal output of type BasicVector to feed into a Saturation block before feeding into the cartpole's actuation port.
The problem I'm encountering right now is that I'm not sure how to get the system's current state data in the DeclareVectorOutputPort's callback function. I was under the assumption I would use the LeafContext parameter in the callback function, OutputControlSignal, obtaining the BasicVector continuous state vector. However, this resulting vector, x_bar is always NaN. Out of desperation (and testing to make sure the rest of my program worked) I set x_bar to the controller's initialization cart_pole_context and have found that the simulation runs with a control signal of 0.0 (as expected). I can also set output to 100 and the cartpole simulation just flies off into endless space (as expected).
TL;DR: What is the proper way to obtain the continuous state vector in a custom controller extending LeafSystem with a DeclareVectorOutputPort?
Thank you for any help! I really appreciate it :) I've been teaching myself so it's been a little arduous haha.
# Combined Energy Shaping (SwingUp) and LQR (Balance) Controller
# with a simple state machine
class SwingUpAndBalanceController(LeafSystem):
def __init__(self, cart_pole, cart_pole_context, input_i, ouput_i, Q, R, x_star):
LeafSystem.__init__(self)
self.DeclareAbstractInputPort("state_input", AbstractValue.Make(FramePoseVector()))
self.DeclareVectorOutputPort("control_signal", BasicVector(1),
self.OutputControlSignal)
(self.K, self.S) = BalancingLQRCtrlr(cart_pole, cart_pole_context,
input_i, ouput_i, Q, R, x_star).get_LQR_matrices()
(self.A, self.B, self.C, self.D) = BalancingLQRCtrlr(cart_pole, cart_pole_context,
input_i, ouput_i,
Q, R, x_star).get_lin_matrices()
self.energy_shaping = EnergyShapingCtrlr(cart_pole, x_star)
self.energy_shaping_context = self.energy_shaping.CreateDefaultContext()
self.cart_pole_context = cart_pole_context
def OutputControlSignal(self, context, output):
#xbar = copy(self.cart_pole_context.get_continuous_state_vector())
xbar = copy(context.get_continuous_state_vector())
xbar_ = np.array([xbar[0], xbar[1], xbar[2], xbar[3]])
xbar_[1] = wrap_to(xbar_[1], 0, 2.0*np.pi) - np.pi
# If x'Sx <= 2, then use LQR ctrlr. Cost-to-go J_star = x^T * S * x
threshold = np.array([2.0])
if (xbar_.dot(self.S.dot(xbar_)) < 2.0):
#output[:] = -self.K.dot(xbar_) # u = -Kx
output.set_value(-self.K.dot(xbar_))
else:
self.energy_shaping.get_input_port(0).FixValue(self.energy_shaping_context,
self.cart_pole_context.get_continuous_state_vector())
output_val = self.energy_shaping.get_output_port(0).Eval(self.energy_shaping_context)
output.set_value(output_val)
print(output)
Here are two things that might help:
If you want to get the state of the cart-pole from MultibodyPlant, you probably want to be connecting to the continuous_state output port, which gives you a normal vector instead of the abstract-type FramePoseVector. In that case, your call to get_input_port().Eval(context) should work just fine.
If you do really want to read the FramePoseVector, then you have to evaluate the input port slightly differently. You can find an example of that here.
I have a dataframe which is of length 177 and I want to calculate and plot the partial auto-correlation function (PACF).
I have the data imported etc and I do:
from statsmodels.tsa.stattools import pacf
ys = pacf(data[key][array].diff(1).dropna(), alpha=0.05, nlags=176, method="ywunbiased")
xs = range(lags+1)
plt.figure()
plt.scatter(xs,ys[0])
plt.grid()
plt.vlines(xs, 0, ys[0])
plt.plot(ys[1])
The method used results in numbers greater than 1 for very long lags (90ish) which is incorrect and I get a RuntimeWarning: invalid value encountered in sqrtreturn rho, np.sqrt(sigmasq) but since I can't see their source code I don't know what this means.
To be honest, when I search for PACF, all the examples only carry out PACF up to 40 lags or 60 or so and they never have any significant PACF after lag=2 and so I couldn't compare to other examples either.
But when I use:
method="ols"
# or
method="ywmle"
the numbers are corrected. So it must be the algo they use to solve it.
I tried importing inspect and getsource method but its useless it just shows that it uses another package and I can't find that.
If you also know where the problem arises from, I would really appreciate the help.
For your reference, the values for data[key][array] are:
[1131.130005, 1144.939941, 1126.209961, 1107.300049, 1120.680054, 1140.839966, 1101.719971, 1104.23999, 1114.579956, 1130.199951, 1173.819946, 1211.920044, 1181.27002, 1203.599976, 1180.589966, 1156.849976, 1191.5, 1191.329956, 1234.180054, 1220.329956, 1228.810059, 1207.01001, 1249.47998, 1248.290039, 1280.079956, 1280.660034, 1294.869995, 1310.609985, 1270.089966, 1270.199951, 1276.660034, 1303.819946, 1335.849976, 1377.939941, 1400.630005, 1418.300049, 1438.23999, 1406.819946, 1420.859985, 1482.369995, 1530.619995, 1503.349976, 1455.27002, 1473.98999, 1526.75, 1549.380005, 1481.140015, 1468.359985, 1378.550049, 1330.630005, 1322.699951, 1385.589966, 1400.380005, 1280.0, 1267.380005, 1282.829956, 1166.359985, 968.75, 896.23999, 903.25, 825.880005, 735.090027, 797.869995, 872.8099980000001, 919.1400150000001, 919.320007, 987.4799800000001, 1020.6199949999999, 1057.079956, 1036.189941, 1095.630005, 1115.099976, 1073.869995, 1104.48999, 1169.430054, 1186.689941, 1089.410034, 1030.709961, 1101.599976, 1049.329956, 1141.199951, 1183.26001, 1180.550049, 1257.640015, 1286.119995, 1327.219971, 1325.829956, 1363.609985, 1345.199951, 1320.640015, 1292.280029, 1218.890015, 1131.420044, 1253.300049, 1246.959961, 1257.599976, 1312.410034, 1365.680054, 1408.469971, 1397.910034, 1310.329956, 1362.160034, 1379.319946, 1406.579956, 1440.670044, 1412.160034, 1416.180054, 1426.189941, 1498.109985, 1514.680054, 1569.189941, 1597.569946, 1630.73999, 1606.280029, 1685.72998, 1632.969971, 1681.550049, 1756.540039, 1805.810059, 1848.359985, 1782.589966, 1859.449951, 1872.339966, 1883.949951, 1923.569946, 1960.22998, 1930.6700440000002, 2003.369995, 1972.290039, 2018.050049, 2067.560059, 2058.899902, 1994.9899899999998, 2104.5, 2067.889893, 2085.51001, 2107.389893, 2063.110107, 2103.840088, 1972.180054, 1920.030029, 2079.360107, 2080.409912, 2043.939941, 1940.2399899999998, 1932.22998, 2059.73999, 2065.300049, 2096.949951, 2098.860107, 2173.600098, 2170.949951, 2168.27002, 2126.149902, 2198.810059, 2238.830078, 2278.8701170000004, 2363.639893, 2362.719971, 2384.199951, 2411.800049, 2423.409912, 2470.300049, 2471.649902, 2519.360107, 2575.26001, 2584.840088, 2673.610107, 2823.810059, 2713.830078, 2640.8701170000004, 2648.050049, 2705.27002, 2718.3701170000004, 2816.290039, 2901.52002, 2913.97998]
Your time series is pretty clearly not stationary, so that Yule-Walker assumptions are violated.
More generally, PACF is usually appropriate with stationary time series. You might difference your data first, before considering the partial autocorrelations.
I am trying to learn linearK estimates on a small linnet object from the CRC spatstat book (chapter 17) and when I use the linearK function, spatstat throws an error. I have documented the process in the comments in the r code below. The error is as below.
Error in seq.default(from = 0, to = right, length.out = npos + 1L) : 'to' cannot be NA, NaN or infinite
I do not understand how to resolve this. I am following this process:
# I have data of points for each data of the week
# d1 is district 1 of the city.
# I did the step below otherwise it was giving me tbl class
d1_data=lapply(split(d1, d1$openDatefactor),as.data.frame)
# I previously create a linnet and divided it into districts of the city
d1_linnet = districts_linnet[["d1"]]
# I create point pattern for each day
d1_ppp = lapply(d1_data, function(x) as.ppp(x, W=Window(d1_linnet)))
plot(d1_ppp[[1]], which.marks="type")
# I am then converting the point pattern to a point pattern on linear network
d1_lpp <- as.lpp(d1_ppp[[1]], L=d1_linnet, W=Window(d1_linnet))
d1_lpp
Point pattern on linear network
3 points
15 columns of marks: ‘status’, ‘number_of_’, ‘zip’, ‘ward’,
‘police_dis’, ‘community_’, ‘type’, ‘days’, ‘NAME’,
‘DISTRICT’, ‘openDatefactor’, ‘OpenDate’, ‘coseDatefactor’,
‘closeDate’ and ‘instance’
Linear network with 4286 vertices and 6183 lines
Enclosing window: polygonal boundary
enclosing rectangle: [441140.9, 448217.7] x [4640080, 4652557] units
# the errors start from plotting this lpp object
plot(d1_lpp)
"show.all" is not a graphical parameter
Show Traceback
Error in plot.window(...) : need finite 'xlim' values
coords(d1_lpp)
x y seg tp
441649.2 4649853 5426 0.5774863
445716.9 4648692 5250 0.5435492
444724.6 4646320 677 0.9189631
3 rows
And then consequently, I also get error on linearK(d1_lpp)
Error in seq.default(from = 0, to = right, length.out = npos + 1L) : 'to' cannot be NA, NaN or infinite
I feel lpp object has the problem, but I find it hard to interpret the errors and how to resolve them. Could someone please guide me?
Thanks
I can confirm there is a bug in plot.lpp when trying to plot the marked point pattern on the linear network. That will hopefully be fixed soon. You can plot the unmarked point pattern using
plot(unmark(d1_lpp))
I cannot reproduce the problem with linearK. Which version of spatstat are you running? In the development version on my laptop spatstat_1.51-0.073 everything works. There has been changes to this code recently, so it is likely that this will be solved by updating to development version (see https://github.com/spatstat/spatstat).
I'm having a issue writing to a netcdf3 file using the netcdf4 functions. I tried using the create variable function but it gives me this error: NetCDF: Attempting netcdf-4 operation on netcdf-3 file
nc = Dataset(root.fileName,'a',format="NETCDF4")
Hycom_U = nc.createVariable('/variables/Hycom_U','float',('time','lat','lon',))
Hycom_V = nc.createVariable('/variables/Hycom_V','f4',('time','lat','lon',))
nc=
root group (NETCDF3_CLASSIC data model, file format NETCDF3):
netcdf_library_version: 4.1.3
format_version: HFRNet_1.0.0
product_version: HFRNet_1.1.05
Conventions: CF-1.0
title: Near-Real Time Surface Ocean Velocity, Hawaii,
2 km Resolution
institution: Scripps Institution of Oceanography
source: Surface Ocean HF-Radar
history: 22-Feb-2017 00:55:46: NetCDF file created
22-Feb-2017 00:55:46: Filtered U and V by GDOP < 1.25 ;
FMRC Best Dataset
references: Terrill, E. et al., 2006. Data Management and Real-time
Distribution in the HF-Radar National Network. Proceedings
of the MTS/IEEE Oceans 2006 Conference, Boston MA,
September 2006.
creator_name: Mark Otero
creator_email: motero#ucsd.edu
creator_url: http://cordc.ucsd.edu/projects/mapping/
summary: Surface ocean velocities estimated from HF-Radar are
representative of the upper 0.3 - 2.5 meters of the
ocean. The main objective of near-real time
processing is to produce the best product from
available data at the time of processing. Radial
velocity measurements are obtained from individual
radar sites through the U.S. HF-Radar Network.
Hourly radial data are processed by unweighted
least-squares on a 2 km resolution grid of Hawaii
to produce near real-time surface current maps.
geospatial_lat_min: 20.487279892
geospatial_lat_max: 21.5720806122
geospatial_lon_min: -158.903594971
geospatial_lon_max: -157.490005493
grid_resolution: 2km
grid_projection: equidistant cylindrical
regional_description: Unites States, Hawaiian Islands
cdm_data_type: GRID
featureType: GRID
location: Proto fmrc:HFRADAR,_US_Hawaii,_2km_Resolution,_Hourly_RTV
History: Translated to CF-1.0 Conventions by Netcdf-Java CDM (NetcdfCFWriter)
Original Dataset = fmrc:HFRADAR,_US_Hawaii,_2km_Resolution,_Hourly_RTV; Translation Date = Thu Feb 23 13:35:32 GMT 2017
dimensions(sizes): time(25), lat(61), lon(77)
variables(dimensions): float32 u(time,lat,lon), float64 time_run(time), float64 time(time), float32 lat(lat), float32 lon(lon), float32 v(time,lat,lon)
groups:
What are the netcdf 3 operations I can use to add data into the file? I found out that I could manually add data by simply doing this nc.variables["Hycom_U"]=U2which directly adds the data, but nothing else. Is there a better way to do this?
I believe the issue is that you're claiming the file to be netCDF4 format:
nc = Dataset(root.fileName,'a',format="NETCDF4")`
but you really want to indicate that it's netCDF3:
nc = Dataset(root.fileName,'a',format="NETCDF3_CLASSIC")
Additional documentation can be found here.
I figured it out! I simply couldn't use a path as a varname.
Hycom_U = nc.createVariable('Hycom_U','float',('time','lat','lon',))
It properly created a variable for me.