diff --git a/docs/OVERVIEW.md b/docs/OVERVIEW.md index d330ed752..c9d050ec3 100644 --- a/docs/OVERVIEW.md +++ b/docs/OVERVIEW.md @@ -224,7 +224,7 @@ The swimmer's cost has two terms: The repository includes additional example tasks: -- Humanoid [Stand](../mjpc/tasks/humanoid/task_stand.xml) | [Walk](../mjpc/tasks/humanoid/task_walk.xml) +- Humanoid [Stand](../mjpc/tasks/humanoid/stand/task.xml) | [Walk](../mjpc/tasks/humanoid/walk/task.xml) - Quadruped [Terrain](../mjpc/tasks/quadruped/task_hill.xml) | [Flat](../mjpc/tasks/quadruped/task_flat.xml) - [Walker](../mjpc/tasks/walker/task.xml) - [In-Hand Manipulation](../mjpc/tasks/hand/task.xml) diff --git a/mjpc/CMakeLists.txt b/mjpc/CMakeLists.txt index 31b97462f..be5734c0e 100644 --- a/mjpc/CMakeLists.txt +++ b/mjpc/CMakeLists.txt @@ -230,8 +230,10 @@ add_library( tasks/cartpole/cartpole.h tasks/hand/hand.cc tasks/hand/hand.h - tasks/humanoid/humanoid.cc - tasks/humanoid/humanoid.h + tasks/humanoid/stand/task.cc + tasks/humanoid/stand/task.h + tasks/humanoid/walk/task.cc + tasks/humanoid/walk/task.h tasks/panda/panda.cc tasks/panda/panda.h tasks/particle/particle.cc diff --git a/mjpc/planners/ilqg/backward_pass.cc b/mjpc/planners/ilqg/backward_pass.cc index 47589562b..c83b9d5e1 100644 --- a/mjpc/planners/ilqg/backward_pass.cc +++ b/mjpc/planners/ilqg/backward_pass.cc @@ -170,7 +170,7 @@ int iLQGBackwardPass::RiccatiStep( boxqp.H.data(), boxqp.g.data(), m, boxqp.lower.data(), boxqp.upper.data()); if (mFree < 0) { - // printf("backward_pass failure\n"); + printf("backward_pass failure\n"); return 0; } @@ -198,7 +198,12 @@ int iLQGBackwardPass::RiccatiStep( } else { // Quut^-1 mju_copy(tmp3, Quu_reg, m * m); - mju_cholFactor(tmp3, m, 0.0); + int rank = mju_cholFactor(tmp3, m, 0.0); + + if (rank < m) { + printf("backward pass failure\n"); + return 0; + } // Kt = - Quut \ Qxut for (int i = 0; i < n; i++) { diff --git a/mjpc/tasks/humanoid/stand/task.cc b/mjpc/tasks/humanoid/stand/task.cc new file mode 100644 index 000000000..850bcf964 --- /dev/null +++ b/mjpc/tasks/humanoid/stand/task.cc @@ -0,0 +1,101 @@ +// 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 "tasks/humanoid/stand/task.h" + +#include + +#include +#include "utilities.h" + + +namespace mjpc { + +// ------------------ Residuals for humanoid stand task ------------ +// Number of residuals: 6 +// Residual (0): Desired height +// Residual (1): Balance: COM_xy - average(feet position)_xy +// Residual (2): Com Vel: should be 0 and equal feet average vel +// Residual (3): Control: minimise control +// Residual (4): Joint vel: minimise joint velocity +// Number of parameters: 1 +// Parameter (0): height_goal +// ---------------------------------------------------------------- +void humanoid::Stand::Residual(const double* parameters, const mjModel* model, + const mjData* data, double* residual) { + int counter = 0; + + // ----- Height: head feet vertical error ----- // + + // feet sensor positions + double* f1_position = mjpc::SensorByName(model, data, "sp0"); + double* f2_position = mjpc::SensorByName(model, data, "sp1"); + double* f3_position = mjpc::SensorByName(model, data, "sp2"); + double* f4_position = mjpc::SensorByName(model, data, "sp3"); + double* head_position = mjpc::SensorByName(model, data, "head_position"); + double head_feet_error = + head_position[2] - 0.25 * (f1_position[2] + f2_position[2] + + f3_position[2] + f4_position[2]); + residual[counter++] = head_feet_error - parameters[0]; + + // ----- Balance: CoM-feet xy error ----- // + + // capture point + double* com_position = mjpc::SensorByName(model, data, "torso_subtreecom"); + double* com_velocity = mjpc::SensorByName(model, data, "torso_subtreelinvel"); + double kFallTime = 0.2; + double capture_point[3] = {com_position[0], com_position[1], com_position[2]}; + mju_addToScl3(capture_point, com_velocity, kFallTime); + + // average feet xy position + double fxy_avg[2] = {0.0}; + mju_addTo(fxy_avg, f1_position, 2); + mju_addTo(fxy_avg, f2_position, 2); + mju_addTo(fxy_avg, f3_position, 2); + mju_addTo(fxy_avg, f4_position, 2); + mju_scl(fxy_avg, fxy_avg, 0.25, 2); + + mju_subFrom(fxy_avg, capture_point, 2); + double com_feet_distance = mju_norm(fxy_avg, 2); + residual[counter++] = com_feet_distance; + + // ----- COM xy velocity should be 0 ----- // + mju_copy(&residual[counter], com_velocity, 2); + counter += 2; + + // ----- joint velocity ----- // + mju_copy(residual + counter, data->qvel + 6, model->nv - 6); + counter += model->nv - 6; + + // ----- action ----- // + mju_copy(&residual[counter], data->ctrl, model->nu); + counter += model->nu; + + // sensor dim sanity check + // TODO: use this pattern everywhere and make this a utility function + int user_sensor_dim = 0; + for (int i = 0; i < model->nsensor; i++) { + if (model->sensor_type[i] == mjSENS_USER) { + user_sensor_dim += model->sensor_dim[i]; + } + } + if (user_sensor_dim != counter) { + mju_error_i( + "mismatch between total user-sensor dimension " + "and actual length of residual %d", + counter); + } +} + +} // namespace mjpc diff --git a/mjpc/tasks/humanoid/stand/task.h b/mjpc/tasks/humanoid/stand/task.h new file mode 100644 index 000000000..4386cb559 --- /dev/null +++ b/mjpc/tasks/humanoid/stand/task.h @@ -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. + +#ifndef MJPC_TASKS_HUMANOID_STAND_TASK_H_ +#define MJPC_TASKS_HUMANOID_STAND_TASK_H_ + +#include + +namespace mjpc { +namespace humanoid { + +struct Stand { + + // ------------------ Residuals for humanoid stand task ------------ + // Number of residuals: 6 + // Residual (0): control + // Residual (1): COM_xy - average(feet position)_xy + // Residual (2): torso_xy - COM_xy + // Residual (3): head_z - feet^{(i)}_position_z - height_goal + // Residual (4): velocity COM_xy + // Residual (5): joint velocity + // Number of parameters: 1 + // Parameter (0): height_goal + // ---------------------------------------------------------------- + static void Residual(const double* parameters, const mjModel* model, + const mjData* data, double* residual); + +}; + +} // namespace humanoid +} // namespace mjpc + +#endif // MJPC_TASKS_HUMANOID_STAND_TASK_H_ diff --git a/mjpc/tasks/humanoid/task_stand.xml b/mjpc/tasks/humanoid/stand/task.xml similarity index 95% rename from mjpc/tasks/humanoid/task_stand.xml rename to mjpc/tasks/humanoid/stand/task.xml index 58d9107da..022f95cfe 100644 --- a/mjpc/tasks/humanoid/task_stand.xml +++ b/mjpc/tasks/humanoid/stand/task.xml @@ -1,6 +1,6 @@ - - + + diff --git a/mjpc/tasks/humanoid/humanoid.cc b/mjpc/tasks/humanoid/walk/task.cc similarity index 65% rename from mjpc/tasks/humanoid/humanoid.cc rename to mjpc/tasks/humanoid/walk/task.cc index bd7ad3664..39a11317a 100644 --- a/mjpc/tasks/humanoid/humanoid.cc +++ b/mjpc/tasks/humanoid/walk/task.cc @@ -12,7 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tasks/humanoid/humanoid.h" +#include "tasks/humanoid/walk/task.h" +#include "mujoco/mjmodel.h" #include @@ -21,82 +22,6 @@ namespace mjpc { -// ------------------ Residuals for humanoid stand task ------------ -// Number of residuals: 6 -// Residual (0): Desired height -// Residual (1): Balance: COM_xy - average(feet position)_xy -// Residual (2): Com Vel: should be 0 and equal feet average vel -// Residual (3): Control: minimise control -// Residual (4): Joint vel: minimise joint velocity -// Number of parameters: 1 -// Parameter (0): height_goal -// ---------------------------------------------------------------- -void Humanoid::ResidualStand(const double* parameters, const mjModel* model, - const mjData* data, double* residual) { - int counter = 0; - - // ----- Height: head feet vertical error ----- // - - // feet sensor positions - double* f1_position = mjpc::SensorByName(model, data, "sp0"); - double* f2_position = mjpc::SensorByName(model, data, "sp1"); - double* f3_position = mjpc::SensorByName(model, data, "sp2"); - double* f4_position = mjpc::SensorByName(model, data, "sp3"); - double* head_position = mjpc::SensorByName(model, data, "head_position"); - double head_feet_error = - head_position[2] - 0.25 * (f1_position[2] + f2_position[2] + - f3_position[2] + f4_position[2]); - residual[counter++] = head_feet_error - parameters[0]; - - // ----- Balance: CoM-feet xy error ----- // - - // capture point - double* com_position = mjpc::SensorByName(model, data, "torso_subtreecom"); - double* com_velocity = mjpc::SensorByName(model, data, "torso_subtreelinvel"); - double kFallTime = 0.2; - double capture_point[3] = {com_position[0], com_position[1], com_position[2]}; - mju_addToScl3(capture_point, com_velocity, kFallTime); - - // average feet xy position - double fxy_avg[2] = {0.0}; - mju_addTo(fxy_avg, f1_position, 2); - mju_addTo(fxy_avg, f2_position, 2); - mju_addTo(fxy_avg, f3_position, 2); - mju_addTo(fxy_avg, f4_position, 2); - mju_scl(fxy_avg, fxy_avg, 0.25, 2); - - mju_subFrom(fxy_avg, capture_point, 2); - double com_feet_distance = mju_norm(fxy_avg, 2); - residual[counter++] = com_feet_distance; - - // ----- COM xy velocity should be 0 ----- // - mju_copy(&residual[counter], com_velocity, 2); - counter += 2; - - // ----- joint velocity ----- // - mju_copy(residual + counter, data->qvel + 6, model->nv - 6); - counter += model->nv - 6; - - // ----- action ----- // - mju_copy(&residual[counter], data->ctrl, model->nu); - counter += model->nu; - - // sensor dim sanity check - // TODO: use this pattern everywhere and make this a utility function - int user_sensor_dim = 0; - for (int i = 0; i < model->nsensor; i++) { - if (model->sensor_type[i] == mjSENS_USER) { - user_sensor_dim += model->sensor_dim[i]; - } - } - if (user_sensor_dim != counter) { - mju_error_i( - "mismatch between total user-sensor dimension " - "and actual length of residual %d", - counter); - } -} - // ------------------ Residuals for humanoid walk task ------------ // Number of residuals: // Residual (0): torso height @@ -111,8 +36,8 @@ void Humanoid::ResidualStand(const double* parameters, const mjModel* model, // Parameter (0): torso height goal // Parameter (1): speed goal // ---------------------------------------------------------------- -void Humanoid::ResidualWalk(const double* parameters, const mjModel* model, - const mjData* data, double* residual) { +void humanoid::Walk::Residual(const double* parameters, const mjModel* model, + const mjData* data, double* residual) { int counter = 0; // ----- torso height ----- // diff --git a/mjpc/tasks/humanoid/humanoid.h b/mjpc/tasks/humanoid/walk/task.h similarity index 56% rename from mjpc/tasks/humanoid/humanoid.h rename to mjpc/tasks/humanoid/walk/task.h index 8950cc7a5..ba6047d40 100644 --- a/mjpc/tasks/humanoid/humanoid.h +++ b/mjpc/tasks/humanoid/walk/task.h @@ -12,26 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MJPC_TASKS_HUMANOID_HUMANOID_H_ -#define MJPC_TASKS_HUMANOID_HUMANOID_H_ +#ifndef MJPC_TASKS_HUMANOID_WALK_TASK_H_ +#define MJPC_TASKS_HUMANOID_WALK_TASK_H_ #include namespace mjpc { -struct Humanoid { - // ------------------ Residuals for humanoid stand task ------------ - // Number of residuals: 6 - // Residual (0): control - // Residual (1): COM_xy - average(feet position)_xy - // Residual (2): torso_xy - COM_xy - // Residual (3): head_z - feet^{(i)}_position_z - height_goal - // Residual (4): velocity COM_xy - // Residual (5): joint velocity - // Number of parameters: 1 - // Parameter (0): height_goal - // ---------------------------------------------------------------- - static void ResidualStand(const double* parameters, const mjModel* model, - const mjData* data, double* residual); +namespace humanoid { + +struct Walk { // ------------------ Residuals for humanoid walk task ------------ // Number of residuals: @@ -47,9 +36,12 @@ struct Humanoid { // Parameter (0): torso height goal // Parameter (1): speed goal // ---------------------------------------------------------------- - static void ResidualWalk(const double* parameters, const mjModel* model, - const mjData* data, double* residual); + static void Residual(const double* parameters, const mjModel* model, + const mjData* data, double* residual); + }; + +} // namespace humanoid } // namespace mjpc -#endif // MJPC_TASKS_HUMANOID_HUMANOID_H_ +#endif // MJPC_TASKS_HUMANOID_WALK_TASK_H_ diff --git a/mjpc/tasks/humanoid/task_walk.xml b/mjpc/tasks/humanoid/walk/task.xml similarity index 96% rename from mjpc/tasks/humanoid/task_walk.xml rename to mjpc/tasks/humanoid/walk/task.xml index 65259ec13..cad112bde 100644 --- a/mjpc/tasks/humanoid/task_walk.xml +++ b/mjpc/tasks/humanoid/walk/task.xml @@ -1,6 +1,6 @@ - - + + diff --git a/mjpc/tasks/tasks.cc b/mjpc/tasks/tasks.cc index 492009dd9..84e00e422 100644 --- a/mjpc/tasks/tasks.cc +++ b/mjpc/tasks/tasks.cc @@ -17,7 +17,8 @@ #include "tasks/acrobot/acrobot.h" #include "tasks/cartpole/cartpole.h" #include "tasks/hand/hand.h" -#include "tasks/humanoid/humanoid.h" +#include "tasks/humanoid/stand/task.h" +#include "tasks/humanoid/walk/task.h" #include "tasks/panda/panda.h" // DEEPMIND INTERNAL IMPORT #include "tasks/particle/particle.h" @@ -35,13 +36,13 @@ namespace { const TaskDefinition kTasksArray[]{ { .name = "Humanoid Stand", - .xml_path = "humanoid/task_stand.xml", - .residual = &Humanoid::ResidualStand, + .xml_path = "humanoid/stand/task.xml", + .residual = &humanoid::Stand::Residual, }, { .name = "Humanoid Walk", - .xml_path = "humanoid/task_walk.xml", - .residual = &Humanoid::ResidualWalk, + .xml_path = "humanoid/walk/task.xml", + .residual = &humanoid::Walk::Residual, }, { .name = "Swimmer",