diff --git a/mjpc/CMakeLists.txt b/mjpc/CMakeLists.txt index f8fb30c3e..525412f2c 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 diff --git a/mjpc/tasks/humanoid/interact/contact_keyframe.cc b/mjpc/tasks/humanoid/interact/contact_keyframe.cc index b009d2e61..b6da72efb 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(); diff --git a/mjpc/tasks/humanoid/interact/contact_keyframe.h b/mjpc/tasks/humanoid/interact/contact_keyframe.h index 1171bbc1a..442b5c18d 100644 --- a/mjpc/tasks/humanoid/interact/contact_keyframe.h +++ b/mjpc/tasks/humanoid/interact/contact_keyframe.h @@ -27,6 +27,14 @@ namespace mjpc::humanoid { 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,22 +49,38 @@ class ContactPair { local_pos2{0.} {} void Reset(); + void GetDistance(mjtNum distance[3], const mjData* data) const; }; class ContactKeyframe { public: std::string name; ContactPair contact_pairs[kNumberOfContactPairsInteract]; - // the direction on the xy-plane for the torso to point towards std::vector facing_target; // 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 }; } // namespace mjpc::humanoid diff --git a/mjpc/tasks/humanoid/interact/interact.cc b/mjpc/tasks/humanoid/interact/interact.cc index 6bb360979..ac038bfb3 100644 --- a/mjpc/tasks/humanoid/interact/interact.cc +++ b/mjpc/tasks/humanoid/interact/interact.cc @@ -25,12 +25,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); } @@ -38,7 +38,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]); } @@ -47,7 +47,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]); } @@ -56,19 +56,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); @@ -84,12 +82,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, @@ -103,8 +100,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); @@ -118,31 +115,16 @@ 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 ------------ +// ---------------- Residuals for humanoid interaction task ----------- // // Number of residuals: 13 // Residual (0): Torso up // Residual (1): Pelvis up @@ -160,7 +142,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; @@ -197,46 +179,70 @@ 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 (residual_.current_task_mode_ != mode) { residual_.current_task_mode_ = (TaskMode)mode; weight = default_weights[residual_.current_task_mode_]; } + + if (!motion_strategy_.HasKeyframes()) 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) { + // success criteria not reached, reset + motion_strategy_.Reset(); + 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(); + 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); + } } // 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..85f277628 100644 --- a/mjpc/tasks/humanoid/interact/interact.h +++ b/mjpc/tasks/humanoid/interact/interact.h @@ -22,6 +22,7 @@ #include "mjpc/task.h" #include "mjpc/tasks/humanoid/interact/contact_keyframe.h" +#include "mjpc/tasks/humanoid/interact/motion_strategy.h" namespace mjpc::humanoid { @@ -47,14 +48,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 +122,7 @@ class Interact : public Task { private: ResidualFn residual_; + MotionStrategy motion_strategy_; // 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..629261390 --- /dev/null +++ b/mjpc/tasks/humanoid/interact/motion_strategy.cc @@ -0,0 +1,85 @@ +// 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(); + current_keyframe_.Reset(); + current_keyframe_index_ = 0; +} + +void MotionStrategy::Reset() { + current_keyframe_index_ = 0; + current_keyframe_ = contact_keyframes_[current_keyframe_index_]; +} + +int MotionStrategy::NextKeyframe() { + current_keyframe_index_ = + (current_keyframe_index_ + 1) % contact_keyframes_.size(); + current_keyframe_ = contact_keyframes_[current_keyframe_index_]; + return current_keyframe_index_; +} + +void MotionStrategy::ClearKeyframes() { + contact_keyframes_.clear(); + current_keyframe_.Reset(); + 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 = current_keyframe_.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; +} + +} // 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..87667395d --- /dev/null +++ b/mjpc/tasks/humanoid/interact/motion_strategy.h @@ -0,0 +1,93 @@ +// 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 "mjpc/tasks/humanoid/interact/contact_keyframe.h" + +namespace mjpc::humanoid { + +/* +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_(ContactKeyframe()), + current_keyframe_index_(0) {} + + void Reset(); + void Clear(); + + bool HasKeyframes() const { return !contact_keyframes_.empty(); } + ContactKeyframe& GetCurrentKeyframe() { return current_keyframe_; } + const ContactKeyframe& GetCurrentKeyframe() const { + return current_keyframe_; + } + 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; + current_keyframe_ = contact_keyframes_[current_keyframe_index_]; + } + void SetContactKeyframes(const std::vector& keyframes) { + contact_keyframes_ = keyframes; + } + + int NextKeyframe(); + void ClearKeyframes(); + bool SaveCurrentKeyframe(); + bool LoadKeyframe(const std::string& kf_name); + + double CalculateTotalKeyframeDistance( + const mjData* data, const ContactKeyframeErrorType error_type = + ContactKeyframeErrorType::kNorm) const; + + private: + std::vector contact_keyframes_; + ContactKeyframe current_keyframe_; + int current_keyframe_index_; + mjtNum current_keyframe_start_time_; + mjtNum current_keyframe_success_start_time_; +}; + +} // namespace mjpc::humanoid + +#endif // MJPC_TASKS_HUMANOID_INTERACT_MOTION_STRATEGY_H_