forked from google-deepmind/mujoco_mpc
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
PiperOrigin-RevId: 505088236 Change-Id: I54aeb33b22050bb9910de680f3111b6fb0ac7fff
- Loading branch information
Showing
19 changed files
with
259 additions
and
135 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 <iostream> | ||
|
|
||
| #include <mujoco/mujoco.h> | ||
| #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 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 <mujoco/mujoco.h> | ||
|
|
||
| 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_ |
4 changes: 2 additions & 2 deletions
4
mjpc/tasks/humanoid/task_stand.xml → mjpc/tasks/humanoid/stand/task.xml
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.