Skip to content

Commit

Permalink
Added support for motion strategies
Browse files Browse the repository at this point in the history
  • Loading branch information
@nkhosh
nkhosh committed Sep 9, 2024
1 parent 1f78168 commit dcf4674
Show file tree
Hide file tree
Showing 7 changed files with 273 additions and 56 deletions.
2 changes: 2 additions & 0 deletions mjpc/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
12 changes: 12 additions & 0 deletions mjpc/tasks/humanoid/interact/contact_keyframe.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down
28 changes: 26 additions & 2 deletions mjpc/tasks/humanoid/interact/contact_keyframe.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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<mjtNum> facing_target;

// 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
};

} // namespace mjpc::humanoid
Expand Down
110 changes: 58 additions & 52 deletions mjpc/tasks/humanoid/interact/interact.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,20 +25,20 @@ 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);
}

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]);
}
Expand All @@ -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]);
}
Expand All @@ -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);
Expand All @@ -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,
Expand All @@ -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);
Expand All @@ -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
Expand All @@ -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;
Expand Down Expand Up @@ -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);
}
}

Expand Down
7 changes: 5 additions & 2 deletions mjpc/tasks/humanoid/interact/interact.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 {

Expand All @@ -47,14 +48,15 @@ const std::vector<std::vector<double>> 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:
Expand Down Expand Up @@ -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,
Expand Down
Loading

0 comments on commit dcf4674

Please sign in to comment.