void
State::print( std::ostream &out, bool basisOnly ) const
{
Config *cfp;
out << "State " << getStateIndex() << ":\n";
if ( basisOnly )
cfp = getBasis();
else
cfp = getConfig();
while ( cfp )
{
cfp->print( out );
out << std::endl;
if ( basisOnly )
cfp = cfp->getNextBasis();
else
cfp = cfp->getNext();
}
out << std::endl;
out << " Actions:" << std::endl;
for ( size_t i = 0; i < myActions.getNumActions(); ++i )
{
const Action &act = myActions.getNthAction( i );
if ( act.print( out ) )
out << std::endl;
}
}
State::ConstPtr LightWorldModel::getStartState(long seed) {
LightWorldState start_state;
start_state.x = params_.start_x;
start_state.y = params_.start_y;
start_state.unlock_attempts_left = params_.initial_unlock_attempts;
start_state.goal_unlocked = false;
start_state.key_picked_up = false;
return complete_state_vector_[getStateIndex(start_state)];
}
void LightWorldModel::getTransitionDynamics(const State::ConstPtr& state_base,
const Action::ConstPtr& action_base,
std::vector<State::ConstPtr>& next_states,
std::vector<float>& rewards,
std::vector<float>& probabilities) const {
boost::shared_ptr<const LightWorldState> state = boost::dynamic_pointer_cast<const LightWorldState>(state_base);
if (!state) {
throw DowncastException("State", "LightWorldState");
}
boost::shared_ptr<const LightWorldAction> action = boost::dynamic_pointer_cast<const LightWorldAction>(action_base);
if (!action) {
throw DowncastException("Action", "LightWorldAction");
}
next_states.clear();
rewards.clear();
probabilities.clear();
if (!isTerminalState(state)) {
if (action->type == PICKUP || action->type == UNLOCK) {
LightWorldState next_state = *state;
float reward = -1.0f;
if (action->type == PICKUP) {
if (state->x == params_.key_x && state->y == params_.key_y) {
next_state.key_picked_up = true;
} else {
reward = params_.incorrect_pickup_reward;
}
} else if (action->type == UNLOCK) {
if (state->unlock_attempts_left > 0) {
next_state.unlock_attempts_left -= 1;
if (state->x == params_.lock_x &&
state->y == params_.lock_y) {
next_state.goal_unlocked = true;
} else {
reward = params_.incorrect_unlock_reward;
}
}
}
rewards.push_back(reward);
probabilities.push_back(1.0f);
int idx = getStateIndex(next_state);
next_states.push_back(complete_state_vector_[idx]);
} else {
// We're performing a navigation action.
int num_valid_nav_actions = 0;
if (state->x > 0) {
++num_valid_nav_actions;
}
if (state->x < params_.grid_size - 1) {
++num_valid_nav_actions;
}
if (state->y > 0) {
++num_valid_nav_actions;
}
if (state->y < params_.grid_size - 1) {
++num_valid_nav_actions;
}
if (state->x > 0) {
// Left action is valid;
LightWorldState next_state = *state;
--next_state.x;
float p = (action->type == LEFT) ?
((1.0f - params_.nondeterminism) + (params_.nondeterminism / num_valid_nav_actions)) :
(params_.nondeterminism / num_valid_nav_actions);
int idx = getStateIndex(next_state);
float reward = -1.0f;
if (isTerminalState(complete_state_vector_[idx])) {
reward += 100.0f;
}
next_states.push_back(complete_state_vector_[idx]);
probabilities.push_back(p);
rewards.push_back(reward);
}
if (state->x < params_.grid_size - 1) {
// Right action is valid;
LightWorldState next_state = *state;
++next_state.x;
float p = (action->type == RIGHT) ?
((1.0f - params_.nondeterminism) + (params_.nondeterminism / num_valid_nav_actions)) :
(params_.nondeterminism / num_valid_nav_actions);
int idx = getStateIndex(next_state);
float reward = -1.0f;
if (isTerminalState(complete_state_vector_[idx])) {
reward += 100.0f;
}
next_states.push_back(complete_state_vector_[idx]);
probabilities.push_back(p);
rewards.push_back(reward);
}
if (state->y > 0) {
// Down action is valid;
LightWorldState next_state = *state;
--next_state.y;
float p = (action->type == DOWN) ?
((1.0f - params_.nondeterminism) + (params_.nondeterminism / num_valid_nav_actions)) :
(params_.nondeterminism / num_valid_nav_actions);
int idx = getStateIndex(next_state);
float reward = -1.0f;
if (isTerminalState(complete_state_vector_[idx])) {
reward += 100.0f;
}
next_states.push_back(complete_state_vector_[idx]);
probabilities.push_back(p);
rewards.push_back(reward);
}
if (state->y < params_.grid_size - 1) {
// Up action is valid;
LightWorldState next_state = *state;
++next_state.y;
float p = (action->type == UP) ?
((1.0f - params_.nondeterminism) + (params_.nondeterminism / num_valid_nav_actions)) :
(params_.nondeterminism / num_valid_nav_actions);
int idx = getStateIndex(next_state);
float reward = -1.0f;
if (isTerminalState(complete_state_vector_[idx])) {
reward += 100.0f;
}
next_states.push_back(complete_state_vector_[idx]);
probabilities.push_back(p);
rewards.push_back(reward);
}
}
}
}
void SimpleGuidanceModel::getTransitionDynamics(const State::ConstPtr& state_base,
const Action::ConstPtr& action_base,
std::vector<State::ConstPtr>& next_states,
std::vector<float>& rewards,
std::vector<float>& probabilities) const {
boost::shared_ptr<const SimpleGuidanceState> state = boost::dynamic_pointer_cast<const SimpleGuidanceState>(state_base);
if (!state) {
throw DowncastException("State", "SimpleGuidanceState");
}
boost::shared_ptr<const SimpleGuidanceAction> action = boost::dynamic_pointer_cast<const SimpleGuidanceAction>(action_base);
if (!action) {
throw DowncastException("Action", "SimpleGuidanceAction");
}
next_states.clear();
rewards.clear();
probabilities.clear();
if (!isTerminalState(state)) {
float p_left = (state->prev_action.type == LEFT) ? 1.0f : 0.0f;
float p_right = (state->prev_action.type == RIGHT) ? 1.0f : 0.0f;
float p_up = (state->prev_action.type == UP) ? 1.0f : 0.0f;
float p_down = (state->prev_action.type == DOWN) ? 1.0f : 0.0f;
if (action->type != NOOP) {
float success_prob = ((1.0f - params_.nondeterminism) + (params_.nondeterminism / 4));
float failure_prob = (params_.nondeterminism / 4);
p_left = (action->type == LEFT) ? success_prob : failure_prob;
p_right = (action->type == RIGHT) ? success_prob : failure_prob;
p_up = (action->type == UP) ? success_prob : failure_prob;
p_down = (action->type == DOWN) ? success_prob : failure_prob;
}
bool left_blocked = (state->x == 0);
bool right_blocked = (state->x == params_.grid_size - 1);
bool up_blocked = (state->y == params_.grid_size - 1);
bool down_blocked = (state->y == 0);
if (left_blocked) {
if (up_blocked) {
p_down += p_left;
} else if (down_blocked) {
p_up += p_left;
} else {
p_up += p_left / 2;
p_down += p_left / 2;
}
p_left = 0.0f;
}
if (right_blocked) {
if (up_blocked) {
p_down += p_right;
} else if (down_blocked) {
p_up += p_right;
} else {
p_up += p_right / 2;
p_down += p_right / 2;
}
p_right = 0.0f;
}
if (up_blocked) {
if (left_blocked) {
p_right += p_up;
} else if (right_blocked) {
p_left += p_up;
} else {
p_right += p_up / 2;
p_left += p_up / 2;
}
p_up = 0.0f;
}
if (down_blocked) {
if (left_blocked) {
p_right += p_down;
} else if (right_blocked) {
p_left += p_down;
} else {
p_right += p_down / 2;
p_left += p_down / 2;
}
p_down = 0.0f;
}
if (p_up != 0.0f) {
SimpleGuidanceState next_state = *state;
next_state.y += 1;
next_state.prev_action.type = UP;
probabilities.push_back(p_up);
rewards.push_back(-1);
next_states.push_back(complete_state_vector_[getStateIndex(next_state)]);
}
if (p_down != 0.0f) {
SimpleGuidanceState next_state = *state;
next_state.y -= 1;
next_state.prev_action.type = DOWN;
probabilities.push_back(p_down);
rewards.push_back(-1);
next_states.push_back(complete_state_vector_[getStateIndex(next_state)]);
}
if (p_left != 0.0f) {
SimpleGuidanceState next_state = *state;
next_state.x -= 1;
next_state.prev_action.type = LEFT;
probabilities.push_back(p_left);
rewards.push_back(-1);
next_states.push_back(complete_state_vector_[getStateIndex(next_state)]);
}
if (p_right != 0.0f) {
SimpleGuidanceState next_state = *state;
next_state.x += 1;
next_state.prev_action.type = RIGHT;
probabilities.push_back(p_right);
rewards.push_back(-1);
next_states.push_back(complete_state_vector_[getStateIndex(next_state)]);
}
if (probabilities.size() == 0) {
std::cout << *state << " " << *action << std::endl;
assert(probabilities.size() == 0);
}
}
}
//TODO: Initialize here
ClothSystem2::ClothSystem2(int numParticles) : PendulumSystem2(numParticles)
{
m_numParticles = numParticles*numParticles;
cloth_size = numParticles;
float seperation = 0.08f;
is_Cloth = true;
bool isVerlet = false;
for (int row = 0; row < numParticles; row++) {
for (int col = 0; col < numParticles; col++) {
//cout << "row: "<< row << endl;
//cout << "col: " << col << endl;
if (isVerlet) {
m_vVecState.push_back(Vector3f((float)col*seperation, (float)-1.0f*row*seperation, 0.0f)); //x
m_vVecState.push_back(Vector3f((float)col*seperation, (float)-1.0f*row*seperation, 0.0f)); //old x
}
else {
m_vVecState.push_back(Vector3f(2.5f, (float)0.5-(-1.0f*row*seperation), (float)(-1.0*cloth_size/2+col)*seperation));
m_vVecState.push_back(Vector3f(0.0f, 0.0f, 0.0f));
particles.push_back(Vector3f(0.0f, (float)0.5 - (-1.0f*row*seperation), (float)(-1.0*cloth_size / 2 + col)*seperation));
}
}
}
structural_force = 60.0f;
shear_force = 60.0f;
flex_force = 60.0f;
//structural springs
struc_rest_length = 0.2f;
for (int row = 0; row < cloth_size; row++) {
for (int col = 0; col < cloth_size; col++) {
if (row < cloth_size - 1) {
springs.push_back(Vector4f(getStateIndex(row, col), getStateIndex(row + 1, col), struc_rest_length, structural_force));
}
if (col < cloth_size - 1) {
springs.push_back(Vector4f(getStateIndex(row, col), getStateIndex(row, col + 1), struc_rest_length, structural_force));
}
}
}
//shear springs
shear_rest_length = 0.28f;
for (int row = 0; row < cloth_size; row++) {
for (int col = 0; col < cloth_size - 1; col++) {
if ((row < cloth_size - 1) && (col < cloth_size - 1)) {
springs.push_back(Vector4f(getStateIndex(row, col), getStateIndex(row + 1, col + 1), shear_rest_length, shear_force));
}
//cout << "SHEAR1 OKAY" << endl;
if ((row > 0) && (col < cloth_size - 1)) {
//cout << "rowcol" << row << endl;
springs.push_back(Vector4f(getStateIndex(row, col), getStateIndex(row - 1, col + 1), shear_rest_length, shear_force));
}
}
}
//flexion
flex_rest_length = 0.4f;
for (int row = 0; row < cloth_size; row++) {
for (int col = 0; col < cloth_size; col++) {
if (col < cloth_size - 2) {
springs.push_back(Vector4f(getStateIndex(row, col), getStateIndex(row, col + 2), flex_rest_length, flex_force));
}
//cout << "FLEX OKAY" << endl;
if (row < cloth_size - 2) {
//cout << "rowcol" << row << endl;
springs.push_back(Vector4f(getStateIndex(row, col), getStateIndex(row + 2, col), flex_rest_length, flex_force));
}
}
}
adjacencyList.resize(m_numParticles);
int face_index = 0;
//faces
for (int row = 0; row < cloth_size - 1; row++) {
for (int col = 0; col < cloth_size - 1; col++) {
int i = getStateIndex(row, col);
faces1.push_back(Vector3f(i + cloth_size, i + cloth_size + 1, i + 1)); //ccw
faces1.push_back(Vector3f(i + cloth_size, i + 1, i));
faces2.push_back(Vector3f(i + cloth_size, i + 1, i + cloth_size + 1)); //cw
faces2.push_back(Vector3f(i + cloth_size, i, i + 1));
adjacencyList[i + cloth_size].push_back(face_index);
adjacencyList[i + cloth_size].push_back(face_index + 1);
adjacencyList[i + cloth_size + 1].push_back(face_index);
adjacencyList[i + 1].push_back(face_index);
adjacencyList[i + 1].push_back(face_index + 1);
adjacencyList[i].push_back(face_index + 1);
face_index += 2;
}
}
std::cout << "faces" << endl;
}
