diff --git a/mjpc/CMakeLists.txt b/mjpc/CMakeLists.txt index f8fb30c3e..ed2874567 100644 --- a/mjpc/CMakeLists.txt +++ b/mjpc/CMakeLists.txt @@ -50,6 +50,8 @@ 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/humanoid/interact/interact.cc tasks/humanoid/interact/interact.h tasks/humanoid/interact/contact_keyframe.cc 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()); diff --git a/mjpc/tasks/CMakeLists.txt b/mjpc/tasks/CMakeLists.txt index c8e345d78..02e140504 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..75b6fe26c --- /dev/null +++ b/mjpc/tasks/h1/h1.xml.patch @@ -0,0 +1,187 @@ +diff --git a/h1.xml b/h1.xml +--- a/h1.xml ++++ b/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/task.xml b/mjpc/tasks/h1/walk/task.xml new file mode 100644 index 000000000..05084dbb7 --- /dev/null +++ b/mjpc/tasks/h1/walk/task.xml @@ -0,0 +1,79 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/mjpc/tasks/h1/walk/walk.cc b/mjpc/tasks/h1/walk/walk.cc new file mode 100644 index 000000000..95e397030 --- /dev/null +++ b/mjpc/tasks/h1/walk/walk.cc @@ -0,0 +1,221 @@ +// 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 "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::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): torso posture +// Residual (5): arms 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 +// Parameter (2): feet distance goal +// Parameter (3): balance speed +// ---------------------------------------------------------------- +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, parameters_[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; + // ----- 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; + // ----- 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_target[2]; + 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); + + double *goal_point = SensorByName(model, data, "goal"); + double *torso_position = SensorByName(model, data, "torso_position"); + mju_sub(forward_target, goal_point, torso_position, 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 + + // Extract the goal forward direction from the goal point + 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(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; + + // ----- 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 - parameters_[2]; + counter += 1; + + // ----- leg cross ----- // + double *right_hip_roll = SensorByName(model, data, "right_hip_roll"); + double *left_hip_roll = SensorByName(model, data, "left_hip_roll"); + + residual[counter++] = *right_hip_roll - 0.15; + residual[counter++] = -(*left_hip_roll) - 0.15; + + // ----- slippage ----- // + // 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]; + + // 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..637f9ddaa --- /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 "mjpc/task.h" +#include + +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 5fc29bdfd..cca1075e7 100644 --- a/mjpc/tasks/tasks.cc +++ b/mjpc/tasks/tasks.cc @@ -25,6 +25,7 @@ #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/humanoid/interact/interact.h" #include "mjpc/tasks/humanoid/stand/stand.h" #include "mjpc/tasks/humanoid/tracking/tracking.h" @@ -52,6 +53,7 @@ std::vector> GetTasks() { 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)