Added support for motion strategies

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)
-

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)

mjpc/task.cc

@@ -202,13 +202,15 @@ 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];
     dim_norm_residual[i] = (int)model->sensor_dim[i];
 
     // 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]);

mjpc/task.h

@@ -139,6 +139,7 @@ class Task {
   std::vector<int> num_norm_parameter;
   std::vector<NormType> norm;
   std::vector<double> weight;
+  std::vector<std::string> weight_names;
   std::vector<double> norm_parameter;
   double risk;
 

mjpc/tasks/humanoid/interact/contact_keyframe.cc

@@ -27,11 +27,61 @@ 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();
 
   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

mjpc/tasks/humanoid/interact/contact_keyframe.h

@@ -21,12 +21,23 @@
 
 #include <mujoco/mujoco.h>
 
+#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<std::string, mjtNum> 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_