Skip to content

Commit

Permalink
Added save/load and sample motion strategies
Browse files Browse the repository at this point in the history
  • Loading branch information
@nkhosh
nkhosh committed Oct 28, 2024
1 parent 9590dda commit c732ce4
Show file tree
Hide file tree
Showing 14 changed files with 5,333 additions and 86 deletions.
42 changes: 40 additions & 2 deletions mjpc/tasks/humanoid/interact/contact_keyframe.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,7 @@ void ContactPair::Reset() {
}
}

void ContactPair::GetDistance(mjtNum distance[3], const mjData* data) const {
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);
Expand All @@ -41,9 +41,47 @@ void ContactPair::GetDistance(mjtNum distance[3], const mjData* data) const {

void ContactKeyframe::Reset() {
name.clear();
for (auto& contact_pair : contact_pairs) contact_pair.Reset();
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
10 changes: 9 additions & 1 deletion mjpc/tasks/humanoid/interact/contact_keyframe.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,9 @@
#include <string>
#include <vector>

#include "nlohmann/json.hpp"
using json = nlohmann::json;

namespace mjpc::humanoid {

// ---------- Constants ----------------- //
Expand Down Expand Up @@ -51,7 +54,7 @@ class ContactPair {
void Reset();

// populates the distance vector between the two contact points
void GetDistance(mjtNum distance[3], const mjData* data) const;
void GetDistance(mjtNum distance[3], const mjData *data) const;
};

class ContactKeyframe {
Expand Down Expand Up @@ -86,6 +89,11 @@ class ContactKeyframe {
// 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_
136 changes: 89 additions & 47 deletions mjpc/tasks/humanoid/interact/interact.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,40 +26,40 @@ std::string Interact::XmlPath() const {
std::string Interact::Name() const { return "Humanoid Interact"; }

// --------------- Helper residual functions ----------------- //
void Interact::ResidualFn::UpResidual(const mjModel* model, const mjData* data,
double* residual, std::string&& name,
int* counter) const {
void Interact::ResidualFn::UpResidual(const mjModel *model, const mjData *data,
double *residual, std::string &&name,
int *counter) const {
name.append("_up");
const double* up_vector = SensorByName(model, data, name);
const double *up_vector = SensorByName(model, data, name);
residual[(*counter)++] = mju_abs(up_vector[2] - 1.0);
}

void Interact::ResidualFn::HeadHeightResidual(const mjModel* model,
const mjData* data,
double* residual,
int* counter) const {
void Interact::ResidualFn::HeadHeightResidual(const mjModel *model,
const mjData *data,
double *residual,
int *counter) const {
const double head_height = SensorByName(model, data, "head_position")[2];
residual[(*counter)++] =
mju_abs(head_height - parameters_[kHeadHeightParameterIndex]);
}

void Interact::ResidualFn::TorsoHeightResidual(const mjModel* model,
const mjData* data,
double* residual,
int* counter) const {
void Interact::ResidualFn::TorsoHeightResidual(const mjModel *model,
const mjData *data,
double *residual,
int *counter) const {
const double torso_height = SensorByName(model, data, "torso_position")[2];
residual[(*counter)++] =
mju_abs(torso_height - parameters_[kTorsoHeightParameterIndex]);
}

void Interact::ResidualFn::KneeFeetXYResidual(const mjModel* model,
const mjData* data,
double* residual,
int* counter) const {
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");
void Interact::ResidualFn::KneeFeetXYResidual(const mjModel *model,
const mjData *data,
double *residual,
int *counter) const {
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_add(knee_xy_avg, knee_right, knee_left, 2);
Expand All @@ -73,13 +73,13 @@ void Interact::ResidualFn::KneeFeetXYResidual(const mjModel* model,
residual[(*counter)++] = mju_norm(knee_xy_avg, 2);
}

void Interact::ResidualFn::COMFeetXYResidual(const mjModel* model,
const mjData* data,
double* residual,
int* counter) const {
double* foot_right = SensorByName(model, data, "foot_right");
double* foot_left = SensorByName(model, data, "foot_left");
double* com_position = SensorByName(model, data, "torso_subtreecom");
void Interact::ResidualFn::COMFeetXYResidual(const mjModel *model,
const mjData *data,
double *residual,
int *counter) const {
double *foot_right = SensorByName(model, data, "foot_right");
double *foot_left = SensorByName(model, data, "foot_left");
double *com_position = SensorByName(model, data, "torso_subtreecom");

double foot_xy_avg[2] = {0.0};
mju_add(foot_xy_avg, foot_right, foot_left, 2);
Expand All @@ -89,17 +89,17 @@ void Interact::ResidualFn::COMFeetXYResidual(const mjModel* model,
residual[(*counter)++] = mju_norm(foot_xy_avg, 2);
}

void Interact::ResidualFn::FacingDirectionResidual(const mjModel* model,
const mjData* data,
double* residual,
int* counter) const {
void Interact::ResidualFn::FacingDirectionResidual(const mjModel *model,
const mjData *data,
double *residual,
int *counter) const {
if (residual_keyframe_.facing_target.empty()) {
residual[(*counter)++] = 0;
return;
}

double* torso_forward = SensorByName(model, data, "torso_forward");
double* torso_position = mjpc::SensorByName(model, data, "torso_position");
double *torso_forward = SensorByName(model, data, "torso_forward");
double *torso_position = mjpc::SensorByName(model, data, "torso_position");

double target[2] = {0.};
mju_sub(target, residual_keyframe_.facing_target.data(), torso_position, 2);
Expand All @@ -108,11 +108,11 @@ void Interact::ResidualFn::FacingDirectionResidual(const mjModel* model,
residual[(*counter)++] = mju_norm(target, 2);
}

void Interact::ResidualFn::ContactResidual(const mjModel* model,
const mjData* data, double* residual,
int* counter) const {
void Interact::ResidualFn::ContactResidual(const mjModel *model,
const mjData *data, double *residual,
int *counter) const {
for (int i = 0; i < kNumberOfContactPairsInteract; i++) {
const ContactPair& contact = residual_keyframe_.contact_pairs[i];
const ContactPair &contact = residual_keyframe_.contact_pairs[i];
if (contact.body1 != kNotSelectedInteract &&
contact.body2 != kNotSelectedInteract) {
double dist[3] = {0.};
Expand All @@ -124,6 +124,39 @@ void Interact::ResidualFn::ContactResidual(const mjModel* model,
}
}

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
Expand All @@ -143,11 +176,11 @@ void Interact::ResidualFn::ContactResidual(const mjModel* model,
// Parameter (0): head_height_goal
// Parameter (1): torso_height_goal
// -------------------------------------------------------------------- //
void Interact::ResidualFn::Residual(const mjModel* model, const mjData* data,
double* residual) const {
void Interact::ResidualFn::Residual(const mjModel *model, const mjData *data,
double *residual) const {
int counter = 0;

double* com_velocity = SensorByName(model, data, "torso_subtreelinvel");
double *com_velocity = SensorByName(model, data, "torso_subtreelinvel");

// ----- task-specific residual terms ------ //
UpResidual(model, data, residual, "torso", &counter);
Expand Down Expand Up @@ -180,25 +213,32 @@ void Interact::ResidualFn::Residual(const mjModel* model, const mjData* data,
}

// -------- Transition for interaction task -------- //
void Interact::TransitionLocked(mjModel* model, mjData* data) {
// If the task mode is changed, sync it with the residual here
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;
// 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 =
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
// timelimit reached but distance criteria not reached, reset
motion_strategy_.Reset();
// sync weights from current keyframe
LoadParamsFromKeyframe(motion_strategy_.GetCurrentKeyframe());
residual_.residual_keyframe_ = motion_strategy_.GetCurrentKeyframe();
motion_strategy_.SetCurrentKeyframeStartTime(data->time);
} else if (total_distance <= current_keyframe.target_distance_tolerance &&
Expand All @@ -207,16 +247,18 @@ void Interact::TransitionLocked(mjModel* model, mjData* data) {
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 {
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++) {
Expand Down
Loading

0 comments on commit c732ce4

Please sign in to comment.