|
| 1 | +// Copyright 2022 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/tasks/cube/solve.h" |
| 16 | + |
| 17 | +#include <random> |
| 18 | +#include <string> |
| 19 | + |
| 20 | +#include <absl/log/check.h> |
| 21 | +#include <absl/log/log.h> |
| 22 | +#include <mujoco/mujoco.h> |
| 23 | +#include "mjpc/task.h" |
| 24 | +#include "mjpc/utilities.h" |
| 25 | + |
| 26 | +namespace mjpc { |
| 27 | +std::string CubeSolve::XmlPath() const { return GetModelPath("cube/task.xml"); } |
| 28 | +std::string CubeSolve::Name() const { return "Cube Solving"; } |
| 29 | + |
| 30 | +// ---------- Residuals for cube solving manipulation task ---- |
| 31 | +// Number of residuals: |
| 32 | +// ------------------------------------------------------------ |
| 33 | +void CubeSolve::ResidualFn::Residual(const mjModel* model, const mjData* data, |
| 34 | + double* residual) const { |
| 35 | + // initialize counter |
| 36 | + int counter = 0; |
| 37 | + |
| 38 | + // lock current mode |
| 39 | + int mode = current_mode_; |
| 40 | + |
| 41 | + // ---------- Residual (0) ---------- |
| 42 | + // goal position |
| 43 | + double* goal_position = SensorByName(model, data, "palm_position"); |
| 44 | + |
| 45 | + // system's position |
| 46 | + double* position = SensorByName(model, data, "cube_position"); |
| 47 | + |
| 48 | + // position error |
| 49 | + mju_sub3(residual + counter, position, goal_position); |
| 50 | + counter += 3; |
| 51 | + |
| 52 | + // ---------- Residual (1) ---------- |
| 53 | + // goal orientation |
| 54 | + double* goal_orientation = SensorByName(model, data, "cube_goal_orientation"); |
| 55 | + |
| 56 | + // system's orientation |
| 57 | + double* orientation = SensorByName(model, data, "cube_orientation"); |
| 58 | + mju_normalize4(goal_orientation); |
| 59 | + |
| 60 | + // orientation error |
| 61 | + mju_subQuat(residual + counter, goal_orientation, orientation); |
| 62 | + counter += 3; |
| 63 | + |
| 64 | + // ---------- Residual (2) ---------- |
| 65 | + double* cube_linear_velocity = |
| 66 | + SensorByName(model, data, "cube_linear_velocity"); |
| 67 | + mju_copy(residual + counter, cube_linear_velocity, 3); |
| 68 | + counter += 3; |
| 69 | + |
| 70 | + // ---------- Residual (3) ---------- |
| 71 | + mju_copy(residual + counter, data->actuator_force, model->nu); |
| 72 | + counter += model->nu; |
| 73 | + |
| 74 | + // ---------- Residual (3) ---------- |
| 75 | + if (mode == kModeManual || mode == kModeSolve) { |
| 76 | + residual[counter + 0] = data->qpos[11] - parameters_[0]; // red |
| 77 | + residual[counter + 1] = data->qpos[12] - parameters_[1]; // orange |
| 78 | + residual[counter + 2] = data->qpos[13] - parameters_[2]; // blue |
| 79 | + residual[counter + 3] = data->qpos[14] - parameters_[3]; // green |
| 80 | + residual[counter + 4] = data->qpos[15] - parameters_[4]; // white |
| 81 | + residual[counter + 5] = data->qpos[16] - parameters_[5]; // yellow |
| 82 | + } else { |
| 83 | + mju_zero(residual + counter, 6); |
| 84 | + } |
| 85 | + counter += 6; |
| 86 | + |
| 87 | + // ---------- Residual (4) ---------- |
| 88 | + mju_sub(residual + counter, data->qpos + 97, model->key_qpos + 97, 24); |
| 89 | + counter += 24; |
| 90 | + |
| 91 | + // ---------- Residual (5) ---------- |
| 92 | + mju_copy(residual + counter, data->qvel + 97, 24); |
| 93 | + counter += 24; |
| 94 | + |
| 95 | + // sensor dim sanity check |
| 96 | + CheckSensorDim(model, counter); |
| 97 | +} |
| 98 | + |
| 99 | +// ----- Transition for cube solving manipulation task ----- |
| 100 | +// If cube is within tolerance or floor -> |
| 101 | +// reset cube into hand. |
| 102 | +// --------------------------------------------------------- |
| 103 | +void CubeSolve::TransitionLocked(mjModel* model, mjData* data) { |
| 104 | + if (transition_model_) { |
| 105 | + if (mode == kModeWait) { |
| 106 | + // wait |
| 107 | + } else if (mode == kModeScramble) { // scramble |
| 108 | + double scramble_param = parameters[6]; |
| 109 | + int num_scramble = ReinterpretAsInt(scramble_param) + 1; |
| 110 | + |
| 111 | + // reset |
| 112 | + mju_copy(data->qpos, model->qpos0, model->nq); |
| 113 | + mj_resetData(transition_model_, transition_data_); |
| 114 | + |
| 115 | + // resize |
| 116 | + face_.resize(num_scramble); |
| 117 | + direction_.resize(num_scramble); |
| 118 | + goal_cache_.resize(6 * num_scramble); |
| 119 | + |
| 120 | + // set transition model |
| 121 | + for (int i = 0; i < num_scramble; i++) { |
| 122 | + // copy goal face orientations |
| 123 | + mju_copy(goal_cache_.data() + i * 6, transition_data_->qpos, 6); |
| 124 | + |
| 125 | + // zero out noise |
| 126 | + for (int j = 0; j < 6; j++) { |
| 127 | + double val = goal_cache_[i * 6 + j]; |
| 128 | + if (mju_abs(val) < 1.0e-4) { |
| 129 | + goal_cache_[i * 6 + j] = 0.0; |
| 130 | + } |
| 131 | + if (val < 0.5 * mjPI * 1.1 && val > 0.5 * mjPI * 0.9) { |
| 132 | + goal_cache_[i * 6 + j] = 0.5 * mjPI; |
| 133 | + } |
| 134 | + if (val < -0.5 * mjPI * 1.1 && val > -0.5 * mjPI * 0.9) { |
| 135 | + goal_cache_[i * 6 + j] = 0.5 * mjPI; |
| 136 | + } |
| 137 | + } |
| 138 | + |
| 139 | + // random face + direction |
| 140 | + std::random_device rd; // Only used once to initialise (seed) engine |
| 141 | + std::mt19937 rng( |
| 142 | + rd()); // Random-number engine used (Mersenne-Twister in this case) |
| 143 | + |
| 144 | + std::uniform_int_distribution<int> uni_face(0, |
| 145 | + 5); // Guaranteed unbiased |
| 146 | + face_[i] = uni_face(rng); |
| 147 | + |
| 148 | + std::uniform_int_distribution<int> uni_direction( |
| 149 | + 0, 1); // Guaranteed unbiased |
| 150 | + direction_[i] = uni_direction(rng); |
| 151 | + if (direction_[i] == 0) { |
| 152 | + direction_[i] = -1; |
| 153 | + } |
| 154 | + |
| 155 | + // set |
| 156 | + for (int t = 0; t < 2000; t++) { |
| 157 | + transition_data_->ctrl[face_[i]] = direction_[i] * 1.57 * t / 2000; |
| 158 | + mj_step(transition_model_, transition_data_); |
| 159 | + mju_copy(data->qpos + 11, transition_data_->qpos, 86); |
| 160 | + } |
| 161 | + } |
| 162 | + |
| 163 | + // set face goal index |
| 164 | + goal_index_ = num_scramble - 1; |
| 165 | + |
| 166 | + // set to wait |
| 167 | + mode = 0; |
| 168 | + } |
| 169 | + |
| 170 | + if (mode == kModeSolve) { // solve |
| 171 | + // set goal |
| 172 | + mju_copy(parameters.data(), goal_cache_.data() + 6 * goal_index_, 6); |
| 173 | + |
| 174 | + // check error |
| 175 | + double error[6]; |
| 176 | + mju_sub(error, data->qpos + 11, parameters.data(), 6); |
| 177 | + |
| 178 | + if (mju_norm(error, 6) < 0.085) { |
| 179 | + if (goal_index_ == 0) { |
| 180 | + // return to wait |
| 181 | + printf("solved!"); |
| 182 | + mode = 0; |
| 183 | + } else { |
| 184 | + goal_index_--; |
| 185 | + } |
| 186 | + } |
| 187 | + } |
| 188 | + } |
| 189 | + |
| 190 | + // check for drop |
| 191 | + if (data->qpos[6] < kResetHeight) { |
| 192 | + // reset cube position + orientation |
| 193 | + mju_copy(data->qpos, model->key_qpos, 7); |
| 194 | + |
| 195 | + // reset cube velocity |
| 196 | + mju_zero(data->qvel, 6); |
| 197 | + } |
| 198 | + |
| 199 | + // check for mode change |
| 200 | + if (residual_.current_mode_ != mode) { |
| 201 | + // update mode for residual |
| 202 | + residual_.current_mode_ = mode; |
| 203 | + } |
| 204 | +} |
| 205 | + |
| 206 | +} // namespace mjpc |
0 commit comments