diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 7423d4307..5c6f29bb8 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -64,7 +64,7 @@ jobs: ${{ matrix.cmake_args }} - name: Build MuJoCo MPC working-directory: build - run: cmake --build . --config=Release ${{ matrix.cmake_build_args }} --target mjpc agent_test cost_derivatives_test norm_test rollout_test threadpool_test trajectory_test utilities_test batch_force_test batch_optimize_test batch_prior_test batch_sensor_test estimator_trajectory_test estimator_utilities_test ${{ matrix.additional_targets }} + run: cmake --build . --config=Release ${{ matrix.cmake_build_args }} --target mjpc agent_test cost_derivatives_test norm_test rollout_test threadpool_test trajectory_test utilities_test batch_force_test batch_optimize_test batch_prior_test batch_sensor_test estimator_trajectory_test estimator_utilities_test kalman_test ${{ matrix.additional_targets }} - name: Test MuJoCo MPC working-directory: build run: ctest -C Release --output-on-failure . diff --git a/grpc/CMakeLists.txt b/grpc/CMakeLists.txt index 37161c3e0..d10df47f2 100644 --- a/grpc/CMakeLists.txt +++ b/grpc/CMakeLists.txt @@ -56,7 +56,6 @@ get_filename_component(agent_service_proto_path "${agent_service_proto}" PATH) get_filename_component(batch_service_proto "./batch.proto" ABSOLUTE) get_filename_component(batch_service_proto_path "${batch_service_proto}" PATH) - # Generated sources set(agent_service_proto_srcs "${CMAKE_CURRENT_BINARY_DIR}/agent.pb.cc") set(agent_service_proto_hdrs "${CMAKE_CURRENT_BINARY_DIR}/agent.pb.h") @@ -209,4 +208,3 @@ target_include_directories(batch_server PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/..) message(BATCH_SERVICE_COMPILE_OPTIONS=${BATCH_SERVICE_COMPILE_OPTIONS}) target_compile_options(batch_server PUBLIC ${BATCH_SERVICE_COMPILE_OPTIONS}) target_link_options(batch_server PRIVATE ${BATCH_SERVICE_LINK_OPTIONS}) - diff --git a/grpc/batch_service_test.cc b/grpc/batch_service_test.cc deleted file mode 100644 index 582c0ca0f..000000000 --- a/grpc/batch_service_test.cc +++ /dev/null @@ -1,71 +0,0 @@ -// Copyright 2023 DeepMind Technologies Limited -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -// Unit tests for the `BatchService` class. - -#include "grpc/batch_service.h" - -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "testing/base/public/gmock.h" -#include "testing/base/public/gunit.h" -#include "grpc/batch.grpc.pb.h" -#include "grpc/batch.pb.h" - -namespace mjpc::batch_grpc { - -using batch::grpc_gen::Batch; - -class BatchServiceTest : public ::testing::Test { - protected: - void SetUp() override { - batch_service = std::make_unique(); - grpc::ServerBuilder builder; - builder.RegisterService(batch_service.get()); - server = builder.BuildAndStart(); - std::shared_ptr channel = - server->InProcessChannel(grpc::ChannelArguments()); - stub = Batch::NewStub(channel); - } - - void TearDown() override { server->Shutdown(); } - - void RunAndCheckInit() { - grpc::ClientContext init_context; - - batch::InitRequest init_request; - - batch::InitResponse init_response; - grpc::Status init_status = - stub->Init(&init_context, init_request, &init_response); - - EXPECT_TRUE(init_status.ok()) << init_status.error_message(); - } - - std::unique_ptr batch_service; - std::unique_ptr stub; - std::unique_ptr server; -}; - -} // namespace mjpc::batch_grpc diff --git a/mjpc/CMakeLists.txt b/mjpc/CMakeLists.txt index fa336dbe9..ede20375f 100644 --- a/mjpc/CMakeLists.txt +++ b/mjpc/CMakeLists.txt @@ -100,9 +100,13 @@ add_library( planners/ilqs/planner.h estimators/batch.cc estimators/batch.h + estimators/estimator.h + estimators/gui.cc + estimators/gui.h estimators/include.cc estimators/include.h - estimators/estimator.h + estimators/kalman.cc + estimators/kalman.h estimators/trajectory.h app.cc app.h diff --git a/mjpc/estimators/batch.cc b/mjpc/estimators/batch.cc index ccd2de24b..d83e43715 100644 --- a/mjpc/estimators/batch.cc +++ b/mjpc/estimators/batch.cc @@ -22,6 +22,8 @@ #include #include "mjpc/array_safety.h" +#include "mjpc/estimators/estimator.h" +#include "mjpc/estimators/gui.h" #include "mjpc/norm.h" #include "mjpc/threadpool.h" #include "mjpc/utilities.h" @@ -85,6 +87,11 @@ void Batch::Initialize(const mjModel* model) { configuration_length_ = GetNumberOrDefault(3, model, "batch_configuration_length"); + // check configuration length + if (configuration_length_ > max_history_) { + mju_error("configuration_length > max_history: increase max history\n"); + } + // -- trajectories -- // configuration.Initialize(nq, configuration_length_); velocity.Initialize(nv, configuration_length_); @@ -257,6 +264,14 @@ void Batch::Initialize(const mjModel* model) { scratch0_covariance_.resize((nv * max_history_) * (nv * max_history_)); scratch1_covariance_.resize((nv * max_history_) * (nv * max_history_)); + // conditioned matrix + mat00_.resize((nv * max_history_) * (nv * max_history_)); + mat10_.resize((nv * max_history_) * (nv * max_history_)); + mat11_.resize((nv * max_history_) * (nv * max_history_)); + condmat_.resize((nv * max_history_) * (nv * max_history_)); + scratch0_condmat_.resize((nv * max_history_) * (nv * max_history_)); + scratch1_condmat_.resize((nv * max_history_) * (nv * max_history_)); + // regularization regularization_ = settings.regularization_initial; @@ -278,19 +293,46 @@ void Batch::Initialize(const mjModel* model) { // settings settings.band_prior = static_cast(GetNumberOrDefault(1, model, "batch_band_covariance")); + + // -- trajectory cache -- // + configuration_cache_.Initialize(nq, configuration_length_); + velocity_cache_.Initialize(nv, configuration_length_); + acceleration_cache_.Initialize(nv, configuration_length_); + act_cache_.Initialize(na, configuration_length_); + times_cache_.Initialize(1, configuration_length_); + + // ctrl + ctrl_cache_.Initialize(model->nu, configuration_length_); + + // prior + configuration_previous_cache_.Initialize(nq, configuration_length_); + + // sensor + sensor_measurement_cache_.Initialize(nsensordata_, configuration_length_); + sensor_prediction_cache_.Initialize(nsensordata_, configuration_length_); + sensor_mask_cache_.Initialize(nsensor_, configuration_length_); + + // force + force_measurement_cache_.Initialize(nv, configuration_length_); + force_prediction_cache_.Initialize(nv, configuration_length_); } // reset memory -void Batch::Reset() { +void Batch::Reset(const mjData* data) { // dimension int nq = model->nq, nv = model->nv, na = model->na; // data mjData* d = data_[0].get(); - // set home keyframe - int home_id = mj_name2id(model, mjOBJ_KEY, "home"); - if (home_id >= 0) mj_resetDataKeyframe(model, d, home_id); + if (data) { + // copy input data + mj_copyData(d, model, data); + } else { + // set home keyframe + int home_id = mj_name2id(model, mjOBJ_KEY, "home"); + if (home_id >= 0) mj_resetDataKeyframe(model, d, home_id); + } // forward evaluation mj_forward(model, d); @@ -299,8 +341,7 @@ void Batch::Reset() { mju_copy(state.data(), d->qpos, nq); mju_copy(state.data() + nq, d->qvel, nv); mju_copy(state.data() + nq + nv, d->act, na); - d->time = 0.0; - time = 0.0; + time = d->time; // covariance mju_eye(covariance.data(), ndstate_); @@ -319,6 +360,9 @@ void Batch::Reset() { GetNumberOrDefault(1.0e-4, model, "estimator_sensor_noise_scale"); std::fill(noise_sensor.begin(), noise_sensor.end(), noise_sensor_scl); + // scale prior + scale_prior = GetNumberOrDefault(1.0e-1, model, "batch_scale_prior"); + // trajectories configuration.Reset(); velocity.Reset(); @@ -336,7 +380,7 @@ void Batch::Reset() { // sensor mask sensor_mask.Reset(); - for (int i = 0; i < nsensor_ * configuration_length_; i++) { + for (int i = 0; i < nsensor_ * max_history_; i++) { sensor_mask.Data()[i] = 1; // sensor on } @@ -461,6 +505,14 @@ void Batch::Reset() { std::fill(scratch0_covariance_.begin(), scratch0_covariance_.end(), 0.0); std::fill(scratch1_covariance_.begin(), scratch1_covariance_.end(), 0.0); + // conditioned matrix + std::fill(mat00_.begin(), mat00_.end(), 0.0); + std::fill(mat10_.begin(), mat10_.end(), 0.0); + std::fill(mat11_.begin(), mat11_.end(), 0.0); + std::fill(condmat_.begin(), condmat_.end(), 0.0); + std::fill(scratch0_condmat_.begin(), scratch0_condmat_.end(), 0.0); + std::fill(scratch1_condmat_.begin(), scratch1_condmat_.end(), 0.0); + // timer std::fill(timer_.prior_step.begin(), timer_.prior_step.end(), 0.0); std::fill(timer_.sensor_step.begin(), timer_.sensor_step.end(), 0.0); @@ -475,79 +527,33 @@ void Batch::Reset() { cost_count_ = 0; solve_status_ = kUnsolved; - // -- initialize -- // - if (settings.filter) { - settings.gradient_tolerance = 1.0e-8; - settings.max_smoother_iterations = 1; - settings.max_search_iterations = 10; - - // timestep - double timestep = model->opt.timestep; - - // set q1 - configuration.Set(state.data(), 1); - configuration_previous.Set(configuration.Get(1), 1); - - // set q0 - double* q0 = configuration.Get(0); - mju_copy(q0, state.data(), nq); - mj_integratePos(model, q0, state.data() + nq, -1.0 * timestep); - configuration_previous.Set(configuration.Get(0), 0); - - // set times - double current_time = -1.0 * timestep; - times.Set(¤t_time, 0); - for (int i = 1; i < configuration_length_; i++) { - // increment time - current_time += timestep; - - // set - times.Set(¤t_time, i); - } - - // -- set initial position-based measurements -- // - - // data - mjData* data = data_[0].get(); - - // set q0 - mju_copy(data->qpos, q0, model->nq); - - // evaluate position - mj_fwdPosition(model, data); - mj_sensorPos(model, data); + // initialize filter + if (settings.filter) InitializeFilter(); - // y0 - double* y0 = sensor_measurement.Get(0); - mju_zero(y0, nsensordata_); + // trajectory cache + configuration_cache_.Reset(); + velocity_cache_.Reset(); + acceleration_cache_.Reset(); + act_cache_.Reset(); + times_cache_.Reset(); + ctrl_cache_.Reset(); - // loop over sensors - for (int i = 0; i < nsensor_; i++) { - // measurement sensor index - int index = sensor_start_ + i; - - // need stage - int sensor_stage = model->sensor_needstage[index]; - - if (sensor_stage == mjSTAGE_POS) { - // address - int sensor_adr = model->sensor_adr[index]; - - // dimension - int sensor_dim = model->sensor_dim[index]; + // prior + configuration_previous_cache_.Reset(); - // set sensor - mju_copy(y0 + sensor_adr - sensor_start_index_, - data->sensordata + sensor_adr, sensor_dim); - } - } + // sensor + sensor_measurement_cache_.Reset(); + sensor_prediction_cache_.Reset(); - // prior weight (skip act) - for (int i = 0; i < ndstate_ - na; i++) { - weight_prior[nv * configuration_length_ * i + i] = - 1.0 / covariance[ndstate_ * i + i]; - } + // sensor mask + sensor_mask_cache_.Reset(); + for (int i = 0; i < nsensor_ * configuration_length_; i++) { + sensor_mask_cache_.Data()[i] = 1; // sensor on } + + // force + force_measurement_cache_.Reset(); + force_prediction_cache_.Reset(); } // update @@ -555,32 +561,25 @@ void Batch::Update(const double* ctrl, const double* sensor) { // start timer auto start = std::chrono::steady_clock::now(); - // check configuration length - if (configuration_length_ != 3) { - mju_error("batch filter only supports configuration length = 3\n"); - } - // dimensions int nq = model->nq, nv = model->nv, na = model->na, nu = model->nu; + // data + mjData* d = data_[0].get(); + // current time index - int t = 1; + int t = current_time_index; // configurations double* q0 = configuration.Get(t - 1); double* q1 = configuration.Get(t); - // data - mjData* d = data_[0].get(); - // -- next qpos -- // // set state mju_copy(d->qpos, q1, model->nq); mj_differentiatePos(model, d->qvel, model->opt.timestep, q0, q1); - // TODO(taylor): set time - // set ctrl mju_copy(d->ctrl, ctrl, nu); @@ -588,6 +587,7 @@ void Batch::Update(const double* ctrl, const double* sensor) { mj_step(model, d); // -- set batch data -- // + // set next qpos configuration.Set(d->qpos, t + 1); configuration_previous.Set(d->qpos, t + 1); @@ -604,7 +604,17 @@ void Batch::Update(const double* ctrl, const double* sensor) { // set force measurement force_measurement.Set(d->qfrc_actuator, t); - // measurement update + // -- measurement update -- // + + // cache configuration length + int configuration_length_cache = configuration_length_; + + // set reduced configuration length for optimization + configuration_length_ = current_time_index + 2; + if (configuration_length_ != configuration_length_cache) { + ShiftResizeTrajectory(0, configuration_length_); + } + // TODO(taylor): preallocate pool ThreadPool pool(1); Optimize(pool); @@ -613,13 +623,77 @@ void Batch::Update(const double* ctrl, const double* sensor) { mju_copy(state.data(), configuration.Get(t + 1), nq); mju_copy(state.data() + nq, velocity.Get(t + 1), nv); mju_copy(state.data() + nq + nv, act.Get(t + 1), na); - time = d->time; + time = times.Get(t + 1)[0]; + + // -- update prior weights -- // + + // dimension + int nvar = nv * configuration_length_; + + // prior weights + double* weights = weight_prior.data(); + + // recursive update + if (settings.recursive_prior_update && + configuration_length_ == configuration_length_cache) { + // condition dimension + int ncondition = nv * (configuration_length_ - 1); + + // compute conditioned matrix + double* condmat = condmat_.data(); + ConditionMatrix(condmat, cost_hessian.data(), mat00_.data(), mat10_.data(), + mat11_.data(), scratch0_condmat_.data(), + scratch1_condmat_.data(), nvar, nv, ncondition); - // update prior weights - // TODO(taylor) + // zero memory + mju_zero(weights, nvar * nvar); + + // set conditioned matrix in prior weights + SetBlockInMatrix(weights, condmat, 1.0, nvar, nvar, ncondition, ncondition, + 0, 0); - // shift trajectories - Shift(1); + // set bottom right to scale_prior * I + for (int i = ncondition; i < nvar; i++) { + weights[nvar * i + i] = scale_prior; + } + } else { + // dimension + int nvar_new = nvar; + if (current_time_index < configuration_length_ - 2) { + nvar_new += nv; + } + + // check same size + if (nvar != nvar_new) { + // get previous weights + double* previous_weights = scratch0_condmat_.data(); + mju_copy(previous_weights, weights, nvar * nvar); + + // set previous in new weights (dimension may have increased) + mju_zero(weights, nvar_new * nvar_new); + SetBlockInMatrix(weights, previous_weights, 1.0, nvar_new, nvar_new, nvar, + nvar, 0, 0); + + // scale_prior * I + for (int i = nvar; i < nvar_new; i++) { + weights[nvar_new * i + i] = scale_prior; + } + } + } + + // restore configuration length + if (configuration_length_ != configuration_length_cache) { + ShiftResizeTrajectory(0, configuration_length_cache); + } + configuration_length_ = configuration_length_cache; + + // check estimation horizon + if (current_time_index < configuration_length_ - 2) { + current_time_index++; + } else { + // shift trajectories once estimation horizon is filled + Shift(1); + } // stop timer timer_.update = 1.0e-3 * GetDuration(start); @@ -1895,6 +1969,159 @@ void Batch::VelocityAccelerationDerivatives() { timer_.velacc_derivatives += GetDuration(start); } +// initialize filter mode +void Batch::InitializeFilter() { + // dimensions + int nq = model->nq, nv = model->nv; + + // filter mode status + current_time_index = 1; + + // filter settings + settings.gradient_tolerance = 1.0e-6; + settings.max_smoother_iterations = 1; + settings.max_search_iterations = 10; + settings.recursive_prior_update = true; + + // timestep + double timestep = model->opt.timestep; + + // set q1 + configuration.Set(state.data(), 1); + configuration_previous.Set(configuration.Get(1), 1); + + // set q0 + double* q0 = configuration.Get(0); + mju_copy(q0, state.data(), nq); + mj_integratePos(model, q0, state.data() + nq, -1.0 * timestep); + configuration_previous.Set(configuration.Get(0), 0); + + // set times + double current_time = time - timestep; + times.Set(¤t_time, 0); + for (int i = 1; i < configuration_length_; i++) { + // increment time + current_time += timestep; + + // set + times.Set(¤t_time, i); + } + + // -- set initial position-based measurements -- // + + // data + mjData* data = data_[0].get(); + + // set q0 + mju_copy(data->qpos, q0, model->nq); + + // evaluate position + mj_fwdPosition(model, data); + mj_sensorPos(model, data); + + // y0 + double* y0 = sensor_measurement.Get(0); + mju_zero(y0, nsensordata_); + + // loop over sensors + for (int i = 0; i < nsensor_; i++) { + // measurement sensor index + int index = sensor_start_ + i; + + // need stage + int sensor_stage = model->sensor_needstage[index]; + + // position sensor + if (sensor_stage == mjSTAGE_POS) { + // address + int sensor_adr = model->sensor_adr[index]; + + // dimension + int sensor_dim = model->sensor_dim[index]; + + // set sensor + mju_copy(y0 + sensor_adr - sensor_start_index_, + data->sensordata + sensor_adr, sensor_dim); + } + } + + // prior weight + int nvar = nv * configuration_length_; + for (int i = 0; i < nvar; i++) { + weight_prior[nvar * i + i] = scale_prior; + } +} + +// shift head and resize trajectories +void Batch::ShiftResizeTrajectory(int new_head, int new_length) { + // reset cache + configuration_cache_.Reset(); + configuration_previous_cache_.Reset(); + velocity_cache_.Reset(); + acceleration_cache_.Reset(); + act_cache_.Reset(); + times_cache_.Reset(); + ctrl_cache_.Reset(); + sensor_measurement_cache_.Reset(); + sensor_prediction_cache_.Reset(); + sensor_mask_cache_.Reset(); + force_measurement_cache_.Reset(); + force_prediction_cache_.Reset(); + + // -- set cache length -- // + int length = configuration_length_; + + configuration_cache_.SetLength(length); + configuration_previous_cache_.SetLength(length); + velocity_cache_.SetLength(length); + acceleration_cache_.SetLength(length); + act_cache_.SetLength(length); + times_cache_.SetLength(length); + ctrl_cache_.SetLength(length); + sensor_measurement_cache_.SetLength(length); + sensor_prediction_cache_.SetLength(length); + sensor_mask_cache_.SetLength(length); + force_measurement_cache_.SetLength(length); + force_prediction_cache_.SetLength(length); + + // copy data to cache + for (int i = 0; i < length; i++) { + configuration_cache_.Set(configuration.Get(i), i); + configuration_previous_cache_.Set(configuration_previous.Get(i), i); + velocity_cache_.Set(velocity.Get(i), i); + acceleration_cache_.Set(acceleration.Get(i), i); + act_cache_.Set(act.Get(i), i); + times_cache_.Set(times.Get(i), i); + ctrl_cache_.Set(ctrl.Get(i), i); + sensor_measurement_cache_.Set(sensor_measurement.Get(i), i); + sensor_prediction_cache_.Set(sensor_prediction.Get(i), i); + sensor_mask_cache_.Set(sensor_mask.Get(i), i); + force_measurement_cache_.Set(force_measurement.Get(i), i); + force_prediction_cache_.Set(force_prediction.Get(i), i); + } + + // set configuration length + SetConfigurationLength(new_length); + + // set trajectory data + int length_copy = std::min(length, new_length); + for (int i = 0; i < length_copy; i++) { + configuration.Set(configuration_cache_.Get(new_head + i), i); + configuration_previous.Set(configuration_previous_cache_.Get(new_head + i), + i); + velocity.Set(velocity_cache_.Get(new_head + i), i); + acceleration.Set(acceleration_cache_.Get(new_head + i), i); + act.Set(act_cache_.Get(new_head + i), i); + times.Set(times_cache_.Get(new_head + i), i); + ctrl.Set(ctrl_cache_.Get(new_head + i), i); + sensor_measurement.Set(sensor_measurement_cache_.Get(new_head + i), i); + sensor_prediction.Set(sensor_prediction_cache_.Get(new_head + i), i); + sensor_mask.Set(sensor_mask_cache_.Get(new_head + i), i); + force_measurement.Set(force_measurement_cache_.Get(new_head + i), i); + force_prediction.Set(force_prediction_cache_.Get(new_head + i), i); + } +} + // compute total cost // TODO(taylor): fix timers double Batch::Cost(double* gradient, double* hessian, ThreadPool& pool) { @@ -2111,7 +2338,7 @@ void Batch::Optimize(ThreadPool& pool) { // -- search direction -- // // check regularization - if (regularization_ >= MAX_REGULARIZATION - 1.0e-6) { + if (regularization_ >= kMaxBatchRegularization - 1.0e-6) { // set solve status solve_status_ = kMaxRegularizationFailure; @@ -2241,12 +2468,12 @@ void Batch::Optimize(ThreadPool& pool) { if (reduction_ratio_ > 0.75) { // decrease regularization_ = - mju_max(MIN_REGULARIZATION, + mju_max(kMinBatchRegularization, regularization_ / settings.regularization_scaling); } else if (reduction_ratio_ < 0.25) { // increase regularization_ = - mju_min(MAX_REGULARIZATION, + mju_min(kMaxBatchRegularization, regularization_ * settings.regularization_scaling); } } @@ -2302,7 +2529,7 @@ void Batch::SearchDirection() { double min_diag = 0.0; while (min_diag <= 0.0) { // failure - if (regularization_ >= MAX_REGULARIZATION) { + if (regularization_ >= kMaxBatchRegularization) { printf("min diag = %f\n", min_diag); mju_error("cost Hessian factorization failure: MAX REGULARIZATION\n"); } @@ -2332,7 +2559,7 @@ void Batch::SearchDirection() { int rank = 0; while (rank < ntotal) { // failure - if (regularization_ >= MAX_REGULARIZATION) { + if (regularization_ >= kMaxBatchRegularization) { mju_error("cost Hessian factorization failure: MAX REGULARIZATION\n"); } @@ -2360,6 +2587,13 @@ void Batch::SearchDirection() { // search direction norm search_direction_norm_ = InfinityNorm(direction, ntotal); + // set regularization + if (regularization_ > 0.0) { + for (int i = 0; i < ntotal; i++) { + hessian[i * ntotal + i] += regularization_; + } + } + // end timer timer_.search_direction += GetDuration(search_direction_start); } @@ -2543,18 +2777,22 @@ std::string StatusString(int code) { } // estimator-specific GUI elements -void Batch::GUI(mjUI& ui, double* process_noise, double* sensor_noise, - double& timestep, int& integrator) { +void Batch::GUI(mjUI& ui, EstimatorGUIData& data) { // ----- estimator ------ // mjuiDef defEstimator[] = { {mjITEM_SECTION, "Estimator Settings", 1, nullptr, "AP"}, // needs new section to satisfy mjMAXUIITEM {mjITEM_BUTTON, "Reset", 2, nullptr, ""}, - {mjITEM_SLIDERNUM, "Timestep", 2, ×tep, "1.0e-3 0.1"}, - {mjITEM_SELECT, "Integrator", 2, &integrator, + {mjITEM_SLIDERNUM, "Timestep", 2, &data.timestep, "1.0e-3 0.1"}, + {mjITEM_SELECT, "Integrator", 2, &data.integrator, "Euler\nRK4\nImplicit\nFastImplicit"}, + {mjITEM_SLIDERINT, "Horizon", 2, &data.horizon, "3 3"}, {mjITEM_END}}; + // set estimation horizon limits + mju::sprintf_arr(defEstimator[4].other, "%i %i", kMinBatchHistory, + kMaxFilterHistory); + // add estimator mjui_add(&ui, defEstimator); @@ -2570,8 +2808,8 @@ void Batch::GUI(mjUI& ui, double* process_noise, double* sensor_noise, // add UI elements for (int i = 0; i < nv; i++) { // element - defProcessNoise[process_noise_shift] = {mjITEM_SLIDERNUM, "", 2, - process_noise + i, "1.0e-8 0.01"}; + defProcessNoise[process_noise_shift] = { + mjITEM_SLIDERNUM, "", 2, data.process_noise.data() + i, "1.0e-8 0.01"}; // set name mju::strcpy_arr(defProcessNoise[process_noise_shift].name, ""); @@ -2689,9 +2927,9 @@ void Batch::GUI(mjUI& ui, double* process_noise, double* sensor_noise, model->name_sensoradr[sensor_start_ + i]); // element - defSensorNoise[sensor_noise_shift] = {mjITEM_SLIDERNUM, "", 2, - sensor_noise + sensor_noise_shift - 1, - "1.0e-8 0.01"}; + defSensorNoise[sensor_noise_shift] = { + mjITEM_SLIDERNUM, "", 2, + data.sensor_noise.data() + sensor_noise_shift - 1, "1.0e-8 0.01"}; // sensor name sensor_str = name_sensor; @@ -2711,6 +2949,69 @@ void Batch::GUI(mjUI& ui, double* process_noise, double* sensor_noise, mjui_add(&ui, defSensorNoise); } +// set GUI data +void Batch::SetGUIData(EstimatorGUIData& data) { + mju_copy(noise_process.data(), data.process_noise.data(), DimensionProcess()); + mju_copy(noise_sensor.data(), data.sensor_noise.data(), DimensionSensor()); + model->opt.timestep = data.timestep; + model->opt.integrator = data.integrator; + + // store estimation horizon + int horizon = data.horizon; + + // changing horizon cases + if (horizon > configuration_length_) { // increase horizon + // -- prior weights resize -- // + int nvar = model->nv * configuration_length_; + int nvar_new = model->nv * horizon; + + // get previous weights + double* weights = weight_prior.data(); + double* previous_weights = scratch0_condmat_.data(); + mju_copy(previous_weights, weights, nvar * nvar); + + // set previous in new weights (dimension may have increased) + mju_zero(weights, nvar_new * nvar_new); + SetBlockInMatrix(weights, previous_weights, 1.0, nvar_new, nvar_new, nvar, + nvar, 0, 0); + + // scale_prior * I + for (int i = nvar; i < nvar_new; i++) { + weights[nvar_new * i + i] = scale_prior; + } + + // modify trajectories + ShiftResizeTrajectory(0, horizon); + + // update configuration length + configuration_length_ = horizon; + } else if (horizon < configuration_length_) { // decrease horizon + // -- prior weights resize -- // + int nvar = model->nv * configuration_length_; + int nvar_new = model->nv * horizon; + + // get previous weights + double* weights = weight_prior.data(); + double* previous_weights = scratch0_condmat_.data(); + BlockFromMatrix(previous_weights, weights, nvar_new, nvar_new, nvar, nvar, + 0, 0); + + // set previous in new weights (dimension may have increased) + mju_zero(weights, nvar * nvar); + mju_copy(weights, previous_weights, nvar_new * nvar_new); + + // compute difference in estimation horizons + int horizon_diff = configuration_length_ - horizon; + + // modify trajectories + ShiftResizeTrajectory(horizon_diff, horizon); + + // update configuration length and current time index + configuration_length_ = horizon; + current_time_index -= horizon_diff; + } +} + // estimator-specific plots void Batch::Plots(mjvFigure* fig_planner, mjvFigure* fig_timer, int planner_shift, int timer_shift, int planning, @@ -2744,7 +3045,7 @@ void Batch::Plots(mjvFigure* fig_planner, mjvFigure* fig_timer, // increase regularization void Batch::IncreaseRegularization() { - regularization_ = mju_min(MAX_REGULARIZATION, + regularization_ = mju_min(kMaxBatchRegularization, regularization_ * settings.regularization_scaling); } diff --git a/mjpc/estimators/batch.h b/mjpc/estimators/batch.h index 9e49fc65a..223d666da 100644 --- a/mjpc/estimators/batch.h +++ b/mjpc/estimators/batch.h @@ -22,6 +22,7 @@ #include #include "mjpc/estimators/estimator.h" +#include "mjpc/estimators/gui.h" #include "mjpc/estimators/trajectory.h" #include "mjpc/norm.h" #include "mjpc/threadpool.h" @@ -32,6 +33,8 @@ namespace mjpc { // defaults inline constexpr int kMinBatchHistory = 3; // minimum configuration trajectory length +inline constexpr int kMaxFilterHistory = + 32; // maximum batch filter estimation horizon // batch estimator status enum BatchStatus : int { @@ -53,8 +56,8 @@ enum SearchType : int { }; // maximum / minimum regularization -const double MAX_REGULARIZATION = 1.0e12; -const double MIN_REGULARIZATION = 1.0e-12; +inline constexpr double kMaxBatchRegularization = 1.0e12; +inline constexpr double kMinBatchRegularization = 1.0e-12; // ----- batch estimator ----- // // based on: "Physically-Consistent Sensor Fusion in Contact-Rich Behaviors" @@ -62,7 +65,10 @@ class Batch : public Estimator { public: // constructor Batch() = default; - Batch(int mode) { settings.filter = mode; } + Batch(int mode) { + settings.filter = mode; + max_history_ = kMaxFilterHistory; + } Batch(const mjModel* model, int length = 3, int max_history_ = 0) { // set max history length this->max_history_ = (max_history_ == 0 ? length : max_history_); @@ -86,7 +92,7 @@ class Batch : public Estimator { void Initialize(const mjModel* model) override; // reset memory - void Reset() override; + void Reset(const mjData* data = nullptr) override; // update void Update(const double* ctrl, const double* sensor) override; @@ -139,6 +145,22 @@ class Batch : public Estimator { mj_integratePos(model, q0, state + nq, -1.0 * model->opt.timestep); }; + // set time + void SetTime(double time) override { + // copy + double time_copy = time; + + // t1 + times.Set(&time_copy, 1); + + // t0 + time_copy -= model->opt.timestep; + times.Set(&time_copy, 0); + + // reset current time index + current_time_index = 1; + } + // set covariance void SetCovariance(const double* covariance) override { mju_copy(this->covariance.data(), covariance, ndstate_ * ndstate_); @@ -146,8 +168,10 @@ class Batch : public Estimator { }; // estimator-specific GUI elements - void GUI(mjUI& ui, double* process_noise, double* sensor_noise, - double& timestep, int& integrator) override; + void GUI(mjUI& ui, EstimatorGUIData& data) override; + + // set GUI data + void SetGUIData(EstimatorGUIData& data) override; // estimator-specific plots void Plots(mjvFigure* fig_planner, mjvFigure* fig_timer, int planner_shift, @@ -290,8 +314,9 @@ class Batch : public Estimator { bool assemble_sensor_norm_hessian = false; // assemble dense sensor norm Hessian bool assemble_force_norm_hessian = - false; // assemble dense force norm Hessian - bool filter = false; // filter mode + false; // assemble dense force norm Hessian + bool filter = false; // filter mode + bool recursive_prior_update = false; // recursively update prior matrix } settings; // finite-difference settings @@ -300,6 +325,9 @@ class Batch : public Estimator { bool flg_actuation = 1; } finite_difference; + // filter mode status + int current_time_index; + private: // convert sequence of configurations to velocities, accelerations void ConfigurationToVelocityAcceleration(); @@ -366,6 +394,12 @@ class Batch : public Estimator { const EstimatorTrajectory& configuration, const double* search_direction, double step_size); + // initialize filter mode + void InitializeFilter(); + + // shift head and resize trajectories + void ShiftResizeTrajectory(int new_head, int new_length); + // reset timers void ResetTimers(); @@ -402,6 +436,8 @@ class Batch : public Estimator { double cost_initial_; double cost_previous_; + // TODO(taylor): underscore + // cost gradient std::vector cost_gradient; // nv * max_history_ @@ -548,6 +584,14 @@ class Batch : public Estimator { std::vector scratch1_covariance_; // (nv * max_history_) * (nv * max_history_) + // conditioned matrix + std::vector mat00_; + std::vector mat10_; + std::vector mat11_; + std::vector condmat_; + std::vector scratch0_condmat_; + std::vector scratch1_condmat_; + // status (internal) int cost_count_; // number of cost evaluations bool cost_skip_ = false; // flag for only evaluating cost derivatives @@ -605,6 +649,20 @@ class Batch : public Estimator { // max history int max_history_ = 3; + + // trajectory cache + EstimatorTrajectory configuration_cache_; // nq x T + EstimatorTrajectory configuration_previous_cache_; // nq x T + EstimatorTrajectory velocity_cache_; // nv x T + EstimatorTrajectory acceleration_cache_; // nv x T + EstimatorTrajectory act_cache_; // na x T + EstimatorTrajectory times_cache_; // 1 x T + EstimatorTrajectory ctrl_cache_; // nu x T + EstimatorTrajectory sensor_measurement_cache_; // ns x T + EstimatorTrajectory sensor_prediction_cache_; // ns x T + EstimatorTrajectory sensor_mask_cache_; // num_sensor x T + EstimatorTrajectory force_measurement_cache_; // nv x T + EstimatorTrajectory force_prediction_cache_; // nv x T }; // estimator status string diff --git a/mjpc/estimators/estimator.h b/mjpc/estimators/estimator.h index 1ec4225d2..4a8c780aa 100644 --- a/mjpc/estimators/estimator.h +++ b/mjpc/estimators/estimator.h @@ -21,6 +21,9 @@ #include +#include "mjpc/estimators/gui.h" +#include "mjpc/utilities.h" + namespace mjpc { // maximum terms @@ -37,7 +40,7 @@ class Estimator { virtual void Initialize(const mjModel* model) = 0; // reset memory - virtual void Reset() = 0; + virtual void Reset(const mjData* data = nullptr) = 0; // update virtual void Update(const double* ctrl, const double* sensor) = 0; @@ -72,12 +75,17 @@ class Estimator { // set state virtual void SetState(const double* state) = 0; + // set time + virtual void SetTime(double time) = 0; + // set covariance virtual void SetCovariance(const double* covariance) = 0; // estimator-specific GUI elements - virtual void GUI(mjUI& ui, double* process_noise, double* sensor_noise, - double& timestep, int& integrator) = 0; + virtual void GUI(mjUI& ui, EstimatorGUIData& data) = 0; + + // set GUI data + virtual void SetGUIData(EstimatorGUIData& data) = 0; // estimator-specific plots virtual void Plots(mjvFigure* fig_planner, mjvFigure* fig_timer, @@ -85,6 +93,183 @@ class Estimator { int* shift) = 0; }; +// ground truth estimator +class GroundTruth : public Estimator { + public: + // constructor + GroundTruth() = default; + GroundTruth(const mjModel* model) { + Initialize(model); + Reset(); + } + + // destructor + ~GroundTruth() { + if (data_) mj_deleteData(data_); + if (model) mj_deleteModel(model); + } + + // initialize + void Initialize(const mjModel* model) override { + // model + if (this->model) mj_deleteModel(this->model); + this->model = mj_copyModel(nullptr, model); + + // data + data_ = mj_makeData(model); + + // timestep + this->model->opt.timestep = GetNumberOrDefault(this->model->opt.timestep, + model, "estimator_timestep"); + + // -- dimensions -- // + ndstate_ = 2 * model->nv + model->na; + + // sensor start index + int sensor_start_ = GetNumberOrDefault(0, model, "estimator_sensor_start"); + + // number of sensors + int nsensor_ = + GetNumberOrDefault(model->nsensor, model, "estimator_number_sensor"); + + // sensor dimension + nsensordata_ = 0; + for (int i = 0; i < nsensor_; i++) { + nsensordata_ += model->sensor_dim[sensor_start_ + i]; + } + + // state + state.resize(model->nq + model->nv + model->na); + + // covariance + covariance.resize(ndstate_ * ndstate_); + + // process noise + noise_process.resize(ndstate_); + + // sensor noise + noise_sensor.resize(nsensordata_); // over allocate + } + + // reset + void Reset(const mjData* data = nullptr) override { + // dimensions + int nq = model->nq, nv = model->nv, na = model->na; + int ndstate = 2 * nv + na; + + if (data) { + mju_copy(state.data(), data->qpos, nq); + mju_copy(state.data() + nq, data->qvel, nv); + mju_copy(state.data() + nq + nv, data->act, na); + time = data->time; + } else { // model default + // set home keyframe + int home_id = mj_name2id(model, mjOBJ_KEY, "home"); + if (home_id >= 0) mj_resetDataKeyframe(model, data_, home_id); + + // state + mju_copy(state.data(), data_->qpos, nq); + mju_copy(state.data() + nq, data_->qvel, nv); + mju_copy(state.data() + nq + nv, data_->act, na); + time = data_->time; + } + + // covariance + mju_eye(covariance.data(), ndstate); + double covariance_scl = + GetNumberOrDefault(1.0e-4, model, "estimator_covariance_initial_scale"); + mju_scl(covariance.data(), covariance.data(), covariance_scl, + ndstate * ndstate); + + // process noise + double noise_process_scl = + GetNumberOrDefault(1.0e-4, model, "estimator_process_noise_scale"); + std::fill(noise_process.begin(), noise_process.end(), noise_process_scl); + + // sensor noise + double noise_sensor_scl = + GetNumberOrDefault(1.0e-4, model, "estimator_sensor_noise_scale"); + std::fill(noise_sensor.begin(), noise_sensor.end(), noise_sensor_scl); + } + + // update + void Update(const double* ctrl, const double* sensor) override{}; + + // get state + double* State() override { return state.data(); }; + + // get covariance + double* Covariance() override { return covariance.data(); }; + + // get time + double& Time() override { return time; }; + + // get model + mjModel* Model() override { return model; }; + + // get data + mjData* Data() override { return data_; }; + + // get process noise + double* ProcessNoise() override { return noise_process.data(); }; + + // get sensor noise + double* SensorNoise() override { return noise_sensor.data(); }; + + // process dimension + int DimensionProcess() const override { return ndstate_; }; + + // sensor dimension + int DimensionSensor() const override { return nsensordata_; }; + + // set state + void SetState(const double* state) override { + mju_copy(this->state.data(), state, ndstate_); + }; + + // set time + void SetTime(double time) override { this->time = time; } + + // set covariance + void SetCovariance(const double* covariance) override { + mju_copy(this->covariance.data(), covariance, ndstate_ * ndstate_); + }; + + // estimator-specific GUI elements + void GUI(mjUI& ui, EstimatorGUIData& data) override{}; + + // set GUI data + void SetGUIData(EstimatorGUIData& data) override{}; + + // estimator-specific plots + void Plots(mjvFigure* fig_planner, mjvFigure* fig_timer, int planner_shift, + int timer_shift, int planning, int* shift) override{}; + + // model + mjModel* model = nullptr; + + // state (nstate_) + std::vector state; + double time; + + // covariance (ndstate_ x ndstate_) + std::vector covariance; + + // process noise (ndstate_) + std::vector noise_process; + + // sensor noise (nsensordata_) + std::vector noise_sensor; + + private: + // data + mjData* data_ = nullptr; + + // dimensions + int ndstate_; + int nsensordata_; +}; + } // namespace mjpc #endif // MJPC_ESTIMATORS_ESTIMATOR_H_ diff --git a/mjpc/estimators/gui.cc b/mjpc/estimators/gui.cc new file mode 100644 index 000000000..9e1b4dc9b --- /dev/null +++ b/mjpc/estimators/gui.cc @@ -0,0 +1,44 @@ +// Copyright 2022 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "mjpc/estimators/gui.h" + +#include + +#include + +#include "mjpc/utilities.h" + +namespace mjpc { + +// Initialize +void EstimatorGUIData::Initialize(const mjModel* model, int nprocess, + int nsensor) { + // timestep + timestep = model->opt.timestep; + + // integrator + integrator = model->opt.integrator; + + // process noise + process_noise.resize(nprocess); + + // sensor noise + sensor_noise.resize(nsensor); + + // estimation horizon + horizon = GetNumberOrDefault(3, model, "batch_configuration_length"); +} + +} // namespace mjpc diff --git a/mjpc/estimators/gui.h b/mjpc/estimators/gui.h new file mode 100644 index 000000000..e773e1e87 --- /dev/null +++ b/mjpc/estimators/gui.h @@ -0,0 +1,54 @@ +// Copyright 2022 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MJPC_ESTIMATORS_GUI_H_ +#define MJPC_ESTIMATORS_GUI_H_ + +#include + +#include + +namespace mjpc { + +// data that is modified by the GUI and later set in Estimator +class EstimatorGUIData { + public: + // constructor + EstimatorGUIData() = default; + + // destructor + ~EstimatorGUIData() = default; + + // Initialize + void Initialize(const mjModel* model, int nprocess, int nsensor); + + // time step + double timestep; + + // integrator + int integrator; + + // process noise + std::vector process_noise; + + // sensor noise + std::vector sensor_noise; + + // estimation horizon + int horizon; +}; + +} // namespace mjpc + +#endif // MJPC_ESTIMATORS_GUI_H_ diff --git a/mjpc/estimators/include.cc b/mjpc/estimators/include.cc index f8717ab31..dfb0386d9 100644 --- a/mjpc/estimators/include.cc +++ b/mjpc/estimators/include.cc @@ -20,7 +20,10 @@ namespace mjpc { -const char kEstimatorNames[] = "Batch"; +const char kEstimatorNames[] = + "Ground Truth\n" + "Kalman\n" + "Batch"; // load all available estimators std::vector> LoadEstimators() { @@ -28,7 +31,9 @@ std::vector> LoadEstimators() { std::vector> estimators; // add estimators - estimators.emplace_back(new mjpc::Batch(1)); // filter mode + estimators.emplace_back(new mjpc::GroundTruth()); // ground truth state + estimators.emplace_back(new mjpc::Kalman()); // extended Kalman filter + estimators.emplace_back(new mjpc::Batch(1)); // recursive batch filter return estimators; } diff --git a/mjpc/estimators/include.h b/mjpc/estimators/include.h index 0cf03d6c0..73f82596d 100644 --- a/mjpc/estimators/include.h +++ b/mjpc/estimators/include.h @@ -19,6 +19,7 @@ #include #include "mjpc/estimators/estimator.h" +#include "mjpc/estimators/kalman.h" namespace mjpc { diff --git a/mjpc/estimators/kalman.cc b/mjpc/estimators/kalman.cc new file mode 100644 index 000000000..11865561e --- /dev/null +++ b/mjpc/estimators/kalman.cc @@ -0,0 +1,566 @@ +// Copyright 2023 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "mjpc/estimators/kalman.h" + +#include +#include +#include + +#include + +#include "mjpc/array_safety.h" +#include "mjpc/estimators/estimator.h" +#include "mjpc/estimators/gui.h" +#include "mjpc/utilities.h" + +namespace mjpc { +namespace mju = ::mujoco::util_mjpc; + +// initialize +void Kalman::Initialize(const mjModel* model) { + // model + if (this->model) mj_deleteModel(this->model); + this->model = mj_copyModel(nullptr, model); + + // data + if (this->data_) mj_deleteData(this->data_); + data_ = mj_makeData(model); + + // timestep + this->model->opt.timestep = GetNumberOrDefault(this->model->opt.timestep, + model, "estimator_timestep"); + + // dimension + nstate_ = model->nq + model->nv + model->na; + ndstate_ = 2 * model->nv + model->na; + + // sensor start index + sensor_start_ = GetNumberOrDefault(0, model, "estimator_sensor_start"); + + // number of sensors + nsensor_ = + GetNumberOrDefault(model->nsensor, model, "estimator_number_sensor"); + + // sensor dimension + nsensordata_ = 0; + for (int i = 0; i < nsensor_; i++) { + nsensordata_ += model->sensor_dim[sensor_start_ + i]; + } + + // sensor start index + sensor_start_index_ = 0; + for (int i = 0; i < sensor_start_; i++) { + sensor_start_index_ += model->sensor_dim[i]; + } + + // state + state.resize(nstate_); + + // covariance + covariance.resize(ndstate_ * ndstate_); + + // process noise + noise_process.resize(ndstate_); + + // sensor noise + noise_sensor.resize(nsensordata_); + + // dynamics Jacobian + dynamics_jacobian_.resize(ndstate_ * ndstate_); + + // sensor Jacobian + sensor_jacobian_.resize(model->nsensordata * ndstate_); + + // sensor error + sensor_error_.resize(nsensordata_); + + // correction + correction_.resize(ndstate_); + + // scratch + tmp0_.resize(ndstate_ * nsensordata_); + tmp1_.resize(nsensordata_ * nsensordata_); + tmp2_.resize(nsensordata_ * ndstate_); + tmp3_.resize(ndstate_ * ndstate_); +} + +// reset memory +void Kalman::Reset(const mjData* data) { + // dimension + int nq = model->nq, nv = model->nv, na = model->na; + + if (data) { + // state + mju_copy(state.data(), data->qpos, nq); + mju_copy(state.data() + nq, data->qvel, nv); + mju_copy(state.data() + nq + nv, data->act, na); + time = data->time; + } else { + // set home keyframe + int home_id = mj_name2id(model, mjOBJ_KEY, "home"); + if (home_id >= 0) mj_resetDataKeyframe(model, data_, home_id); + + // state + mju_copy(state.data(), data_->qpos, nq); + mju_copy(state.data() + nq, data_->qvel, nv); + mju_copy(state.data() + nq + nv, data_->act, na); + time = data_->time; + } + + // covariance + mju_eye(covariance.data(), ndstate_); + double covariance_scl = + GetNumberOrDefault(1.0e-4, model, "estimator_covariance_initial_scale"); + mju_scl(covariance.data(), covariance.data(), covariance_scl, + ndstate_ * ndstate_); + + // process noise + double noise_process_scl = + GetNumberOrDefault(1.0e-4, model, "estimator_process_noise_scale"); + std::fill(noise_process.begin(), noise_process.end(), noise_process_scl); + + // sensor noise + double noise_sensor_scl = + GetNumberOrDefault(1.0e-4, model, "estimator_sensor_noise_scale"); + std::fill(noise_sensor.begin(), noise_sensor.end(), noise_sensor_scl); + + // dynamics Jacobian + mju_zero(dynamics_jacobian_.data(), ndstate_ * ndstate_); + + // sensor Jacobian + mju_zero(sensor_jacobian_.data(), model->nsensordata * ndstate_); + + // sensor error + mju_zero(sensor_error_.data(), nsensordata_); + + // correction + mju_zero(correction_.data(), ndstate_); + + // timer + timer_measurement_ = 0.0; + timer_prediction_ = 0.0; + + // scratch + std::fill(tmp0_.begin(), tmp0_.end(), 0.0); + std::fill(tmp1_.begin(), tmp1_.end(), 0.0); + std::fill(tmp2_.begin(), tmp2_.end(), 0.0); + std::fill(tmp3_.begin(), tmp3_.end(), 0.0); +} + +// update measurement +void Kalman::UpdateMeasurement(const double* ctrl, const double* sensor) { + // start timer + auto start = std::chrono::steady_clock::now(); + + // dimensions + int nq = model->nq, nv = model->nv, na = model->na, nu = model->nu; + + // set state + mju_copy(data_->qpos, state.data(), nq); + mju_copy(data_->qvel, state.data() + nq, nv); + mju_copy(data_->act, state.data() + nq + nv, na); + + // set ctrl + mju_copy(data_->ctrl, ctrl, nu); + + // forward to get sensor + mj_forward(model, data_); + + mju_sub(sensor_error_.data(), sensor + sensor_start_index_, + data_->sensordata + sensor_start_index_, nsensordata_); + + // -- Kalman gain: P * C' (C * P * C' + R)^-1 -- // + + // sensor Jacobian + mjd_transitionFD(model, data_, settings.epsilon, settings.flg_centered, NULL, + NULL, sensor_jacobian_.data(), NULL); + + // grab rows + double* C = sensor_jacobian_.data() + sensor_start_index_ * ndstate_; + + // P * C' = tmp0 + mju_mulMatMatT(tmp0_.data(), covariance.data(), C, ndstate_, ndstate_, + nsensordata_); + + // C * P * C' = C * tmp0 = tmp1 + mju_mulMatMat(tmp1_.data(), C, tmp0_.data(), nsensordata_, ndstate_, + nsensordata_); + + // C * P * C' + R + for (int i = 0; i < nsensordata_; i++) { + tmp1_[nsensordata_ * i + i] += noise_sensor[i]; + } + + // factorize: C * P * C' + R + int rank = mju_cholFactor(tmp1_.data(), nsensordata_, 0.0); + if (rank < nsensordata_) { + // TODO(taylor): remove and return status + mju_error("measurement update rank: (%i / %i)\n", rank, nsensordata_); + } + + // -- correction: (P * C') * (C * P * C' + R)^-1 * sensor_error -- // + + // tmp2 = (C * P * C' + R) \ sensor_error + mju_cholSolve(tmp2_.data(), tmp1_.data(), sensor_error_.data(), nsensordata_); + + // correction = (P * C') * (C * P * C' + R) \ sensor_error = tmp0 * tmp2 + mju_mulMatVec(correction_.data(), tmp0_.data(), tmp2_.data(), ndstate_, + nsensordata_); + + // -- state update -- // + + // configuration + mj_integratePos(model, state.data(), correction_.data(), 1.0); + + // velocity + act + mju_addTo(state.data() + nq, correction_.data() + nv, nv + na); + + // -- covariance update -- // + // TODO(taylor): Joseph form update ? + + // tmp2 = (C * P * C' + R)^-1 (C * P) = tmp1 \ tmp0' + for (int i = 0; i < ndstate_; i++) { + mju_cholSolve(tmp2_.data() + nsensordata_ * i, tmp1_.data(), + tmp0_.data() + nsensordata_ * i, nsensordata_); + } + + // tmp3 = (P * C') * (C * P * C' + R)^-1 (C * P) = tmp0 * tmp2' + mju_mulMatMatT(tmp3_.data(), tmp0_.data(), tmp2_.data(), ndstate_, + nsensordata_, ndstate_); + + // covariance -= tmp3 + mju_subFrom(covariance.data(), tmp3_.data(), ndstate_ * ndstate_); + + // symmetrize + mju_symmetrize(covariance.data(), covariance.data(), ndstate_); + + // stop timer (ms) + timer_measurement_ = 1.0e-3 * GetDuration(start); +} + +// update time +void Kalman::UpdatePrediction() { + // start timer + auto start = std::chrono::steady_clock::now(); + + // dimensions + int nq = model->nq, nv = model->nv, na = model->na; + + // set state + mju_copy(data_->qpos, state.data(), nq); + mju_copy(data_->qvel, state.data() + nq, nv); + mju_copy(data_->act, state.data() + nq + nv, na); + + // dynamics Jacobian + mjd_transitionFD(model, data_, settings.epsilon, settings.flg_centered, + dynamics_jacobian_.data(), NULL, NULL, NULL); + + // integrate state + mj_step(model, data_); + + // update state + mju_copy(state.data(), data_->qpos, nq); + mju_copy(state.data() + nq, data_->qvel, nv); + mju_copy(state.data() + nq + nv, data_->act, na); + + // -- update covariance: P = A * P * A' -- // + + // tmp = P * A' + mju_mulMatMatT(tmp3_.data(), covariance.data(), dynamics_jacobian_.data(), + ndstate_, ndstate_, ndstate_); + + // P = A * tmp + mju_mulMatMat(covariance.data(), dynamics_jacobian_.data(), tmp3_.data(), + ndstate_, ndstate_, ndstate_); + + // process noise + for (int i = 0; i < ndstate_; i++) { + covariance[ndstate_ * i + i] += noise_process[i]; + } + + // symmetrize + mju_symmetrize(covariance.data(), covariance.data(), ndstate_); + + // stop timer + timer_prediction_ = 1.0e-3 * GetDuration(start); +} + +// estimator-specific GUI elements +void Kalman::GUI(mjUI& ui, EstimatorGUIData& data) { + // ----- estimator ------ // + mjuiDef defEstimator[] = { + {mjITEM_SECTION, "Estimator Settings", 1, nullptr, + "AP"}, // needs new section to satisfy mjMAXUIITEM + {mjITEM_BUTTON, "Reset", 2, nullptr, ""}, + {mjITEM_SLIDERNUM, "Timestep", 2, &data.timestep, "1.0e-3 0.1"}, + {mjITEM_SELECT, "Integrator", 2, &data.integrator, + "Euler\nRK4\nImplicit\nFastImplicit"}, + {mjITEM_END}}; + + // add estimator + mjui_add(&ui, defEstimator); + + // -- process noise -- // + int nv = model->nv; + int process_noise_shift = 0; + mjuiDef defProcessNoise[kMaxProcessNoise + 2]; + + // separator + defProcessNoise[0] = {mjITEM_SEPARATOR, "Process Noise Covariance", 1}; + process_noise_shift++; + + // add UI elements + for (int i = 0; i < DimensionProcess(); i++) { + // element + defProcessNoise[process_noise_shift] = { + mjITEM_SLIDERNUM, "", 2, data.process_noise.data() + i, "1.0e-8 0.01"}; + + // set name + mju::strcpy_arr(defProcessNoise[process_noise_shift].name, ""); + + // shift + process_noise_shift++; + } + + // name UI elements + int jnt_shift = 1; + std::string jnt_name_pos; + std::string jnt_name_vel; + + // loop over joints + for (int i = 0; i < model->njnt; i++) { + int name_jntadr = model->name_jntadr[i]; + std::string jnt_name(model->names + name_jntadr); + + // get joint type + int jnt_type = model->jnt_type[i]; + + // free + switch (jnt_type) { + case mjJNT_FREE: + // position + jnt_name_pos = jnt_name + " (pos 0)"; + mju::strcpy_arr(defProcessNoise[jnt_shift + 0].name, + jnt_name_pos.c_str()); + + jnt_name_pos = jnt_name + " (pos 1)"; + mju::strcpy_arr(defProcessNoise[jnt_shift + 1].name, + jnt_name_pos.c_str()); + + jnt_name_pos = jnt_name + " (pos 2)"; + mju::strcpy_arr(defProcessNoise[jnt_shift + 2].name, + jnt_name_pos.c_str()); + + jnt_name_pos = jnt_name + " (pos 3)"; + mju::strcpy_arr(defProcessNoise[jnt_shift + 3].name, + jnt_name_pos.c_str()); + + jnt_name_pos = jnt_name + " (pos 4)"; + mju::strcpy_arr(defProcessNoise[jnt_shift + 4].name, + jnt_name_pos.c_str()); + + jnt_name_pos = jnt_name + " (pos 5)"; + mju::strcpy_arr(defProcessNoise[jnt_shift + 5].name, + jnt_name_pos.c_str()); + + // velocity + jnt_name_vel = jnt_name + " (vel 0)"; + mju::strcpy_arr(defProcessNoise[nv + jnt_shift + 0].name, + jnt_name_vel.c_str()); + + jnt_name_vel = jnt_name + " (vel 1)"; + mju::strcpy_arr(defProcessNoise[nv + jnt_shift + 1].name, + jnt_name_vel.c_str()); + + jnt_name_vel = jnt_name + " (vel 2)"; + mju::strcpy_arr(defProcessNoise[nv + jnt_shift + 2].name, + jnt_name_vel.c_str()); + + jnt_name_vel = jnt_name + " (vel 3)"; + mju::strcpy_arr(defProcessNoise[nv + jnt_shift + 3].name, + jnt_name_vel.c_str()); + + jnt_name_vel = jnt_name + " (vel 4)"; + mju::strcpy_arr(defProcessNoise[nv + jnt_shift + 4].name, + jnt_name_vel.c_str()); + + jnt_name_vel = jnt_name + " (vel 5)"; + mju::strcpy_arr(defProcessNoise[nv + jnt_shift + 5].name, + jnt_name_vel.c_str()); + + // shift + jnt_shift += 6; + break; + case mjJNT_BALL: + // position + jnt_name_pos = jnt_name + " (pos 0)"; + mju::strcpy_arr(defProcessNoise[jnt_shift + 0].name, + jnt_name_pos.c_str()); + + jnt_name_pos = jnt_name + " (pos 1)"; + mju::strcpy_arr(defProcessNoise[jnt_shift + 1].name, + jnt_name_pos.c_str()); + + jnt_name_pos = jnt_name + " (pos 2)"; + mju::strcpy_arr(defProcessNoise[jnt_shift + 2].name, + jnt_name_pos.c_str()); + + // velocity + jnt_name_vel = jnt_name + " (vel 0)"; + mju::strcpy_arr(defProcessNoise[nv + jnt_shift + 0].name, + jnt_name_vel.c_str()); + + jnt_name_vel = jnt_name + " (vel 1)"; + mju::strcpy_arr(defProcessNoise[nv + jnt_shift + 1].name, + jnt_name_vel.c_str()); + + jnt_name_vel = jnt_name + " (vel 2)"; + mju::strcpy_arr(defProcessNoise[nv + jnt_shift + 2].name, + jnt_name_vel.c_str()); + + // shift + jnt_shift += 3; + break; + case mjJNT_HINGE: + // position + jnt_name_pos = jnt_name + " (pos)"; + mju::strcpy_arr(defProcessNoise[jnt_shift].name, jnt_name_pos.c_str()); + + // velocity + jnt_name_vel = jnt_name + " (vel)"; + mju::strcpy_arr(defProcessNoise[nv + jnt_shift].name, + jnt_name_vel.c_str()); + + // shift + jnt_shift++; + break; + case mjJNT_SLIDE: + // position + jnt_name_pos = jnt_name + " (pos)"; + mju::strcpy_arr(defProcessNoise[jnt_shift].name, jnt_name_pos.c_str()); + + // velocity + jnt_name_vel = jnt_name + " (vel)"; + mju::strcpy_arr(defProcessNoise[nv + jnt_shift].name, + jnt_name_vel.c_str()); + + // shift + jnt_shift++; + break; + } + } + + // loop over act + std::string act_str; + for (int i = 0; i < model->na; i++) { + act_str = "act (" + std::to_string(i) + ")"; + mju::strcpy_arr(defProcessNoise[nv + jnt_shift + i].name, act_str.c_str()); + } + + // end + defProcessNoise[process_noise_shift] = {mjITEM_END}; + + // add process noise + mjui_add(&ui, defProcessNoise); + + // -- sensor noise -- // + int sensor_noise_shift = 0; + mjuiDef defSensorNoise[kMaxSensorNoise + 2]; + + // separator + defSensorNoise[0] = {mjITEM_SEPARATOR, "Sensor Noise Covariance", 1}; + sensor_noise_shift++; + + // loop over sensors + std::string sensor_str; + for (int i = 0; i < nsensor_; i++) { + std::string name_sensor(model->names + + model->name_sensoradr[sensor_start_ + i]); + int dim_sensor = model->sensor_dim[sensor_start_ + i]; + + // loop over sensor elements + for (int j = 0; j < dim_sensor; j++) { + // element + defSensorNoise[sensor_noise_shift] = { + mjITEM_SLIDERNUM, "", 2, + data.sensor_noise.data() + sensor_noise_shift - 1, "1.0e-8 0.01"}; + + // sensor name + sensor_str = name_sensor; + + // add element index + if (dim_sensor > 1) { + sensor_str += " (" + std::to_string(j) + ")"; + } + + // set sensor name + mju::strcpy_arr(defSensorNoise[sensor_noise_shift].name, + sensor_str.c_str()); + + // shift + sensor_noise_shift++; + } + } + + // end + defSensorNoise[sensor_noise_shift] = {mjITEM_END}; + + // add sensor noise + mjui_add(&ui, defSensorNoise); +} + +// set GUI data +void Kalman::SetGUIData(EstimatorGUIData& data) { + mju_copy(noise_process.data(), data.process_noise.data(), DimensionProcess()); + mju_copy(noise_sensor.data(), data.sensor_noise.data(), DimensionSensor()); + model->opt.timestep = data.timestep; + model->opt.integrator = data.integrator; +} + +// estimator-specific plots +void Kalman::Plots(mjvFigure* fig_planner, mjvFigure* fig_timer, + int planner_shift, int timer_shift, int planning, + int* shift) { + // Kalman info + double estimator_bounds[2] = {-6, 6}; + + // covariance trace + double trace = Trace(covariance.data(), DimensionProcess()); + mjpc::PlotUpdateData(fig_planner, estimator_bounds, + fig_planner->linedata[planner_shift + 0][0] + 1, + mju_log10(trace), 100, planner_shift + 0, 0, 1, -100); + + // legend + mju::strcpy_arr(fig_planner->linename[planner_shift + 0], "Covariance Trace"); + + // Kalman timers + double timer_bounds[2] = {0.0, 1.0}; + + // measurement update + PlotUpdateData(fig_timer, timer_bounds, + fig_timer->linedata[timer_shift + 0][0] + 1, + TimerMeasurement(), 100, timer_shift + 0, 0, 1, -100); + + // prediction update + PlotUpdateData(fig_timer, timer_bounds, + fig_timer->linedata[timer_shift + 1][0] + 1, TimerPrediction(), + 100, timer_shift + 1, 0, 1, -100); + + // legend + mju::strcpy_arr(fig_timer->linename[timer_shift + 0], "Measurement Update"); + mju::strcpy_arr(fig_timer->linename[timer_shift + 1], "Prediction Update"); +} + +} // namespace mjpc diff --git a/mjpc/estimators/kalman.h b/mjpc/estimators/kalman.h new file mode 100644 index 000000000..baf23f946 --- /dev/null +++ b/mjpc/estimators/kalman.h @@ -0,0 +1,186 @@ +// Copyright 2023 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MJPC_ESTIMATORS_KALMAN_H_ +#define MJPC_ESTIMATORS_KALMAN_H_ + +#include +#include + +#include + +#include "mjpc/estimators/estimator.h" +#include "mjpc/estimators/gui.h" +#include "mjpc/utilities.h" + +namespace mjpc { + +// https://stanford.edu/class/ee363/lectures/kf.pdf +class Kalman : public Estimator { + public: + // constructor + Kalman() = default; + Kalman(const mjModel* model) { + Initialize(model); + Reset(); + } + + // destructor + ~Kalman() { + if (data_) mj_deleteData(data_); + if (model) mj_deleteModel(model); + } + + // initialize + void Initialize(const mjModel* model) override; + + // reset memory + void Reset(const mjData* data = nullptr) override; + + // update measurement + void UpdateMeasurement(const double* ctrl, const double* sensor); + + // update time + void UpdatePrediction(); + + // update + void Update(const double* ctrl, const double* sensor) override { + // correct state with latest measurement + UpdateMeasurement(ctrl, sensor); + + // propagate state forward in time with model + UpdatePrediction(); + + // set time + time = data_->time; + } + + // get state + double* State() override { return state.data(); }; + + // get covariance + double* Covariance() override { return covariance.data(); }; + + // get time + double& Time() override { return time; }; + + // get model + mjModel* Model() override { return model; }; + + // get data + mjData* Data() override { return data_; }; + + // get process noise + double* ProcessNoise() override { return noise_process.data(); }; + + // get sensor noise + double* SensorNoise() override { return noise_sensor.data(); }; + + // dimension process + int DimensionProcess() const override { return ndstate_; }; + + // dimension sensor + int DimensionSensor() const override { return nsensordata_; }; + + // set state + void SetState(const double* state) override { + mju_copy(this->state.data(), state, ndstate_); + }; + + // set time + void SetTime(double time) override { this->time = time; } + + // set covariance + void SetCovariance(const double* covariance) override { + mju_copy(this->covariance.data(), covariance, ndstate_ * ndstate_); + } + + // get measurement timer (ms) + double TimerMeasurement() const { return timer_measurement_; } + + // get prediction timer (ms) + double TimerPrediction() const { return timer_prediction_; } + + // estimator-specific GUI elements + void GUI(mjUI& ui, EstimatorGUIData& data) override; + + // set GUI data + void SetGUIData(EstimatorGUIData& data) override; + + // estimator-specific plots + void Plots(mjvFigure* fig_planner, mjvFigure* fig_timer, int planner_shift, + int timer_shift, int planning, int* shift) override; + + // model + mjModel* model = nullptr; + + // state (nstate_) + std::vector state; + double time; + + // covariance (ndstate_ x ndstate_) + std::vector covariance; + + // process noise (ndstate_) + std::vector noise_process; + + // sensor noise (nsensordata_) + std::vector noise_sensor; + + // settings + struct Settings { + double epsilon = 1.0e-6; + bool flg_centered = false; + } settings; + + private: + // dimensions + int nstate_; + int ndstate_; + int nsensordata_; + int nsensor_; + + // sensor indexing + int sensor_start_; + int sensor_start_index_; + + // data + mjData* data_ = nullptr; + + // correction (ndstate_) + std::vector correction_; + + // sensor Jacobian (nsensordata x ndstate_) + std::vector sensor_jacobian_; + + // dynamics Jacobian (ndstate_ x ndstate_) + std::vector dynamics_jacobian_; + + // sensor error (nsensordata_) + std::vector sensor_error_; + + // timer (ms) + double timer_measurement_; + double timer_prediction_; + + // scratch + std::vector tmp0_; + std::vector tmp1_; + std::vector tmp2_; + std::vector tmp3_; +}; + +} // namespace mjpc + +#endif // MJPC_ESTIMATORS_KALMAN_H_ diff --git a/mjpc/test/estimator/CMakeLists.txt b/mjpc/test/estimator/CMakeLists.txt index 5b58f03fe..b0a5ce67b 100644 --- a/mjpc/test/estimator/CMakeLists.txt +++ b/mjpc/test/estimator/CMakeLists.txt @@ -29,3 +29,6 @@ target_link_libraries(estimator_trajectory_test gmock) test(estimator_utilities_test) target_link_libraries(estimator_utilities_test load simulation gmock) + +test(kalman_test) +target_link_libraries(kalman_test load simulation gmock) \ No newline at end of file diff --git a/mjpc/test/estimator/estimator_utilities_test.cc b/mjpc/test/estimator/estimator_utilities_test.cc index c72b3b3b5..1dd27064d 100644 --- a/mjpc/test/estimator/estimator_utilities_test.cc +++ b/mjpc/test/estimator/estimator_utilities_test.cc @@ -48,7 +48,7 @@ TEST(FiniteDifferenceVelocityAcceleration, Particle2D) { }; // trajectories - int T = 200; + int T = 10; std::vector qpos(nq * T); std::vector qvel(nv * T); std::vector qacc(nv * T); diff --git a/mjpc/test/estimator/kalman_test.cc b/mjpc/test/estimator/kalman_test.cc new file mode 100644 index 000000000..57a119f3f --- /dev/null +++ b/mjpc/test/estimator/kalman_test.cc @@ -0,0 +1,115 @@ +// Copyright 2023 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "mjpc/estimators/kalman.h" + +#include + +#include +#include + +#include "gtest/gtest.h" +#include "mjpc/estimators/trajectory.h" +#include "mjpc/test/load.h" +#include "mjpc/test/simulation.h" +#include "mjpc/utilities.h" + +namespace mjpc { +namespace { + +TEST(Estimator, Kalman) { + // load model + mjModel* model = LoadTestModel("estimator/particle/task_imu.xml"); + mjData* data = mj_makeData(model); + + // ----- rollout ----- // + int T = 200; + Simulation sim(model, T); + auto controller = [](double* ctrl, double time) {}; + double qpos0[1] = {0.25}; + sim.SetState(qpos0, NULL); + sim.Rollout(controller); + + // ----- Kalman ----- // + + // initialize filter + Kalman kalman(model); + + // set initial state + mju_copy(kalman.state.data(), sim.qpos.Get(0), model->nq); + mju_copy(kalman.state.data() + model->nq, sim.qvel.Get(0), model->nv); + + // initialize covariance + mju_eye(kalman.covariance.data(), 2 * model->nv); + mju_scl(kalman.covariance.data(), kalman.covariance.data(), 1.0e-5, + (2 * model->nv) * (2 * model->nv)); + + // initial process noise + mju_fill(kalman.noise_process.data(), 1.0e-5, 2 * model->nv); + + // initialize sensor noise + mju_fill(kalman.noise_sensor.data(), 1.0e-5, model->nsensordata); + + // Kalman trajectories + EstimatorTrajectory kalman_qpos(model->nq, T); + EstimatorTrajectory kalman_qvel(model->nv, T); + EstimatorTrajectory kalman_timer_measurement(1, T); + EstimatorTrajectory kalman_timer_prediction(1, T); + + // noisy sensor + std::vector noisy_sensor(model->nsensordata); + absl::BitGen gen_; + + for (int t = 0; t < T; t++) { + // noisy sensor + mju_copy(noisy_sensor.data(), sim.sensor.Get(t), model->nsensordata); + for (int i = 0; i < model->nsensordata; i++) { + noisy_sensor[i] += 0.0 * absl::Gaussian(gen_, 0.0, 1.0); + } + // measurement update + kalman.UpdateMeasurement(sim.ctrl.Get(t), noisy_sensor.data()); + + // cache state + kalman_qpos.Set(kalman.state.data(), t); + kalman_qvel.Set(kalman.state.data() + model->nq, t); + + // cache timer + double timer_measurement = kalman.TimerMeasurement(); + kalman_timer_measurement.Set(&timer_measurement, t); + + // prediction update + kalman.UpdatePrediction(); + + // cache timer + double timer_prediction = kalman.TimerPrediction(); + kalman_timer_prediction.Set(&timer_prediction, t); + + // test qpos + std::vector pos_error(model->nq); + mju_sub(pos_error.data(), kalman_qpos.Get(t), sim.qpos.Get(t), model->nq); + EXPECT_NEAR(mju_norm(pos_error.data(), model->nq), 0.0, 1.0e-4); + + // test qvel + std::vector vel_error(model->nv); + mju_sub(vel_error.data(), kalman_qvel.Get(t), sim.qvel.Get(t), model->nv); + EXPECT_NEAR(mju_norm(vel_error.data(), model->nv), 0.0, 1.0e-4); + } + + // delete data + model + mj_deleteData(data); + mj_deleteModel(model); +} + +} // namespace +} // namespace mjpc diff --git a/mjpc/test/testdata/estimator/box/task3Drot.xml b/mjpc/test/testdata/estimator/box/task3Drot.xml new file mode 100644 index 000000000..448c1f1fe --- /dev/null +++ b/mjpc/test/testdata/estimator/box/task3Drot.xml @@ -0,0 +1,65 @@ + + diff --git a/mjpc/test/testdata/estimator/box/task3Drot2.xml b/mjpc/test/testdata/estimator/box/task3Drot2.xml new file mode 100644 index 000000000..8a14a7ca3 --- /dev/null +++ b/mjpc/test/testdata/estimator/box/task3Drot2.xml @@ -0,0 +1,75 @@ + + diff --git a/mjpc/test/testdata/estimator/particle/task_imu.xml b/mjpc/test/testdata/estimator/particle/task_imu.xml new file mode 100644 index 000000000..611f63b38 --- /dev/null +++ b/mjpc/test/testdata/estimator/particle/task_imu.xml @@ -0,0 +1,56 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/mjpc/test/testdata/estimator/quadruped/task.xml b/mjpc/test/testdata/estimator/quadruped/task.xml index c8e1141c8..27d38f8f9 100644 --- a/mjpc/test/testdata/estimator/quadruped/task.xml +++ b/mjpc/test/testdata/estimator/quadruped/task.xml @@ -1,5 +1,4 @@ - @@ -31,10 +30,6 @@ - - - - diff --git a/mjpc/test/testdata/estimator/tblock/task.xml b/mjpc/test/testdata/estimator/tblock/task.xml deleted file mode 100644 index 42eefedf7..000000000 --- a/mjpc/test/testdata/estimator/tblock/task.xml +++ /dev/null @@ -1,32 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/python/mujoco_mpc/batch.py b/python/mujoco_mpc/batch.py index 644ef2430..a8383535b 100644 --- a/python/mujoco_mpc/batch.py +++ b/python/mujoco_mpc/batch.py @@ -504,7 +504,7 @@ def status_code(code): elif code == 3: return "SMALL_DIRECTION_FAILURE" elif code == 4: - return "MAX_REGULARIZATION_FAILURE" + return "kMaxBatchRegularization_FAILURE" elif code == 5: return "COST_DIFFERENCE_FAILURE" elif code == 6: diff --git a/python/setup_batch.py b/python/setup_batch.py index cbc45767a..bd07a3b66 100644 --- a/python/setup_batch.py +++ b/python/setup_batch.py @@ -59,9 +59,7 @@ def run(self): from grpc_tools import protoc # pylint: disable=import-outside-toplevel batch_proto_filename = "batch.proto" - batch_proto_source_path = Path( - "..", "grpc", batch_proto_filename - ).resolve() + batch_proto_source_path = Path("..", "grpc", batch_proto_filename).resolve() assert self.build_lib is not None build_lib_path = Path(self.build_lib).resolve() proto_module_relative_path = Path(