diff --git a/CMakeLists.txt b/CMakeLists.txt index 8239b4118..a3cb79520 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -212,5 +212,5 @@ endif() set(MJPC_COMPILE_OPTIONS "${AVX_COMPILE_OPTIONS}" "${EXTRA_COMPILE_OPTIONS}") set(MJPC_LINK_OPTIONS "${EXTRA_LINK_OPTIONS}") +add_definitions(-DSOURCE_DIR=\"${CMAKE_SOURCE_DIR}\") add_subdirectory(mjpc) - diff --git a/mjpc/CMakeLists.txt b/mjpc/CMakeLists.txt index f8fb30c3e..f49c65e12 100644 --- a/mjpc/CMakeLists.txt +++ b/mjpc/CMakeLists.txt @@ -54,6 +54,8 @@ add_library( tasks/humanoid/interact/interact.h tasks/humanoid/interact/contact_keyframe.cc tasks/humanoid/interact/contact_keyframe.h + tasks/humanoid/interact/motion_strategy.cc + tasks/humanoid/interact/motion_strategy.h tasks/humanoid/stand/stand.cc tasks/humanoid/stand/stand.h tasks/humanoid/tracking/tracking.cc @@ -163,6 +165,7 @@ target_link_libraries( mujoco::platform_ui_adapter threadpool Threads::Threads + nlohmann_json::nlohmann_json ) target_include_directories(libmjpc PUBLIC @@ -236,3 +239,7 @@ endif() if(MJPC_BUILD_GRPC_SERVICE) add_subdirectory(grpc) endif() + +include(FetchContent) +FetchContent_Declare(json URL https://github.com/nlohmann/json/releases/download/v3.11.3/json.tar.xz) +FetchContent_MakeAvailable(json) \ No newline at end of file diff --git a/mjpc/task.cc b/mjpc/task.cc index 3c0ee3d91..4778337b5 100644 --- a/mjpc/task.cc +++ b/mjpc/task.cc @@ -202,6 +202,7 @@ void Task::Reset(const mjModel* model) { // loop over sensors int parameter_shift = 0; + weight_names.resize(num_term); for (int i = 0; i < num_term; i++) { // residual dimension num_residual += model->sensor_dim[i]; @@ -209,6 +210,7 @@ void Task::Reset(const mjModel* model) { // user data: [norm, weight, weight_lower, weight_upper, parameters...] double* s = model->sensor_user + i * model->nuser_sensor; + char* name = model->names + model->name_sensoradr[i]; // check number of parameters int norm_parameter_dimension = NormParameterDimension(s[0]); @@ -231,6 +233,7 @@ void Task::Reset(const mjModel* model) { } weight[i] = s[1]; + weight_names[i] = name; num_norm_parameter[i] = norm_parameter_dimension; mju_copy(DataAt(norm_parameter, parameter_shift), s + 4, num_norm_parameter[i]); diff --git a/mjpc/task.h b/mjpc/task.h index 5f8d0cdc5..4579e00c1 100644 --- a/mjpc/task.h +++ b/mjpc/task.h @@ -139,6 +139,7 @@ class Task { std::vector num_norm_parameter; std::vector norm; std::vector weight; + std::vector weight_names; std::vector norm_parameter; double risk; diff --git a/mjpc/tasks/humanoid/interact/contact_keyframe.cc b/mjpc/tasks/humanoid/interact/contact_keyframe.cc index b009d2e61..84a7ac375 100644 --- a/mjpc/tasks/humanoid/interact/contact_keyframe.cc +++ b/mjpc/tasks/humanoid/interact/contact_keyframe.cc @@ -27,6 +27,18 @@ void ContactPair::Reset() { } } +void ContactPair::GetDistance(mjtNum distance[3], const mjData* data) const { + mjtNum selected_global_pos1[3] = {0.}; + mju_mulMatVec(selected_global_pos1, data->xmat + 9 * body1, local_pos1, 3, 3); + mju_addTo3(selected_global_pos1, data->xpos + 3 * body1); + + mjtNum selected_global_pos2[3] = {0.}; + mju_mulMatVec(selected_global_pos2, data->xmat + 9 * body2, local_pos2, 3, 3); + mju_addTo3(selected_global_pos2, data->xpos + 3 * body2); + + mju_sub3(distance, selected_global_pos1, selected_global_pos2); +} + void ContactKeyframe::Reset() { name.clear(); for (auto& contact_pair : contact_pairs) contact_pair.Reset(); @@ -34,4 +46,42 @@ void ContactKeyframe::Reset() { facing_target.clear(); weight.clear(); } + +void to_json(json& j, const ContactPair& contact_pair) { + j = json{{"body1", contact_pair.body1}, + {"body2", contact_pair.body2}, + {"geom1", contact_pair.geom1}, + {"geom2", contact_pair.geom2}, + {"local_pos1", contact_pair.local_pos1}, + {"local_pos2", contact_pair.local_pos2}}; +} + +void from_json(const json& j, ContactPair& contact_pair) { + j.at("body1").get_to(contact_pair.body1); + j.at("body2").get_to(contact_pair.body2); + j.at("geom1").get_to(contact_pair.geom1); + j.at("geom2").get_to(contact_pair.geom2); + j.at("local_pos1").get_to(contact_pair.local_pos1); + j.at("local_pos2").get_to(contact_pair.local_pos2); +} + +void to_json(json& j, const ContactKeyframe& keyframe) { + j = json{{"name", keyframe.name}, + {"contacts", keyframe.contact_pairs}, + {"facing_target", keyframe.facing_target}, + {"time_limit", keyframe.time_limit}, + {"success_sustain_time", keyframe.success_sustain_time}, + {"target_distance_tolerance", keyframe.target_distance_tolerance}, + {"weight", keyframe.weight}}; +} + +void from_json(const json& j, ContactKeyframe& keyframe) { + j.at("name").get_to(keyframe.name); + j.at("contacts").get_to(keyframe.contact_pairs); + j.at("facing_target").get_to(keyframe.facing_target); + j.at("time_limit").get_to(keyframe.time_limit); + j.at("success_sustain_time").get_to(keyframe.success_sustain_time); + j.at("target_distance_tolerance").get_to(keyframe.target_distance_tolerance); + j.at("weight").get_to(keyframe.weight); +} } // namespace mjpc::humanoid diff --git a/mjpc/tasks/humanoid/interact/contact_keyframe.h b/mjpc/tasks/humanoid/interact/contact_keyframe.h index 53090f05f..8b3481140 100644 --- a/mjpc/tasks/humanoid/interact/contact_keyframe.h +++ b/mjpc/tasks/humanoid/interact/contact_keyframe.h @@ -21,12 +21,23 @@ #include +#include "nlohmann/json.hpp" +using json = nlohmann::json; + namespace mjpc::humanoid { // ---------- Constants ----------------- // constexpr int kNotSelectedInteract = -1; constexpr int kNumberOfContactPairsInteract = 5; +// ---------- Enums --------------------- // +enum ContactKeyframeErrorType : int { + kMax = 0, + kMean = 1, + kSum = 2, + kNorm = 3, +}; + class ContactPair { public: int body1, body2, geom1, geom2; @@ -41,6 +52,9 @@ class ContactPair { local_pos2{0.} {} void Reset(); + + // populates the distance vector between the two contact points + void GetDistance(mjtNum distance[3], const mjData* data) const; }; class ContactKeyframe { @@ -54,11 +68,32 @@ class ContactKeyframe { // weight of all residual terms (name -> value map) std::map weight; - ContactKeyframe() : name(""), contact_pairs{}, facing_target(), weight() {} + ContactKeyframe() + : name(""), + contact_pairs{}, + facing_target(), + weight(), + time_limit(10.), + success_sustain_time(2.), + target_distance_tolerance(0.1) {} void Reset(); + + mjtNum time_limit; // maximum time (in seconds) allowed for attempting a + // single keyframe before resetting + mjtNum success_sustain_time; // minimum time (in seconds) that the objective + // needs to be satisfied within the distance + // threshold to consider the keyframe successful + mjtNum target_distance_tolerance; // the proximity to the keyframe objective + // that needs to be maintained for a + // certain time }; +void to_json(json& j, const ContactPair& contact_pair); +void from_json(const json& j, ContactPair& contact_pair); +void to_json(json& j, const ContactKeyframe& keyframe); +void from_json(const json& j, ContactKeyframe& keyframe); + } // namespace mjpc::humanoid #endif // MJPC_TASKS_HUMANOID_INTERACT_CONTACT_KEYFRAME_H_ diff --git a/mjpc/tasks/humanoid/interact/interact.cc b/mjpc/tasks/humanoid/interact/interact.cc index a64b95854..b89ef6fc4 100644 --- a/mjpc/tasks/humanoid/interact/interact.cc +++ b/mjpc/tasks/humanoid/interact/interact.cc @@ -27,12 +27,12 @@ std::string Interact::XmlPath() const { } std::string Interact::Name() const { return "Humanoid Interact"; } -// ------------------ Residuals for humanoid contact keyframes task ------------ +// --------------- Helper residual functions ----------------- // void Interact::ResidualFn::UpResidual(const mjModel* model, const mjData* data, double* residual, std::string&& name, int* counter) const { name.append("_up"); - double* up_vector = SensorByName(model, data, name); + const double* up_vector = SensorByName(model, data, name); residual[(*counter)++] = mju_abs(up_vector[2] - 1.0); } @@ -40,7 +40,7 @@ void Interact::ResidualFn::HeadHeightResidual(const mjModel* model, const mjData* data, double* residual, int* counter) const { - double head_height = SensorByName(model, data, "head_position")[2]; + const double head_height = SensorByName(model, data, "head_position")[2]; residual[(*counter)++] = mju_abs(head_height - parameters_[kHeadHeightParameterIndex]); } @@ -49,7 +49,7 @@ void Interact::ResidualFn::TorsoHeightResidual(const mjModel* model, const mjData* data, double* residual, int* counter) const { - double torso_height = SensorByName(model, data, "torso_position")[2]; + const double torso_height = SensorByName(model, data, "torso_position")[2]; residual[(*counter)++] = mju_abs(torso_height - parameters_[kTorsoHeightParameterIndex]); } @@ -58,19 +58,17 @@ void Interact::ResidualFn::KneeFeetXYResidual(const mjModel* model, const mjData* data, double* residual, int* counter) const { - double* knee_right = SensorByName(model, data, "knee_right"); - double* knee_left = SensorByName(model, data, "knee_left"); - double* foot_right = SensorByName(model, data, "foot_right"); - double* foot_left = SensorByName(model, data, "foot_left"); + const double* knee_right = SensorByName(model, data, "knee_right"); + const double* knee_left = SensorByName(model, data, "knee_left"); + const double* foot_right = SensorByName(model, data, "foot_right"); + const double* foot_left = SensorByName(model, data, "foot_left"); double knee_xy_avg[2] = {0.0}; - mju_addTo(knee_xy_avg, knee_left, 2); - mju_addTo(knee_xy_avg, knee_right, 2); + mju_add(knee_xy_avg, knee_right, knee_left, 2); mju_scl(knee_xy_avg, knee_xy_avg, 0.5, 2); double foot_xy_avg[2] = {0.0}; - mju_addTo(foot_xy_avg, foot_left, 2); - mju_addTo(foot_xy_avg, foot_right, 2); + mju_add(foot_xy_avg, foot_right, foot_left, 2); mju_scl(foot_xy_avg, foot_xy_avg, 0.5, 2); mju_subFrom(knee_xy_avg, foot_xy_avg, 2); @@ -86,12 +84,11 @@ void Interact::ResidualFn::COMFeetXYResidual(const mjModel* model, double* com_position = SensorByName(model, data, "torso_subtreecom"); double foot_xy_avg[2] = {0.0}; - mju_addTo(foot_xy_avg, foot_left, 2); - mju_addTo(foot_xy_avg, foot_right, 2); + mju_add(foot_xy_avg, foot_right, foot_left, 2); mju_scl(foot_xy_avg, foot_xy_avg, 0.5, 2); - mju_subFrom(com_position, foot_xy_avg, 2); - residual[(*counter)++] = mju_norm(com_position, 2); + mju_subFrom(foot_xy_avg, com_position, 2); + residual[(*counter)++] = mju_norm(foot_xy_avg, 2); } void Interact::ResidualFn::FacingDirectionResidual(const mjModel* model, @@ -105,8 +102,8 @@ void Interact::ResidualFn::FacingDirectionResidual(const mjModel* model, double* torso_forward = SensorByName(model, data, "torso_forward"); double* torso_position = mjpc::SensorByName(model, data, "torso_position"); - double target[2] = {0.}; + double target[2] = {0.}; mju_sub(target, residual_keyframe_.facing_target.data(), torso_position, 2); mju_normalize(target, 2); mju_subFrom(target, torso_forward, 2); @@ -120,31 +117,49 @@ void Interact::ResidualFn::ContactResidual(const mjModel* model, const ContactPair& contact = residual_keyframe_.contact_pairs[i]; if (contact.body1 != kNotSelectedInteract && contact.body2 != kNotSelectedInteract) { - mjtNum tmp1[3]; - mjtNum selected_global_pos1[3]; - mju_mulMatVec(tmp1, data->xmat + 9 * contact.body1, contact.local_pos1, 3, - 3); - mju_add3(selected_global_pos1, tmp1, data->xpos + 3 * contact.body1); - - mjtNum tmp2[3]; - mjtNum selected_global_pos2[3]; - mju_mulMatVec(tmp2, data->xmat + 9 * contact.body2, contact.local_pos2, 3, - 3); - mju_add3(selected_global_pos2, tmp2, data->xpos + 3 * contact.body2); - double dist[3] = {0.}; - mju_addTo(dist, selected_global_pos1, 3); - mju_subFrom(dist, selected_global_pos2, 3); - residual[(*counter)++] = mju_abs(dist[0]); - residual[(*counter)++] = mju_abs(dist[1]); - residual[(*counter)++] = mju_abs(dist[2]); + contact.GetDistance(dist, data); + for (int j = 0; j < 3; j++) residual[(*counter)++] = mju_abs(dist[j]); } else { for (int j = 0; j < 3; j++) residual[(*counter)++] = 0; } } } -// ------------------ Residuals for humanoid interaction task ------------ +void Interact::SaveParamsToKeyframe(ContactKeyframe& kf) const { + for (int i = 0; i < weight_names.size(); i++) { + kf.weight[weight_names[i]] = weight[i]; + } + kf.target_distance_tolerance = parameters[kDistanceToleranceParameterIndex]; + kf.time_limit = parameters[kTimeLimitParameterIndex]; + kf.success_sustain_time = parameters[kSustainTimeParameterIndex]; +} + +void Interact::LoadParamsFromKeyframe(const ContactKeyframe& kf) { + ContactKeyframe current_keyframe = motion_strategy_.GetCurrentKeyframe(); + weight.clear(); + int index = 0; + for (auto& w : weight_names) { + if (kf.weight.find(w) != kf.weight.end()) { + weight.push_back(kf.weight.at(w)); + } else { + double default_weight = + default_weights[residual_.current_task_mode_][index]; + std::printf( + "Keyframe %s does not have weight for %s, set default %.1f.\n", + kf.name.c_str(), w.c_str(), default_weight); + weight.push_back(default_weight); + } + current_keyframe.weight[w] = weight[index]; + index++; + } + current_keyframe.name = kf.name; + parameters[kDistanceToleranceParameterIndex] = kf.target_distance_tolerance; + parameters[kTimeLimitParameterIndex] = kf.time_limit; + parameters[kSustainTimeParameterIndex] = kf.success_sustain_time; +} + +// ---------------- Residuals for humanoid interaction task ----------- // // Number of residuals: 13 // Residual (0): Torso up // Residual (1): Pelvis up @@ -162,7 +177,7 @@ void Interact::ResidualFn::ContactResidual(const mjModel* model, // Number of parameters: 2 // Parameter (0): head_height_goal // Parameter (1): torso_height_goal -// ---------------------------------------------------------------- +// -------------------------------------------------------------------- // void Interact::ResidualFn::Residual(const mjModel* model, const mjData* data, double* residual) const { int counter = 0; @@ -199,46 +214,78 @@ void Interact::ResidualFn::Residual(const mjModel* model, const mjData* data, CheckSensorDim(model, counter); } -// -------- Transition for interaction task -------- +// -------- Transition for interaction task -------- // void Interact::TransitionLocked(mjModel* model, mjData* data) { - // If the task mode is changed, sync it with the residual here + // If the task mode is changed, sync it with the residual here if (residual_.current_task_mode_ != mode) { residual_.current_task_mode_ = (TaskMode)mode; weight = default_weights[residual_.current_task_mode_]; } + + // If the motion strategy is not initialized, load the given strategy + if (!motion_strategy_.HasKeyframes()) { + motion_strategy_.LoadStrategy("armchair_cross_leg"); + LoadParamsFromKeyframe(motion_strategy_.GetCurrentKeyframe()); + return; + } + + const ContactKeyframe& current_keyframe = + motion_strategy_.GetCurrentKeyframe(); + const double total_distance = motion_strategy_.CalculateTotalKeyframeDistance( + data, ContactKeyframeErrorType::kNorm); + + if (data->time - motion_strategy_.GetCurrentKeyframeStartTime() > + current_keyframe.time_limit && + total_distance > current_keyframe.target_distance_tolerance) { + // timelimit reached but distance criteria not reached, reset + motion_strategy_.Reset(); + LoadParamsFromKeyframe(motion_strategy_.GetCurrentKeyframe()); + residual_.residual_keyframe_ = motion_strategy_.GetCurrentKeyframe(); + motion_strategy_.SetCurrentKeyframeStartTime(data->time); + } else if (total_distance <= current_keyframe.target_distance_tolerance && + data->time - + motion_strategy_.GetCurrentKeyframeSuccessStartTime() > + current_keyframe.success_sustain_time) { + // success criteria reached, go to the next keyframe + motion_strategy_.NextKeyframe(); + LoadParamsFromKeyframe(motion_strategy_.GetCurrentKeyframe()); + residual_.residual_keyframe_ = motion_strategy_.GetCurrentKeyframe(); + } else if (total_distance > current_keyframe.target_distance_tolerance) { + // keyframe error is more than tolerance, update the success start time + motion_strategy_.SetCurrentKeyframeSuccessStartTime(data->time); + } + SaveParamsToKeyframe(motion_strategy_.GetCurrentKeyframe()); } // draw task-related geometry in the scene void Interact::ModifyScene(const mjModel* model, const mjData* data, mjvScene* scene) const { // add visuals for the contact points + mjtNum global_pos[3] = {0.}; for (int i = 0; i < kNumberOfContactPairsInteract; i++) { - double sz[3] = {0.03}; const ContactPair contact = residual_.residual_keyframe_.contact_pairs[i]; if (contact.body1 != kNotSelectedInteract) { - mjtNum global[3]; - mju_mulMatVec(global, data->xmat + 9 * contact.body1, contact.local_pos1, - 3, 3); - mju_addTo(global, data->xpos + 3 * contact.body1, 3); - AddGeom(scene, mjGEOM_SPHERE, sz, global, nullptr, - CONTACT_POINTS_COLOR[i]); + mju_mulMatVec(global_pos, data->xmat + 9 * contact.body1, + contact.local_pos1, 3, 3); + mju_addTo(global_pos, data->xpos + 3 * contact.body1, 3); + AddGeom(scene, mjGEOM_SPHERE, kVisualPointSize, global_pos, nullptr, + kContactPairColor[i]); } if (contact.body2 != kNotSelectedInteract) { - mjtNum global[3]; - mju_mulMatVec(global, data->xmat + 9 * contact.body2, contact.local_pos2, - 3, 3); - mju_addTo(global, data->xpos + 3 * contact.body2, 3); - AddGeom(scene, mjGEOM_SPHERE, sz, global, nullptr, - CONTACT_POINTS_COLOR[i]); + mju_mulMatVec(global_pos, data->xmat + 9 * contact.body2, + contact.local_pos2, 3, 3); + mju_addTo(global_pos, data->xpos + 3 * contact.body2, 3); + AddGeom(scene, mjGEOM_SPHERE, kVisualPointSize, global_pos, nullptr, + kContactPairColor[i]); } } // add visuals for the facing direction if (!residual_.residual_keyframe_.facing_target.empty()) { - double sz[3] = {0.02}; mjtNum pos[3] = {residual_.residual_keyframe_.facing_target[0], residual_.residual_keyframe_.facing_target[1], 0}; - AddGeom(scene, mjGEOM_SPHERE, sz, pos, nullptr, FACING_DIRECTION_COLOR); + AddGeom(scene, mjGEOM_SPHERE, kVisualPointSize, pos, nullptr, + kFacingDirectionColor); } } diff --git a/mjpc/tasks/humanoid/interact/interact.h b/mjpc/tasks/humanoid/interact/interact.h index df8eb2a20..2c5de834d 100644 --- a/mjpc/tasks/humanoid/interact/interact.h +++ b/mjpc/tasks/humanoid/interact/interact.h @@ -22,13 +22,17 @@ #include "mjpc/task.h" #include "mjpc/tasks/humanoid/interact/contact_keyframe.h" +#include "mjpc/tasks/humanoid/interact/motion_strategy.h" namespace mjpc::humanoid { // ---------- Constants ----------------- // +constexpr int kNumberOfFreeJoints = 0; constexpr int kHeadHeightParameterIndex = 0; constexpr int kTorsoHeightParameterIndex = 1; -constexpr int kNumberOfFreeJoints = 0; +constexpr int kDistanceToleranceParameterIndex = 2; +constexpr int kTimeLimitParameterIndex = 3; +constexpr int kSustainTimeParameterIndex = 4; // ---------- Enums ----------------- // enum TaskMode : int { @@ -47,14 +51,15 @@ const std::vector> default_weights = { }; // ----------- Default colors for the contact pair points ------------ // -constexpr float CONTACT_POINTS_COLOR[kNumberOfContactPairsInteract][4] = { +constexpr float kContactPairColor[kNumberOfContactPairsInteract][4] = { {0., 0., 1., 0.8}, // blue {0., 1., 0., 0.8}, // green {0., 1., 1., 0.8}, // cyan {1., 0., 0., 0.8}, // red {1., 0., 1., 0.8}, // magenta }; -constexpr float FACING_DIRECTION_COLOR[] = {1., 1., 1., 0.8}; +constexpr float kFacingDirectionColor[] = {1., 1., 1., 0.8}; +constexpr double kVisualPointSize[3] = {0.02}; class Interact : public Task { public: @@ -120,6 +125,10 @@ class Interact : public Task { private: ResidualFn residual_; + MotionStrategy motion_strategy_; + + void SaveParamsToKeyframe(ContactKeyframe& kf) const; + void LoadParamsFromKeyframe(const ContactKeyframe& kf); // draw task-related geometry in the scene void ModifyScene(const mjModel* model, const mjData* data, diff --git a/mjpc/tasks/humanoid/interact/motion_strategy.cc b/mjpc/tasks/humanoid/interact/motion_strategy.cc new file mode 100644 index 000000000..22916ed30 --- /dev/null +++ b/mjpc/tasks/humanoid/interact/motion_strategy.cc @@ -0,0 +1,147 @@ +// 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/humanoid/interact/motion_strategy.h" + +#include "mjpc/utilities.h" + +namespace mjpc::humanoid { + +void MotionStrategy::Clear() { + contact_keyframes_.clear(); + contact_keyframes_[current_keyframe_index_].Reset(); + current_keyframe_index_ = 0; +} + +void MotionStrategy::Reset() { current_keyframe_index_ = 0; } + +int MotionStrategy::NextKeyframe() { + current_keyframe_index_ = + (current_keyframe_index_ + 1) % contact_keyframes_.size(); + return current_keyframe_index_; +} + +void MotionStrategy::ClearKeyframes() { + contact_keyframes_.clear(); + current_keyframe_index_ = 0; +} + +double MotionStrategy::CalculateTotalKeyframeDistance( + const mjData* data, const ContactKeyframeErrorType error_type) const { + double error[kNumberOfContactPairsInteract] = {0.}; + + // iterate through all pairs + for (int i = 0; i < kNumberOfContactPairsInteract; i++) { + const ContactPair& contact = + contact_keyframes_[current_keyframe_index_].contact_pairs[i]; + if (contact.body1 != kNotSelectedInteract && + contact.body2 != kNotSelectedInteract) { + double dist[3] = {0.}; + contact.GetDistance(dist, data); + error[i] = mju_norm3(dist); + } + } + + double total_error = 0.; + switch (error_type) { + case ContactKeyframeErrorType::kMax: + total_error = + *std::max_element(error, error + kNumberOfContactPairsInteract); + break; + case ContactKeyframeErrorType::kMean: + total_error = + std::accumulate(error, error + kNumberOfContactPairsInteract, + kNumberOfContactPairsInteract) / + kNumberOfContactPairsInteract; + break; + case ContactKeyframeErrorType::kSum: + total_error = + std::accumulate(error, error + kNumberOfContactPairsInteract, + kNumberOfContactPairsInteract); + break; + case ContactKeyframeErrorType::kNorm: + default: + total_error = mju_norm(error, kNumberOfContactPairsInteract); + break; + } + return total_error; +} + +bool MotionStrategy::SaveStrategy(const std::string& name, + const std::string& path) { + std::string filename(path); + filename.append(name); + filename.append(".json"); + + try { + json json_keyframes = json::array(); + json json_kf; + for (auto& kf: contact_keyframes_) { + to_json(json_kf, kf); + json_keyframes.push_back(json_kf); + } + + std::ofstream of(filename); + if (!of.is_open()) { + std::printf("\nFailed to open file %s", filename.c_str()); + return false; + } + of << std::setw(4) << json_keyframes << std::endl; + of.close(); + std::printf("\nKeyframe sequence saved as %s", filename.c_str()); + return true; + } catch (const std::exception &e) { + std::cerr << e.what() << '\n'; + return false; + } +} + +bool MotionStrategy::LoadStrategy(const std::string& name, + const std::string& path) { + std::string filename(path); + filename.append(name); + filename.append(".json"); + try { + std::ifstream file(filename); + if (!file.good()) { + std::printf("\nFile %s does not exist", filename.c_str()); + return false; + } + contact_keyframes_.clear(); + json json_keyframes = json::parse(file); + ContactKeyframe kf; + for (auto& json_kf: json_keyframes) { + from_json(json_kf, kf); + contact_keyframes_.push_back(kf); + } + + current_keyframe_index_ = 0; + std::printf("\nKeyframe sequence loaded from %s", filename.c_str()); + return true; + } catch (const std::exception &e) { + std::cerr << e.what() << '\n'; + return false; + } +} + +void to_json(json& j, const MotionStrategy& strategy) { + j = json{{"contact_keyframes", strategy.GetContactKeyframes()}}; +} + +void from_json(const json& j, MotionStrategy& strategy) { + strategy.SetContactKeyframes( + j.at("contact_keyframes").get>()); +} + +} // namespace mjpc::humanoid diff --git a/mjpc/tasks/humanoid/interact/motion_strategy.h b/mjpc/tasks/humanoid/interact/motion_strategy.h new file mode 100644 index 000000000..548756321 --- /dev/null +++ b/mjpc/tasks/humanoid/interact/motion_strategy.h @@ -0,0 +1,102 @@ +// Copyright 2022 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MJPC_TASKS_HUMANOID_INTERACT_MOTION_STRATEGY_H_ +#define MJPC_TASKS_HUMANOID_INTERACT_MOTION_STRATEGY_H_ + +#include + +#include +#include +#include +#include + +#include "mjpc/tasks/humanoid/interact/contact_keyframe.h" +#include "nlohmann/json.hpp" +using json = nlohmann::json; + +namespace mjpc::humanoid { + +constexpr char kMotionStrategyFilePath[] = + SOURCE_DIR "/mjpc/tasks/humanoid/interact/strategies/"; + +/* +This class holds the motion strategy, e.g. given a sequence of keyframes, it +manages the initial and current state and any changes to the sequence. +*/ +class MotionStrategy { + public: + MotionStrategy() = default; + ~MotionStrategy() = default; + + explicit MotionStrategy(const std::vector& keyframes) + : contact_keyframes_(keyframes), current_keyframe_index_(0) {} + + void Reset(); + void Clear(); + + bool HasKeyframes() const { return !contact_keyframes_.empty(); } + ContactKeyframe& GetCurrentKeyframe() { + return contact_keyframes_[current_keyframe_index_]; + } + const ContactKeyframe& GetCurrentKeyframe() const { + return contact_keyframes_[current_keyframe_index_]; + } + const int GetCurrentKeyframeIndex() const { return current_keyframe_index_; } + const std::vector& GetContactKeyframes() const { + return contact_keyframes_; + } + const int GetKeyframesCount() const { return contact_keyframes_.size(); } + const mjtNum GetCurrentKeyframeStartTime() const { + return current_keyframe_start_time_; + } + void SetCurrentKeyframeStartTime(const mjtNum start_time) { + current_keyframe_start_time_ = start_time; + } + const mjtNum GetCurrentKeyframeSuccessStartTime() const { + return current_keyframe_success_start_time_; + } + void SetCurrentKeyframeSuccessStartTime(const mjtNum start_time) { + current_keyframe_success_start_time_ = start_time; + } + void UpdateCurrentKeyframe(const int index) { + current_keyframe_index_ = index; + } + void SetContactKeyframes(const std::vector& keyframes) { + contact_keyframes_ = keyframes; + } + bool SaveStrategy(const std::string& name, + const std::string& path = kMotionStrategyFilePath); + bool LoadStrategy(const std::string& name, + const std::string& path = kMotionStrategyFilePath); + + int NextKeyframe(); + void ClearKeyframes(); + + double CalculateTotalKeyframeDistance( + const mjData* data, const ContactKeyframeErrorType error_type = + ContactKeyframeErrorType::kNorm) const; + + private: + std::vector contact_keyframes_; + int current_keyframe_index_; + mjtNum current_keyframe_start_time_; + mjtNum current_keyframe_success_start_time_; +}; + +void to_json(json& j, const MotionStrategy& strategy); +void from_json(const json& j, MotionStrategy& strategy); +} // namespace mjpc::humanoid + +#endif // MJPC_TASKS_HUMANOID_INTERACT_MOTION_STRATEGY_H_ diff --git a/mjpc/tasks/humanoid/interact/strategies/armchair_cross_leg.json b/mjpc/tasks/humanoid/interact/strategies/armchair_cross_leg.json new file mode 100644 index 000000000..64a70fb63 --- /dev/null +++ b/mjpc/tasks/humanoid/interact/strategies/armchair_cross_leg.json @@ -0,0 +1,737 @@ +[ + { + "contacts": [ + { + "body1": 4, + "body2": 21, + "geom1": 5, + "geom2": 20, + "local_pos1": [ + -0.084515, + -0.003438, + -0.062752 + ], + "local_pos2": [ + -0.077371, + 0.042382, + 0.17 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + -0.061753, + -0.020781, + -0.032963 + ], + "local_pos2": [ + -0.368475, + -0.015674, + 0.54212 + ] + }, + { + "body1": 7, + "body2": 0, + "geom1": 9, + "geom2": 0, + "local_pos1": [ + 0.058889, + 0.014053, + -0.02573 + ], + "local_pos2": [ + 0.059334, + -0.217544, + 0.0 + ] + }, + { + "body1": 12, + "body2": 0, + "geom1": 12, + "geom2": 0, + "local_pos1": [ + 0.066571, + -0.012282, + -0.0248 + ], + "local_pos2": [ + 0.088113, + 0.237827, + 0.0 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + -0.08998, + 0.011944, + -0.056593 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + } + ], + "facing_target": [], + "name": "armchair1", + "success_sustain_time": 1.0, + "target_distance_tolerance": 0.1, + "time_limit": 5.0, + "weight": { + "COM Feet XY": 0.0, + "CoM Vel.": 10.0, + "Contact": 100.0, + "Control": 0.8, + "Facing Dir": 0.0, + "Head Height": 0.0, + "Joint Vel.": 0.01, + "Knee Feet XY": 30.0, + "LFoot Up": 5.0, + "Pelvis Up": 35.0, + "RFoot Up": 5.0, + "Torse Target": 0.0, + "Torso Height": 20.0, + "Torso Up": 33.5 + } + }, + { + "contacts": [ + { + "body1": 4, + "body2": 21, + "geom1": 5, + "geom2": 20, + "local_pos1": [ + -0.084515, + -0.003438, + -0.062752 + ], + "local_pos2": [ + -0.092556, + 0.029488, + 0.17 + ] + }, + { + "body1": 19, + "body2": 21, + "geom1": 18, + "geom2": 23, + "local_pos1": [ + 0.064681, + -0.020846, + 0.043366 + ], + "local_pos2": [ + 0.071386, + 0.337136, + 0.4 + ] + }, + { + "body1": 7, + "body2": 0, + "geom1": 9, + "geom2": 0, + "local_pos1": [ + 0.058889, + 0.014053, + -0.02573 + ], + "local_pos2": [ + 0.095554, + -0.013252, + 0.0 + ] + }, + { + "body1": 11, + "body2": 5, + "geom1": 11, + "geom2": 6, + "local_pos1": [ + -0.043389, + 0.022768, + -0.192149 + ], + "local_pos2": [ + 0.038766, + -0.021936, + -0.372823 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + -0.08998, + 0.011944, + -0.056593 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + } + ], + "facing_target": [], + "name": "cross_leg2", + "success_sustain_time": 0.1, + "target_distance_tolerance": 0.15, + "time_limit": 5.0, + "weight": { + "COM Feet XY": 0.0, + "CoM Vel.": 10.0, + "Contact": 100.0, + "Control": 0.8, + "Facing Dir": 0.0, + "Head Height": 0.0, + "Joint Vel.": 0.01, + "Knee Feet XY": 0.0, + "LFoot Up": 2.0, + "Pelvis Up": 30.0, + "RFoot Up": 6.0, + "Torse Target": 0.0, + "Torso Height": 11.0, + "Torso Up": 15.0 + } + }, + { + "contacts": [ + { + "body1": 4, + "body2": 21, + "geom1": 5, + "geom2": 20, + "local_pos1": [ + -0.084515, + -0.003438, + -0.062752 + ], + "local_pos2": [ + -0.092556, + 0.029488, + 0.17 + ] + }, + { + "body1": 19, + "body2": 21, + "geom1": 18, + "geom2": 23, + "local_pos1": [ + 0.021537272060050648, + -0.004767054472087867, + -0.018293243619553066 + ], + "local_pos2": [ + -0.09448593902482194, + 0.34203181769059077, + 0.4000000000000001 + ] + }, + { + "body1": 7, + "body2": 0, + "geom1": 9, + "geom2": 0, + "local_pos1": [ + 0.058889, + 0.014053, + -0.02573 + ], + "local_pos2": [ + 0.095554, + -0.013252, + 0.0 + ] + }, + { + "body1": 11, + "body2": 5, + "geom1": 11, + "geom2": 6, + "local_pos1": [ + -0.048367, + 0.00785, + -0.062881 + ], + "local_pos2": [ + 0.038766, + -0.021936, + -0.372823 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + -0.08998, + 0.011944, + -0.056593 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + } + ], + "facing_target": [], + "name": "cross_leg3", + "success_sustain_time": 1.5, + "target_distance_tolerance": 0.15, + "time_limit": 5.0, + "weight": { + "COM Feet XY": 0.0, + "CoM Vel.": 10.0, + "Contact": 100.0, + "Control": 0.8, + "Facing Dir": 0.0, + "Head Height": 0.0, + "Joint Vel.": 0.01, + "Knee Feet XY": 0.0, + "LFoot Up": 2.0, + "Pelvis Up": 30.0, + "RFoot Up": 6.0, + "Torse Target": 0.0, + "Torso Height": 11.0, + "Torso Up": 15.0 + } + }, + { + "contacts": [ + { + "body1": 4, + "body2": 21, + "geom1": 5, + "geom2": 20, + "local_pos1": [ + -0.084515, + -0.003438, + -0.062752 + ], + "local_pos2": [ + -0.08014673480749479, + -0.03976267195304996, + 0.17000000000000032 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + -0.061753, + -0.020781, + -0.032963 + ], + "local_pos2": [ + -0.368475, + -0.015674, + 0.54212 + ] + }, + { + "body1": 7, + "body2": 0, + "geom1": 9, + "geom2": 0, + "local_pos1": [ + 0.058889, + 0.014053, + -0.02573 + ], + "local_pos2": [ + 0.059334, + -0.217544, + 0.0 + ] + }, + { + "body1": 12, + "body2": 0, + "geom1": 12, + "geom2": 0, + "local_pos1": [ + 0.066571, + -0.012282, + -0.0248 + ], + "local_pos2": [ + 0.088113, + 0.237827, + 0.0 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + -0.08998, + 0.011944, + -0.056593 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + } + ], + "facing_target": [], + "name": "armchair1", + "success_sustain_time": 2, + "target_distance_tolerance": 0.1, + "time_limit": 5.0, + "weight": { + "COM Feet XY": 0.0, + "CoM Vel.": 10.0, + "Contact": 100.0, + "Control": 0.8, + "Facing Dir": 0.0, + "Head Height": 0.0, + "Joint Vel.": 0.01, + "Knee Feet XY": 30.0, + "LFoot Up": 5.0, + "Pelvis Up": 31.5, + "RFoot Up": 5.0, + "Torse Target": 0.0, + "Torso Height": 20.0, + "Torso Up": 31.5 + } + }, + { + "contacts": [ + { + "body1": 4, + "body2": 21, + "geom1": 5, + "geom2": 20, + "local_pos1": [ + -0.084515, + -0.003438, + -0.062752 + ], + "local_pos2": [ + -0.092556, + 0.029488, + 0.17 + ] + }, + { + "body1": 16, + "body2": 21, + "geom1": 15, + "geom2": 24, + "local_pos1": [ + 0.039681060012276254, + 0.018789468493973872, + -0.004143711957572487 + ], + "local_pos2": [ + 0.06069253443111744, + -0.3490693397120561, + 0.4000000000000001 + ] + }, + { + "body1": 12, + "body2": 0, + "geom1": 12, + "geom2": 0, + "local_pos1": [ + 0.07272529036074993, + -0.0029474710441867325, + -0.017509945740931912 + ], + "local_pos2": [ + 0.095554, + -0.013252, + 0.0 + ] + }, + { + "body1": 6, + "body2": 10, + "geom1": 7, + "geom2": 10, + "local_pos1": [ + -0.04898023184503099, + -0.0013917213825888515, + -0.178876283210221 + ], + "local_pos2": [ + 0.031124442999517297, + 0.03747898359476125, + -0.3594168783479228 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + -0.08998, + 0.011944, + -0.056593 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + } + ], + "facing_target": [], + "name": "cross_leg_r2", + "success_sustain_time": 0.1, + "target_distance_tolerance": 0.15, + "time_limit": 5.0, + "weight": { + "COM Feet XY": 0.0, + "CoM Vel.": 10.0, + "Contact": 100.0, + "Control": 0.8, + "Facing Dir": 0.0, + "Head Height": 0.0, + "Joint Vel.": 0.01, + "Knee Feet XY": 0.0, + "LFoot Up": 6.5, + "Pelvis Up": 30.0, + "RFoot Up": 2.5, + "Torse Target": 0.0, + "Torso Height": 11.0, + "Torso Up": 15.0 + } + }, + { + "contacts": [ + { + "body1": 4, + "body2": 21, + "geom1": 5, + "geom2": 20, + "local_pos1": [ + -0.084515, + -0.003438, + -0.062752 + ], + "local_pos2": [ + -0.092556, + 0.029488, + 0.17 + ] + }, + { + "body1": 16, + "body2": 21, + "geom1": 15, + "geom2": 24, + "local_pos1": [ + 0.039681060012276254, + 0.018789468493973872, + -0.004143711957572487 + ], + "local_pos2": [ + 0.06069253443111744, + -0.3490693397120561, + 0.4000000000000001 + ] + }, + { + "body1": 12, + "body2": 0, + "geom1": 12, + "geom2": 0, + "local_pos1": [ + 0.07272529036074993, + -0.0029474710441867325, + -0.017509945740931912 + ], + "local_pos2": [ + 0.095554, + -0.013252, + 0.0 + ] + }, + { + "body1": 6, + "body2": 10, + "geom1": 7, + "geom2": 10, + "local_pos1": [ + -0.04897752674128333, + 0.00148387139143712, + -0.0897084728192637 + ], + "local_pos2": [ + 0.03486590634446894, + 0.03735299636408763, + -0.3519189894756823 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + -0.08998, + 0.011944, + -0.056593 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + } + ], + "facing_target": [], + "name": "cross_leg_r3", + "success_sustain_time": 1.5, + "target_distance_tolerance": 0.15, + "time_limit": 5.0, + "weight": { + "COM Feet XY": 0.0, + "CoM Vel.": 10.0, + "Contact": 100.0, + "Control": 0.8, + "Facing Dir": 0.0, + "Head Height": 0.0, + "Joint Vel.": 0.01, + "Knee Feet XY": 0.0, + "LFoot Up": 7.000000000000001, + "Pelvis Up": 30.0, + "RFoot Up": 3.5000000000000004, + "Torse Target": 0.0, + "Torso Height": 11.0, + "Torso Up": 15.0 + } + }, + { + "contacts": [ + { + "body1": 4, + "body2": 21, + "geom1": 5, + "geom2": 20, + "local_pos1": [ + -0.084515, + -0.003438, + -0.062752 + ], + "local_pos2": [ + -0.08414333223402826, + -0.0017080035532556614, + 0.1700000000000001 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + -0.061753, + -0.020781, + -0.032963 + ], + "local_pos2": [ + -0.368475, + -0.015674, + 0.54212 + ] + }, + { + "body1": 7, + "body2": 0, + "geom1": 9, + "geom2": 0, + "local_pos1": [ + 0.058889, + 0.014053, + -0.02573 + ], + "local_pos2": [ + 0.059334, + -0.217544, + 0.0 + ] + }, + { + "body1": 12, + "body2": 0, + "geom1": 12, + "geom2": 0, + "local_pos1": [ + 0.066571, + -0.012282, + -0.0248 + ], + "local_pos2": [ + 0.088113, + 0.237827, + 0.0 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + -0.08998, + 0.011944, + -0.056593 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + } + ], + "facing_target": [], + "name": "armchair1", + "success_sustain_time": 1.0, + "target_distance_tolerance": 0.1, + "time_limit": 5.0, + "weight": { + "COM Feet XY": 0.0, + "CoM Vel.": 10.0, + "Contact": 100.0, + "Control": 0.8, + "Facing Dir": 0.0, + "Head Height": 0.0, + "Joint Vel.": 0.01, + "Knee Feet XY": 30.0, + "LFoot Up": 5.0, + "Pelvis Up": 31.5, + "RFoot Up": 5.0, + "Torse Target": 0.0, + "Torso Height": 20.0, + "Torso Up": 31.5 + } + } +] \ No newline at end of file diff --git a/mjpc/tasks/humanoid/interact/strategies/armchair_relax.json b/mjpc/tasks/humanoid/interact/strategies/armchair_relax.json new file mode 100644 index 000000000..d8da83ea4 --- /dev/null +++ b/mjpc/tasks/humanoid/interact/strategies/armchair_relax.json @@ -0,0 +1,737 @@ +[ + { + "contacts": [ + { + "body1": 4, + "body2": 21, + "geom1": 5, + "geom2": 20, + "local_pos1": [ + -0.084515, + -0.003438, + -0.062752 + ], + "local_pos2": [ + -0.077371, + 0.042382, + 0.17 + ] + }, + { + "body1": 1, + "body2": 21, + "geom1": 2, + "geom2": 22, + "local_pos1": [ + -0.064812, + -0.022854, + -0.144406 + ], + "local_pos2": [ + -0.330955, + 0.001345, + 0.441806 + ] + }, + { + "body1": 7, + "body2": 0, + "geom1": 9, + "geom2": 0, + "local_pos1": [ + 0.058889, + 0.014053, + -0.02573 + ], + "local_pos2": [ + 0.059334, + -0.217544, + 0.0 + ] + }, + { + "body1": 12, + "body2": 0, + "geom1": 12, + "geom2": 0, + "local_pos1": [ + 0.066571, + -0.012282, + -0.0248 + ], + "local_pos2": [ + 0.088113, + 0.237827, + 0.0 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + -0.08998, + 0.011944, + -0.056593 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + } + ], + "facing_target": [], + "name": "armchair_relax1", + "success_sustain_time": 0.5, + "target_distance_tolerance": 0.2, + "time_limit": 5.0, + "weight": { + "COM Feet XY": 0.0, + "CoM Vel.": 10.0, + "Contact": 100.0, + "Control": 0.2, + "Facing Dir": 0.0, + "Head Height": 0.0, + "Joint Vel.": 0.01, + "Knee Feet XY": 30.0, + "LFoot Up": 5.0, + "Pelvis Up": 5.0, + "RFoot Up": 5.0, + "Torse Target": 0.0, + "Torso Height": 0.0, + "Torso Up": 5.5 + } + }, + { + "contacts": [ + { + "body1": 4, + "body2": 21, + "geom1": 5, + "geom2": 20, + "local_pos1": [ + -0.084515, + -0.003438, + -0.062752 + ], + "local_pos2": [ + 0.038509, + 0.001444, + 0.17 + ] + }, + { + "body1": 1, + "body2": 21, + "geom1": 1, + "geom2": 22, + "local_pos1": [ + -0.059872, + -0.006664, + -0.036268 + ], + "local_pos2": [ + -0.330955, + 0.001345, + 0.441806 + ] + }, + { + "body1": 7, + "body2": 0, + "geom1": 9, + "geom2": 0, + "local_pos1": [ + 0.058889, + 0.014053, + -0.02573 + ], + "local_pos2": [ + 0.059334, + -0.217544, + 0.0 + ] + }, + { + "body1": 12, + "body2": 0, + "geom1": 12, + "geom2": 0, + "local_pos1": [ + 0.066571, + -0.012282, + -0.0248 + ], + "local_pos2": [ + 0.088113, + 0.237827, + 0.0 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + -0.08998, + 0.011944, + -0.056593 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + } + ], + "facing_target": [], + "name": "armchair_relax2", + "success_sustain_time": 0.3, + "target_distance_tolerance": 0.3, + "time_limit": 5.0, + "weight": { + "COM Feet XY": 0.0, + "CoM Vel.": 10.0, + "Contact": 100.0, + "Control": 0.2, + "Facing Dir": 0.0, + "Head Height": 0.0, + "Joint Vel.": 0.01, + "Knee Feet XY": 0.0, + "LFoot Up": 5.0, + "Pelvis Up": 5.0, + "RFoot Up": 5.0, + "Torse Target": 0.0, + "Torso Height": 0.0, + "Torso Up": 5.5 + } + }, + { + "contacts": [ + { + "body1": 4, + "body2": 21, + "geom1": 5, + "geom2": 20, + "local_pos1": [ + -0.084515, + -0.003438, + -0.062752 + ], + "local_pos2": [ + 0.038509, + 0.001444, + 0.17 + ] + }, + { + "body1": 1, + "body2": 21, + "geom1": 2, + "geom2": 22, + "local_pos1": [ + -0.066968, + 0.003691, + -0.138831 + ], + "local_pos2": [ + -0.294421, + 0.257243, + 0.344129 + ] + }, + { + "body1": 6, + "body2": 21, + "geom1": 7, + "geom2": 20, + "local_pos1": [ + -0.043896, + -0.021775, + -0.069169 + ], + "local_pos2": [ + 0.25, + -0.346188, + 0.155922 + ] + }, + { + "body1": 12, + "body2": 0, + "geom1": 12, + "geom2": 0, + "local_pos1": [ + 0.066571, + -0.012282, + -0.0248 + ], + "local_pos2": [ + 0.11838, + -0.178174, + 0.0 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + -0.08998, + 0.011944, + -0.056593 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + } + ], + "facing_target": [], + "name": "armchair_relax3", + "success_sustain_time": 0.2, + "target_distance_tolerance": 0.4, + "time_limit": 5.0, + "weight": { + "COM Feet XY": 0.0, + "CoM Vel.": 10.0, + "Contact": 100.0, + "Control": 0.2, + "Facing Dir": 0.0, + "Head Height": 0.0, + "Joint Vel.": 0.01, + "Knee Feet XY": 0.0, + "LFoot Up": 5.0, + "Pelvis Up": 0.0, + "RFoot Up": 5.0, + "Torse Target": 0.0, + "Torso Height": 0.0, + "Torso Up": 0.0 + } + }, + { + "contacts": [ + { + "body1": 4, + "body2": 21, + "geom1": 5, + "geom2": 20, + "local_pos1": [ + -0.084515, + -0.003438, + -0.062752 + ], + "local_pos2": [ + 0.038509, + 0.001444, + 0.17 + ] + }, + { + "body1": 19, + "body2": 21, + "geom1": 18, + "geom2": 23, + "local_pos1": [ + 0.021663, + 0.01204, + -0.008418 + ], + "local_pos2": [ + 0.138935, + 0.312639, + 0.4 + ] + }, + { + "body1": 6, + "body2": 21, + "geom1": 7, + "geom2": 24, + "local_pos1": [ + -0.043896, + -0.021775, + -0.069169 + ], + "local_pos2": [ + 0.128166, + -0.37206, + 0.4 + ] + }, + { + "body1": 12, + "body2": 0, + "geom1": 12, + "geom2": 0, + "local_pos1": [ + 0.066571, + -0.012282, + -0.0248 + ], + "local_pos2": [ + 0.11838, + -0.178174, + 0.0 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + -0.08998, + 0.011944, + -0.056593 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + } + ], + "facing_target": [], + "name": "armchair_relax4", + "success_sustain_time": 0.1, + "target_distance_tolerance": 0.2, + "time_limit": 5.0, + "weight": { + "COM Feet XY": 0.0, + "CoM Vel.": 10.0, + "Contact": 100.0, + "Control": 0.2, + "Facing Dir": 0.0, + "Head Height": 0.0, + "Joint Vel.": 0.01, + "Knee Feet XY": 0.0, + "LFoot Up": 5.0, + "Pelvis Up": 0.0, + "RFoot Up": 5.0, + "Torse Target": 0.0, + "Torso Height": 0.0, + "Torso Up": 0.0 + } + }, + { + "contacts": [ + { + "body1": 4, + "body2": 21, + "geom1": 5, + "geom2": 20, + "local_pos1": [ + -0.084515, + -0.003438, + -0.062752 + ], + "local_pos2": [ + 0.038509, + 0.001444, + 0.17 + ] + }, + { + "body1": 1, + "body2": 21, + "geom1": 2, + "geom2": 23, + "local_pos1": [ + -0.069989, + 0.008408, + -0.118845 + ], + "local_pos2": [ + -0.137655, + 0.303035, + 0.4 + ] + }, + { + "body1": 6, + "body2": 21, + "geom1": 7, + "geom2": 24, + "local_pos1": [ + -0.043896, + -0.021775, + -0.069169 + ], + "local_pos2": [ + -0.132634, + -0.354813, + 0.4 + ] + }, + { + "body1": 11, + "body2": 21, + "geom1": 11, + "geom2": 20, + "local_pos1": [ + -0.047545, + 0.01185, + -0.070724 + ], + "local_pos2": [ + 0.245098, + -0.341528, + 0.17 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + -0.08998, + 0.011944, + -0.056593 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + } + ], + "facing_target": [], + "name": "armchair_relax5", + "success_sustain_time": 0.5, + "target_distance_tolerance": 0.2, + "time_limit": 5.0, + "weight": { + "COM Feet XY": 0.0, + "CoM Vel.": 10.0, + "Contact": 100.0, + "Control": 0.2, + "Facing Dir": 0.0, + "Head Height": 0.0, + "Joint Vel.": 0.01, + "Knee Feet XY": 0.0, + "LFoot Up": 5.0, + "Pelvis Up": 0.0, + "RFoot Up": 5.0, + "Torse Target": 0.0, + "Torso Height": 0.0, + "Torso Up": 0.0 + } + }, + { + "contacts": [ + { + "body1": 4, + "body2": 21, + "geom1": 5, + "geom2": 20, + "local_pos1": [ + -0.084515, + -0.003438, + -0.062752 + ], + "local_pos2": [ + 0.038509, + 0.001444, + 0.17 + ] + }, + { + "body1": 1, + "body2": 21, + "geom1": 2, + "geom2": 23, + "local_pos1": [ + -0.069989, + 0.008408, + -0.118845 + ], + "local_pos2": [ + -0.137655, + 0.303035, + 0.4 + ] + }, + { + "body1": 6, + "body2": 21, + "geom1": 7, + "geom2": 24, + "local_pos1": [ + -0.043896, + -0.021775, + -0.069169 + ], + "local_pos2": [ + -0.132634, + -0.354813, + 0.4 + ] + }, + { + "body1": 11, + "body2": 21, + "geom1": 11, + "geom2": 24, + "local_pos1": [ + -0.047545, + 0.01185, + -0.070724 + ], + "local_pos2": [ + 0.092992, + -0.360093, + 0.4 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + -0.08998, + 0.011944, + -0.056593 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + } + ], + "facing_target": [], + "name": "armchair_relax6", + "success_sustain_time": 0.5, + "target_distance_tolerance": 0.15, + "time_limit": 5.0, + "weight": { + "COM Feet XY": 0.0, + "CoM Vel.": 10.0, + "Contact": 100.0, + "Control": 0.2, + "Facing Dir": 0.0, + "Head Height": 0.0, + "Joint Vel.": 0.01, + "Knee Feet XY": 0.0, + "LFoot Up": 5.0, + "Pelvis Up": 0.0, + "RFoot Up": 5.0, + "Torse Target": 0.0, + "Torso Height": 0.0, + "Torso Up": 15.0 + } + }, + { + "contacts": [ + { + "body1": 4, + "body2": 21, + "geom1": 5, + "geom2": 20, + "local_pos1": [ + -0.084515, + -0.003438, + -0.062752 + ], + "local_pos2": [ + 0.038509, + 0.001444, + 0.17 + ] + }, + { + "body1": 1, + "body2": 21, + "geom1": 2, + "geom2": 23, + "local_pos1": [ + -0.069989, + 0.008408, + -0.118845 + ], + "local_pos2": [ + -0.137655, + 0.303035, + 0.4 + ] + }, + { + "body1": 6, + "body2": 21, + "geom1": 7, + "geom2": 24, + "local_pos1": [ + -0.043896, + -0.021775, + -0.069169 + ], + "local_pos2": [ + -0.132634, + -0.354813, + 0.4 + ] + }, + { + "body1": 11, + "body2": 21, + "geom1": 11, + "geom2": 24, + "local_pos1": [ + -0.047545, + 0.01185, + -0.070724 + ], + "local_pos2": [ + 0.092992, + -0.360093, + 0.4 + ] + }, + { + "body1": 17, + "body2": 5, + "geom1": 16, + "geom2": 6, + "local_pos1": [ + 0.020934, + 0.022527, + 0.025579 + ], + "local_pos2": [ + 0.04905, + 0.016951, + -0.373849 + ] + } + ], + "facing_target": [], + "name": "armchair_relax7", + "success_sustain_time": 1.0, + "target_distance_tolerance": 0.2, + "time_limit": 5.0, + "weight": { + "COM Feet XY": 0.0, + "CoM Vel.": 10.0, + "Contact": 100.0, + "Control": 0.2, + "Facing Dir": 0.0, + "Head Height": 0.0, + "Joint Vel.": 0.01, + "Knee Feet XY": 0.0, + "LFoot Up": 5.0, + "Pelvis Up": 0.0, + "RFoot Up": 5.0, + "Torse Target": 0.0, + "Torso Height": 0.0, + "Torso Up": 15.0 + } + } +] \ No newline at end of file diff --git a/mjpc/tasks/humanoid/interact/strategies/bench_lie_down_get_up.json b/mjpc/tasks/humanoid/interact/strategies/bench_lie_down_get_up.json new file mode 100644 index 000000000..cf41464ae --- /dev/null +++ b/mjpc/tasks/humanoid/interact/strategies/bench_lie_down_get_up.json @@ -0,0 +1,522 @@ +[ + { + "contacts": [ + { + "body1": 4, + "body2": 21, + "geom1": 5, + "geom2": 20, + "local_pos1": [ + -0.08877256044254017, + -0.06499463635824664, + -0.05805458578072561 + ], + "local_pos2": [ + -0.0810423397252018, + 0.18566574372627143, + 0.2999999999999998 + ] + }, + { + "body1": 4, + "body2": 21, + "geom1": 5, + "geom2": 20, + "local_pos1": [ + -0.08833072302468692, + 0.06921710240139245, + -0.05857398988392399 + ], + "local_pos2": [ + 0.15594672574533297, + 0.13713688205130092, + 0.2999999999999998 + ] + }, + { + "body1": 7, + "body2": 21, + "geom1": 9, + "geom2": 20, + "local_pos1": [ + -0.07386936066650113, + 0.01431608640675599, + -0.026370427492967898 + ], + "local_pos2": [ + 0.2406356297801423, + -0.3546633205183136, + 0.2999999999999998 + ] + }, + { + "body1": 12, + "body2": 0, + "geom1": 12, + "geom2": 0, + "local_pos1": [ + 0.05926145649731817, + -0.0072834776383078415, + -0.022476986942815233 + ], + "local_pos2": [ + -0.019343248635854238, + -0.1311980878907235, + 0.0 + ] + }, + { + "body1": 17, + "body2": 21, + "geom1": 16, + "geom2": 20, + "local_pos1": [ + 0.0390346373641039, + -0.00501065259306588, + -0.0071547499220646505 + ], + "local_pos2": [ + -0.22234577654288745, + -0.1493229369591099, + 0.2999999999999998 + ] + } + ], + "facing_target": [], + "name": "bench_turn1", + "success_sustain_time": 0.35000000000000003, + "target_distance_tolerance": 0.2, + "time_limit": 8.0, + "weight": { + "COM Feet XY": 0.0, + "CoM Vel.": 0.0, + "Contact": 80.0, + "Control": 0.14, + "Facing Dir": 0.0, + "Head Height": 0.0, + "Joint Vel.": 0.01, + "Knee Feet XY": 0.0, + "LFoot Up": 5.0, + "Pelvis Up": 43.0, + "RFoot Up": 5.0, + "Torso Height": 0.0, + "Torso Up": 44.0 + } + }, + { + "contacts": [ + { + "body1": 4, + "body2": 21, + "geom1": 5, + "geom2": 20, + "local_pos1": [ + -0.10791761397703721, + 0.020049000704914808, + 0.019248198684076616 + ], + "local_pos2": [ + -0.015976131032660823, + 0.18472821144169815, + 0.2999999999999998 + ] + }, + { + "body1": 17, + "body2": 21, + "geom1": 16, + "geom2": 20, + "local_pos1": [ + 0.026991208907812144, + 0.0009155922502382058, + -0.02950654728242904 + ], + "local_pos2": [ + -0.19463568712772983, + 0.23968290711781615, + 0.2999999999999998 + ] + }, + { + "body1": 7, + "body2": 21, + "geom1": 8, + "geom2": 20, + "local_pos1": [ + 0.05254595714036825, + -0.0039044162515324506, + -0.020400595227484632 + ], + "local_pos2": [ + -0.05279386157304666, + -0.3989411952269357, + 0.2999999999999998 + ] + }, + { + "body1": 12, + "body2": 21, + "geom1": 12, + "geom2": 20, + "local_pos1": [ + -0.07500181523853829, + -0.031239401837805575, + -0.01590187579818933 + ], + "local_pos2": [ + 0.2479331215813, + -0.40308262447062093, + 0.2999999999999998 + ] + }, + { + "body1": 20, + "body2": 21, + "geom1": 19, + "geom2": 20, + "local_pos1": [ + 0.02300446618888568, + -0.0231594941915172, + 0.023117793237121893 + ], + "local_pos2": [ + 0.22605250779942343, + 0.2705342202394923, + 0.2999999999999998 + ] + } + ], + "facing_target": [], + "name": "bench_turn2", + "success_sustain_time": 0.1499999999999998, + "target_distance_tolerance": 0.2, + "time_limit": 8.0000000001, + "weight": { + "COM Feet XY": 0.0, + "CoM Vel.": 0.0, + "Contact": 80.0, + "Control": 0.2, + "Facing Dir": 0.0, + "Head Height": 0.0, + "Joint Vel.": 0.01, + "Knee Feet XY": 0.0, + "LFoot Up": 5.0, + "Pelvis Up": 20.0, + "RFoot Up": 5.0, + "Torso Height": 0.0, + "Torso Up": 44.0 + } + }, + { + "contacts": [ + { + "body1": 4, + "body2": 21, + "geom1": 5, + "geom2": 20, + "local_pos1": [ + -0.08505375416076873, + 0.017745595347700368, + 0.06219332013639195 + ], + "local_pos2": [ + -0.02006660210759087, + 0.37675402066400265, + 0.2999999999999998 + ] + }, + { + "body1": 1, + "body2": 21, + "geom1": 1, + "geom2": 20, + "local_pos1": [ + -0.06970561639868951, + 0.023516916655307238, + 0.006413036915350601 + ], + "local_pos2": [ + -0.008700087636983933, + 0.7773444794307389, + 0.2999999999999998 + ] + }, + { + "body1": 7, + "body2": 21, + "geom1": 9, + "geom2": 20, + "local_pos1": [ + -0.09676747966488825, + 0.007740031729418488, + 0.0027192969303500175 + ], + "local_pos2": [ + -0.12237407598707872, + -0.4781135912075054, + 0.30000000000000004 + ] + }, + { + "body1": 12, + "body2": 21, + "geom1": 13, + "geom2": 20, + "local_pos1": [ + -0.0952403065294925, + 0.0009633680645035774, + 0.003204092627242093 + ], + "local_pos2": [ + 0.06009547927690512, + -0.4871230723872224, + 0.2999999999999998 + ] + }, + { + "body1": 20, + "body2": 21, + "geom1": 19, + "geom2": 20, + "local_pos1": [ + 0.02300446618888568, + -0.0231594941915172, + 0.023117793237121893 + ], + "local_pos2": [ + 0.18097965693862983, + 0.33288768001569555, + 0.2999999999999998 + ] + } + ], + "facing_target": [], + "name": "bench_turn3", + "success_sustain_time": 2.0, + "target_distance_tolerance": 0.2, + "time_limit": 8.00000000000001, + "weight": { + "COM Feet XY": 0.0, + "CoM Vel.": 0.0, + "Contact": 80.0, + "Control": 0.2, + "Facing Dir": 0.0, + "Head Height": 0.0, + "Joint Vel.": 0.2, + "Knee Feet XY": 0.0, + "LFoot Up": 0.0, + "Pelvis Up": 0.0, + "RFoot Up": 0.0, + "Torso Height": 0.0, + "Torso Up": 0.0 + } + }, + { + "contacts": [ + { + "body1": 4, + "body2": 21, + "geom1": 5, + "geom2": 20, + "local_pos1": [ + -0.08877256044254017, + -0.06499463635824664, + -0.05805458578072561 + ], + "local_pos2": [ + -0.0810423397252018, + 0.18566574372627143, + 0.2999999999999998 + ] + }, + { + "body1": 4, + "body2": 21, + "geom1": 5, + "geom2": 20, + "local_pos1": [ + -0.08833072302468692, + 0.06921710240139245, + -0.05857398988392399 + ], + "local_pos2": [ + 0.15594672574533297, + 0.13713688205130092, + 0.2999999999999998 + ] + }, + { + "body1": 7, + "body2": 21, + "geom1": 9, + "geom2": 20, + "local_pos1": [ + -0.07386936066650113, + 0.01431608640675599, + -0.026370427492967898 + ], + "local_pos2": [ + 0.2406356297801423, + -0.3546633205183136, + 0.2999999999999998 + ] + }, + { + "body1": 12, + "body2": 0, + "geom1": 12, + "geom2": 0, + "local_pos1": [ + 0.05926145649731817, + -0.0072834776383078415, + -0.022476986942815233 + ], + "local_pos2": [ + -0.019343248635854238, + -0.1311980878907235, + 0.0 + ] + }, + { + "body1": 17, + "body2": 21, + "geom1": 16, + "geom2": 20, + "local_pos1": [ + 0.0390346373641039, + -0.00501065259306588, + -0.0071547499220646505 + ], + "local_pos2": [ + -0.22234577654288745, + 0.14493229369591099, + 0.2999999999999998 + ] + } + ], + "facing_target": [], + "name": "bench_turn1", + "success_sustain_time": 0.5, + "target_distance_tolerance": 0.2, + "time_limit": 10.0, + "weight": { + "COM Feet XY": 0.0, + "CoM Vel.": 0.0, + "Contact": 80.0, + "Control": 0.025, + "Facing Dir": 0.0, + "Head Height": 0.0, + "Joint Vel.": 0.01, + "Knee Feet XY": 0.0, + "LFoot Up": 5.0, + "Pelvis Up": 20.0, + "RFoot Up": 5.0, + "Torso Height": 0.0, + "Torso Up": 44.0 + } + }, + { + "contacts": [ + { + "body1": 4, + "body2": 21, + "geom1": 5, + "geom2": 20, + "local_pos1": [ + -0.09757891714865231, + 0.005253409256739172, + -0.04562358615938966 + ], + "local_pos2": [ + -0.08133443799725554, + 0.004423856897148193, + 0.30000000000000004 + ] + }, + { + "body1": 17, + "body2": 21, + "geom1": 16, + "geom2": 20, + "local_pos1": [ + 0.03480944515858375, + 0.019494586957991278, + -0.0028746490372002498 + ], + "local_pos2": [ + 0.06854300222125487, + -0.25979611890309373, + 0.2999999999999998 + ] + }, + { + "body1": 20, + "body2": 21, + "geom1": 19, + "geom2": 20, + "local_pos1": [ + 0.031114980128544967, + 0.02253972690352163, + -0.011127386158265251 + ], + "local_pos2": [ + 0.12056425904251716, + 0.26577136382178945, + 0.2999999999999998 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + 0.0, + 0.0, + 0.0 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + 0.0, + 0.0, + 0.0 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + } + ], + "facing_target": [], + "name": "bench_turn1", + "success_sustain_time": 2.0, + "target_distance_tolerance": 0.2, + "time_limit": 10.0, + "weight": { + "COM Feet XY": 0.0, + "CoM Vel.": 10.0, + "Contact": 100.0, + "Control": 0.8, + "Facing Dir": 0.0, + "Head Height": 0.0, + "Joint Vel.": 0.01, + "Knee Feet XY": 30.0, + "LFoot Up": 5.0, + "Pelvis Up": 50.0, + "RFoot Up": 5.0, + "Torso Height": 20.0, + "Torso Up": 50.0 + } + } +] \ No newline at end of file diff --git a/mjpc/tasks/humanoid/interact/strategies/bench_shuffle.json b/mjpc/tasks/humanoid/interact/strategies/bench_shuffle.json new file mode 100644 index 000000000..0a33b47d0 --- /dev/null +++ b/mjpc/tasks/humanoid/interact/strategies/bench_shuffle.json @@ -0,0 +1,522 @@ +[ + { + "contacts": [ + { + "body1": 4, + "body2": 21, + "geom1": 5, + "geom2": 20, + "local_pos1": [ + -0.09757891714865231, + 0.005253409256739172, + -0.04562358615938966 + ], + "local_pos2": [ + -0.08133443799725554, + 0.004423856897148193, + 0.30000000000000004 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + 0.0, + 0.0, + 0.0 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + 0.0, + 0.0, + 0.0 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + 0.0, + 0.0, + 0.0 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + 0.0, + 0.0, + 0.0 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + } + ], + "facing_target": [], + "name": "bench_sit", + "success_sustain_time": 1.0, + "target_distance_tolerance": 0.1, + "time_limit": 10.0, + "weight": { + "COM Feet XY": 0.0, + "CoM Vel.": 10.0, + "Contact": 100.0, + "Control": 0.001, + "Facing Dir": 0.0, + "Head Height": 0.0, + "Joint Vel.": 0.01, + "Knee Feet XY": 50.0, + "LFoot Up": 30.0, + "Pelvis Up": 10.0, + "RFoot Up": 30.0, + "Torso Height": 0.0, + "Torso Up": 100.0 + } + }, + { + "contacts": [ + { + "body1": 4, + "body2": 21, + "geom1": 5, + "geom2": 20, + "local_pos1": [ + -0.09757891714865231, + 0.005253409256739172, + -0.04562358615938966 + ], + "local_pos2": [ + -0.07084339623391711, + 0.6979072666503298, + 0.2999999999999998 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + 0.03480944515858375, + 0.019494586957991278, + -0.0028746490372002498 + ], + "local_pos2": [ + 0.02759996250688912, + 0.40739572193347073, + 0.2999999999999998 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + 0.0, + 0.0, + 0.0 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + 0.0, + 0.0, + 0.0 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + 0.0, + 0.0, + 0.0 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + } + ], + "facing_target": [], + "name": "bench_left", + "success_sustain_time": 1, + "target_distance_tolerance": 0.1, + "time_limit": 10.0, + "weight": { + "COM Feet XY": 0.0, + "CoM Vel.": 10.0, + "Contact": 100.0, + "Control": 0.001, + "Facing Dir": 0.0, + "Head Height": 0.0, + "Joint Vel.": 0.01, + "Knee Feet XY": 50.0, + "LFoot Up": 30.0, + "Pelvis Up": 10.0, + "RFoot Up": 30.0, + "Torso Height": 0.0, + "Torso Up": 100.0 + } + }, + { + "contacts": [ + { + "body1": 4, + "body2": 21, + "geom1": 5, + "geom2": 20, + "local_pos1": [ + -0.09757891714865231, + 0.005253409256739172, + -0.04562358615938966 + ], + "local_pos2": [ + -0.08133443799725554, + 0.004423856897148193, + 0.30000000000000004 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + 0.0, + 0.0, + 0.0 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + 0.0, + 0.0, + 0.0 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + 0.0, + 0.0, + 0.0 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + 0.0, + 0.0, + 0.0 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + } + ], + "facing_target": [], + "name": "bench_sit", + "success_sustain_time": 1, + "target_distance_tolerance": 0.1, + "time_limit": 10.0, + "weight": { + "COM Feet XY": 0.0, + "CoM Vel.": 10.0, + "Contact": 100.0, + "Control": 0.001, + "Facing Dir": 0.0, + "Head Height": 0.0, + "Joint Vel.": 0.01, + "Knee Feet XY": 50.0, + "LFoot Up": 30.0, + "Pelvis Up": 10.0, + "RFoot Up": 30.0, + "Torso Height": 0.0, + "Torso Up": 100.0 + } + }, + { + "contacts": [ + { + "body1": 4, + "body2": 21, + "geom1": 5, + "geom2": 20, + "local_pos1": [ + -0.09757891714865231, + 0.005253409256739172, + -0.04562358615938966 + ], + "local_pos2": [ + -0.10894194078012431, + -0.6477750554744135, + 0.2999999999999998 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + 0.0, + 0.0, + 0.0 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + 0.031114980128544967, + 0.02253972690352163, + -0.011127386158265251 + ], + "local_pos2": [ + 0.060806845885136185, + -0.3755029203614153, + 0.2999999999999998 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + 0.0, + 0.0, + 0.0 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + 0.0, + 0.0, + 0.0 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + } + ], + "facing_target": [], + "name": "bench_right", + "success_sustain_time": 1, + "target_distance_tolerance": 0.1, + "time_limit": 10.0, + "weight": { + "COM Feet XY": 0.0, + "CoM Vel.": 10.0, + "Contact": 100.0, + "Control": 0.001, + "Facing Dir": 0.0, + "Head Height": 0.0, + "Joint Vel.": 0.01, + "Knee Feet XY": 50.0, + "LFoot Up": 30.0, + "Pelvis Up": 10.0, + "RFoot Up": 30.0, + "Torso Height": 0.0, + "Torso Up": 100.0 + } + }, + { + "contacts": [ + { + "body1": 4, + "body2": 21, + "geom1": 5, + "geom2": 20, + "local_pos1": [ + -0.09757891714865231, + 0.005253409256739172, + -0.04562358615938966 + ], + "local_pos2": [ + -0.08133443799725554, + 0.004423856897148193, + 0.30000000000000004 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + 0.0, + 0.0, + 0.0 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + 0.0, + 0.0, + 0.0 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + 0.0, + 0.0, + 0.0 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + 0.0, + 0.0, + 0.0 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + } + ], + "facing_target": [], + "name": "bench_sit", + "success_sustain_time": 1, + "target_distance_tolerance": 0.1, + "time_limit": 10.0, + "weight": { + "COM Feet XY": 0.0, + "CoM Vel.": 10.0, + "Contact": 100.0, + "Control": 0.001, + "Facing Dir": 0.0, + "Head Height": 0.0, + "Joint Vel.": 0.01, + "Knee Feet XY": 50.0, + "LFoot Up": 30.0, + "Pelvis Up": 10.0, + "RFoot Up": 30.0, + "Torso Height": 0.0, + "Torso Up": 100.0 + } + } +] \ No newline at end of file diff --git a/mjpc/tasks/humanoid/interact/strategies/stand_sit_stand.json b/mjpc/tasks/humanoid/interact/strategies/stand_sit_stand.json new file mode 100644 index 000000000..ef2a69f19 --- /dev/null +++ b/mjpc/tasks/humanoid/interact/strategies/stand_sit_stand.json @@ -0,0 +1,629 @@ +[ + { + "contacts": [ + { + "body1": 4, + "body2": 21, + "geom1": 5, + "geom2": 20, + "local_pos1": [ + -0.030287911573965005, + 0.004815256870587602, + -0.0894100602586016 + ], + "local_pos2": [ + -0.08357137165674333, + -0.0006287156427891016, + 0.1700000000000001 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + 0.030902021002374276, + -0.010539980375938231, + 0.023107875532885222 + ], + "local_pos2": [ + 0.17496449635445133, + 0.3452783409875556, + 0.39999999999999986 + ] + }, + { + "body1": 7, + "body2": 0, + "geom1": 8, + "geom2": 0, + "local_pos1": [ + 0.09050005753122557, + -0.0003754283050522459, + -0.010679658740233588 + ], + "local_pos2": [ + -0.012219168395155133, + -0.20868620608504912, + 0.0 + ] + }, + { + "body1": 12, + "body2": 0, + "geom1": 12, + "geom2": 0, + "local_pos1": [ + 0.09052240930622941, + -0.008209380523737207, + -0.020974930769408352 + ], + "local_pos2": [ + -0.0024607824491855723, + 0.177495228535462, + 0.0 + ] + }, + { + "body1": 1, + "body2": 0, + "geom1": 1, + "geom2": 0, + "local_pos1": [ + 0.06999055671344898, + -0.024455730480426652, + 0.0011497699510759733 + ], + "local_pos2": [ + -0.21394008810379944, + -0.00546991901340337, + 0.9 + ] + } + ], + "facing_target": [], + "name": "sit_leanforward1", + "success_sustain_time": 0.325, + "target_distance_tolerance": 0.18, + "time_limit": 5.0, + "weight": { + "COM Feet XY": 0.0, + "CoM Vel.": 0.0, + "Contact": 100.0, + "Control": 0.1, + "Facing Dir": 0.0, + "Head Height": 0.0, + "Joint Vel.": 0.01, + "Knee Feet XY": 0.0, + "LFoot Up": 0.0, + "Pelvis Up": 0.0, + "RFoot Up": 0.0, + "Torso Height": 0.0, + "Torso Up": 0.0 + } + }, + { + "contacts": [ + { + "body1": 4, + "body2": 21, + "geom1": 5, + "geom2": 20, + "local_pos1": [ + -0.109999, + -0.045599, + 0.000472 + ], + "local_pos2": [ + -0.143933, + 0.004582, + 0.17 + ] + }, + { + "body1": 1, + "body2": 21, + "geom1": 1, + "geom2": 21, + "local_pos1": [ + -0.061753, + -0.020781, + -0.032963 + ], + "local_pos2": [ + -0.368475, + -0.015674, + 0.54212 + ] + }, + { + "body1": 7, + "body2": 0, + "geom1": 9, + "geom2": 0, + "local_pos1": [ + 0.058889, + 0.014053, + -0.02573 + ], + "local_pos2": [ + 0.059334, + -0.217544, + 0.0 + ] + }, + { + "body1": 12, + "body2": 0, + "geom1": 12, + "geom2": 0, + "local_pos1": [ + 0.066571, + -0.012282, + -0.0248 + ], + "local_pos2": [ + 0.088113, + 0.237827, + 0.0 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + 0.0, + 0.0, + 0.0 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + } + ], + "facing_target": [], + "name": "armchair0", + "success_sustain_time": 1.0, + "target_distance_tolerance": 0.3, + "time_limit": 5.0, + "weight": { + "COM Feet XY": 0.0, + "CoM Vel.": 10.0, + "Contact": 100.0, + "Control": 0.8, + "Facing Dir": 0.0, + "Head Height": 0.0, + "Joint Vel.": 0.01, + "Knee Feet XY": 30.0, + "LFoot Up": 5.0, + "Pelvis Up": 10.0, + "RFoot Up": 5.0, + "Torse Target": 0.0, + "Torso Height": 20.0, + "Torso Up": 10.0 + } + }, + { + "contacts": [ + { + "body1": 4, + "body2": 21, + "geom1": 5, + "geom2": 20, + "local_pos1": [ + -0.030287911573965005, + 0.004815256870587602, + -0.0894100602586016 + ], + "local_pos2": [ + -0.08357137165674333, + -0.0006287156427891016, + 0.1700000000000001 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + 0.030902021002374276, + -0.010539980375938231, + 0.023107875532885222 + ], + "local_pos2": [ + 0.17496449635445133, + 0.3452783409875556, + 0.39999999999999986 + ] + }, + { + "body1": 7, + "body2": 0, + "geom1": 8, + "geom2": 0, + "local_pos1": [ + 0.09050005753122557, + -0.0003754283050522459, + -0.010679658740233588 + ], + "local_pos2": [ + -0.012219168395155133, + -0.20868620608504912, + 0.0 + ] + }, + { + "body1": 12, + "body2": 0, + "geom1": 12, + "geom2": 0, + "local_pos1": [ + 0.09052240930622941, + -0.008209380523737207, + -0.020974930769408352 + ], + "local_pos2": [ + -0.0024607824491855723, + 0.177495228535462, + 0.0 + ] + }, + { + "body1": 1, + "body2": 0, + "geom1": 1, + "geom2": 0, + "local_pos1": [ + 0.06999055671344898, + -0.024455730480426652, + 0.0011497699510759733 + ], + "local_pos2": [ + -0.21394008810379944, + -0.00546991901340337, + 0.9 + ] + } + ], + "facing_target": [], + "name": "sit_leanforward1", + "success_sustain_time": 0.325, + "target_distance_tolerance": 0.18, + "time_limit": 5.0, + "weight": { + "COM Feet XY": 0.0, + "CoM Vel.": 0.0, + "Contact": 100.0, + "Control": 0.1, + "Facing Dir": 0.0, + "Head Height": 0.0, + "Joint Vel.": 0.01, + "Knee Feet XY": 0.0, + "LFoot Up": 0.0, + "Pelvis Up": 0.0, + "RFoot Up": 0.0, + "Torso Height": 0.0, + "Torso Up": 0.0 + } + }, + { + "contacts": [ + { + "body1": 4, + "body2": 21, + "geom1": 5, + "geom2": 20, + "local_pos1": [ + -0.030287911573965005, + 0.004815256870587602, + -0.0894100602586016 + ], + "local_pos2": [ + -0.08357137165674333, + -0.0006287156427891016, + 0.1700000000000001 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + 0.030902021002374276, + -0.010539980375938231, + 0.023107875532885222 + ], + "local_pos2": [ + 0.17496449635445133, + 0.3452783409875556, + 0.39999999999999986 + ] + }, + { + "body1": 7, + "body2": 0, + "geom1": 8, + "geom2": 0, + "local_pos1": [ + 0.09050005753122557, + -0.0003754283050522459, + -0.010679658740233588 + ], + "local_pos2": [ + -0.012219168395155133, + -0.20868620608504912, + 0.0 + ] + }, + { + "body1": 12, + "body2": 0, + "geom1": 12, + "geom2": 0, + "local_pos1": [ + 0.09052240930622941, + -0.008209380523737207, + -0.020974930769408352 + ], + "local_pos2": [ + -0.0024607824491855723, + 0.177495228535462, + 0.0 + ] + }, + { + "body1": 1, + "body2": 0, + "geom1": 1, + "geom2": 0, + "local_pos1": [ + 0.06999055671344898, + -0.024455730480426652, + 0.0011497699510759733 + ], + "local_pos2": [ + -0.1, + 0.0, + 0.85 + ] + } + ], + "facing_target": [], + "name": "sit_leanforward2", + "success_sustain_time": 0.325, + "target_distance_tolerance": 0.18, + "time_limit": 5.0, + "weight": { + "COM Feet XY": 0.0, + "CoM Vel.": 0.0, + "Contact": 100.0, + "Control": 0.1, + "Facing Dir": 0.0, + "Head Height": 0.0, + "Joint Vel.": 0.01, + "Knee Feet XY": 0.0, + "LFoot Up": 0.0, + "Pelvis Up": 0.0, + "RFoot Up": 0.0, + "Torso Height": 0.0, + "Torso Up": 0.0 + } + }, + { + "contacts": [ + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + 0.0, + 0.0, + 0.0 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + 0.0, + 0.0, + 0.0 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + 0.088251, + 0.005847, + -0.019046 + ], + "local_pos2": [ + 0.065443, + -0.1864, + 0.0 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + 0.049769, + -0.014707, + -0.026163 + ], + "local_pos2": [ + 0.068107, + 0.182984, + 0.0 + ] + }, + { + "body1": 1, + "body2": 0, + "geom1": 1, + "geom2": 0, + "local_pos1": [ + 0.037814, + -0.003873, + -0.058907 + ], + "local_pos2": [ + 0.013783, + -0.006972, + 1.2 + ] + } + ], + "facing_target": [], + "name": "sit2stand", + "success_sustain_time": 1.0, + "target_distance_tolerance": 0.5, + "time_limit": 5.0, + "weight": { + "COM Feet XY": 100.0, + "CoM Vel.": 0.0, + "Contact": 0.0, + "Control": 0.025, + "Facing Dir": 0.0, + "Head Height": 80.0, + "Joint Vel.": 0.01, + "Knee Feet XY": 0.0, + "LFoot Up": 1.0, + "Pelvis Up": 0.0, + "RFoot Up": 1.0, + "Torse Target": 0.0, + "Torso Height": 0.0, + "Torso Up": 10.0 + } + }, + { + "contacts": [ + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + 0.0, + 0.0, + 0.0 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + 0.0, + 0.0, + 0.0 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + 0.088251, + 0.005847, + -0.019046 + ], + "local_pos2": [ + 0.065443, + -0.1864, + 0.0 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + 0.049769, + -0.014707, + -0.026163 + ], + "local_pos2": [ + 0.068107, + 0.182984, + 0.0 + ] + }, + { + "body1": 1, + "body2": 0, + "geom1": 1, + "geom2": 0, + "local_pos1": [ + 0.037814, + -0.003873, + -0.058907 + ], + "local_pos2": [ + 0.013783, + -0.006972, + 1.2 + ] + } + ], + "facing_target": [], + "name": "stand", + "success_sustain_time": 1.0, + "target_distance_tolerance": 0.5, + "time_limit": 5.0, + "weight": { + "COM Feet XY": 100.0, + "CoM Vel.": 0.0, + "Contact": 0.0, + "Control": 0.025, + "Facing Dir": 0.0, + "Head Height": 80.0, + "Joint Vel.": 0.01, + "Knee Feet XY": 0.0, + "LFoot Up": 1.0, + "Pelvis Up": 0.0, + "RFoot Up": 1.0, + "Torse Target": 0.0, + "Torso Height": 0.0, + "Torso Up": 10.0 + } + } +] \ No newline at end of file diff --git a/mjpc/tasks/humanoid/interact/strategies/stool_rotate.json b/mjpc/tasks/humanoid/interact/strategies/stool_rotate.json new file mode 100644 index 000000000..838cf6f9c --- /dev/null +++ b/mjpc/tasks/humanoid/interact/strategies/stool_rotate.json @@ -0,0 +1,938 @@ +[ + { + "contacts": [ + { + "body1": 4, + "body2": 21, + "geom1": 5, + "geom2": 20, + "local_pos1": [ + -0.050036342004474946, + -0.0003074581098543223, + -0.08483995614679576 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.4 + ] + }, + { + "body1": 7, + "body2": 0, + "geom1": 8, + "geom2": 0, + "local_pos1": [ + 0.07605052653852291, + -0.008441791958672439, + -0.022178780452783223 + ], + "local_pos2": [ + 0.42369680832196044, + -0.15296410750287368, + 0.0 + ] + }, + { + "body1": 12, + "body2": 0, + "geom1": 13, + "geom2": 0, + "local_pos1": [ + 0.0826109548709775, + 0.0014912095167608993, + -0.01424027282327213 + ], + "local_pos2": [ + 0.4349132891612387, + 0.18469982807799706, + 0.0 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + 0.0, + 0.0, + 0.0 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + 0.0, + 0.0, + 0.0 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + } + ], + "facing_target": [], + "name": "rot1", + "success_sustain_time": 1.0, + "target_distance_tolerance": 0.1, + "time_limit": 5.0, + "weight": { + "COM Feet XY": 0.0, + "CoM Vel.": 10.0, + "Contact": 100.0, + "Control": 0.8, + "Facing Dir": 0.0, + "Head Height": 0.0, + "Joint Vel.": 0.01, + "Knee Feet XY": 14.499999999999998, + "LFoot Up": 7.000000000000001, + "Pelvis Up": 49.5, + "RFoot Up": 9.0, + "Torso Height": 0.0, + "Torso Up": 100 + } + }, + { + "contacts": [ + { + "body1": 4, + "body2": 21, + "geom1": 5, + "geom2": 20, + "local_pos1": [ + -0.050036342004474946, + -0.0003074581098543223, + -0.08483995614679576 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.4 + ] + }, + { + "body1": 7, + "body2": 0, + "geom1": 8, + "geom2": 0, + "local_pos1": [ + 0.07605052653852291, + -0.008441791958672439, + -0.022178780452783223 + ], + "local_pos2": [ + 0.42, + 0.18, + 0.0 + ] + }, + { + "body1": 12, + "body2": 0, + "geom1": 13, + "geom2": 0, + "local_pos1": [ + 0.0826109548709775, + 0.0014912095167608993, + -0.01424027282327213 + ], + "local_pos2": [ + 0.16970562748477142, + 0.42426406871192845, + 0.0 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + 0.0, + 0.0, + 0.0 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + 0.0, + 0.0, + 0.0 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + } + ], + "facing_target": [], + "name": "rot2", + "success_sustain_time": 1.0, + "target_distance_tolerance": 0.1, + "time_limit": 5.0, + "weight": { + "COM Feet XY": 0.0, + "CoM Vel.": 10.0, + "Contact": 100.0, + "Control": 0.8, + "Facing Dir": 0.0, + "Head Height": 0.0, + "Joint Vel.": 0.01, + "Knee Feet XY": 14.499999999999998, + "LFoot Up": 7.000000000000001, + "Pelvis Up": 49.5, + "RFoot Up": 9.0, + "Torso Height": 0.0, + "Torso Up": 100 + } + }, + { + "contacts": [ + { + "body1": 4, + "body2": 21, + "geom1": 5, + "geom2": 20, + "local_pos1": [ + -0.050036342004474946, + -0.0003074581098543223, + -0.08483995614679576 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.4 + ] + }, + { + "body1": 7, + "body2": 0, + "geom1": 8, + "geom2": 0, + "local_pos1": [ + 0.07605052653852291, + -0.008441791958672439, + -0.022178780452783223 + ], + "local_pos2": [ + 0.16970562748477142, + 0.42426406871192845, + 0.0 + ] + }, + { + "body1": 12, + "body2": 0, + "geom1": 13, + "geom2": 0, + "local_pos1": [ + 0.0826109548709775, + 0.0014912095167608993, + -0.01424027282327213 + ], + "local_pos2": [ + -0.17999999999999997, + 0.42, + 0.0 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + 0.0, + 0.0, + 0.0 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + 0.0, + 0.0, + 0.0 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + } + ], + "facing_target": [], + "name": "rot3", + "success_sustain_time": 1.0, + "target_distance_tolerance": 0.1, + "time_limit": 5.0, + "weight": { + "COM Feet XY": 0.0, + "CoM Vel.": 10.0, + "Contact": 100.0, + "Control": 0.8, + "Facing Dir": 0.0, + "Head Height": 0.0, + "Joint Vel.": 0.01, + "Knee Feet XY": 14.499999999999998, + "LFoot Up": 7.000000000000001, + "Pelvis Up": 49.5, + "RFoot Up": 9.0, + "Torso Height": 0.0, + "Torso Up": 100 + } + }, + { + "contacts": [ + { + "body1": 4, + "body2": 21, + "geom1": 5, + "geom2": 20, + "local_pos1": [ + -0.050036342004474946, + -0.0003074581098543223, + -0.08483995614679576 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.4 + ] + }, + { + "body1": 7, + "body2": 0, + "geom1": 8, + "geom2": 0, + "local_pos1": [ + 0.07605052653852291, + -0.008441791958672439, + -0.022178780452783223 + ], + "local_pos2": [ + -0.17999999999999997, + 0.42, + 0.0 + ] + }, + { + "body1": 12, + "body2": 0, + "geom1": 13, + "geom2": 0, + "local_pos1": [ + 0.0826109548709775, + 0.0014912095167608993, + -0.01424027282327213 + ], + "local_pos2": [ + -0.42426406871192845, + 0.16970562748477142, + 0.0 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + 0.0, + 0.0, + 0.0 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + 0.0, + 0.0, + 0.0 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + } + ], + "facing_target": [], + "name": "rot4", + "success_sustain_time": 1.0, + "target_distance_tolerance": 0.1, + "time_limit": 5.0, + "weight": { + "COM Feet XY": 0.0, + "CoM Vel.": 10.0, + "Contact": 100.0, + "Control": 0.8, + "Facing Dir": 0.0, + "Head Height": 0.0, + "Joint Vel.": 0.01, + "Knee Feet XY": 14.499999999999998, + "LFoot Up": 7.000000000000001, + "Pelvis Up": 49.5, + "RFoot Up": 9.0, + "Torso Height": 0.0, + "Torso Up": 100 + } + }, + { + "contacts": [ + { + "body1": 4, + "body2": 21, + "geom1": 5, + "geom2": 20, + "local_pos1": [ + -0.050036342004474946, + -0.0003074581098543223, + -0.08483995614679576 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.4 + ] + }, + { + "body1": 7, + "body2": 0, + "geom1": 8, + "geom2": 0, + "local_pos1": [ + 0.07605052653852291, + -0.008441791958672439, + -0.022178780452783223 + ], + "local_pos2": [ + -0.42426406871192845, + 0.16970562748477142, + 0.0 + ] + }, + { + "body1": 12, + "body2": 0, + "geom1": 13, + "geom2": 0, + "local_pos1": [ + 0.0826109548709775, + 0.0014912095167608993, + -0.01424027282327213 + ], + "local_pos2": [ + -0.42, + -0.17999999999999994, + 0.0 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + 0.0, + 0.0, + 0.0 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + 0.0, + 0.0, + 0.0 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + } + ], + "facing_target": [], + "name": "rot5", + "success_sustain_time": 1.0, + "target_distance_tolerance": 0.1, + "time_limit": 5.0, + "weight": { + "COM Feet XY": 0.0, + "CoM Vel.": 10.0, + "Contact": 100.0, + "Control": 0.8, + "Facing Dir": 0.0, + "Head Height": 0.0, + "Joint Vel.": 0.01, + "Knee Feet XY": 14.499999999999998, + "LFoot Up": 7.000000000000001, + "Pelvis Up": 49.5, + "RFoot Up": 9.0, + "Torso Height": 0.0, + "Torso Up": 100 + } + }, + { + "contacts": [ + { + "body1": 4, + "body2": 21, + "geom1": 5, + "geom2": 20, + "local_pos1": [ + -0.050036342004474946, + -0.0003074581098543223, + -0.08483995614679576 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.4 + ] + }, + { + "body1": 7, + "body2": 0, + "geom1": 8, + "geom2": 0, + "local_pos1": [ + 0.07605052653852291, + -0.008441791958672439, + -0.022178780452783223 + ], + "local_pos2": [ + -0.42, + -0.17999999999999994, + 0.0 + ] + }, + { + "body1": 12, + "body2": 0, + "geom1": 13, + "geom2": 0, + "local_pos1": [ + 0.0826109548709775, + 0.0014912095167608993, + -0.01424027282327213 + ], + "local_pos2": [ + -0.16970562748477147, + -0.42426406871192845, + 0.0 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + 0.0, + 0.0, + 0.0 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + 0.0, + 0.0, + 0.0 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + } + ], + "facing_target": [], + "name": "rot6", + "success_sustain_time": 1.0, + "target_distance_tolerance": 0.1, + "time_limit": 5.0, + "weight": { + "COM Feet XY": 0.0, + "CoM Vel.": 10.0, + "Contact": 100.0, + "Control": 0.8, + "Facing Dir": 0.0, + "Head Height": 0.0, + "Joint Vel.": 0.01, + "Knee Feet XY": 14.499999999999998, + "LFoot Up": 7.000000000000001, + "Pelvis Up": 49.5, + "RFoot Up": 9.0, + "Torso Height": 0.0, + "Torso Up": 100 + } + }, + { + "contacts": [ + { + "body1": 4, + "body2": 21, + "geom1": 5, + "geom2": 20, + "local_pos1": [ + -0.050036342004474946, + -0.0003074581098543223, + -0.08483995614679576 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.4 + ] + }, + { + "body1": 7, + "body2": 0, + "geom1": 8, + "geom2": 0, + "local_pos1": [ + 0.07605052653852291, + -0.008441791958672439, + -0.022178780452783223 + ], + "local_pos2": [ + -0.16970562748477147, + -0.42426406871192845, + 0.0 + ] + }, + { + "body1": 12, + "body2": 0, + "geom1": 13, + "geom2": 0, + "local_pos1": [ + 0.0826109548709775, + 0.0014912095167608993, + -0.01424027282327213 + ], + "local_pos2": [ + 0.1799999999999999, + -0.42000000000000004, + 0.0 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + 0.0, + 0.0, + 0.0 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + 0.0, + 0.0, + 0.0 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + } + ], + "facing_target": [], + "name": "rot7", + "success_sustain_time": 1.0, + "target_distance_tolerance": 0.1, + "time_limit": 5.0, + "weight": { + "COM Feet XY": 0.0, + "CoM Vel.": 10.0, + "Contact": 100.0, + "Control": 0.8, + "Facing Dir": 0.0, + "Head Height": 0.0, + "Joint Vel.": 0.01, + "Knee Feet XY": 14.499999999999998, + "LFoot Up": 7.000000000000001, + "Pelvis Up": 49.5, + "RFoot Up": 9.0, + "Torso Height": 0.0, + "Torso Up": 100 + } + }, + { + "contacts": [ + { + "body1": 4, + "body2": 21, + "geom1": 5, + "geom2": 20, + "local_pos1": [ + -0.050036342004474946, + -0.0003074581098543223, + -0.08483995614679576 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.4 + ] + }, + { + "body1": 7, + "body2": 0, + "geom1": 8, + "geom2": 0, + "local_pos1": [ + 0.07605052653852291, + -0.008441791958672439, + -0.022178780452783223 + ], + "local_pos2": [ + 0.1799999999999999, + -0.42000000000000004, + 0.0 + ] + }, + { + "body1": 12, + "body2": 0, + "geom1": 13, + "geom2": 0, + "local_pos1": [ + 0.0826109548709775, + 0.0014912095167608993, + -0.01424027282327213 + ], + "local_pos2": [ + 0.42426406871192845, + -0.1697056274847715, + 0.0 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + 0.0, + 0.0, + 0.0 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + 0.0, + 0.0, + 0.0 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + } + ], + "facing_target": [], + "name": "rot8", + "success_sustain_time": 1.0, + "target_distance_tolerance": 0.1, + "time_limit": 5.0, + "weight": { + "COM Feet XY": 0.0, + "CoM Vel.": 10.0, + "Contact": 100.0, + "Control": 0.8, + "Facing Dir": 0.0, + "Head Height": 0.0, + "Joint Vel.": 0.01, + "Knee Feet XY": 14.499999999999998, + "LFoot Up": 7.000000000000001, + "Pelvis Up": 49.5, + "RFoot Up": 9.0, + "Torso Height": 0.0, + "Torso Up": 100 + } + }, + { + "contacts": [ + { + "body1": 4, + "body2": 21, + "geom1": 5, + "geom2": 20, + "local_pos1": [ + -0.050036342004474946, + -0.0003074581098543223, + -0.08483995614679576 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.4 + ] + }, + { + "body1": 7, + "body2": 0, + "geom1": 8, + "geom2": 0, + "local_pos1": [ + 0.07605052653852291, + -0.008441791958672439, + -0.022178780452783223 + ], + "local_pos2": [ + 0.42369680832196044, + -0.15296410750287368, + 0.0 + ] + }, + { + "body1": 12, + "body2": 0, + "geom1": 13, + "geom2": 0, + "local_pos1": [ + 0.0826109548709775, + 0.0014912095167608993, + -0.01424027282327213 + ], + "local_pos2": [ + 0.4349132891612387, + 0.18469982807799706, + 0.0 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + 0.0, + 0.0, + 0.0 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + 0.0, + 0.0, + 0.0 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + } + ], + "facing_target": [], + "name": "rot1", + "success_sustain_time": 1.0, + "target_distance_tolerance": 0.1, + "time_limit": 5.0, + "weight": { + "COM Feet XY": 0.0, + "CoM Vel.": 10.0, + "Contact": 100.0, + "Control": 0.8, + "Facing Dir": 0.0, + "Head Height": 0.0, + "Joint Vel.": 0.01, + "Knee Feet XY": 14.499999999999998, + "LFoot Up": 7.000000000000001, + "Pelvis Up": 49.5, + "RFoot Up": 9.0, + "Torso Height": 0.0, + "Torso Up": 100 + } + } +] \ No newline at end of file diff --git a/mjpc/tasks/humanoid/interact/strategies/table_climb.json b/mjpc/tasks/humanoid/interact/strategies/table_climb.json new file mode 100644 index 000000000..f0db451ce --- /dev/null +++ b/mjpc/tasks/humanoid/interact/strategies/table_climb.json @@ -0,0 +1,986 @@ +[ + { + "contacts": [ + { + "body1": 17, + "body2": 21, + "geom1": 16, + "geom2": 20, + "local_pos1": [ + 0.023724, + 0.028041, + 0.015839 + ], + "local_pos2": [ + 0.222266, + -0.068316, + -0.65 + ] + }, + { + "body1": 20, + "body2": 21, + "geom1": 19, + "geom2": 20, + "local_pos1": [ + 0.013216, + -0.036975, + 0.007628 + ], + "local_pos2": [ + 0.235804, + 0.38269, + -0.65 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + 0.0, + 0.0, + 0.0 + ], + "local_pos2": [ + 0.001054, + -0.049976, + -0.001148 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + 0.0, + 0.0, + 0.0 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + }, + { + "body1": 1, + "body2": 0, + "geom1": 1, + "geom2": 0, + "local_pos1": [ + 0.0, + 0.0, + 0.0 + ], + "local_pos2": [ + 0.46436500000000003, + 0.217577, + 1.170778 + ] + } + ], + "facing_target": [ + 0.661723, + 0.273219, + 1.163555 + ], + "name": "dc0", + "success_sustain_time": 0, + "target_distance_tolerance": 0.3, + "time_limit": 5.0, + "weight": { + "COM Feet XY": 0.0, + "CoM Vel.": 10.0, + "Contact": 100.0, + "Control": 0.025, + "Facing Dir": 0.0, + "Head Height": 0.0, + "Joint Vel.": 0.01, + "Knee Feet XY": 30.0, + "LFoot Up": 5.0, + "Pelvis Up": 10.0, + "RFoot Up": 5.0, + "Torse Target": 0.0, + "Torso Height": 20.0, + "Torso Up": 10.0 + } + }, + { + "contacts": [ + { + "body1": 17, + "body2": 21, + "geom1": 16, + "geom2": 20, + "local_pos1": [ + 0.023724, + 0.028041, + 0.015839 + ], + "local_pos2": [ + 0.222266, + -0.068316, + -0.65 + ] + }, + { + "body1": 20, + "body2": 21, + "geom1": 19, + "geom2": 20, + "local_pos1": [ + 0.013216, + -0.036975, + 0.007628 + ], + "local_pos2": [ + 0.235804, + 0.38269, + -0.65 + ] + }, + { + "body1": 6, + "body2": 21, + "geom1": 7, + "geom2": 20, + "local_pos1": [ + 0.041843, + 0.00132, + 0.025465 + ], + "local_pos2": [ + 0.194429, + -0.463069, + -0.65 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + 0.0, + 0.0, + 0.0 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + }, + { + "body1": 1, + "body2": 0, + "geom1": 1, + "geom2": 0, + "local_pos1": [ + 0.0, + 0.0, + 0.0 + ], + "local_pos2": [ + 0.564365, + 0.217577, + 1.170778 + ] + } + ], + "facing_target": [ + 0.0, + 0.0, + 0.0 + ], + "name": "dc1", + "success_sustain_time": 0, + "target_distance_tolerance": 0.3, + "time_limit": 5.0, + "weight": { + "COM Feet XY": 0.0, + "CoM Vel.": 10.0, + "Contact": 100.0, + "Control": 0.025, + "Facing Dir": 0.0, + "Head Height": 80.0, + "Joint Vel.": 0.01, + "Knee Feet XY": 0.0, + "LFoot Up": 1.0, + "Pelvis Up": 15.0, + "RFoot Up": 0.0, + "Torse Target": 0.0, + "Torso Height": 80.0, + "Torso Up": 0.0 + } + }, + { + "contacts": [ + { + "body1": 17, + "body2": 21, + "geom1": 16, + "geom2": 20, + "local_pos1": [ + 0.023724, + 0.028041, + 0.015839 + ], + "local_pos2": [ + -0.136627, + 0.14919, + -0.65 + ] + }, + { + "body1": 20, + "body2": 21, + "geom1": 19, + "geom2": 20, + "local_pos1": [ + 0.013216, + -0.036975, + 0.007628 + ], + "local_pos2": [ + 0.188926, + 0.406365, + -0.65 + ] + }, + { + "body1": 6, + "body2": 21, + "geom1": 7, + "geom2": 20, + "local_pos1": [ + 0.041843, + 0.00132, + 0.025465 + ], + "local_pos2": [ + 0.069227, + -0.397974, + -0.65 + ] + }, + { + "body1": -1, + "body2": -1, + "geom1": -1, + "geom2": -1, + "local_pos1": [ + 0.0, + 0.0, + 0.0 + ], + "local_pos2": [ + 0.0, + 0.0, + 0.0 + ] + }, + { + "body1": 1, + "body2": 0, + "geom1": 1, + "geom2": 0, + "local_pos1": [ + 0.0, + 0.0, + 0.0 + ], + "local_pos2": [ + 0.664365, + 0.217577, + 1.370778 + ] + } + ], + "facing_target": [ + 0.0, + 0.0, + 0.0 + ], + "name": "dc2", + "success_sustain_time": 0, + "target_distance_tolerance": 0.3, + "time_limit": 5.0, + "weight": { + "COM Feet XY": 0.0, + "CoM Vel.": 10.0, + "Contact": 100.0, + "Control": 0.025, + "Facing Dir": 0.0, + "Head Height": 80.0, + "Joint Vel.": 0.01, + "Knee Feet XY": 0.0, + "LFoot Up": 0.0, + "Pelvis Up": 15.0, + "RFoot Up": 0.0, + "Torse Target": 0.0, + "Torso Height": 80.0, + "Torso Up": 0.0 + } + }, + { + "contacts": [ + { + "body1": 17, + "body2": 21, + "geom1": 16, + "geom2": 20, + "local_pos1": [ + 0.023724, + 0.028041, + 0.015839 + ], + "local_pos2": [ + -0.136627, + 0.14919, + -0.65 + ] + }, + { + "body1": 20, + "body2": 21, + "geom1": 19, + "geom2": 20, + "local_pos1": [ + 0.013216, + -0.036975, + 0.007628 + ], + "local_pos2": [ + 0.188926, + 0.406365, + -0.65 + ] + }, + { + "body1": 6, + "body2": 21, + "geom1": 7, + "geom2": 20, + "local_pos1": [ + 0.041843, + 0.00132, + 0.025465 + ], + "local_pos2": [ + 0.069227, + -0.397974, + -0.65 + ] + }, + { + "body1": 11, + "body2": 21, + "geom1": 11, + "geom2": 20, + "local_pos1": [ + 0.047473, + 0.003463, + 0.011632 + ], + "local_pos2": [ + 0.338378, + -0.271057, + -0.65 + ] + }, + { + "body1": 1, + "body2": 0, + "geom1": 1, + "geom2": 0, + "local_pos1": [ + 0.0, + 0.0, + 0.0 + ], + "local_pos2": [ + 0.664365, + 0.217577, + 1.25 + ] + } + ], + "facing_target": [ + 0.0, + 0.0, + 0.0 + ], + "name": "dc3", + "success_sustain_time": 0, + "target_distance_tolerance": 0.3, + "time_limit": 10.0, + "weight": { + "COM Feet XY": 0.0, + "CoM Vel.": 10.0, + "Contact": 100.0, + "Control": 0.025, + "Facing Dir": 0.0, + "Head Height": 0.0, + "Joint Vel.": 0.01, + "Knee Feet XY": 0.0, + "LFoot Up": 0.0, + "Pelvis Up": 0.0, + "RFoot Up": 0.0, + "Torse Target": 0.0, + "Torso Height": 20.0, + "Torso Up": 0.0, + "control": 0.025 + } + }, + { + "contacts": [ + { + "body1": 17, + "body2": 21, + "geom1": 16, + "geom2": 20, + "local_pos1": [ + 0.023724, + 0.028041, + 0.015839 + ], + "local_pos2": [ + -0.192455, + 0.385982, + -0.65 + ] + }, + { + "body1": 20, + "body2": 21, + "geom1": 19, + "geom2": 20, + "local_pos1": [ + 0.013216, + -0.036975, + 0.007628 + ], + "local_pos2": [ + 0.188926, + 0.406365, + -0.65 + ] + }, + { + "body1": 6, + "body2": 21, + "geom1": 7, + "geom2": 20, + "local_pos1": [ + 0.041843, + 0.00132, + 0.025465 + ], + "local_pos2": [ + -0.123912, + -0.191276, + -0.65 + ] + }, + { + "body1": 11, + "body2": 21, + "geom1": 11, + "geom2": 20, + "local_pos1": [ + 0.047473, + 0.003463, + 0.011632 + ], + "local_pos2": [ + 0.227636, + -0.188019, + -0.65 + ] + }, + { + "body1": 1, + "body2": 0, + "geom1": 1, + "geom2": 0, + "local_pos1": [ + 0.0, + 0.0, + 0.0 + ], + "local_pos2": [ + 0.664365, + 0.217577, + 1.25 + ] + } + ], + "facing_target": [ + 0.0, + 0.0, + 0.0 + ], + "name": "dc4", + "success_sustain_time": 0, + "target_distance_tolerance": 0.3, + "time_limit": 10.0, + "weight": { + "COM Feet XY": 0.0, + "CoM Vel.": 10.0, + "Contact": 100.0, + "Control": 0.025, + "Facing Dir": 0.0, + "Head Height": 0.0, + "Joint Vel.": 0.01, + "Knee Feet XY": 0.0, + "LFoot Up": 0.0, + "Pelvis Up": 0.0, + "RFoot Up": 0.0, + "Torse Target": 0.0, + "Torso Height": 20.0, + "Torso Up": 0.0, + "control": 0.025 + } + }, + { + "contacts": [ + { + "body1": 17, + "body2": 21, + "geom1": 16, + "geom2": 20, + "local_pos1": [ + 0.023724, + 0.028041, + 0.015839 + ], + "local_pos2": [ + -0.192455, + 0.385982, + -0.65 + ] + }, + { + "body1": 20, + "body2": 21, + "geom1": 19, + "geom2": 20, + "local_pos1": [ + 0.013216, + -0.036975, + 0.007628 + ], + "local_pos2": [ + 0.188926, + 0.406365, + -0.65 + ] + }, + { + "body1": 6, + "body2": 21, + "geom1": 7, + "geom2": 20, + "local_pos1": [ + 0.041843, + 0.00132, + 0.025465 + ], + "local_pos2": [ + -0.26613221534593, + -0.20127031186771216, + -0.6499999999999997 + ] + }, + { + "body1": 11, + "body2": 21, + "geom1": 11, + "geom2": 20, + "local_pos1": [ + 0.047473, + 0.003463, + 0.011632 + ], + "local_pos2": [ + 0.227636, + -0.188019, + -0.65 + ] + }, + { + "body1": 1, + "body2": 0, + "geom1": 1, + "geom2": 0, + "local_pos1": [ + 0.0, + 0.0, + 0.0 + ], + "local_pos2": [ + 0.664365, + 0.217577, + 1.25 + ] + } + ], + "facing_target": [ + 0.0, + 0.0, + 0.0 + ], + "name": "dc4r", + "success_sustain_time": 0, + "target_distance_tolerance": 0.3, + "time_limit": 10.0, + "weight": { + "COM Feet XY": 0.0, + "CoM Vel.": 10.0, + "Contact": 100.0, + "Control": 0.025, + "Facing Dir": 0.0, + "Head Height": 0.0, + "Joint Vel.": 0.01, + "Knee Feet XY": 0.0, + "LFoot Up": 0.0, + "Pelvis Up": 0.0, + "RFoot Up": 0.0, + "Torse Target": 0.0, + "Torso Height": 20.0, + "Torso Up": 0.0, + "control": 0.025 + } + }, + { + "contacts": [ + { + "body1": 17, + "body2": 21, + "geom1": 16, + "geom2": 20, + "local_pos1": [ + 0.023724, + 0.028041, + 0.015839 + ], + "local_pos2": [ + -0.2011172807887297, + 0.40361013124817924, + -0.6499999999999997 + ] + }, + { + "body1": 20, + "body2": 21, + "geom1": 19, + "geom2": 20, + "local_pos1": [ + 0.013216, + -0.036975, + 0.007628 + ], + "local_pos2": [ + 0.10020646728498389, + 0.12608536391239245, + -0.6499999999999997 + ] + }, + { + "body1": 6, + "body2": 21, + "geom1": 7, + "geom2": 20, + "local_pos1": [ + 0.041843, + 0.00132, + 0.025465 + ], + "local_pos2": [ + -0.3281356337575013, + -0.2104603502436433, + -0.6499999999999997 + ] + }, + { + "body1": 11, + "body2": 21, + "geom1": 11, + "geom2": 20, + "local_pos1": [ + 0.047473, + 0.003463, + 0.011632 + ], + "local_pos2": [ + -0.06587309788600315, + -0.34935120556885224, + -0.6499999999999997 + ] + }, + { + "body1": 1, + "body2": 0, + "geom1": 1, + "geom2": 0, + "local_pos1": [ + 0.0, + 0.0, + 0.0 + ], + "local_pos2": [ + 0.504365, + 0.217577, + 1.25 + ] + } + ], + "facing_target": [ + 0.9937455427896383, + 0.002661271462101933 + ], + "name": "dc3r", + "success_sustain_time": 0, + "target_distance_tolerance": 0.3, + "time_limit": 10.0, + "weight": { + "COM Feet XY": 0.0, + "CoM Vel.": 10.0, + "Contact": 100.0, + "Control": 0.025, + "Facing Dir": 0.0, + "Head Height": 0.0, + "Joint Vel.": 0.01, + "Knee Feet XY": 0.0, + "LFoot Up": 0.0, + "Pelvis Up": 0.0, + "RFoot Up": 0.0, + "Torse Target": 0.0, + "Torso Height": 20.0, + "Torso Up": 0.0, + "control": 0.025 + } + }, + { + "contacts": [ + { + "body1": 17, + "body2": 21, + "geom1": 16, + "geom2": 20, + "local_pos1": [ + 0.023724, + 0.028041, + 0.015839 + ], + "local_pos2": [ + -0.25836413506395406, + 0.37698803305651346, + -0.6499999999999997 + ] + }, + { + "body1": 20, + "body2": 21, + "geom1": 19, + "geom2": 20, + "local_pos1": [ + 0.013216, + -0.036975, + 0.007628 + ], + "local_pos2": [ + 0.10020646728498389, + 0.12608536391239245, + -0.6499999999999997 + ] + }, + { + "body1": 7, + "body2": 0, + "geom1": 8, + "geom2": 0, + "local_pos1": [ + 0.09109086212404559, + -0.01286144721516809, + -0.023971571801248766 + ], + "local_pos2": [ + 0.8435301479306072, + -0.1338938989737709, + 0.0 + ] + }, + { + "body1": 11, + "body2": 21, + "geom1": 11, + "geom2": 20, + "local_pos1": [ + 0.015451453519300296, + -0.0008102148747241404, + 0.04649296867266253 + ], + "local_pos2": [ + -0.20129710233346074, + -0.4012243191349012, + -0.6499999999999997 + ] + }, + { + "body1": 1, + "body2": 0, + "geom1": 1, + "geom2": 0, + "local_pos1": [ + 0.0, + 0.0, + 0.0 + ], + "local_pos2": [ + 0.60365, + 0.217577, + 1.25 + ] + } + ], + "facing_target": [ + 0.0, + 0.0, + 0.0 + ], + "name": "dc2r_better", + "success_sustain_time": 0, + "target_distance_tolerance": 0.3, + "time_limit": 5.0, + "weight": { + "COM Feet XY": 0.0, + "CoM Vel.": 10.0, + "Contact": 100.0, + "Control": 0.025, + "Facing Dir": 0.0, + "Head Height": 80.0, + "Joint Vel.": 0.01, + "Knee Feet XY": 0.0, + "LFoot Up": 0.0, + "Pelvis Up": 15.0, + "RFoot Up": 1.0, + "Torse Target": 0.0, + "Torso Height": 80.0, + "Torso Up": 14.499999999999998 + } + }, + { + "contacts": [ + { + "body1": 17, + "body2": 21, + "geom1": 16, + "geom2": 20, + "local_pos1": [ + 0.023724, + 0.028041, + 0.015839 + ], + "local_pos2": [ + -0.25836413506395406, + 0.37698803305651346, + -0.6499999999999997 + ] + }, + { + "body1": 20, + "body2": 21, + "geom1": 19, + "geom2": 20, + "local_pos1": [ + 0.013216, + -0.036975, + 0.007628 + ], + "local_pos2": [ + -0.18592045338148644, + 0.04843571250747633, + -0.6500000000000001 + ] + }, + { + "body1": 7, + "body2": 0, + "geom1": 8, + "geom2": 0, + "local_pos1": [ + 0.09109086212404559, + -0.01286144721516809, + -0.023971571801248766 + ], + "local_pos2": [ + 1.0246263758660046, + 0.0693224095694065, + 0.0 + ] + }, + { + "body1": 12, + "body2": 0, + "geom1": 12, + "geom2": 0, + "local_pos1": [ + 0.09155750767538509, + -0.011389750538185636, + -0.023126909554058526 + ], + "local_pos2": [ + 0.9093769416095232, + -0.3298691846534352, + 0.0 + ] + }, + { + "body1": 1, + "body2": 0, + "geom1": 1, + "geom2": 0, + "local_pos1": [ + 0.0, + 0.0, + 0.0 + ], + "local_pos2": [ + 0.70365, + -0.117577, + 1.25 + ] + } + ], + "facing_target": [ + 0.0, + 0.0, + 0.0 + ], + "name": "dc1r_better", + "success_sustain_time": 0.5, + "target_distance_tolerance": 0.3, + "time_limit": 5.0, + "weight": { + "COM Feet XY": 27.0, + "CoM Vel.": 10.0, + "Contact": 100.0, + "Control": 0.025, + "Facing Dir": 0.0, + "Head Height": 75.0, + "Joint Vel.": 0.01, + "Knee Feet XY": 23.5, + "LFoot Up": 30.5, + "Pelvis Up": 31.5, + "RFoot Up": 33.5, + "Torse Target": 0.0, + "Torso Height": 80.0, + "Torso Up": 28.999999999999996 + } + } +] \ No newline at end of file diff --git a/mjpc/tasks/humanoid/interact/task.xml b/mjpc/tasks/humanoid/interact/task.xml index 5db8ddcc5..6e1667af0 100644 --- a/mjpc/tasks/humanoid/interact/task.xml +++ b/mjpc/tasks/humanoid/interact/task.xml @@ -3,15 +3,15 @@ - - + + - + @@ -35,6 +35,9 @@ + + +