From 6d54f28c872a8b0baf718cba72d660d51db09612 Mon Sep 17 00:00:00 2001 From: Antonio Langella Date: Fri, 26 Apr 2024 21:49:42 +0200 Subject: [PATCH 01/24] [REF] Increased max message size for grpc servers --- mjpc/grpc/agent_server.cc | 2 +- mjpc/grpc/direct_server.cc | 2 +- mjpc/grpc/filter_server.cc | 2 +- mjpc/grpc/ui_agent_server.cc | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/mjpc/grpc/agent_server.cc b/mjpc/grpc/agent_server.cc index 841eee731..0468cb3a8 100644 --- a/mjpc/grpc/agent_server.cc +++ b/mjpc/grpc/agent_server.cc @@ -48,7 +48,7 @@ int main(int argc, char** argv) { mjpc::agent_grpc::AgentService service(mjpc::GetTasks(), absl::GetFlag(FLAGS_mjpc_workers)); - builder.SetMaxReceiveMessageSize(40 * 1024 * 1024); + builder.SetMaxReceiveMessageSize(80 * 1024 * 1024); builder.RegisterService(&service); std::unique_ptr server(builder.BuildAndStart()); diff --git a/mjpc/grpc/direct_server.cc b/mjpc/grpc/direct_server.cc index cdd3b6ceb..04edd6128 100644 --- a/mjpc/grpc/direct_server.cc +++ b/mjpc/grpc/direct_server.cc @@ -43,7 +43,7 @@ int main(int argc, char** argv) { builder.AddListeningPort(server_address, server_credentials); mjpc::direct_grpc::DirectService service; - builder.SetMaxReceiveMessageSize(40 * 1024 * 1024); + builder.SetMaxReceiveMessageSize(80 * 1024 * 1024); builder.RegisterService(&service); std::unique_ptr server(builder.BuildAndStart()); diff --git a/mjpc/grpc/filter_server.cc b/mjpc/grpc/filter_server.cc index c8b343324..93306a313 100644 --- a/mjpc/grpc/filter_server.cc +++ b/mjpc/grpc/filter_server.cc @@ -43,7 +43,7 @@ int main(int argc, char** argv) { builder.AddListeningPort(server_address, server_credentials); filter_grpc::FilterService service; - builder.SetMaxReceiveMessageSize(40 * 1024 * 1024); + builder.SetMaxReceiveMessageSize(80 * 1024 * 1024); builder.RegisterService(&service); std::unique_ptr server(builder.BuildAndStart()); diff --git a/mjpc/grpc/ui_agent_server.cc b/mjpc/grpc/ui_agent_server.cc index fde7da6f7..7d66d4c8c 100644 --- a/mjpc/grpc/ui_agent_server.cc +++ b/mjpc/grpc/ui_agent_server.cc @@ -50,7 +50,7 @@ int main(int argc, char** argv) { mjpc::MjpcApp app(mjpc::GetTasks()); mjpc::agent_grpc::UiAgentService service(app.Sim()); - builder.SetMaxReceiveMessageSize(40 * 1024 * 1024); + builder.SetMaxReceiveMessageSize(80 * 1024 * 1024); builder.RegisterService(&service); std::unique_ptr server(builder.BuildAndStart()); From 7d67c12beb854f63f38acb9c85afc443c42cf5e1 Mon Sep 17 00:00:00 2001 From: Antonio Langella Date: Tue, 7 May 2024 17:02:12 +0200 Subject: [PATCH 02/24] [ADD] Add Unitree H1 Walk task [ADD] Add Unitree H1 Walk baseline task --- mjpc/CMakeLists.txt | 4 + mjpc/tasks/CMakeLists.txt | 12 ++ mjpc/tasks/h1/h1.xml.patch | 188 ++++++++++++++++++ mjpc/tasks/h1/walk-baseline/task.xml | 94 +++++++++ mjpc/tasks/h1/walk-baseline/walk-baseline.cc | 180 +++++++++++++++++ mjpc/tasks/h1/walk-baseline/walk-baseline.h | 66 +++++++ mjpc/tasks/h1/walk/task.xml | 96 +++++++++ mjpc/tasks/h1/walk/walk.cc | 194 +++++++++++++++++++ mjpc/tasks/h1/walk/walk.h | 66 +++++++ mjpc/tasks/tasks.cc | 4 + python/mujoco_mpc/demos/agent/h1_walk.py | 71 +++++++ 11 files changed, 975 insertions(+) create mode 100644 mjpc/tasks/h1/h1.xml.patch create mode 100644 mjpc/tasks/h1/walk-baseline/task.xml create mode 100644 mjpc/tasks/h1/walk-baseline/walk-baseline.cc create mode 100644 mjpc/tasks/h1/walk-baseline/walk-baseline.h create mode 100644 mjpc/tasks/h1/walk/task.xml create mode 100644 mjpc/tasks/h1/walk/walk.cc create mode 100644 mjpc/tasks/h1/walk/walk.h create mode 100644 python/mujoco_mpc/demos/agent/h1_walk.py diff --git a/mjpc/CMakeLists.txt b/mjpc/CMakeLists.txt index c450b7c97..15538029e 100644 --- a/mjpc/CMakeLists.txt +++ b/mjpc/CMakeLists.txt @@ -48,6 +48,10 @@ add_library( tasks/cartpole/cartpole.h tasks/fingers/fingers.cc tasks/fingers/fingers.h + tasks/h1/walk/walk.cc + tasks/h1/walk/walk.h + tasks/h1/walk-baseline/walk-baseline.cc + tasks/h1/walk-baseline/walk-baseline.h tasks/humanoid/stand/stand.cc tasks/humanoid/stand/stand.h tasks/humanoid/tracking/tracking.cc diff --git a/mjpc/tasks/CMakeLists.txt b/mjpc/tasks/CMakeLists.txt index 3cbfa14a7..4e705f1d0 100644 --- a/mjpc/tasks/CMakeLists.txt +++ b/mjpc/tasks/CMakeLists.txt @@ -195,6 +195,18 @@ add_custom_target( COMMAND patch -o ${CMAKE_CURRENT_BINARY_DIR}/op3/op3_modified.xml ${CMAKE_CURRENT_BINARY_DIR}/op3/op3.xml <${CMAKE_CURRENT_SOURCE_DIR}/op3/op3.xml.patch + #H1 WALK TASK + # copy h1 model from Menagerie + COMMAND ${CMAKE_COMMAND} -E copy + ${menagerie_SOURCE_DIR}/unitree_h1/h1.xml + ${CMAKE_CURRENT_BINARY_DIR}/h1/h1.xml + COMMAND ${CMAKE_COMMAND} -E copy_directory + ${menagerie_SOURCE_DIR}/unitree_h1/assets + ${CMAKE_CURRENT_BINARY_DIR}/h1/assets + # modified h1 model for task + COMMAND patch -o ${CMAKE_CURRENT_BINARY_DIR}/h1/h1_modified.xml + ${CMAKE_CURRENT_BINARY_DIR}/h1/h1.xml + <${CMAKE_CURRENT_SOURCE_DIR}/h1/h1.xml.patch COMMAND ${Python_EXECUTABLE} ${CMAKE_CURRENT_BINARY_DIR}/manipulation/merge_panda_robotiq.py diff --git a/mjpc/tasks/h1/h1.xml.patch b/mjpc/tasks/h1/h1.xml.patch new file mode 100644 index 000000000..71eff07cf --- /dev/null +++ b/mjpc/tasks/h1/h1.xml.patch @@ -0,0 +1,188 @@ +diff --git a/home/antonio/uni/tesi/mujoco_mpc/build/_deps/menagerie-src/unitree_h1/h1.xml b/home/antonio/uni/tesi/mujoco_mpc/mjpc/tasks/h1/h1.xml +index 32d3fb9..dfe3948 100644 +--- a/home/antonio/uni/tesi/mujoco_mpc/build/_deps/menagerie-src/unitree_h1/h1.xml ++++ b/home/antonio/uni/tesi/mujoco_mpc/mjpc/tasks/h1/h1.xml +@@ -1,5 +1,5 @@ + +- ++ + + + +@@ -62,84 +62,86 @@ + diaginertia="0.0490211 0.0445821 0.00824619"/> + + +- +- +- +- +- +- +- +- +- +- +- +- +- +- +- +- +- +- +- +- +- +- +- +- +- +- +- +- +- +- +- ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ + +- + +- +- +- +- +- +- +- +- +- +- +- +- +- +- +- +- +- +- +- +- +- +- +- +- +- +- +- +- +- +- +- +- ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ + +- + + + +@@ -153,6 +155,7 @@ + + + ++ + + +@@ -239,4 +242,4 @@ + + + +- ++ +\ No newline at end of file diff --git a/mjpc/tasks/h1/walk-baseline/task.xml b/mjpc/tasks/h1/walk-baseline/task.xml new file mode 100644 index 000000000..9fdb04780 --- /dev/null +++ b/mjpc/tasks/h1/walk-baseline/task.xml @@ -0,0 +1,94 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/mjpc/tasks/h1/walk-baseline/walk-baseline.cc b/mjpc/tasks/h1/walk-baseline/walk-baseline.cc new file mode 100644 index 000000000..3bf7b5608 --- /dev/null +++ b/mjpc/tasks/h1/walk-baseline/walk-baseline.cc @@ -0,0 +1,180 @@ +// 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/tasks/h1/walk-baseline/walk-baseline.h" + +#include +#include + +#include +#include "mjpc/task.h" +#include "mjpc/utilities.h" + +namespace mjpc::h1 { +std::string WalkBaseline::XmlPath() const { + return GetModelPath("h1/walk-baseline/task.xml"); +} +std::string WalkBaseline::Name() const { return "H1 WalkBaseline Baseline"; } + +// ------------------ Residuals for humanoid walk task ------------ +// Number of residuals: +// Residual (0): torso height +// Residual (1): pelvis-feet aligment +// Residual (2): balance +// Residual (3): upright +// Residual (4): posture +// Residual (5): walk +// Residual (6): move feet +// Residual (7): control +// Number of parameters: +// Parameter (0): torso height goal +// Parameter (1): speed goal +// ---------------------------------------------------------------- +void WalkBaseline::ResidualFn::Residual(const mjModel* model, const mjData* data, + double* residual) const { + int counter = 0; + + // ----- torso height ----- // + double torso_height = SensorByName(model, data, "torso_position")[2]; + residual[counter++] = torso_height - parameters_[0]; + + // ----- pelvis / feet ----- // + double* foot_right = SensorByName(model, data, "foot_right"); + double* foot_left = SensorByName(model, data, "foot_left"); + double pelvis_height = SensorByName(model, data, "pelvis_position")[2]; + residual[counter++] = + 0.5 * (foot_left[2] + foot_right[2]) - pelvis_height - 0.2; + + // ----- balance ----- // + // capture point + double* subcom = SensorByName(model, data, "torso_subcom"); + double* subcomvel = SensorByName(model, data, "torso_subcomvel"); + + double capture_point[3]; + mju_addScl(capture_point, subcom, subcomvel, 0.3, 3); + capture_point[2] = 1.0e-3; + + // project onto line segment + + double axis[3]; + double center[3]; + double vec[3]; + double pcp[3]; + mju_sub3(axis, foot_right, foot_left); + axis[2] = 1.0e-3; + double length = 0.5 * mju_normalize3(axis) - 0.05; + mju_add3(center, foot_right, foot_left); + mju_scl3(center, center, 0.5); + mju_sub3(vec, capture_point, center); + + // project onto axis + double t = mju_dot3(vec, axis); + + // clamp + t = mju_max(-length, mju_min(length, t)); + mju_scl3(vec, axis, t); + mju_add3(pcp, vec, center); + pcp[2] = 1.0e-3; + + // is standing + double standing = + torso_height / mju_sqrt(torso_height * torso_height + 0.45 * 0.45) - 0.4; + + mju_sub(&residual[counter], capture_point, pcp, 2); + mju_scl(&residual[counter], &residual[counter], standing, 2); + + counter += 2; + + // ----- upright ----- // + double* torso_up = SensorByName(model, data, "torso_up"); + double* pelvis_up = SensorByName(model, data, "pelvis_up"); + double* foot_right_up = SensorByName(model, data, "foot_right_up"); + double* foot_left_up = SensorByName(model, data, "foot_left_up"); + double z_ref[3] = {0.0, 0.0, 1.0}; + + // torso + residual[counter++] = torso_up[2] - 1.0; + + // pelvis + residual[counter++] = 0.3 * (pelvis_up[2] - 1.0); + + // right foot + mju_sub3(&residual[counter], foot_right_up, z_ref); + mju_scl3(&residual[counter], &residual[counter], 0.1 * standing); + counter += 3; + + mju_sub3(&residual[counter], foot_left_up, z_ref); + mju_scl3(&residual[counter], &residual[counter], 0.1 * standing); + counter += 3; + // ----- posture up ----- // + mju_copy(&residual[counter], data->qpos + 7, model->nq - 7); + counter += model->nq - 7; + // ----- walk ----- // + double* torso_forward = SensorByName(model, data, "torso_forward"); + double* pelvis_forward = SensorByName(model, data, "pelvis_forward"); + double* foot_right_forward = SensorByName(model, data, "foot_right_forward"); + double* foot_left_forward = SensorByName(model, data, "foot_left_forward"); + + double forward[2]; + mju_copy(forward, torso_forward, 2); + mju_addTo(forward, pelvis_forward, 2); + mju_addTo(forward, foot_right_forward, 2); + mju_addTo(forward, foot_left_forward, 2); + mju_normalize(forward, 2); + + // com vel + double* waist_lower_subcomvel = + SensorByName(model, data, "waist_lower_subcomvel"); + double* torso_velocity = SensorByName(model, data, "torso_velocity"); + double com_vel[2]; + mju_add(com_vel, waist_lower_subcomvel, torso_velocity, 2); + mju_scl(com_vel, com_vel, 0.5, 2); + + // walk forward + residual[counter++] = + standing * (mju_dot(com_vel, forward, 2) - parameters_[1]); + + // ----- move feet ----- // + double* foot_right_vel = SensorByName(model, data, "foot_right_velocity"); + double* foot_left_vel = SensorByName(model, data, "foot_left_velocity"); + double move_feet[2]; + mju_copy(move_feet, com_vel, 2); + mju_addToScl(move_feet, foot_right_vel, -0.5, 2); + mju_addToScl(move_feet, foot_left_vel, -0.5, 2); + + mju_copy(&residual[counter], move_feet, 2); + mju_scl(&residual[counter], &residual[counter], standing, 2); + counter += 2; + + // ----- control ----- // + 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::h1 diff --git a/mjpc/tasks/h1/walk-baseline/walk-baseline.h b/mjpc/tasks/h1/walk-baseline/walk-baseline.h new file mode 100644 index 000000000..e7a320867 --- /dev/null +++ b/mjpc/tasks/h1/walk-baseline/walk-baseline.h @@ -0,0 +1,66 @@ +// 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_H1_WALK_BASELINE_TASK_H_ +#define MJPC_TASKS_H1_WALK_BASELINE_TASK_H_ + +#include +#include "mjpc/task.h" + +namespace mjpc { +namespace h1 { + +class WalkBaseline : public Task { + public: + class ResidualFn : public mjpc::BaseResidualFn { + public: + explicit ResidualFn(const WalkBaseline* task) : mjpc::BaseResidualFn(task) {} + + // ------------------ Residuals for humanoid walk task ------------ + // Number of residuals: + // Residual (0): torso height + // Residual (1): pelvis-feet aligment + // Residual (2): balance + // Residual (3): upright + // Residual (4): posture + // Residual (5): walk + // Residual (6): move feet + // Residual (7): control + // Number of parameters: + // Parameter (0): torso height goal + // Parameter (1): speed goal + // ---------------------------------------------------------------- + void Residual(const mjModel* model, const mjData* data, + double* residual) const override; + }; + + WalkBaseline() : residual_(this) {} + + std::string Name() const override; + std::string XmlPath() const override; + + protected: + std::unique_ptr ResidualLocked() const override { + return std::make_unique(this); + } + ResidualFn* InternalResidual() override { return &residual_; } + + private: + ResidualFn residual_; +}; + +} // namespace h1 +} // namespace mjpc + +#endif // MJPC_TASKS_H1_WALK_BASELINE_TASK_H_ diff --git a/mjpc/tasks/h1/walk/task.xml b/mjpc/tasks/h1/walk/task.xml new file mode 100644 index 000000000..d77ec5b0b --- /dev/null +++ b/mjpc/tasks/h1/walk/task.xml @@ -0,0 +1,96 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/mjpc/tasks/h1/walk/walk.cc b/mjpc/tasks/h1/walk/walk.cc new file mode 100644 index 000000000..99fca8931 --- /dev/null +++ b/mjpc/tasks/h1/walk/walk.cc @@ -0,0 +1,194 @@ +// 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/tasks/h1/walk/walk.h" + +#include +#include + +#include +#include "mjpc/task.h" +#include "mjpc/utilities.h" + +namespace mjpc::h1 { +std::string Walk::XmlPath() const { + return GetModelPath("h1/walk/task.xml"); +} +std::string Walk::Name() const { return "H1 Walk"; } + +// ------------------ Residuals for humanoid walk task ------------ +// Number of residuals: +// Residual (0): torso height +// Residual (1): pelvis-feet aligment +// Residual (2): balance +// Residual (3): upright +// Residual (4): posture +// Residual (5): walk +// Residual (6): move feet +// Residual (7): control +// Number of parameters: +// Parameter (0): torso height goal +// Parameter (1): speed goal +// ---------------------------------------------------------------- +void Walk::ResidualFn::Residual(const mjModel* model, const mjData* data, + double* residual) const { + int counter = 0; + + // ----- torso height ----- // + double torso_height = SensorByName(model, data, "torso_position")[2]; + residual[counter++] = torso_height - parameters_[0]; + + // ----- pelvis / feet ----- // + double* foot_right = SensorByName(model, data, "foot_right"); + double* foot_left = SensorByName(model, data, "foot_left"); + double pelvis_height = SensorByName(model, data, "pelvis_position")[2]; + residual[counter++] = + 0.5 * (foot_left[2] + foot_right[2]) - pelvis_height - 0.2; + + // ----- balance ----- // + // capture point + double* subcom = SensorByName(model, data, "torso_subcom"); + double* subcomvel = SensorByName(model, data, "torso_subcomvel"); + + double capture_point[3]; + mju_addScl(capture_point, subcom, subcomvel, 0.3, 3); + capture_point[2] = 1.0e-3; + + // project onto line segment + + double axis[3]; + double center[3]; + double vec[3]; + double pcp[3]; + mju_sub3(axis, foot_right, foot_left); + axis[2] = 1.0e-3; + double length = 0.5 * mju_normalize3(axis) - 0.05; + mju_add3(center, foot_right, foot_left); + mju_scl3(center, center, 0.5); + mju_sub3(vec, capture_point, center); + + // project onto axis + double t = mju_dot3(vec, axis); + + // clamp + t = mju_max(-length, mju_min(length, t)); + mju_scl3(vec, axis, t); + mju_add3(pcp, vec, center); + pcp[2] = 1.0e-3; + + // is standing + double standing = + torso_height / mju_sqrt(torso_height * torso_height + 0.45 * 0.45) - 0.4; + + mju_sub(&residual[counter], capture_point, pcp, 2); + mju_scl(&residual[counter], &residual[counter], standing, 2); + + counter += 2; + + // ----- upright ----- // + double* torso_up = SensorByName(model, data, "torso_up"); + double* pelvis_up = SensorByName(model, data, "pelvis_up"); + double* foot_right_up = SensorByName(model, data, "foot_right_up"); + double* foot_left_up = SensorByName(model, data, "foot_left_up"); + double z_ref[3] = {0.0, 0.0, 1.0}; + + // torso + residual[counter++] = torso_up[2] - 1.0; + + // pelvis + residual[counter++] = 0.3 * (pelvis_up[2] - 1.0); + + // right foot + mju_sub3(&residual[counter], foot_right_up, z_ref); + mju_scl3(&residual[counter], &residual[counter], 0.1 * standing); + counter += 3; + + mju_sub3(&residual[counter], foot_left_up, z_ref); + mju_scl3(&residual[counter], &residual[counter], 0.1 * standing); + counter += 3; + // ----- posture up ----- // + mju_copy(&residual[counter], data->qpos + 17, model->nq - 17); //First 7 are freejoint coord, the other 10 are lower body joints + counter += model->nq - 17; + // ----- posture down ----- // + mju_copy(&residual[counter], data->qpos + 7, model->nq - 16); //First 7 are freejoint coord, the other 10 are lower body joints + counter += model->nq - 16; + // ----- walk ----- // + double* torso_forward = SensorByName(model, data, "torso_forward"); + double* pelvis_forward = SensorByName(model, data, "pelvis_forward"); + double* foot_right_forward = SensorByName(model, data, "foot_right_forward"); + double* foot_left_forward = SensorByName(model, data, "foot_left_forward"); + + double forward[2]; + mju_copy(forward, torso_forward, 2); + mju_addTo(forward, pelvis_forward, 2); + mju_addTo(forward, foot_right_forward, 2); + mju_addTo(forward, foot_left_forward, 2); + mju_normalize(forward, 2); + + // com vel + // double* waist_lower_subcomvel = + // SensorByName(model, data, "waist_lower_subcomvel"); + // double* torso_velocity = SensorByName(model, data, "torso_velocity"); + // double com_vel[2]; + // mju_add(com_vel, waist_lower_subcomvel, torso_velocity, 2); + // mju_scl(com_vel, com_vel, 0.5, 2); + + double com_vel[2]; + mju_copy(com_vel,subcomvel,2); //subcomvel is the velocity of the robot's CoM + + // walk forward + residual[counter++] = + standing * (mju_dot(com_vel, forward, 2) - parameters_[1]); + + // ----- move feet ----- // + double* foot_right_vel = SensorByName(model, data, "foot_right_velocity"); + double* foot_left_vel = SensorByName(model, data, "foot_left_velocity"); + double move_feet[2]; + mju_copy(move_feet, com_vel, 2); + mju_addToScl(move_feet, foot_right_vel, -0.5, 2); + mju_addToScl(move_feet, foot_left_vel, -0.5, 2); + + mju_copy(&residual[counter], move_feet, 2); + mju_scl(&residual[counter], &residual[counter], standing, 2); + counter += 2; + + // ----- control ----- // + mju_copy(&residual[counter], data->ctrl, model->nu); + counter += model->nu; + + // ----- feet distance ----- // + double feet_axis[2]; + mju_copy(feet_axis, foot_right, 2); + mju_addToScl(feet_axis, foot_left, -1, 2); + double feet_distance = mju_norm(feet_axis, 2); + residual[counter] = feet_distance - 0.4; + counter += 1; + + // 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::h1 diff --git a/mjpc/tasks/h1/walk/walk.h b/mjpc/tasks/h1/walk/walk.h new file mode 100644 index 000000000..188299d1e --- /dev/null +++ b/mjpc/tasks/h1/walk/walk.h @@ -0,0 +1,66 @@ +// 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_H1_WALK_TASK_H_ +#define MJPC_TASKS_H1_WALK_TASK_H_ + +#include +#include "mjpc/task.h" + +namespace mjpc { +namespace h1 { + +class Walk : public Task { + public: + class ResidualFn : public mjpc::BaseResidualFn { + public: + explicit ResidualFn(const Walk* task) : mjpc::BaseResidualFn(task) {} + + // ------------------ Residuals for humanoid walk task ------------ + // Number of residuals: + // Residual (0): torso height + // Residual (1): pelvis-feet aligment + // Residual (2): balance + // Residual (3): upright + // Residual (4): posture + // Residual (5): walk + // Residual (6): move feet + // Residual (7): control + // Number of parameters: + // Parameter (0): torso height goal + // Parameter (1): speed goal + // ---------------------------------------------------------------- + void Residual(const mjModel* model, const mjData* data, + double* residual) const override; + }; + + Walk() : residual_(this) {} + + std::string Name() const override; + std::string XmlPath() const override; + + protected: + std::unique_ptr ResidualLocked() const override { + return std::make_unique(this); + } + ResidualFn* InternalResidual() override { return &residual_; } + + private: + ResidualFn residual_; +}; + +} // namespace h1 +} // namespace mjpc + +#endif // MJPC_TASKS_H1_WALK_TASK_H_ diff --git a/mjpc/tasks/tasks.cc b/mjpc/tasks/tasks.cc index 8676f7016..f79756781 100644 --- a/mjpc/tasks/tasks.cc +++ b/mjpc/tasks/tasks.cc @@ -24,6 +24,8 @@ #include "mjpc/tasks/bimanual/reorient/reorient.h" #include "mjpc/tasks/cartpole/cartpole.h" #include "mjpc/tasks/fingers/fingers.h" +#include "mjpc/tasks/h1/walk/walk.h" +#include "mjpc/tasks/h1/walk-baseline/walk-baseline.h" #include "mjpc/tasks/humanoid/stand/stand.h" #include "mjpc/tasks/humanoid/tracking/tracking.h" #include "mjpc/tasks/humanoid/walk/walk.h" @@ -49,6 +51,8 @@ std::vector> GetTasks() { std::make_shared(), std::make_shared(), std::make_shared(), + std::make_shared(), + std::make_shared(), std::make_shared(), std::make_shared(), std::make_shared(), diff --git a/python/mujoco_mpc/demos/agent/h1_walk.py b/python/mujoco_mpc/demos/agent/h1_walk.py new file mode 100644 index 000000000..ced902fff --- /dev/null +++ b/python/mujoco_mpc/demos/agent/h1_walk.py @@ -0,0 +1,71 @@ +# 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. + +import datetime as dt +import mujoco +from mujoco_mpc import agent as agent_lib +import pathlib +import matplotlib.animation as animation +import matplotlib.pyplot as plt +from multiprocessing import Process, Queue + +def runGraph(q): + # Plot data + fig = plt.figure() + axs = [] + xs = [] + ys = [] + + for i in range(1, 21): + axs.append(fig.add_subplot(5, 4, i)) + ys.append([]) + + def update_plots(i, xs, ys): + action = q.get() + # Add x and y to lists + xs.append(dt.datetime.now().strftime('%H:%M:%S.%f')) + xs = xs[-20:] + for i in range(0, len(action)): + ys[i].append(action[i]) + ys[i] = ys[i][-20:] + # Draw x and y lists + axs[i].clear() + axs[i].plot(xs, ys[i]) + return axs + anim = animation.FuncAnimation(fig, lambda i: update_plots(i, xs, ys), frames=200, interval=1, blit=True) + plt.show(block=True) + +# Cartpole model +model_path = ( + pathlib.Path(__file__).parent.parent.parent + / "../../build/mjpc/tasks/h1/walk/task.xml" +) +model = mujoco.MjModel.from_xml_path(str(model_path)) + + +# Run GUI +with agent_lib.Agent( + server_binary_path=pathlib.Path(agent_lib.__file__).parent + / "mjpc" + / "ui_agent_server", + task_id="H1 Walk", + model=model, + real_time_speed=0.5, +) as agent: + q = Queue() + p = Process(target=runGraph, args=(q,)) + p.start() + while True: + action = agent.get_action() + q.put(action) From 5b89bc5b604bd61ccb1dd12f07c4cfe7573f8c76 Mon Sep 17 00:00:00 2001 From: Antonio Langella Date: Tue, 7 May 2024 17:06:34 +0200 Subject: [PATCH 03/24] [ADD] Add utility for visualization in mujoco --- tuning/mjv_utils.py | 50 +++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 50 insertions(+) create mode 100644 tuning/mjv_utils.py diff --git a/tuning/mjv_utils.py b/tuning/mjv_utils.py new file mode 100644 index 000000000..41337fd04 --- /dev/null +++ b/tuning/mjv_utils.py @@ -0,0 +1,50 @@ +import mujoco + +class Geometry: + def __init__(self, geom_id, geom_type, geom_rgba): + self.geom_id = geom_id + self.geom_type = geom_type + self.geom_rgba = geom_rgba + self.geom_pos = [0, 0, 0] + self.geom_mat = [1, 0, 0, 0, 1, 0, 0, 0, 1] + self.geom_size = [1, 1, 1] + + def update(self, ref): + raise NotImplementedError("update() must be implemented by the subclass") + +class Line(Geometry): + def __init__(self, geom_id, geom_rgba): + super().__init__(geom_id, mujoco.mjtGeom.mjGEOM_LINE, geom_rgba) + self.start = None + self.end = None + self.width = 1 + + def update(self, ref): + mujoco.mjv_connector(ref, mujoco.mjtGeom.mjGEOM_LINE, self.width, self.start(), self.end()) + + +class GeomManager: + def __init__(self, scene): + if not isinstance(scene, mujoco._structs.MjvScene): + raise ValueError("scene must be a mujoco.mjvScene") + self.scene = scene + self.geoms = [] + + def add_geom(self, geom): + self.geoms.append(geom) + mujoco.mjv_initGeom(self.scene.geoms[self.scene.ngeom], + geom.geom_type, + geom.geom_size, + geom.geom_pos, + geom.geom_mat, + geom.geom_rgba) + self.scene.ngeom += 1 + return geom + + def add_line(self, geom_rgba = [1, 0, 0, 1]): + geom = Line(self.scene.ngeom, geom_rgba) + return self.add_geom(geom) + + def update(self): + for geom in self.geoms: + geom.update(self.scene.geoms[geom.geom_id]) \ No newline at end of file From 3d3017f156897c03db86f83c8a560c358403f4f7 Mon Sep 17 00:00:00 2001 From: Antonio Langella Date: Tue, 7 May 2024 17:06:51 +0200 Subject: [PATCH 04/24] [ADD] Add utility for plotting live graphs --- tuning/plotting_utils.py | 148 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 148 insertions(+) create mode 100644 tuning/plotting_utils.py diff --git a/tuning/plotting_utils.py b/tuning/plotting_utils.py new file mode 100644 index 000000000..8f890055a --- /dev/null +++ b/tuning/plotting_utils.py @@ -0,0 +1,148 @@ +import matplotlib.pyplot as plt +import numpy as np +import math +from multiprocessing import Process, Queue +import time +import dearpygui.dearpygui as dpg +from collections import deque + +DEQUE_MAX_LEN = 200 + +class _Plot: + def __init__(self, id, name, window_id, y_range = None): + self._id = id + self._window_id = window_id + self._name = name + self._data_count = 0 + self._y_range = y_range + self._xdata = deque(maxlen=DEQUE_MAX_LEN) + self._ydata = deque(maxlen=DEQUE_MAX_LEN) + + def _initialize_gui(self): + with dpg.plot(height=200, width=500): + dpg.add_plot_axis(dpg.mvXAxis, label="Time", tag=f"{self._window_id}-{self._name}-xaxis", time=True, no_tick_labels=True) + dpg.add_plot_axis(dpg.mvYAxis, label=self._name, tag=f"{self._window_id}-{self._name}-yaxis") + dpg.add_line_series([], [], tag=f'{self._window_id}-{self._name}-line', parent=f"{self._window_id}-{self._name}-yaxis") + if self._y_range: + dpg.set_axis_limits(f"{self._window_id}-{self._name}-yaxis", self._y_range[0], self._y_range[1]) + + + def _update_plot(self, x_data, y_data): + assert len(x_data) == len(y_data) + self._xdata.extend(x_data) + self._ydata.extend(y_data) + dpg.configure_item(f'{self._window_id}-{self._name}-line', x=list(self._xdata), y=list(self._ydata)) + dpg.fit_axis_data(f"{self._window_id}-{self._name}-xaxis") + if not self._y_range: + dpg.fit_axis_data(f"{self._window_id}-{self._name}-yaxis") + +class _Window: + def __init__(self, id, shape: tuple[2], name: str): + self._id = id + self._grid_shape: tuple[2] = shape + self._plots: list[_Plot] = [] + self._name = name + + @property + def max_num_plots(self): + return math.prod(self._grid_shape) + + @property + def num_plots(self): + return len(self._plots) + + def _initialize_gui(self): + with dpg.tab(tag = self._name, label=self._name): + with dpg.subplots(rows=self._grid_shape[0], columns=self._grid_shape[1], tag=f"{self._id}-subplots", height=1300, width=1100): + for plot in self._plots: + plot._initialize_gui() + + def _add_plot(self, name, plot_id = -1, y_range = None): + if plot_id == -1: + plot_id = len(self._plots) + assert plot_id < self.max_num_plots + plot = _Plot(id = plot_id, name=name, window_id=self._id, y_range=y_range) + self._plots.insert(plot_id, plot) + return plot + + def _add_points(self, x_data, y_data: list[np.ndarray]): + #assert (type(y_data) == list and len(points) == self.num_plots) or (type(points) == np.ndarray and points.shape[0] == self.num_plots) + y_data = np.stack(y_data, axis=-1) + for i, plot in enumerate(self._plots): + plot._update_plot(x_data, y_data[i, :]) + +class PlotManager(Process): + def __init__(self): + super(PlotManager, self).__init__() + self._windows: list[_Window] = [] + self._queues: list[Queue] = [] + self.daemon = True + + @property + def num_windows(self): + return len(self._windows) + + @property + def num_plots(self): + return sum(map(lambda window: window.num_plots, self._windows)) + + def add_window(self, shape: tuple[2], name: str): + w = _Window(id = len(self._windows), shape=shape, name=name) + q = Queue(maxsize=DEQUE_MAX_LEN) + self._windows.append(w) + self._queues.append(q) + return w._id + + def add_plot(self, window_id: int, plot_name: str, plot_range = None): + return self._windows[window_id]._add_plot(plot_name, y_range=plot_range) + + def send_data(self, window_id: int, data: tuple[float, np.ndarray]): + self._queues[window_id].put(data) + + def __initialize_gui(self): + dpg.create_context() + with dpg.window(tag = "Main Window"): + with dpg.tab_bar(): + for window in self._windows: + window._initialize_gui() + + def __flush_queue(self, queue_id: int): + tmp = [] + while not self._queues[queue_id].empty(): + tmp.append(self._queues[queue_id].get()) + if len(tmp) == 0: + return + x_data, y_data = zip(*tmp) + self._windows[queue_id]._add_points(x_data, y_data) + + def run(self): + self.__initialize_gui() + dpg.create_viewport(width=1100, height=1300, title='Updating plot data') + dpg.setup_dearpygui() + dpg.show_viewport() + dpg.set_primary_window("Main Window", True) + while dpg.is_dearpygui_running(): + start = time.perf_counter_ns() + for i in range(self.num_windows): + self.__flush_queue(i) + dpg.render_dearpygui_frame() + end = time.perf_counter_ns() + print(f"Time taken to render frame: {(end - start) / 1e6} ms") + print(f"FPS: {1 / ((end - start) / 1e9)}") + dpg.destroy_context() + +if __name__ == "__main__": + manager = PlotManager() + w = manager.add_window((2, 1), "Test window") + manager.add_plot(w, "Foot distance") + manager.add_plot(w, "Test distance") + w = manager.add_window((1, 2), "Test window 2") + manager.add_plot(w, "Foot distance") + manager.add_plot(w, "Test distance") + + manager.start() + for i in range(100000): + time.sleep(0.02) + manager.send_data(0, (time.time(), np.asarray([i, 100-i]))) + manager.send_data(1, (time.time(), np.asarray([i*2, np.sin(i/2.0)]))) + manager.join() \ No newline at end of file From 22147db6536264f1272b39621d9435cda0ed77fe Mon Sep 17 00:00:00 2001 From: Antonio Langella Date: Tue, 7 May 2024 17:07:25 +0200 Subject: [PATCH 05/24] [ADD] Add utility for live parameter editing --- tuning/slider_manager.py | 82 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 82 insertions(+) create mode 100644 tuning/slider_manager.py diff --git a/tuning/slider_manager.py b/tuning/slider_manager.py new file mode 100644 index 000000000..7efeec3bb --- /dev/null +++ b/tuning/slider_manager.py @@ -0,0 +1,82 @@ +import dearpygui.dearpygui as dpg +from multiprocessing import Process, Array +import time + +class SliderManager(Process): + def __init__(self, name = "Slider Manager"): + super(SliderManager, self).__init__() + self.daemon = True + self.name = name + self.sliders_metadata = [] + self.__slider_values = None + self.slider_values_local = [] + + def __initialize_gui(self): + print("Initializing sliders") + for metadata in self.sliders_metadata: + slider = dpg.add_slider_float(label=metadata["name"], default_value=metadata["default_value"], min_value=metadata["min_value"], max_value=metadata["max_value"], callback=self.__update_slider_values) + metadata.update({"ref": slider}) + dpg.add_button(label="Reset", callback=self.__reset_sliders) + + def __update_slider_values(self): + with self.__slider_values.get_lock(): + for i, metadata in enumerate(self.sliders_metadata): + self.__slider_values[i] = dpg.get_value(metadata["ref"]) + + def __reset_sliders(self): + with self.__slider_values.get_lock(): + for i, metadata in enumerate(self.sliders_metadata): + dpg.set_value(metadata["ref"], metadata["default_value"]) + self.__slider_values[i] = metadata["default_value"] + + def start(self) -> None: + self.__slider_values = Array('f', [metadata["default_value"] for metadata in self.sliders_metadata]) + with self.__slider_values.get_lock(): + self.slider_values_local = self.__slider_values[:] + return super().start() + + def run(self): + dpg.create_context() + + with dpg.window(tag = "Main Window"): + self.__initialize_gui() + + dpg.create_viewport(width=900, height=600, title='Updating plot data') + dpg.setup_dearpygui() + dpg.show_viewport() + dpg.set_viewport_title(self.name) + dpg.set_primary_window("Main Window", True) + dpg.start_dearpygui() + dpg.destroy_context() + + def add_slider(self, name: str, default_value: float, min_value: float, max_value: float): + metadata = { + "name": name, + "default_value": default_value, + "min_value": min_value, + "max_value": max_value + } + self.sliders_metadata.append(metadata) + + def get_slider_values(self, blocking = True): + lock = self.__slider_values.get_lock() + is_lock_acquired = lock.acquire(block=blocking) + if is_lock_acquired: + self.slider_values_local = self.__slider_values[:] + lock.release() + return self.slider_values_local + + +if __name__ == "__main__": + slider_manager = SliderManager() + slider_manager.add_slider("Slider 1", 0.0, 0.0, 1.0) + slider_manager.add_slider("Slider 2", 0.0, 0.0, 1.0) + slider_manager.start() + + try: + while True: + print(slider_manager.get_slider_values(blocking=False)) + time.sleep(0.01) + except KeyboardInterrupt: + slider_manager.kill() + slider_manager.join() \ No newline at end of file From e256ed2bd87fdf9650cc4cdee0732f3f05276a39 Mon Sep 17 00:00:00 2001 From: Antonio Langella Date: Sun, 12 May 2024 23:18:56 +0200 Subject: [PATCH 06/24] [REF] Model checkpoint for H1 walk --- mjpc/tasks/h1/walk/task.xml | 26 +++++++++++++++++------- mjpc/tasks/h1/walk/walk.cc | 40 ++++++++++++++++++++++++++++++++++--- mjpc/tasks/h1/walk/walk.h | 1 + 3 files changed, 57 insertions(+), 10 deletions(-) diff --git a/mjpc/tasks/h1/walk/task.xml b/mjpc/tasks/h1/walk/task.xml index d77ec5b0b..cdad3f458 100644 --- a/mjpc/tasks/h1/walk/task.xml +++ b/mjpc/tasks/h1/walk/task.xml @@ -6,18 +6,23 @@ + + + - - + + - + + + - - + + - + + - + + + + + @@ -92,5 +102,7 @@ + + diff --git a/mjpc/tasks/h1/walk/walk.cc b/mjpc/tasks/h1/walk/walk.cc index 99fca8931..0928249da 100644 --- a/mjpc/tasks/h1/walk/walk.cc +++ b/mjpc/tasks/h1/walk/walk.cc @@ -37,9 +37,13 @@ std::string Walk::Name() const { return "H1 Walk"; } // Residual (5): walk // Residual (6): move feet // Residual (7): control +// Residual (8): feet distance +// Residual (9): slippage // Number of parameters: // Parameter (0): torso height goal // Parameter (1): speed goal +// Parameter (2): feet distance goal +// Parameter (3): balance speed // ---------------------------------------------------------------- void Walk::ResidualFn::Residual(const mjModel* model, const mjData* data, double* residual) const { @@ -62,7 +66,7 @@ void Walk::ResidualFn::Residual(const mjModel* model, const mjData* data, double* subcomvel = SensorByName(model, data, "torso_subcomvel"); double capture_point[3]; - mju_addScl(capture_point, subcom, subcomvel, 0.3, 3); + mju_addScl(capture_point, subcom, subcomvel, parameters_[3], 3); capture_point[2] = 1.0e-3; // project onto line segment @@ -129,6 +133,7 @@ void Walk::ResidualFn::Residual(const mjModel* model, const mjData* data, double* foot_right_forward = SensorByName(model, data, "foot_right_forward"); double* foot_left_forward = SensorByName(model, data, "foot_left_forward"); + double forward_target[2]; double forward[2]; mju_copy(forward, torso_forward, 2); mju_addTo(forward, pelvis_forward, 2); @@ -136,6 +141,11 @@ void Walk::ResidualFn::Residual(const mjModel* model, const mjData* data, mju_addTo(forward, foot_left_forward, 2); mju_normalize(forward, 2); + double* goal_point = SensorByName(model, data, "goal"); + double* torso_position = SensorByName(model, data, "torso_position"); + mju_sub(forward_target, goal_point, torso_position, 2); + mju_normalize(forward_target, 2); + // com vel // double* waist_lower_subcomvel = // SensorByName(model, data, "waist_lower_subcomvel"); @@ -147,9 +157,13 @@ void Walk::ResidualFn::Residual(const mjModel* model, const mjData* data, double com_vel[2]; mju_copy(com_vel,subcomvel,2); //subcomvel is the velocity of the robot's CoM + // face forward + residual[counter++] = + standing * (mju_dot(forward, forward_target, 2) - 1); + // walk forward residual[counter++] = - standing * (mju_dot(com_vel, forward, 2) - parameters_[1]); + standing * (mju_dot(com_vel, forward_target, 2) - parameters_[1]); // ----- move feet ----- // double* foot_right_vel = SensorByName(model, data, "foot_right_velocity"); @@ -172,8 +186,17 @@ void Walk::ResidualFn::Residual(const mjModel* model, const mjData* data, mju_copy(feet_axis, foot_right, 2); mju_addToScl(feet_axis, foot_left, -1, 2); double feet_distance = mju_norm(feet_axis, 2); - residual[counter] = feet_distance - 0.4; + residual[counter] = feet_distance - parameters_[2]; counter += 1; + // ----- slippage ----- // + double* foot_right_ang_velocity = SensorByName(model, data, "foot_right_ang_velocity"); + double* foot_left_ang_velocity = SensorByName(model, data, "foot_left_ang_velocity"); + double* right_foot_xbody = SensorByName(model, data, "foot_right_xbody"); + double* left_foot_xbody = SensorByName(model, data, "foot_right_xbody"); + + + residual[counter++] = (tanh(-(right_foot_xbody[2]-0.0645)/0.001)+1)*0.5*foot_right_ang_velocity[2]; + residual[counter++] = (tanh(-(left_foot_xbody[2]-0.0645)/0.001)+1)*0.5*foot_left_ang_velocity[2]; // sensor dim sanity check // TODO: use this pattern everywhere and make this a utility function @@ -191,4 +214,15 @@ void Walk::ResidualFn::Residual(const mjModel* model, const mjData* data, } } +double Walk::ResidualFn::StepHeight(double time, double footphase, + double duty_ratio) const { + double angle = fmod(time + mjPI - footphase, 2*mjPI) - mjPI; + double value = 0; + if (duty_ratio < 1) { + angle *= 0.5 / (1 - duty_ratio); + value = mju_cos(mju_clip(angle, -mjPI/2, mjPI/2)); + } + return mju_abs(value) < 1e-6 ? 0.0 : value; +} + } // namespace mjpc::h1 diff --git a/mjpc/tasks/h1/walk/walk.h b/mjpc/tasks/h1/walk/walk.h index 188299d1e..12708d94c 100644 --- a/mjpc/tasks/h1/walk/walk.h +++ b/mjpc/tasks/h1/walk/walk.h @@ -43,6 +43,7 @@ class Walk : public Task { // ---------------------------------------------------------------- void Residual(const mjModel* model, const mjData* data, double* residual) const override; + double StepHeight(double time, double footphase, double duty_ratio) const; }; Walk() : residual_(this) {} From 12a9a216ac24f8f8b1e26afadacf8f6098faaecd Mon Sep 17 00:00:00 2001 From: Antonio Langella Date: Sun, 12 May 2024 23:20:06 +0200 Subject: [PATCH 07/24] [DOC] Fix comment --- mjpc/tasks/h1/walk/walk.cc | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/mjpc/tasks/h1/walk/walk.cc b/mjpc/tasks/h1/walk/walk.cc index 0928249da..6ee2e8ca2 100644 --- a/mjpc/tasks/h1/walk/walk.cc +++ b/mjpc/tasks/h1/walk/walk.cc @@ -34,11 +34,12 @@ std::string Walk::Name() const { return "H1 Walk"; } // Residual (2): balance // Residual (3): upright // Residual (4): posture -// Residual (5): walk -// Residual (6): move feet -// Residual (7): control -// Residual (8): feet distance -// Residual (9): slippage +// Residual (5): face towards goal +// Residual (6): walk towards goal +// Residual (7): move feet +// Residual (8): control +// Residual (9): feet distance +// Residual (10): slippage // Number of parameters: // Parameter (0): torso height goal // Parameter (1): speed goal From cbb6cf30156ffb869e995628f3672c011416e077 Mon Sep 17 00:00:00 2001 From: Antonio Langella Date: Sun, 21 Jul 2024 18:10:02 +0200 Subject: [PATCH 08/24] [ADD] Point plotting in mujoco --- tuning/mjv_utils.py | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/tuning/mjv_utils.py b/tuning/mjv_utils.py index 41337fd04..10aab73a9 100644 --- a/tuning/mjv_utils.py +++ b/tuning/mjv_utils.py @@ -22,6 +22,14 @@ def __init__(self, geom_id, geom_rgba): def update(self, ref): mujoco.mjv_connector(ref, mujoco.mjtGeom.mjGEOM_LINE, self.width, self.start(), self.end()) +class Point(Geometry): + def __init__(self, geom_id, geom_rgba, size=0.025): + super().__init__(geom_id, mujoco.mjtGeom.mjGEOM_SPHERE, geom_rgba) + self.location = None + self.geom_size = [size, size, size] + + def update(self, ref): + ref.pos = self.location() class GeomManager: def __init__(self, scene): @@ -41,6 +49,10 @@ def add_geom(self, geom): self.scene.ngeom += 1 return geom + def add_point(self, geom_rgba = [1, 0, 0, 1]): + geom = Point(self.scene.ngeom, geom_rgba) + return self.add_geom(geom) + def add_line(self, geom_rgba = [1, 0, 0, 1]): geom = Line(self.scene.ngeom, geom_rgba) return self.add_geom(geom) From 2edb239b05c6d7aef280a266d65073ba78ad3e7a Mon Sep 17 00:00:00 2001 From: Antonio Langella Date: Sun, 21 Jul 2024 19:05:01 +0200 Subject: [PATCH 09/24] [ADD] python script for h1 walk tuning --- tuning/h1_walk_tuning.py | 239 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 239 insertions(+) create mode 100644 tuning/h1_walk_tuning.py diff --git a/tuning/h1_walk_tuning.py b/tuning/h1_walk_tuning.py new file mode 100644 index 000000000..d49c74a45 --- /dev/null +++ b/tuning/h1_walk_tuning.py @@ -0,0 +1,239 @@ +import copy +import datetime as dt +import matplotlib.pyplot as plt +import mediapy as media +import mujoco +import mujoco.viewer +from multiprocessing import Process, Queue +import numpy as np +import pathlib +import time + +from mujoco_mpc import agent as agent_lib +from mjv_utils import Line, GeomManager +from plotting_utils import PlotManager +from slider_manager import SliderManager + +def step_function(x, s=0.001, step_x=0.0, orientation=1): + return 0.5*(np.tanh(orientation*(x-step_x)/s)+1) + +def add_fw_cost(geom_manager, data): + line = geom_manager.add_line() + line.start = lambda: data.site('upper_torso').xpos + line.end = lambda: data.site('upper_torso').xpos + data.sensor('torso_forward').data + line = geom_manager.add_line() + line.start = lambda: data.body('pelvis').xpos + line.end = lambda: data.body('pelvis').xpos + data.sensor('pelvis_forward').data + + # Right foot info + point = geom_manager.add_point() + point.location = lambda: data.sensor('foot_right_xbody').data + point = geom_manager.add_point(geom_rgba = [0,1,0,1]) + point.location = lambda: data.sensor('foot_right').data + line = geom_manager.add_line() + line.start = lambda: data.body('right_ankle_link').xpos + line.end = lambda: data.body('right_ankle_link').xpos + data.sensor('foot_right_forward').data + + # Left foot info + line = geom_manager.add_line() + line.start = lambda: data.sensor('foot_left_xbody').data + line.end = lambda: data.sensor('foot_left_xbody').data + data.sensor('foot_left_forward').data + line = geom_manager.add_line(geom_rgba=[0, 1, 0, 1]) + line.start = lambda: data.sensor('foot_left_xbody').data + line.end = lambda: data.sensor('foot_left_xbody').data + data.sensor('foot_left_left').data + line = geom_manager.add_line(geom_rgba=[0, 0, 1, 1]) + line.start = lambda: data.sensor('foot_left_xbody').data + line.end = lambda: data.sensor('foot_left_xbody').data + data.sensor('foot_left_up').data + +def add_foot_distance(geom_manager, data): + line = geom_manager.add_line(geom_rgba=[0, 0, 1, 1]) + line.start = lambda: data.body('right_ankle_link').xpos + line.end = lambda: data.body('left_ankle_link').xpos + +def configure_plot_manager(plot_manager: PlotManager, model): + w = plot_manager.add_window((6, 1), "Cost function terms") + plot_manager.add_plot(w, "Foot distance") + plot_manager.add_plot(w, "Right foot velocity x") + plot_manager.add_plot(w, "Right foot velocity y") + plot_manager.add_plot(w, "Right foot velocity z") + plot_manager.add_plot(w, "Left ankle height") + plot_manager.add_plot(w, "Right ankle height") + w = plot_manager.add_window((10, 2), "Errors") + for i in range(1,model.njnt): + plot_manager.add_plot(w, model.joint(i).name, (-0.4, 0.4)) + w = plot_manager.add_window((10, 2), "Debug") + for i in range(1,model.njnt): + plot_manager.add_plot(w, model.joint(i).name, (-0.4, 0.4)) + +def add_slider_foreach_joint(slider_manager, model, default_value, min_value, max_value, prefix=""): + for i in range(1,model.njnt): + joint = model.joint(i) + default_value_val = default_value(i, joint) if callable(default_value) else default_value + min_value_val = min_value(i, joint) if callable(min_value) else min_value + max_value_val = max_value(i, joint) if callable(max_value) else max_value + slider_manager.add_slider(prefix + joint.name, default_value_val, min_value_val, max_value_val) + +def main(): + SIMULATION_RUNNING = False + i = 0 + steps_per_planning_iteration = 10 + # kp = np.asarray([200, 200, 200, 300, 40, 200, 200, 200, 300, 40, 300, 100, 100, 100, 100, 100, 100, 100, 10]) + # kd = np.asarray([5, 5, 5, 6, 2, 5, 5, 5, 6, 2, 6, 2, 2, 2, 2, 2, 2, 2, 2]) + kp = np.asarray([150, 500, 250, 350, 250, 150, 500, 250, 350, 250, 500, 150, 50, 50, 50, 150, 50, 50, 50]) + kd = np.asarray([0, 35, 10, 10, 0, 0, 35, 10, 10, 0, 10, 5, 5, 5, 1.5, 5, 5, 5, 1.5]) + #200 5 hip + #300 6 knee torso + #40 2 ankle + #100 2 shoulder1 shoulder2 elbow + def handle_key(key): + nonlocal SIMULATION_RUNNING + if key == ord(" "): + print("SIMULATION PAUSED/RESUMED") + SIMULATION_RUNNING = not SIMULATION_RUNNING + # model + model_path = ( + pathlib.Path(__file__).parent + / "../build/mjpc/tasks/h1/walk/task.xml" + ) + model = mujoco.MjModel.from_xml_path(str(model_path)) + model.opt.timestep = 0.002 + # data + data = mujoco.MjData(model) + # agent + agent = agent_lib.Agent(task_id="H1 Walk", + model=model, + server_binary_path=pathlib.Path(agent_lib.__file__).parent + / "mjpc" + / "agent_server") + + # weights + #agent.set_cost_weights({"Heigth": 0, "Velocity": 0.15}) + print("Cost weights:", agent.get_cost_weights()) + + # parameters + agent.set_task_parameter("Torso", 1.3) + agent.set_task_parameter("Speed", 0.7) + print("Parameters:", agent.get_task_parameters()) + + # rollout + mujoco.mj_resetData(model, data) + + # mocap + mocap_path = [np.asarray([2.0, 2.0, 0.25]), + np.asarray([2.0, -2.0, 0.25]), + np.asarray([-2.0, -2.0, 0.25]), + np.asarray([-2.0, 2.0, 0.25])] + current_mocap = 0 + data.mocap_pos[0] = mocap_path[current_mocap] + + + # simulate + with mujoco.viewer.launch_passive(model, data, key_callback=handle_key) as viewer: + start = time.time() + + geom_manager = GeomManager(viewer.user_scn) + add_fw_cost(geom_manager, data) + add_foot_distance(geom_manager, data) + + plot_manager = PlotManager() + configure_plot_manager(plot_manager, model) + plot_manager.start() + + kp_slider_manager = SliderManager(name = "kp") + kd_slider_manager = SliderManager(name = "kd") + ref_slider_manager = SliderManager(name = "ref") + add_slider_foreach_joint(kp_slider_manager, model, lambda i, j: kp[i-1], 0.0, 800.0) + add_slider_foreach_joint(kd_slider_manager, model, lambda i, j: kd[i-1], 0.0, 50.0) + add_slider_foreach_joint(ref_slider_manager, model, lambda _, j: j.qpos0, lambda _, j: j.range[0], lambda _, j: j.range[1]) + add_slider_foreach_joint(ref_slider_manager, model, 0, -1.0, 1.0, prefix="b_") + add_slider_foreach_joint(ref_slider_manager, model, 0, 0, 0.5, prefix="A_") + add_slider_foreach_joint(ref_slider_manager, model, 0, 0, 2.0, prefix="f_") + kp_slider_manager.start() + kd_slider_manager.start() + ref_slider_manager.start() + + + while viewer.is_running(): + while not SIMULATION_RUNNING: + time.sleep(0.1) + step_start = time.time() + + # GET CONTROLLER PARAMETERS + # kp = np.asarray(kp_slider_manager.get_slider_values(blocking=False)) + # kd = np.asarray(kd_slider_manager.get_slider_values(blocking=False)) + + # GET REFERENCE + + # set planner state + agent.set_state( + time=data.time, + qpos=data.qpos, + qvel=data.qvel, + act=data.act, + mocap_pos=data.mocap_pos, + mocap_quat=data.mocap_quat, + userdata=data.userdata, + ) + if i % steps_per_planning_iteration == 0: + agent.planner_step() + # traj = agent.best_trajectory() + # traj_states = traj['states'] + # traj_actions = traj['actions'] + # q_ref = traj[1,7:26] #19 + # q_dot_ref = traj[1,32:51] #19 + + + + # reference from slider + # slider_values = ref_slider_manager.get_slider_values(blocking=False) + # b = np.asarray(slider_values[:19]) + # A = np.asarray(slider_values[19:38]) + # f = np.asarray(slider_values[38:]) + # q_ref = A * np.sin(2*np.pi*data.time*f) + b + # q_dot_ref = A*2*np.pi*f*np.cos(2*np.pi*data.time*f) + + # READ STATE + q = data.qpos[7:26] + q_dot = data.qvel[6:25] + + + + #CONTROL LAW + + # set ctrl from agent policy + data.ctrl = agent.get_action(nominal_action=True) + + mujoco.mj_step1(model, data) + # data.ctrl = np.multiply((q_ref-q), kp) + np.multiply((q_dot_ref-q_dot), kd) + # data.ctrl += data.qfrc_bias[6:] + #data.ctrl = traj_actions[0, :] + plot_manager.send_data(2, (time.time(), data.ctrl)) + # SIMULATION STEP + + mujoco.mj_step2(model, data) + plot_manager.send_data(0, (time.time(), [mujoco.mju_norm(data.body('right_ankle_link').xpos[:2] - data.body('left_ankle_link').xpos[:2]), + *data.sensor('foot_right_ang_velocity').data, + data.body('left_ankle_link').xpos[2], + data.body('right_ankle_link').xpos[2]])) + #plot_manager.send_data(1, (time.time(), q_ref-data.qpos[7:26])) + + # update graphics + geom_manager.update() + + # Pick up changes to the physics state, apply perturbations, update options from GUI. + viewer.sync() + + # update mocap if goal reached + if np.linalg.norm(data.sensor('torso_position').data[:2] - data.mocap_pos[0][:2]) < 0.1: + current_mocap = (current_mocap + 1) % len(mocap_path) + data.mocap_pos[0] = mocap_path[current_mocap] + + i+=1 + time_until_next_step = model.opt.timestep - (time.time() - step_start) + if time_until_next_step > 0: + time.sleep(time_until_next_step) + elif i < 1: + print("Simulation not running in real time!") + +if __name__ == "__main__": + main() \ No newline at end of file From 97d7924a59138561a5e04de9e9ede4749f10536f Mon Sep 17 00:00:00 2001 From: Antonio Langella Date: Thu, 25 Jul 2024 11:52:08 +0200 Subject: [PATCH 10/24] [ADD] Add a new cost term which penalizes the feet crossing --- mjpc/tasks/h1/walk/task.xml | 7 +++++++ mjpc/tasks/h1/walk/walk.cc | 12 ++++++++++-- 2 files changed, 17 insertions(+), 2 deletions(-) diff --git a/mjpc/tasks/h1/walk/task.xml b/mjpc/tasks/h1/walk/task.xml index cdad3f458..24f2f17d4 100644 --- a/mjpc/tasks/h1/walk/task.xml +++ b/mjpc/tasks/h1/walk/task.xml @@ -3,9 +3,14 @@ + + + + + @@ -46,6 +51,7 @@ + @@ -94,6 +100,7 @@ + diff --git a/mjpc/tasks/h1/walk/walk.cc b/mjpc/tasks/h1/walk/walk.cc index 6ee2e8ca2..af7237163 100644 --- a/mjpc/tasks/h1/walk/walk.cc +++ b/mjpc/tasks/h1/walk/walk.cc @@ -39,7 +39,8 @@ std::string Walk::Name() const { return "H1 Walk"; } // Residual (7): move feet // Residual (8): control // Residual (9): feet distance -// Residual (10): slippage +// Residual (10): feet cross +// Residual (11): slippage // Number of parameters: // Parameter (0): torso height goal // Parameter (1): speed goal @@ -189,11 +190,18 @@ void Walk::ResidualFn::Residual(const mjModel* model, const mjData* data, double feet_distance = mju_norm(feet_axis, 2); residual[counter] = feet_distance - parameters_[2]; counter += 1; + + // ----- feet cross ----- // + double *left_foot_left_axis = SensorByName(model, data, "foot_left_left"); + mju_sub3(vec, foot_right, foot_left); + residual[counter++] = mju_dot3(vec, left_foot_left_axis) - 0.2; + + // ----- slippage ----- // double* foot_right_ang_velocity = SensorByName(model, data, "foot_right_ang_velocity"); double* foot_left_ang_velocity = SensorByName(model, data, "foot_left_ang_velocity"); double* right_foot_xbody = SensorByName(model, data, "foot_right_xbody"); - double* left_foot_xbody = SensorByName(model, data, "foot_right_xbody"); + double* left_foot_xbody = SensorByName(model, data, "foot_left_xbody"); residual[counter++] = (tanh(-(right_foot_xbody[2]-0.0645)/0.001)+1)*0.5*foot_right_ang_velocity[2]; From cfc9ad6aa0ac8ceb149209316e853e6c29505e18 Mon Sep 17 00:00:00 2001 From: Antonio Langella Date: Sun, 4 Aug 2024 09:45:20 +0200 Subject: [PATCH 11/24] [REM] Useless files for the PR --- tuning/h1_walk_tuning.py | 239 --------------------------------------- tuning/mjv_utils.py | 62 ---------- tuning/plotting_utils.py | 148 ------------------------ tuning/slider_manager.py | 82 -------------- 4 files changed, 531 deletions(-) delete mode 100644 tuning/h1_walk_tuning.py delete mode 100644 tuning/mjv_utils.py delete mode 100644 tuning/plotting_utils.py delete mode 100644 tuning/slider_manager.py diff --git a/tuning/h1_walk_tuning.py b/tuning/h1_walk_tuning.py deleted file mode 100644 index d49c74a45..000000000 --- a/tuning/h1_walk_tuning.py +++ /dev/null @@ -1,239 +0,0 @@ -import copy -import datetime as dt -import matplotlib.pyplot as plt -import mediapy as media -import mujoco -import mujoco.viewer -from multiprocessing import Process, Queue -import numpy as np -import pathlib -import time - -from mujoco_mpc import agent as agent_lib -from mjv_utils import Line, GeomManager -from plotting_utils import PlotManager -from slider_manager import SliderManager - -def step_function(x, s=0.001, step_x=0.0, orientation=1): - return 0.5*(np.tanh(orientation*(x-step_x)/s)+1) - -def add_fw_cost(geom_manager, data): - line = geom_manager.add_line() - line.start = lambda: data.site('upper_torso').xpos - line.end = lambda: data.site('upper_torso').xpos + data.sensor('torso_forward').data - line = geom_manager.add_line() - line.start = lambda: data.body('pelvis').xpos - line.end = lambda: data.body('pelvis').xpos + data.sensor('pelvis_forward').data - - # Right foot info - point = geom_manager.add_point() - point.location = lambda: data.sensor('foot_right_xbody').data - point = geom_manager.add_point(geom_rgba = [0,1,0,1]) - point.location = lambda: data.sensor('foot_right').data - line = geom_manager.add_line() - line.start = lambda: data.body('right_ankle_link').xpos - line.end = lambda: data.body('right_ankle_link').xpos + data.sensor('foot_right_forward').data - - # Left foot info - line = geom_manager.add_line() - line.start = lambda: data.sensor('foot_left_xbody').data - line.end = lambda: data.sensor('foot_left_xbody').data + data.sensor('foot_left_forward').data - line = geom_manager.add_line(geom_rgba=[0, 1, 0, 1]) - line.start = lambda: data.sensor('foot_left_xbody').data - line.end = lambda: data.sensor('foot_left_xbody').data + data.sensor('foot_left_left').data - line = geom_manager.add_line(geom_rgba=[0, 0, 1, 1]) - line.start = lambda: data.sensor('foot_left_xbody').data - line.end = lambda: data.sensor('foot_left_xbody').data + data.sensor('foot_left_up').data - -def add_foot_distance(geom_manager, data): - line = geom_manager.add_line(geom_rgba=[0, 0, 1, 1]) - line.start = lambda: data.body('right_ankle_link').xpos - line.end = lambda: data.body('left_ankle_link').xpos - -def configure_plot_manager(plot_manager: PlotManager, model): - w = plot_manager.add_window((6, 1), "Cost function terms") - plot_manager.add_plot(w, "Foot distance") - plot_manager.add_plot(w, "Right foot velocity x") - plot_manager.add_plot(w, "Right foot velocity y") - plot_manager.add_plot(w, "Right foot velocity z") - plot_manager.add_plot(w, "Left ankle height") - plot_manager.add_plot(w, "Right ankle height") - w = plot_manager.add_window((10, 2), "Errors") - for i in range(1,model.njnt): - plot_manager.add_plot(w, model.joint(i).name, (-0.4, 0.4)) - w = plot_manager.add_window((10, 2), "Debug") - for i in range(1,model.njnt): - plot_manager.add_plot(w, model.joint(i).name, (-0.4, 0.4)) - -def add_slider_foreach_joint(slider_manager, model, default_value, min_value, max_value, prefix=""): - for i in range(1,model.njnt): - joint = model.joint(i) - default_value_val = default_value(i, joint) if callable(default_value) else default_value - min_value_val = min_value(i, joint) if callable(min_value) else min_value - max_value_val = max_value(i, joint) if callable(max_value) else max_value - slider_manager.add_slider(prefix + joint.name, default_value_val, min_value_val, max_value_val) - -def main(): - SIMULATION_RUNNING = False - i = 0 - steps_per_planning_iteration = 10 - # kp = np.asarray([200, 200, 200, 300, 40, 200, 200, 200, 300, 40, 300, 100, 100, 100, 100, 100, 100, 100, 10]) - # kd = np.asarray([5, 5, 5, 6, 2, 5, 5, 5, 6, 2, 6, 2, 2, 2, 2, 2, 2, 2, 2]) - kp = np.asarray([150, 500, 250, 350, 250, 150, 500, 250, 350, 250, 500, 150, 50, 50, 50, 150, 50, 50, 50]) - kd = np.asarray([0, 35, 10, 10, 0, 0, 35, 10, 10, 0, 10, 5, 5, 5, 1.5, 5, 5, 5, 1.5]) - #200 5 hip - #300 6 knee torso - #40 2 ankle - #100 2 shoulder1 shoulder2 elbow - def handle_key(key): - nonlocal SIMULATION_RUNNING - if key == ord(" "): - print("SIMULATION PAUSED/RESUMED") - SIMULATION_RUNNING = not SIMULATION_RUNNING - # model - model_path = ( - pathlib.Path(__file__).parent - / "../build/mjpc/tasks/h1/walk/task.xml" - ) - model = mujoco.MjModel.from_xml_path(str(model_path)) - model.opt.timestep = 0.002 - # data - data = mujoco.MjData(model) - # agent - agent = agent_lib.Agent(task_id="H1 Walk", - model=model, - server_binary_path=pathlib.Path(agent_lib.__file__).parent - / "mjpc" - / "agent_server") - - # weights - #agent.set_cost_weights({"Heigth": 0, "Velocity": 0.15}) - print("Cost weights:", agent.get_cost_weights()) - - # parameters - agent.set_task_parameter("Torso", 1.3) - agent.set_task_parameter("Speed", 0.7) - print("Parameters:", agent.get_task_parameters()) - - # rollout - mujoco.mj_resetData(model, data) - - # mocap - mocap_path = [np.asarray([2.0, 2.0, 0.25]), - np.asarray([2.0, -2.0, 0.25]), - np.asarray([-2.0, -2.0, 0.25]), - np.asarray([-2.0, 2.0, 0.25])] - current_mocap = 0 - data.mocap_pos[0] = mocap_path[current_mocap] - - - # simulate - with mujoco.viewer.launch_passive(model, data, key_callback=handle_key) as viewer: - start = time.time() - - geom_manager = GeomManager(viewer.user_scn) - add_fw_cost(geom_manager, data) - add_foot_distance(geom_manager, data) - - plot_manager = PlotManager() - configure_plot_manager(plot_manager, model) - plot_manager.start() - - kp_slider_manager = SliderManager(name = "kp") - kd_slider_manager = SliderManager(name = "kd") - ref_slider_manager = SliderManager(name = "ref") - add_slider_foreach_joint(kp_slider_manager, model, lambda i, j: kp[i-1], 0.0, 800.0) - add_slider_foreach_joint(kd_slider_manager, model, lambda i, j: kd[i-1], 0.0, 50.0) - add_slider_foreach_joint(ref_slider_manager, model, lambda _, j: j.qpos0, lambda _, j: j.range[0], lambda _, j: j.range[1]) - add_slider_foreach_joint(ref_slider_manager, model, 0, -1.0, 1.0, prefix="b_") - add_slider_foreach_joint(ref_slider_manager, model, 0, 0, 0.5, prefix="A_") - add_slider_foreach_joint(ref_slider_manager, model, 0, 0, 2.0, prefix="f_") - kp_slider_manager.start() - kd_slider_manager.start() - ref_slider_manager.start() - - - while viewer.is_running(): - while not SIMULATION_RUNNING: - time.sleep(0.1) - step_start = time.time() - - # GET CONTROLLER PARAMETERS - # kp = np.asarray(kp_slider_manager.get_slider_values(blocking=False)) - # kd = np.asarray(kd_slider_manager.get_slider_values(blocking=False)) - - # GET REFERENCE - - # set planner state - agent.set_state( - time=data.time, - qpos=data.qpos, - qvel=data.qvel, - act=data.act, - mocap_pos=data.mocap_pos, - mocap_quat=data.mocap_quat, - userdata=data.userdata, - ) - if i % steps_per_planning_iteration == 0: - agent.planner_step() - # traj = agent.best_trajectory() - # traj_states = traj['states'] - # traj_actions = traj['actions'] - # q_ref = traj[1,7:26] #19 - # q_dot_ref = traj[1,32:51] #19 - - - - # reference from slider - # slider_values = ref_slider_manager.get_slider_values(blocking=False) - # b = np.asarray(slider_values[:19]) - # A = np.asarray(slider_values[19:38]) - # f = np.asarray(slider_values[38:]) - # q_ref = A * np.sin(2*np.pi*data.time*f) + b - # q_dot_ref = A*2*np.pi*f*np.cos(2*np.pi*data.time*f) - - # READ STATE - q = data.qpos[7:26] - q_dot = data.qvel[6:25] - - - - #CONTROL LAW - - # set ctrl from agent policy - data.ctrl = agent.get_action(nominal_action=True) - - mujoco.mj_step1(model, data) - # data.ctrl = np.multiply((q_ref-q), kp) + np.multiply((q_dot_ref-q_dot), kd) - # data.ctrl += data.qfrc_bias[6:] - #data.ctrl = traj_actions[0, :] - plot_manager.send_data(2, (time.time(), data.ctrl)) - # SIMULATION STEP - - mujoco.mj_step2(model, data) - plot_manager.send_data(0, (time.time(), [mujoco.mju_norm(data.body('right_ankle_link').xpos[:2] - data.body('left_ankle_link').xpos[:2]), - *data.sensor('foot_right_ang_velocity').data, - data.body('left_ankle_link').xpos[2], - data.body('right_ankle_link').xpos[2]])) - #plot_manager.send_data(1, (time.time(), q_ref-data.qpos[7:26])) - - # update graphics - geom_manager.update() - - # Pick up changes to the physics state, apply perturbations, update options from GUI. - viewer.sync() - - # update mocap if goal reached - if np.linalg.norm(data.sensor('torso_position').data[:2] - data.mocap_pos[0][:2]) < 0.1: - current_mocap = (current_mocap + 1) % len(mocap_path) - data.mocap_pos[0] = mocap_path[current_mocap] - - i+=1 - time_until_next_step = model.opt.timestep - (time.time() - step_start) - if time_until_next_step > 0: - time.sleep(time_until_next_step) - elif i < 1: - print("Simulation not running in real time!") - -if __name__ == "__main__": - main() \ No newline at end of file diff --git a/tuning/mjv_utils.py b/tuning/mjv_utils.py deleted file mode 100644 index 10aab73a9..000000000 --- a/tuning/mjv_utils.py +++ /dev/null @@ -1,62 +0,0 @@ -import mujoco - -class Geometry: - def __init__(self, geom_id, geom_type, geom_rgba): - self.geom_id = geom_id - self.geom_type = geom_type - self.geom_rgba = geom_rgba - self.geom_pos = [0, 0, 0] - self.geom_mat = [1, 0, 0, 0, 1, 0, 0, 0, 1] - self.geom_size = [1, 1, 1] - - def update(self, ref): - raise NotImplementedError("update() must be implemented by the subclass") - -class Line(Geometry): - def __init__(self, geom_id, geom_rgba): - super().__init__(geom_id, mujoco.mjtGeom.mjGEOM_LINE, geom_rgba) - self.start = None - self.end = None - self.width = 1 - - def update(self, ref): - mujoco.mjv_connector(ref, mujoco.mjtGeom.mjGEOM_LINE, self.width, self.start(), self.end()) - -class Point(Geometry): - def __init__(self, geom_id, geom_rgba, size=0.025): - super().__init__(geom_id, mujoco.mjtGeom.mjGEOM_SPHERE, geom_rgba) - self.location = None - self.geom_size = [size, size, size] - - def update(self, ref): - ref.pos = self.location() - -class GeomManager: - def __init__(self, scene): - if not isinstance(scene, mujoco._structs.MjvScene): - raise ValueError("scene must be a mujoco.mjvScene") - self.scene = scene - self.geoms = [] - - def add_geom(self, geom): - self.geoms.append(geom) - mujoco.mjv_initGeom(self.scene.geoms[self.scene.ngeom], - geom.geom_type, - geom.geom_size, - geom.geom_pos, - geom.geom_mat, - geom.geom_rgba) - self.scene.ngeom += 1 - return geom - - def add_point(self, geom_rgba = [1, 0, 0, 1]): - geom = Point(self.scene.ngeom, geom_rgba) - return self.add_geom(geom) - - def add_line(self, geom_rgba = [1, 0, 0, 1]): - geom = Line(self.scene.ngeom, geom_rgba) - return self.add_geom(geom) - - def update(self): - for geom in self.geoms: - geom.update(self.scene.geoms[geom.geom_id]) \ No newline at end of file diff --git a/tuning/plotting_utils.py b/tuning/plotting_utils.py deleted file mode 100644 index 8f890055a..000000000 --- a/tuning/plotting_utils.py +++ /dev/null @@ -1,148 +0,0 @@ -import matplotlib.pyplot as plt -import numpy as np -import math -from multiprocessing import Process, Queue -import time -import dearpygui.dearpygui as dpg -from collections import deque - -DEQUE_MAX_LEN = 200 - -class _Plot: - def __init__(self, id, name, window_id, y_range = None): - self._id = id - self._window_id = window_id - self._name = name - self._data_count = 0 - self._y_range = y_range - self._xdata = deque(maxlen=DEQUE_MAX_LEN) - self._ydata = deque(maxlen=DEQUE_MAX_LEN) - - def _initialize_gui(self): - with dpg.plot(height=200, width=500): - dpg.add_plot_axis(dpg.mvXAxis, label="Time", tag=f"{self._window_id}-{self._name}-xaxis", time=True, no_tick_labels=True) - dpg.add_plot_axis(dpg.mvYAxis, label=self._name, tag=f"{self._window_id}-{self._name}-yaxis") - dpg.add_line_series([], [], tag=f'{self._window_id}-{self._name}-line', parent=f"{self._window_id}-{self._name}-yaxis") - if self._y_range: - dpg.set_axis_limits(f"{self._window_id}-{self._name}-yaxis", self._y_range[0], self._y_range[1]) - - - def _update_plot(self, x_data, y_data): - assert len(x_data) == len(y_data) - self._xdata.extend(x_data) - self._ydata.extend(y_data) - dpg.configure_item(f'{self._window_id}-{self._name}-line', x=list(self._xdata), y=list(self._ydata)) - dpg.fit_axis_data(f"{self._window_id}-{self._name}-xaxis") - if not self._y_range: - dpg.fit_axis_data(f"{self._window_id}-{self._name}-yaxis") - -class _Window: - def __init__(self, id, shape: tuple[2], name: str): - self._id = id - self._grid_shape: tuple[2] = shape - self._plots: list[_Plot] = [] - self._name = name - - @property - def max_num_plots(self): - return math.prod(self._grid_shape) - - @property - def num_plots(self): - return len(self._plots) - - def _initialize_gui(self): - with dpg.tab(tag = self._name, label=self._name): - with dpg.subplots(rows=self._grid_shape[0], columns=self._grid_shape[1], tag=f"{self._id}-subplots", height=1300, width=1100): - for plot in self._plots: - plot._initialize_gui() - - def _add_plot(self, name, plot_id = -1, y_range = None): - if plot_id == -1: - plot_id = len(self._plots) - assert plot_id < self.max_num_plots - plot = _Plot(id = plot_id, name=name, window_id=self._id, y_range=y_range) - self._plots.insert(plot_id, plot) - return plot - - def _add_points(self, x_data, y_data: list[np.ndarray]): - #assert (type(y_data) == list and len(points) == self.num_plots) or (type(points) == np.ndarray and points.shape[0] == self.num_plots) - y_data = np.stack(y_data, axis=-1) - for i, plot in enumerate(self._plots): - plot._update_plot(x_data, y_data[i, :]) - -class PlotManager(Process): - def __init__(self): - super(PlotManager, self).__init__() - self._windows: list[_Window] = [] - self._queues: list[Queue] = [] - self.daemon = True - - @property - def num_windows(self): - return len(self._windows) - - @property - def num_plots(self): - return sum(map(lambda window: window.num_plots, self._windows)) - - def add_window(self, shape: tuple[2], name: str): - w = _Window(id = len(self._windows), shape=shape, name=name) - q = Queue(maxsize=DEQUE_MAX_LEN) - self._windows.append(w) - self._queues.append(q) - return w._id - - def add_plot(self, window_id: int, plot_name: str, plot_range = None): - return self._windows[window_id]._add_plot(plot_name, y_range=plot_range) - - def send_data(self, window_id: int, data: tuple[float, np.ndarray]): - self._queues[window_id].put(data) - - def __initialize_gui(self): - dpg.create_context() - with dpg.window(tag = "Main Window"): - with dpg.tab_bar(): - for window in self._windows: - window._initialize_gui() - - def __flush_queue(self, queue_id: int): - tmp = [] - while not self._queues[queue_id].empty(): - tmp.append(self._queues[queue_id].get()) - if len(tmp) == 0: - return - x_data, y_data = zip(*tmp) - self._windows[queue_id]._add_points(x_data, y_data) - - def run(self): - self.__initialize_gui() - dpg.create_viewport(width=1100, height=1300, title='Updating plot data') - dpg.setup_dearpygui() - dpg.show_viewport() - dpg.set_primary_window("Main Window", True) - while dpg.is_dearpygui_running(): - start = time.perf_counter_ns() - for i in range(self.num_windows): - self.__flush_queue(i) - dpg.render_dearpygui_frame() - end = time.perf_counter_ns() - print(f"Time taken to render frame: {(end - start) / 1e6} ms") - print(f"FPS: {1 / ((end - start) / 1e9)}") - dpg.destroy_context() - -if __name__ == "__main__": - manager = PlotManager() - w = manager.add_window((2, 1), "Test window") - manager.add_plot(w, "Foot distance") - manager.add_plot(w, "Test distance") - w = manager.add_window((1, 2), "Test window 2") - manager.add_plot(w, "Foot distance") - manager.add_plot(w, "Test distance") - - manager.start() - for i in range(100000): - time.sleep(0.02) - manager.send_data(0, (time.time(), np.asarray([i, 100-i]))) - manager.send_data(1, (time.time(), np.asarray([i*2, np.sin(i/2.0)]))) - manager.join() \ No newline at end of file diff --git a/tuning/slider_manager.py b/tuning/slider_manager.py deleted file mode 100644 index 7efeec3bb..000000000 --- a/tuning/slider_manager.py +++ /dev/null @@ -1,82 +0,0 @@ -import dearpygui.dearpygui as dpg -from multiprocessing import Process, Array -import time - -class SliderManager(Process): - def __init__(self, name = "Slider Manager"): - super(SliderManager, self).__init__() - self.daemon = True - self.name = name - self.sliders_metadata = [] - self.__slider_values = None - self.slider_values_local = [] - - def __initialize_gui(self): - print("Initializing sliders") - for metadata in self.sliders_metadata: - slider = dpg.add_slider_float(label=metadata["name"], default_value=metadata["default_value"], min_value=metadata["min_value"], max_value=metadata["max_value"], callback=self.__update_slider_values) - metadata.update({"ref": slider}) - dpg.add_button(label="Reset", callback=self.__reset_sliders) - - def __update_slider_values(self): - with self.__slider_values.get_lock(): - for i, metadata in enumerate(self.sliders_metadata): - self.__slider_values[i] = dpg.get_value(metadata["ref"]) - - def __reset_sliders(self): - with self.__slider_values.get_lock(): - for i, metadata in enumerate(self.sliders_metadata): - dpg.set_value(metadata["ref"], metadata["default_value"]) - self.__slider_values[i] = metadata["default_value"] - - def start(self) -> None: - self.__slider_values = Array('f', [metadata["default_value"] for metadata in self.sliders_metadata]) - with self.__slider_values.get_lock(): - self.slider_values_local = self.__slider_values[:] - return super().start() - - def run(self): - dpg.create_context() - - with dpg.window(tag = "Main Window"): - self.__initialize_gui() - - dpg.create_viewport(width=900, height=600, title='Updating plot data') - dpg.setup_dearpygui() - dpg.show_viewport() - dpg.set_viewport_title(self.name) - dpg.set_primary_window("Main Window", True) - dpg.start_dearpygui() - dpg.destroy_context() - - def add_slider(self, name: str, default_value: float, min_value: float, max_value: float): - metadata = { - "name": name, - "default_value": default_value, - "min_value": min_value, - "max_value": max_value - } - self.sliders_metadata.append(metadata) - - def get_slider_values(self, blocking = True): - lock = self.__slider_values.get_lock() - is_lock_acquired = lock.acquire(block=blocking) - if is_lock_acquired: - self.slider_values_local = self.__slider_values[:] - lock.release() - return self.slider_values_local - - -if __name__ == "__main__": - slider_manager = SliderManager() - slider_manager.add_slider("Slider 1", 0.0, 0.0, 1.0) - slider_manager.add_slider("Slider 2", 0.0, 0.0, 1.0) - slider_manager.start() - - try: - while True: - print(slider_manager.get_slider_values(blocking=False)) - time.sleep(0.01) - except KeyboardInterrupt: - slider_manager.kill() - slider_manager.join() \ No newline at end of file From f902df5eadcf4903e7b5a014debf312dd64897ef Mon Sep 17 00:00:00 2001 From: Antonio Langella Date: Sun, 4 Aug 2024 09:50:11 +0200 Subject: [PATCH 12/24] [REM] H1 walk baseline task, as it shouldn't be in the PR --- mjpc/CMakeLists.txt | 2 - mjpc/tasks/h1/walk-baseline/task.xml | 94 ---------- mjpc/tasks/h1/walk-baseline/walk-baseline.cc | 180 ------------------- mjpc/tasks/h1/walk-baseline/walk-baseline.h | 66 ------- mjpc/tasks/tasks.cc | 2 - 5 files changed, 344 deletions(-) delete mode 100644 mjpc/tasks/h1/walk-baseline/task.xml delete mode 100644 mjpc/tasks/h1/walk-baseline/walk-baseline.cc delete mode 100644 mjpc/tasks/h1/walk-baseline/walk-baseline.h diff --git a/mjpc/CMakeLists.txt b/mjpc/CMakeLists.txt index 15538029e..e5800794f 100644 --- a/mjpc/CMakeLists.txt +++ b/mjpc/CMakeLists.txt @@ -50,8 +50,6 @@ add_library( tasks/fingers/fingers.h tasks/h1/walk/walk.cc tasks/h1/walk/walk.h - tasks/h1/walk-baseline/walk-baseline.cc - tasks/h1/walk-baseline/walk-baseline.h tasks/humanoid/stand/stand.cc tasks/humanoid/stand/stand.h tasks/humanoid/tracking/tracking.cc diff --git a/mjpc/tasks/h1/walk-baseline/task.xml b/mjpc/tasks/h1/walk-baseline/task.xml deleted file mode 100644 index 9fdb04780..000000000 --- a/mjpc/tasks/h1/walk-baseline/task.xml +++ /dev/null @@ -1,94 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/mjpc/tasks/h1/walk-baseline/walk-baseline.cc b/mjpc/tasks/h1/walk-baseline/walk-baseline.cc deleted file mode 100644 index 3bf7b5608..000000000 --- a/mjpc/tasks/h1/walk-baseline/walk-baseline.cc +++ /dev/null @@ -1,180 +0,0 @@ -// 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/tasks/h1/walk-baseline/walk-baseline.h" - -#include -#include - -#include -#include "mjpc/task.h" -#include "mjpc/utilities.h" - -namespace mjpc::h1 { -std::string WalkBaseline::XmlPath() const { - return GetModelPath("h1/walk-baseline/task.xml"); -} -std::string WalkBaseline::Name() const { return "H1 WalkBaseline Baseline"; } - -// ------------------ Residuals for humanoid walk task ------------ -// Number of residuals: -// Residual (0): torso height -// Residual (1): pelvis-feet aligment -// Residual (2): balance -// Residual (3): upright -// Residual (4): posture -// Residual (5): walk -// Residual (6): move feet -// Residual (7): control -// Number of parameters: -// Parameter (0): torso height goal -// Parameter (1): speed goal -// ---------------------------------------------------------------- -void WalkBaseline::ResidualFn::Residual(const mjModel* model, const mjData* data, - double* residual) const { - int counter = 0; - - // ----- torso height ----- // - double torso_height = SensorByName(model, data, "torso_position")[2]; - residual[counter++] = torso_height - parameters_[0]; - - // ----- pelvis / feet ----- // - double* foot_right = SensorByName(model, data, "foot_right"); - double* foot_left = SensorByName(model, data, "foot_left"); - double pelvis_height = SensorByName(model, data, "pelvis_position")[2]; - residual[counter++] = - 0.5 * (foot_left[2] + foot_right[2]) - pelvis_height - 0.2; - - // ----- balance ----- // - // capture point - double* subcom = SensorByName(model, data, "torso_subcom"); - double* subcomvel = SensorByName(model, data, "torso_subcomvel"); - - double capture_point[3]; - mju_addScl(capture_point, subcom, subcomvel, 0.3, 3); - capture_point[2] = 1.0e-3; - - // project onto line segment - - double axis[3]; - double center[3]; - double vec[3]; - double pcp[3]; - mju_sub3(axis, foot_right, foot_left); - axis[2] = 1.0e-3; - double length = 0.5 * mju_normalize3(axis) - 0.05; - mju_add3(center, foot_right, foot_left); - mju_scl3(center, center, 0.5); - mju_sub3(vec, capture_point, center); - - // project onto axis - double t = mju_dot3(vec, axis); - - // clamp - t = mju_max(-length, mju_min(length, t)); - mju_scl3(vec, axis, t); - mju_add3(pcp, vec, center); - pcp[2] = 1.0e-3; - - // is standing - double standing = - torso_height / mju_sqrt(torso_height * torso_height + 0.45 * 0.45) - 0.4; - - mju_sub(&residual[counter], capture_point, pcp, 2); - mju_scl(&residual[counter], &residual[counter], standing, 2); - - counter += 2; - - // ----- upright ----- // - double* torso_up = SensorByName(model, data, "torso_up"); - double* pelvis_up = SensorByName(model, data, "pelvis_up"); - double* foot_right_up = SensorByName(model, data, "foot_right_up"); - double* foot_left_up = SensorByName(model, data, "foot_left_up"); - double z_ref[3] = {0.0, 0.0, 1.0}; - - // torso - residual[counter++] = torso_up[2] - 1.0; - - // pelvis - residual[counter++] = 0.3 * (pelvis_up[2] - 1.0); - - // right foot - mju_sub3(&residual[counter], foot_right_up, z_ref); - mju_scl3(&residual[counter], &residual[counter], 0.1 * standing); - counter += 3; - - mju_sub3(&residual[counter], foot_left_up, z_ref); - mju_scl3(&residual[counter], &residual[counter], 0.1 * standing); - counter += 3; - // ----- posture up ----- // - mju_copy(&residual[counter], data->qpos + 7, model->nq - 7); - counter += model->nq - 7; - // ----- walk ----- // - double* torso_forward = SensorByName(model, data, "torso_forward"); - double* pelvis_forward = SensorByName(model, data, "pelvis_forward"); - double* foot_right_forward = SensorByName(model, data, "foot_right_forward"); - double* foot_left_forward = SensorByName(model, data, "foot_left_forward"); - - double forward[2]; - mju_copy(forward, torso_forward, 2); - mju_addTo(forward, pelvis_forward, 2); - mju_addTo(forward, foot_right_forward, 2); - mju_addTo(forward, foot_left_forward, 2); - mju_normalize(forward, 2); - - // com vel - double* waist_lower_subcomvel = - SensorByName(model, data, "waist_lower_subcomvel"); - double* torso_velocity = SensorByName(model, data, "torso_velocity"); - double com_vel[2]; - mju_add(com_vel, waist_lower_subcomvel, torso_velocity, 2); - mju_scl(com_vel, com_vel, 0.5, 2); - - // walk forward - residual[counter++] = - standing * (mju_dot(com_vel, forward, 2) - parameters_[1]); - - // ----- move feet ----- // - double* foot_right_vel = SensorByName(model, data, "foot_right_velocity"); - double* foot_left_vel = SensorByName(model, data, "foot_left_velocity"); - double move_feet[2]; - mju_copy(move_feet, com_vel, 2); - mju_addToScl(move_feet, foot_right_vel, -0.5, 2); - mju_addToScl(move_feet, foot_left_vel, -0.5, 2); - - mju_copy(&residual[counter], move_feet, 2); - mju_scl(&residual[counter], &residual[counter], standing, 2); - counter += 2; - - // ----- control ----- // - 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::h1 diff --git a/mjpc/tasks/h1/walk-baseline/walk-baseline.h b/mjpc/tasks/h1/walk-baseline/walk-baseline.h deleted file mode 100644 index e7a320867..000000000 --- a/mjpc/tasks/h1/walk-baseline/walk-baseline.h +++ /dev/null @@ -1,66 +0,0 @@ -// 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_H1_WALK_BASELINE_TASK_H_ -#define MJPC_TASKS_H1_WALK_BASELINE_TASK_H_ - -#include -#include "mjpc/task.h" - -namespace mjpc { -namespace h1 { - -class WalkBaseline : public Task { - public: - class ResidualFn : public mjpc::BaseResidualFn { - public: - explicit ResidualFn(const WalkBaseline* task) : mjpc::BaseResidualFn(task) {} - - // ------------------ Residuals for humanoid walk task ------------ - // Number of residuals: - // Residual (0): torso height - // Residual (1): pelvis-feet aligment - // Residual (2): balance - // Residual (3): upright - // Residual (4): posture - // Residual (5): walk - // Residual (6): move feet - // Residual (7): control - // Number of parameters: - // Parameter (0): torso height goal - // Parameter (1): speed goal - // ---------------------------------------------------------------- - void Residual(const mjModel* model, const mjData* data, - double* residual) const override; - }; - - WalkBaseline() : residual_(this) {} - - std::string Name() const override; - std::string XmlPath() const override; - - protected: - std::unique_ptr ResidualLocked() const override { - return std::make_unique(this); - } - ResidualFn* InternalResidual() override { return &residual_; } - - private: - ResidualFn residual_; -}; - -} // namespace h1 -} // namespace mjpc - -#endif // MJPC_TASKS_H1_WALK_BASELINE_TASK_H_ diff --git a/mjpc/tasks/tasks.cc b/mjpc/tasks/tasks.cc index f79756781..362c586a2 100644 --- a/mjpc/tasks/tasks.cc +++ b/mjpc/tasks/tasks.cc @@ -25,7 +25,6 @@ #include "mjpc/tasks/cartpole/cartpole.h" #include "mjpc/tasks/fingers/fingers.h" #include "mjpc/tasks/h1/walk/walk.h" -#include "mjpc/tasks/h1/walk-baseline/walk-baseline.h" #include "mjpc/tasks/humanoid/stand/stand.h" #include "mjpc/tasks/humanoid/tracking/tracking.h" #include "mjpc/tasks/humanoid/walk/walk.h" @@ -52,7 +51,6 @@ std::vector> GetTasks() { std::make_shared(), std::make_shared(), std::make_shared(), - std::make_shared(), std::make_shared(), std::make_shared(), std::make_shared(), From 994054c1d6df9a7914936e3bb38d3c52727d23ed Mon Sep 17 00:00:00 2001 From: Antonio Langella Date: Sun, 4 Aug 2024 10:09:05 +0200 Subject: [PATCH 13/24] [REM] Useless comments --- mjpc/tasks/h1/walk/task.xml | 70 ++++++++----------------------------- 1 file changed, 15 insertions(+), 55 deletions(-) diff --git a/mjpc/tasks/h1/walk/task.xml b/mjpc/tasks/h1/walk/task.xml index 24f2f17d4..e3ef018d1 100644 --- a/mjpc/tasks/h1/walk/task.xml +++ b/mjpc/tasks/h1/walk/task.xml @@ -28,14 +28,6 @@ - - @@ -47,69 +39,37 @@ - + - - - - - - + - - - - + + + + - - + + - - + + - - - - - + + + + + From d45fde7a13a74cb31189ac5e578dec01bb4436ce Mon Sep 17 00:00:00 2001 From: Antonio Langella Date: Sun, 4 Aug 2024 10:24:02 +0200 Subject: [PATCH 14/24] [DOC] Updated comments --- mjpc/tasks/h1/walk/walk.cc | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/mjpc/tasks/h1/walk/walk.cc b/mjpc/tasks/h1/walk/walk.cc index af7237163..cd9697906 100644 --- a/mjpc/tasks/h1/walk/walk.cc +++ b/mjpc/tasks/h1/walk/walk.cc @@ -33,14 +33,15 @@ std::string Walk::Name() const { return "H1 Walk"; } // Residual (1): pelvis-feet aligment // Residual (2): balance // Residual (3): upright -// Residual (4): posture -// Residual (5): face towards goal -// Residual (6): walk towards goal -// Residual (7): move feet -// Residual (8): control -// Residual (9): feet distance -// Residual (10): feet cross -// Residual (11): slippage +// Residual (4): upper posture +// Residual (5): lower posture +// Residual (6): face towards goal +// Residual (7): walk towards goal +// Residual (8): move feet +// Residual (9): control +// Residual (10): feet distance +// Residual (11): leg cross +// Residual (12): slippage // Number of parameters: // Parameter (0): torso height goal // Parameter (1): speed goal From cb20fdbe1a6f6e96ab6a791f27f128417abb8df0 Mon Sep 17 00:00:00 2001 From: Antonio Langella Date: Wed, 14 Aug 2024 10:23:55 +0200 Subject: [PATCH 15/24] [REF] Reduced agent horizon to 0.4s --- mjpc/tasks/h1/walk/task.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mjpc/tasks/h1/walk/task.xml b/mjpc/tasks/h1/walk/task.xml index e3ef018d1..0961619e4 100644 --- a/mjpc/tasks/h1/walk/task.xml +++ b/mjpc/tasks/h1/walk/task.xml @@ -19,7 +19,7 @@ - + From 636f8b75d5bd8187ca122e73ddc42e03672a98d8 Mon Sep 17 00:00:00 2001 From: Antonio Langella Date: Wed, 14 Aug 2024 10:24:58 +0200 Subject: [PATCH 16/24] [REF] Changed agent integrator to 'implicitfast' --- mjpc/tasks/h1/walk/task.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/mjpc/tasks/h1/walk/task.xml b/mjpc/tasks/h1/walk/task.xml index 0961619e4..10a95f774 100644 --- a/mjpc/tasks/h1/walk/task.xml +++ b/mjpc/tasks/h1/walk/task.xml @@ -21,6 +21,7 @@ + From 9dfcc14bb7b6e4e75b0b04274efe070f7d62d1b5 Mon Sep 17 00:00:00 2001 From: Antonio Langella Date: Wed, 14 Aug 2024 16:25:15 +0200 Subject: [PATCH 17/24] [DOC] Added spaces after '//' --- mjpc/tasks/h1/walk/walk.cc | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/mjpc/tasks/h1/walk/walk.cc b/mjpc/tasks/h1/walk/walk.cc index cd9697906..d2e51ca5a 100644 --- a/mjpc/tasks/h1/walk/walk.cc +++ b/mjpc/tasks/h1/walk/walk.cc @@ -125,10 +125,10 @@ void Walk::ResidualFn::Residual(const mjModel* model, const mjData* data, mju_scl3(&residual[counter], &residual[counter], 0.1 * standing); counter += 3; // ----- posture up ----- // - mju_copy(&residual[counter], data->qpos + 17, model->nq - 17); //First 7 are freejoint coord, the other 10 are lower body joints + mju_copy(&residual[counter], data->qpos + 17, model->nq - 17); // First 7 are freejoint coord, the other 10 are lower body joints counter += model->nq - 17; // ----- posture down ----- // - mju_copy(&residual[counter], data->qpos + 7, model->nq - 16); //First 7 are freejoint coord, the other 10 are lower body joints + mju_copy(&residual[counter], data->qpos + 7, model->nq - 16); // First 7 are freejoint coord, the other 10 are lower body joints counter += model->nq - 16; // ----- walk ----- // double* torso_forward = SensorByName(model, data, "torso_forward"); @@ -158,7 +158,7 @@ void Walk::ResidualFn::Residual(const mjModel* model, const mjData* data, // mju_scl(com_vel, com_vel, 0.5, 2); double com_vel[2]; - mju_copy(com_vel,subcomvel,2); //subcomvel is the velocity of the robot's CoM + mju_copy(com_vel,subcomvel,2); // subcomvel is the velocity of the robot's CoM // face forward residual[counter++] = From d73cd6a3460cdcbce64c2a13ac2d8f9cf0dd3d95 Mon Sep 17 00:00:00 2001 From: Antonio Langella Date: Wed, 14 Aug 2024 16:26:40 +0200 Subject: [PATCH 18/24] [REM] Removed useless function and comments --- mjpc/tasks/h1/walk/walk.cc | 19 ------------------- mjpc/tasks/h1/walk/walk.h | 1 - 2 files changed, 20 deletions(-) diff --git a/mjpc/tasks/h1/walk/walk.cc b/mjpc/tasks/h1/walk/walk.cc index d2e51ca5a..a475bdf49 100644 --- a/mjpc/tasks/h1/walk/walk.cc +++ b/mjpc/tasks/h1/walk/walk.cc @@ -149,14 +149,6 @@ void Walk::ResidualFn::Residual(const mjModel* model, const mjData* data, mju_sub(forward_target, goal_point, torso_position, 2); mju_normalize(forward_target, 2); - // com vel - // double* waist_lower_subcomvel = - // SensorByName(model, data, "waist_lower_subcomvel"); - // double* torso_velocity = SensorByName(model, data, "torso_velocity"); - // double com_vel[2]; - // mju_add(com_vel, waist_lower_subcomvel, torso_velocity, 2); - // mju_scl(com_vel, com_vel, 0.5, 2); - double com_vel[2]; mju_copy(com_vel,subcomvel,2); // subcomvel is the velocity of the robot's CoM @@ -224,15 +216,4 @@ void Walk::ResidualFn::Residual(const mjModel* model, const mjData* data, } } -double Walk::ResidualFn::StepHeight(double time, double footphase, - double duty_ratio) const { - double angle = fmod(time + mjPI - footphase, 2*mjPI) - mjPI; - double value = 0; - if (duty_ratio < 1) { - angle *= 0.5 / (1 - duty_ratio); - value = mju_cos(mju_clip(angle, -mjPI/2, mjPI/2)); - } - return mju_abs(value) < 1e-6 ? 0.0 : value; -} - } // namespace mjpc::h1 diff --git a/mjpc/tasks/h1/walk/walk.h b/mjpc/tasks/h1/walk/walk.h index 12708d94c..188299d1e 100644 --- a/mjpc/tasks/h1/walk/walk.h +++ b/mjpc/tasks/h1/walk/walk.h @@ -43,7 +43,6 @@ class Walk : public Task { // ---------------------------------------------------------------- void Residual(const mjModel* model, const mjData* data, double* residual) const override; - double StepHeight(double time, double footphase, double duty_ratio) const; }; Walk() : residual_(this) {} From 47228abb9846518bb44512f6e75cad73c3a75e98 Mon Sep 17 00:00:00 2001 From: Antonio Langella Date: Wed, 14 Aug 2024 16:28:55 +0200 Subject: [PATCH 19/24] [REF] Formatted code with 'clang-format' vscode extension --- mjpc/tasks/h1/walk/walk.cc | 87 ++++++++++++++++++++------------------ mjpc/tasks/h1/walk/walk.h | 24 +++++------ 2 files changed, 57 insertions(+), 54 deletions(-) diff --git a/mjpc/tasks/h1/walk/walk.cc b/mjpc/tasks/h1/walk/walk.cc index a475bdf49..e728ba412 100644 --- a/mjpc/tasks/h1/walk/walk.cc +++ b/mjpc/tasks/h1/walk/walk.cc @@ -17,14 +17,12 @@ #include #include -#include #include "mjpc/task.h" #include "mjpc/utilities.h" +#include namespace mjpc::h1 { -std::string Walk::XmlPath() const { - return GetModelPath("h1/walk/task.xml"); -} +std::string Walk::XmlPath() const { return GetModelPath("h1/walk/task.xml"); } std::string Walk::Name() const { return "H1 Walk"; } // ------------------ Residuals for humanoid walk task ------------ @@ -48,8 +46,8 @@ std::string Walk::Name() const { return "H1 Walk"; } // Parameter (2): feet distance goal // Parameter (3): balance speed // ---------------------------------------------------------------- -void Walk::ResidualFn::Residual(const mjModel* model, const mjData* data, - double* residual) const { +void Walk::ResidualFn::Residual(const mjModel *model, const mjData *data, + double *residual) const { int counter = 0; // ----- torso height ----- // @@ -57,16 +55,16 @@ void Walk::ResidualFn::Residual(const mjModel* model, const mjData* data, residual[counter++] = torso_height - parameters_[0]; // ----- pelvis / feet ----- // - double* foot_right = SensorByName(model, data, "foot_right"); - double* foot_left = SensorByName(model, data, "foot_left"); + double *foot_right = SensorByName(model, data, "foot_right"); + double *foot_left = SensorByName(model, data, "foot_left"); double pelvis_height = SensorByName(model, data, "pelvis_position")[2]; residual[counter++] = 0.5 * (foot_left[2] + foot_right[2]) - pelvis_height - 0.2; // ----- balance ----- // // capture point - double* subcom = SensorByName(model, data, "torso_subcom"); - double* subcomvel = SensorByName(model, data, "torso_subcomvel"); + double *subcom = SensorByName(model, data, "torso_subcom"); + double *subcomvel = SensorByName(model, data, "torso_subcomvel"); double capture_point[3]; mju_addScl(capture_point, subcom, subcomvel, parameters_[3], 3); @@ -96,7 +94,7 @@ void Walk::ResidualFn::Residual(const mjModel* model, const mjData* data, // is standing double standing = - torso_height / mju_sqrt(torso_height * torso_height + 0.45 * 0.45) - 0.4; + torso_height / mju_sqrt(torso_height * torso_height + 0.45 * 0.45) - 0.4; mju_sub(&residual[counter], capture_point, pcp, 2); mju_scl(&residual[counter], &residual[counter], standing, 2); @@ -104,10 +102,10 @@ void Walk::ResidualFn::Residual(const mjModel* model, const mjData* data, counter += 2; // ----- upright ----- // - double* torso_up = SensorByName(model, data, "torso_up"); - double* pelvis_up = SensorByName(model, data, "pelvis_up"); - double* foot_right_up = SensorByName(model, data, "foot_right_up"); - double* foot_left_up = SensorByName(model, data, "foot_left_up"); + double *torso_up = SensorByName(model, data, "torso_up"); + double *pelvis_up = SensorByName(model, data, "pelvis_up"); + double *foot_right_up = SensorByName(model, data, "foot_right_up"); + double *foot_left_up = SensorByName(model, data, "foot_left_up"); double z_ref[3] = {0.0, 0.0, 1.0}; // torso @@ -125,16 +123,20 @@ void Walk::ResidualFn::Residual(const mjModel* model, const mjData* data, mju_scl3(&residual[counter], &residual[counter], 0.1 * standing); counter += 3; // ----- posture up ----- // - mju_copy(&residual[counter], data->qpos + 17, model->nq - 17); // First 7 are freejoint coord, the other 10 are lower body joints + mju_copy(&residual[counter], data->qpos + 17, + model->nq - 17); // First 7 are freejoint coord, the other 10 are + // lower body joints counter += model->nq - 17; // ----- posture down ----- // - mju_copy(&residual[counter], data->qpos + 7, model->nq - 16); // First 7 are freejoint coord, the other 10 are lower body joints + mju_copy(&residual[counter], data->qpos + 7, + model->nq - 16); // First 7 are freejoint coord, the other 10 are + // lower body joints counter += model->nq - 16; // ----- walk ----- // - double* torso_forward = SensorByName(model, data, "torso_forward"); - double* pelvis_forward = SensorByName(model, data, "pelvis_forward"); - double* foot_right_forward = SensorByName(model, data, "foot_right_forward"); - double* foot_left_forward = SensorByName(model, data, "foot_left_forward"); + double *torso_forward = SensorByName(model, data, "torso_forward"); + double *pelvis_forward = SensorByName(model, data, "pelvis_forward"); + double *foot_right_forward = SensorByName(model, data, "foot_right_forward"); + double *foot_left_forward = SensorByName(model, data, "foot_left_forward"); double forward_target[2]; double forward[2]; @@ -144,25 +146,25 @@ void Walk::ResidualFn::Residual(const mjModel* model, const mjData* data, mju_addTo(forward, foot_left_forward, 2); mju_normalize(forward, 2); - double* goal_point = SensorByName(model, data, "goal"); - double* torso_position = SensorByName(model, data, "torso_position"); + double *goal_point = SensorByName(model, data, "goal"); + double *torso_position = SensorByName(model, data, "torso_position"); mju_sub(forward_target, goal_point, torso_position, 2); mju_normalize(forward_target, 2); double com_vel[2]; - mju_copy(com_vel,subcomvel,2); // subcomvel is the velocity of the robot's CoM + mju_copy(com_vel, subcomvel, + 2); // subcomvel is the velocity of the robot's CoM // face forward - residual[counter++] = - standing * (mju_dot(forward, forward_target, 2) - 1); + residual[counter++] = standing * (mju_dot(forward, forward_target, 2) - 1); // walk forward residual[counter++] = standing * (mju_dot(com_vel, forward_target, 2) - parameters_[1]); // ----- move feet ----- // - double* foot_right_vel = SensorByName(model, data, "foot_right_velocity"); - double* foot_left_vel = SensorByName(model, data, "foot_left_velocity"); + double *foot_right_vel = SensorByName(model, data, "foot_right_velocity"); + double *foot_left_vel = SensorByName(model, data, "foot_left_velocity"); double move_feet[2]; mju_copy(move_feet, com_vel, 2); mju_addToScl(move_feet, foot_right_vel, -0.5, 2); @@ -188,17 +190,19 @@ void Walk::ResidualFn::Residual(const mjModel* model, const mjData* data, double *left_foot_left_axis = SensorByName(model, data, "foot_left_left"); mju_sub3(vec, foot_right, foot_left); residual[counter++] = mju_dot3(vec, left_foot_left_axis) - 0.2; - // ----- slippage ----- // - double* foot_right_ang_velocity = SensorByName(model, data, "foot_right_ang_velocity"); - double* foot_left_ang_velocity = SensorByName(model, data, "foot_left_ang_velocity"); - double* right_foot_xbody = SensorByName(model, data, "foot_right_xbody"); - double* left_foot_xbody = SensorByName(model, data, "foot_left_xbody"); - - - residual[counter++] = (tanh(-(right_foot_xbody[2]-0.0645)/0.001)+1)*0.5*foot_right_ang_velocity[2]; - residual[counter++] = (tanh(-(left_foot_xbody[2]-0.0645)/0.001)+1)*0.5*foot_left_ang_velocity[2]; + double *foot_right_ang_velocity = + SensorByName(model, data, "foot_right_ang_velocity"); + double *foot_left_ang_velocity = + SensorByName(model, data, "foot_left_ang_velocity"); + double *right_foot_xbody = SensorByName(model, data, "foot_right_xbody"); + double *left_foot_xbody = SensorByName(model, data, "foot_left_xbody"); + + residual[counter++] = (tanh(-(right_foot_xbody[2] - 0.0645) / 0.001) + 1) * + 0.5 * foot_right_ang_velocity[2]; + residual[counter++] = (tanh(-(left_foot_xbody[2] - 0.0645) / 0.001) + 1) * + 0.5 * foot_left_ang_velocity[2]; // sensor dim sanity check // TODO: use this pattern everywhere and make this a utility function @@ -209,11 +213,10 @@ void Walk::ResidualFn::Residual(const mjModel* model, const mjData* data, } } if (user_sensor_dim != counter) { - mju_error_i( - "mismatch between total user-sensor dimension" - "and actual length of residual %d", - counter); + mju_error_i("mismatch between total user-sensor dimension" + "and actual length of residual %d", + counter); } } -} // namespace mjpc::h1 +} // namespace mjpc::h1 diff --git a/mjpc/tasks/h1/walk/walk.h b/mjpc/tasks/h1/walk/walk.h index 188299d1e..637f9ddaa 100644 --- a/mjpc/tasks/h1/walk/walk.h +++ b/mjpc/tasks/h1/walk/walk.h @@ -15,17 +15,17 @@ #ifndef MJPC_TASKS_H1_WALK_TASK_H_ #define MJPC_TASKS_H1_WALK_TASK_H_ -#include #include "mjpc/task.h" +#include namespace mjpc { namespace h1 { class Walk : public Task { - public: +public: class ResidualFn : public mjpc::BaseResidualFn { - public: - explicit ResidualFn(const Walk* task) : mjpc::BaseResidualFn(task) {} + public: + explicit ResidualFn(const Walk *task) : mjpc::BaseResidualFn(task) {} // ------------------ Residuals for humanoid walk task ------------ // Number of residuals: @@ -41,8 +41,8 @@ class Walk : public Task { // Parameter (0): torso height goal // Parameter (1): speed goal // ---------------------------------------------------------------- - void Residual(const mjModel* model, const mjData* data, - double* residual) const override; + void Residual(const mjModel *model, const mjData *data, + double *residual) const override; }; Walk() : residual_(this) {} @@ -50,17 +50,17 @@ class Walk : public Task { std::string Name() const override; std::string XmlPath() const override; - protected: +protected: std::unique_ptr ResidualLocked() const override { return std::make_unique(this); } - ResidualFn* InternalResidual() override { return &residual_; } + ResidualFn *InternalResidual() override { return &residual_; } - private: +private: ResidualFn residual_; }; -} // namespace h1 -} // namespace mjpc +} // namespace h1 +} // namespace mjpc -#endif // MJPC_TASKS_H1_WALK_TASK_H_ +#endif // MJPC_TASKS_H1_WALK_TASK_H_ From 65132ee0e53bc6245f335fcc02ddb862dda576d1 Mon Sep 17 00:00:00 2001 From: Antonio Langella Date: Wed, 14 Aug 2024 16:31:55 +0200 Subject: [PATCH 20/24] [REF] Removed links to local machine in patch file --- mjpc/tasks/h1/h1.xml.patch | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/mjpc/tasks/h1/h1.xml.patch b/mjpc/tasks/h1/h1.xml.patch index 71eff07cf..75b6fe26c 100644 --- a/mjpc/tasks/h1/h1.xml.patch +++ b/mjpc/tasks/h1/h1.xml.patch @@ -1,7 +1,6 @@ -diff --git a/home/antonio/uni/tesi/mujoco_mpc/build/_deps/menagerie-src/unitree_h1/h1.xml b/home/antonio/uni/tesi/mujoco_mpc/mjpc/tasks/h1/h1.xml -index 32d3fb9..dfe3948 100644 ---- a/home/antonio/uni/tesi/mujoco_mpc/build/_deps/menagerie-src/unitree_h1/h1.xml -+++ b/home/antonio/uni/tesi/mujoco_mpc/mjpc/tasks/h1/h1.xml +diff --git a/h1.xml b/h1.xml +--- a/h1.xml ++++ b/h1.xml @@ -1,5 +1,5 @@ - From 9954bc07e95a050bdcdc07614fa6511730bc9114 Mon Sep 17 00:00:00 2001 From: Antonio Langella Date: Tue, 3 Sep 2024 23:07:16 +0200 Subject: [PATCH 21/24] - [ADD] Robot now follows orientation reference (mocap quaternion) after reaching position reference (mocap position) - [REF] Improved leg cross detection in cost function --- mjpc/tasks/h1/walk/task.xml | 7 +++++-- mjpc/tasks/h1/walk/walk.cc | 33 ++++++++++++++++++++------------- 2 files changed, 25 insertions(+), 15 deletions(-) diff --git a/mjpc/tasks/h1/walk/task.xml b/mjpc/tasks/h1/walk/task.xml index 10a95f774..530e70ce8 100644 --- a/mjpc/tasks/h1/walk/task.xml +++ b/mjpc/tasks/h1/walk/task.xml @@ -39,12 +39,12 @@ - + - + @@ -66,11 +66,14 @@ + + + diff --git a/mjpc/tasks/h1/walk/walk.cc b/mjpc/tasks/h1/walk/walk.cc index e728ba412..9106971cf 100644 --- a/mjpc/tasks/h1/walk/walk.cc +++ b/mjpc/tasks/h1/walk/walk.cc @@ -149,19 +149,21 @@ void Walk::ResidualFn::Residual(const mjModel *model, const mjData *data, double *goal_point = SensorByName(model, data, "goal"); double *torso_position = SensorByName(model, data, "torso_position"); mju_sub(forward_target, goal_point, torso_position, 2); - mju_normalize(forward_target, 2); - + double goal_distance = mju_normalize(forward_target, 2); + // A function of the distance to the goal used to disable goal tracking when the goal is too close. + // To do this, we use a tanh function that tends to 0 when the goal is less than 30cm away and 1 otherwise. + double goal_distance_factor = std::tanh((goal_distance - 0.3) / 0.01) / 2.0 + 0.5; double com_vel[2]; - mju_copy(com_vel, subcomvel, - 2); // subcomvel is the velocity of the robot's CoM + mju_copy(com_vel, subcomvel, 2); // subcomvel is the velocity of the robot's CoM - // face forward - residual[counter++] = standing * (mju_dot(forward, forward_target, 2) - 1); + // Extract the goal forward direction from the goal point + double *goal_forward = SensorByName(model, data, "goal_forward"); - // walk forward - residual[counter++] = - standing * (mju_dot(com_vel, forward_target, 2) - parameters_[1]); + // face goal + residual[counter++] = standing * ((goal_distance_factor * mju_dot(forward, forward_target, 2) - 1) + (1.0-goal_distance_factor) * mju_dot(forward, goal_forward, 2) - 1); + // walk forward + residual[counter++] = standing * (mju_dot(com_vel, forward_target, 2) - parameters_[1]) * goal_distance_factor; // ----- move feet ----- // double *foot_right_vel = SensorByName(model, data, "foot_right_velocity"); double *foot_left_vel = SensorByName(model, data, "foot_left_velocity"); @@ -186,10 +188,15 @@ void Walk::ResidualFn::Residual(const mjModel *model, const mjData *data, residual[counter] = feet_distance - parameters_[2]; counter += 1; - // ----- feet cross ----- // - double *left_foot_left_axis = SensorByName(model, data, "foot_left_left"); - mju_sub3(vec, foot_right, foot_left); - residual[counter++] = mju_dot3(vec, left_foot_left_axis) - 0.2; + // ----- leg cross ----- // + //double *left_foot_left_axis = SensorByName(model, data, "foot_left_left"); + double *right_hip_roll = SensorByName(model, data, "right_hip_roll"); + double *left_hip_roll = SensorByName(model, data, "left_hip_roll"); + + // mju_sub3(vec, foot_right, foot_left); + // residual[counter++] = mju_dot3(vec, left_foot_left_axis) + 0.3; + residual[counter++] = *right_hip_roll - 0.15; + residual[counter++] = -(*left_hip_roll) - 0.15; // ----- slippage ----- // double *foot_right_ang_velocity = From 0e81d030caa83c9c71128b9d081d56e612eb5f52 Mon Sep 17 00:00:00 2001 From: Antonio Langella Date: Sat, 7 Sep 2024 15:28:36 +0200 Subject: [PATCH 22/24] - [FIX] Fixed a bug in the "Face goal" residual calculation - [REF] Split "Posture up" residual in "Posture arms" and "Posture torso" for a better control on the task - [FIX] Reduced "Leg cross" weight to 1 as it produced unwanted behaviors - [REM] Removed unused residual "Posture down" --- mjpc/tasks/h1/walk/task.xml | 6 +++--- mjpc/tasks/h1/walk/walk.cc | 40 +++++++++++++------------------------ 2 files changed, 17 insertions(+), 29 deletions(-) diff --git a/mjpc/tasks/h1/walk/task.xml b/mjpc/tasks/h1/walk/task.xml index 530e70ce8..bd57a6850 100644 --- a/mjpc/tasks/h1/walk/task.xml +++ b/mjpc/tasks/h1/walk/task.xml @@ -37,14 +37,14 @@ - - + + - + diff --git a/mjpc/tasks/h1/walk/walk.cc b/mjpc/tasks/h1/walk/walk.cc index 9106971cf..fd1c1d451 100644 --- a/mjpc/tasks/h1/walk/walk.cc +++ b/mjpc/tasks/h1/walk/walk.cc @@ -71,7 +71,6 @@ void Walk::ResidualFn::Residual(const mjModel *model, const mjData *data, capture_point[2] = 1.0e-3; // project onto line segment - double axis[3]; double center[3]; double vec[3]; @@ -93,8 +92,7 @@ void Walk::ResidualFn::Residual(const mjModel *model, const mjData *data, pcp[2] = 1.0e-3; // is standing - double standing = - torso_height / mju_sqrt(torso_height * torso_height + 0.45 * 0.45) - 0.4; + double standing = torso_height / mju_sqrt(torso_height * torso_height + 0.45 * 0.45) - 0.4; mju_sub(&residual[counter], capture_point, pcp, 2); mju_scl(&residual[counter], &residual[counter], standing, 2); @@ -122,16 +120,11 @@ void Walk::ResidualFn::Residual(const mjModel *model, const mjData *data, mju_sub3(&residual[counter], foot_left_up, z_ref); mju_scl3(&residual[counter], &residual[counter], 0.1 * standing); counter += 3; - // ----- posture up ----- // - mju_copy(&residual[counter], data->qpos + 17, - model->nq - 17); // First 7 are freejoint coord, the other 10 are - // lower body joints + // ----- torso & arms posture ----- // + // We ignore the 7 dof of the freejoint and the 10 dof of the lower body joints + mju_copy(&residual[counter], data->qpos + 17, model->nq - 17); + counter += model->nq - 17; - // ----- posture down ----- // - mju_copy(&residual[counter], data->qpos + 7, - model->nq - 16); // First 7 are freejoint coord, the other 10 are - // lower body joints - counter += model->nq - 16; // ----- walk ----- // double *torso_forward = SensorByName(model, data, "torso_forward"); double *pelvis_forward = SensorByName(model, data, "pelvis_forward"); @@ -159,11 +152,12 @@ void Walk::ResidualFn::Residual(const mjModel *model, const mjData *data, // Extract the goal forward direction from the goal point double *goal_forward = SensorByName(model, data, "goal_forward"); - // face goal - residual[counter++] = standing * ((goal_distance_factor * mju_dot(forward, forward_target, 2) - 1) + (1.0-goal_distance_factor) * mju_dot(forward, goal_forward, 2) - 1); + // Face towards goal point when the goal is far away, and face towards the goal forward direction when the goal is close. + residual[counter++] = standing * (goal_distance_factor * (mju_dot(forward, forward_target, 2) - 1) + (1.0-goal_distance_factor) * (mju_dot(forward, goal_forward, 2) - 1)); - // walk forward + // Walk towards the goal point when the goal is far away residual[counter++] = standing * (mju_dot(com_vel, forward_target, 2) - parameters_[1]) * goal_distance_factor; + // ----- move feet ----- // double *foot_right_vel = SensorByName(model, data, "foot_right_velocity"); double *foot_left_vel = SensorByName(model, data, "foot_left_velocity"); @@ -189,27 +183,21 @@ void Walk::ResidualFn::Residual(const mjModel *model, const mjData *data, counter += 1; // ----- leg cross ----- // - //double *left_foot_left_axis = SensorByName(model, data, "foot_left_left"); double *right_hip_roll = SensorByName(model, data, "right_hip_roll"); double *left_hip_roll = SensorByName(model, data, "left_hip_roll"); - // mju_sub3(vec, foot_right, foot_left); - // residual[counter++] = mju_dot3(vec, left_foot_left_axis) + 0.3; residual[counter++] = *right_hip_roll - 0.15; residual[counter++] = -(*left_hip_roll) - 0.15; // ----- slippage ----- // - double *foot_right_ang_velocity = - SensorByName(model, data, "foot_right_ang_velocity"); - double *foot_left_ang_velocity = - SensorByName(model, data, "foot_left_ang_velocity"); + // This term is experimental and is used to penalize the robot for slipping. + double *foot_right_ang_velocity = SensorByName(model, data, "foot_right_ang_velocity"); + double *foot_left_ang_velocity = SensorByName(model, data, "foot_left_ang_velocity"); double *right_foot_xbody = SensorByName(model, data, "foot_right_xbody"); double *left_foot_xbody = SensorByName(model, data, "foot_left_xbody"); - residual[counter++] = (tanh(-(right_foot_xbody[2] - 0.0645) / 0.001) + 1) * - 0.5 * foot_right_ang_velocity[2]; - residual[counter++] = (tanh(-(left_foot_xbody[2] - 0.0645) / 0.001) + 1) * - 0.5 * foot_left_ang_velocity[2]; + residual[counter++] = (tanh(-(right_foot_xbody[2] - 0.0645) / 0.001) + 1) * 0.5 * foot_right_ang_velocity[2]; + residual[counter++] = (tanh(-(left_foot_xbody[2] - 0.0645) / 0.001) + 1) * 0.5 * foot_left_ang_velocity[2]; // sensor dim sanity check // TODO: use this pattern everywhere and make this a utility function From 0282b2723e3b97e280b85362d4dba12aa5db729b Mon Sep 17 00:00:00 2001 From: Antonio Langella Date: Sat, 7 Sep 2024 15:28:53 +0200 Subject: [PATCH 23/24] [DOC] Updated comment --- mjpc/tasks/h1/walk/walk.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mjpc/tasks/h1/walk/walk.cc b/mjpc/tasks/h1/walk/walk.cc index fd1c1d451..9fecd2146 100644 --- a/mjpc/tasks/h1/walk/walk.cc +++ b/mjpc/tasks/h1/walk/walk.cc @@ -31,8 +31,8 @@ std::string Walk::Name() const { return "H1 Walk"; } // Residual (1): pelvis-feet aligment // Residual (2): balance // Residual (3): upright -// Residual (4): upper posture -// Residual (5): lower posture +// Residual (4): torso posture +// Residual (5): arms posture // Residual (6): face towards goal // Residual (7): walk towards goal // Residual (8): move feet From 6cb23e4a1b3bb6a1a3382e927012a117fd2ea7e9 Mon Sep 17 00:00:00 2001 From: Antonio Langella Date: Mon, 23 Sep 2024 16:10:38 +0200 Subject: [PATCH 24/24] [REF] 'Face Goal' now takes into account the four orientations separately --- mjpc/tasks/h1/walk/task.xml | 2 +- mjpc/tasks/h1/walk/walk.cc | 6 +++++- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/mjpc/tasks/h1/walk/task.xml b/mjpc/tasks/h1/walk/task.xml index bd57a6850..05084dbb7 100644 --- a/mjpc/tasks/h1/walk/task.xml +++ b/mjpc/tasks/h1/walk/task.xml @@ -39,7 +39,7 @@ - + diff --git a/mjpc/tasks/h1/walk/walk.cc b/mjpc/tasks/h1/walk/walk.cc index 9fecd2146..95e397030 100644 --- a/mjpc/tasks/h1/walk/walk.cc +++ b/mjpc/tasks/h1/walk/walk.cc @@ -153,7 +153,11 @@ void Walk::ResidualFn::Residual(const mjModel *model, const mjData *data, double *goal_forward = SensorByName(model, data, "goal_forward"); // Face towards goal point when the goal is far away, and face towards the goal forward direction when the goal is close. - residual[counter++] = standing * (goal_distance_factor * (mju_dot(forward, forward_target, 2) - 1) + (1.0-goal_distance_factor) * (mju_dot(forward, goal_forward, 2) - 1)); + //residual[counter++] = standing * (goal_distance_factor * (mju_dot(forward, forward_target, 2) - 1) + (1.0-goal_distance_factor) * (mju_dot(forward, goal_forward, 2) - 1)); + residual[counter++] = standing * (goal_distance_factor * (mju_dot(torso_forward, forward_target, 2) - 1) + (1.0-goal_distance_factor) * (mju_dot(torso_forward, goal_forward, 2) - 1)); + residual[counter++] = standing * (goal_distance_factor * (mju_dot(pelvis_forward, forward_target, 2) - 1) + (1.0-goal_distance_factor) * (mju_dot(pelvis_forward, goal_forward, 2) - 1)); + residual[counter++] = standing * (goal_distance_factor * (mju_dot(foot_right_forward, forward_target, 2) - 1) + (1.0-goal_distance_factor) * (mju_dot(foot_right_forward, goal_forward, 2) - 1)); + residual[counter++] = standing * (goal_distance_factor * (mju_dot(foot_left_forward, forward_target, 2) - 1) + (1.0-goal_distance_factor) * (mju_dot(foot_left_forward, goal_forward, 2) - 1)); // Walk towards the goal point when the goal is far away residual[counter++] = standing * (mju_dot(com_vel, forward_target, 2) - parameters_[1]) * goal_distance_factor;