|
| 1 | +// Copyright 2024 DeepMind Technologies Limited |
| 2 | +// |
| 3 | +// Licensed under the Apache License, Version 2.0 (the "License"); |
| 4 | +// you may not use this file except in compliance with the License. |
| 5 | +// You may obtain a copy of the License at |
| 6 | +// |
| 7 | +// http://www.apache.org/licenses/LICENSE-2.0 |
| 8 | +// |
| 9 | +// Unless required by applicable law or agreed to in writing, software |
| 10 | +// distributed under the License is distributed on an "AS IS" BASIS, |
| 11 | +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| 12 | +// See the License for the specific language governing permissions and |
| 13 | +// limitations under the License. |
| 14 | + |
| 15 | +#include "mjpc/testspeed.h" |
| 16 | + |
| 17 | +#include <chrono> |
| 18 | +#include <cmath> |
| 19 | +#include <iostream> |
| 20 | +#include <string> |
| 21 | +#include <vector> |
| 22 | + |
| 23 | +#include <mujoco/mujoco.h> |
| 24 | + |
| 25 | +#include "mjpc/agent.h" |
| 26 | +#include "mjpc/states/state.h" |
| 27 | +#include "mjpc/task.h" |
| 28 | +#include "mjpc/threadpool.h" |
| 29 | +#include "mjpc/utilities.h" |
| 30 | +#include "mjpc/tasks/tasks.h" |
| 31 | + |
| 32 | +namespace mjpc { |
| 33 | + |
| 34 | +namespace { |
| 35 | +Task* task; |
| 36 | +void residual_callback(const mjModel* model, mjData* data, int stage) { |
| 37 | + if (stage == mjSTAGE_ACC) { |
| 38 | + task->Residual(model, data, data->sensordata); |
| 39 | + } |
| 40 | +} |
| 41 | +} // namespace |
| 42 | + |
| 43 | +// Run synchronous planning, print timing info,return 0 if nothing failed. |
| 44 | +int TestSpeed(std::string task_name, int planner_thread_count, |
| 45 | + int steps_per_planning_iteration, double total_time) { |
| 46 | + std::cout << "Test MJPC Speed\n"; |
| 47 | + std::cout << " MuJoCo version " << mj_versionString() << "\n"; |
| 48 | + if (mjVERSION_HEADER != mj_version()) { |
| 49 | + mju_error("Headers and library have Different versions"); |
| 50 | + } |
| 51 | + std::cout << " Hardware threads: " << NumAvailableHardwareThreads() << "\n"; |
| 52 | + |
| 53 | + Agent agent; |
| 54 | + agent.SetTaskList(GetTasks()); |
| 55 | + agent.gui_task_id = agent.GetTaskIdByName(task_name); |
| 56 | + if (agent.gui_task_id == -1) { |
| 57 | + std::cerr << "Invalid --task flag: '" << task_name |
| 58 | + << "'. Valid values:\n"; |
| 59 | + std::cerr << agent.GetTaskNames(); |
| 60 | + return -1; |
| 61 | + } |
| 62 | + auto load_model = agent.LoadModel(); |
| 63 | + mjModel* model = load_model.model.release(); |
| 64 | + if (!model) { |
| 65 | + std::cerr << load_model.error << "\n"; |
| 66 | + return 1; |
| 67 | + } |
| 68 | + mjData* data = mj_makeData(model); |
| 69 | + mj_forward(model, data); |
| 70 | + |
| 71 | + int home_id = mj_name2id(model, mjOBJ_KEY, "home"); |
| 72 | + if (home_id >= 0) { |
| 73 | + std::cout << "home_id: " << home_id << "\n"; |
| 74 | + mj_resetDataKeyframe(model, data, home_id); |
| 75 | + } |
| 76 | + |
| 77 | + // the planner and its initial configuration is set in the XML |
| 78 | + agent.estimator_enabled = false; |
| 79 | + agent.Initialize(model); |
| 80 | + agent.Allocate(); |
| 81 | + agent.Reset(data->ctrl); |
| 82 | + agent.plan_enabled = true; |
| 83 | + |
| 84 | + // make task available for global callback: |
| 85 | + task = agent.ActiveTask(); |
| 86 | + mjcb_sensor = &residual_callback; |
| 87 | + |
| 88 | + std::cout << " Planning threads: " << planner_thread_count << "\n"; |
| 89 | + ThreadPool pool(planner_thread_count); |
| 90 | + |
| 91 | + int total_steps = ceil(total_time / model->opt.timestep); |
| 92 | + int current_time = 0; |
| 93 | + double total_cost = 0; |
| 94 | + auto loop_start = std::chrono::steady_clock::now(); |
| 95 | + for (int i = 0; i < total_steps; i++) { |
| 96 | + agent.ActiveTask()->Transition(model, data); |
| 97 | + agent.state.Set(model, data); |
| 98 | + |
| 99 | + agent.ActivePlanner().ActionFromPolicy( |
| 100 | + data->ctrl, agent.state.state().data(), |
| 101 | + agent.state.time(), /*use_previous=*/false); |
| 102 | + mj_step(model, data); |
| 103 | + double cost = agent.ActiveTask()->CostValue(data->sensordata); |
| 104 | + total_cost += cost; |
| 105 | + |
| 106 | + if (i % steps_per_planning_iteration == 0) { agent.PlanIteration(&pool); } |
| 107 | + |
| 108 | + if (floor(data->time) > current_time) { |
| 109 | + current_time++; |
| 110 | + std::cout << "sim time: " << current_time << ", cost: " << cost << "\n"; |
| 111 | + } |
| 112 | + } |
| 113 | + auto wall_run_time = std::chrono::duration_cast<std::chrono::microseconds>( |
| 114 | + std::chrono::steady_clock::now() - loop_start) |
| 115 | + .count() / |
| 116 | + 1e6; |
| 117 | + std::cout << "Total wall time (" |
| 118 | + << (int)ceil(total_steps / steps_per_planning_iteration) |
| 119 | + << " planning steps): " << wall_run_time << " s (" |
| 120 | + << total_time / wall_run_time << "x realtime)\n"; |
| 121 | + std::cout << "Average cost per step (lower is better): " |
| 122 | + << total_cost / total_steps << "\n"; |
| 123 | + |
| 124 | + mj_deleteData(data); |
| 125 | + mj_deleteModel(model); |
| 126 | + return 0; |
| 127 | +} |
| 128 | +} // namespace mjpc |
0 commit comments