void ChompOptimizer::perturbTrajectory()
{
//int mid_point = (free_vars_start_ + free_vars_end_) / 2;
if(worst_collision_cost_state_ < 0)
return;
int mid_point = worst_collision_cost_state_;
planning_models::RobotState *random_state(state_);
random_state.getJointStateGroup(planning_group_)->setToRandomValues();
std::vector<double> vals;
random_state.getJointStateGroup(planning_group_)->getGroupStateValues(vals);
double* ptr = &vals[0];
Eigen::Map<Eigen::VectorXd> random_matrix(ptr, vals.size());
//Eigen::VectorXd random_matrix = vals;
// convert the state into an increment
random_matrix -= group_trajectory_.getTrajectoryPoint(mid_point).transpose();
// project the increment orthogonal to joint velocities
group_trajectory_.getJointVelocities(mid_point, joint_state_velocities_);
joint_state_velocities_.normalize();
random_matrix = (Eigen::MatrixXd::Identity(num_joints_, num_joints_) - joint_state_velocities_
* joint_state_velocities_.transpose()) * random_matrix;
int mp_free_vars_index = mid_point - free_vars_start_;
for(int i = 0; i < num_joints_; i++)
{
group_trajectory_.getFreeJointTrajectoryBlock(i)
+= joint_costs_[i].getQuadraticCostInverse().col(mp_free_vars_index) * random_state_(i);
}
}
int main() {
// Standard deviation of truncated normal distribution in proposal function
double proposal_stddev = 0.1;
// Side length of the box
double side_length = 15.0;
// Number of oppositely charged particle pairs
unsigned particle_num = 20;
// Thermodynamic beta
double beta = 300.0;
// Number of samples
std::size_t sample_num = 100000;
// Output
bool save_output = true;
std::string filename = "out.tsv";
// Convenience typedefs
using Ensemble = CanonicalEnsemble<Particle2D>;
using PotentialPtr = double (*)(const Particle2D &, const Particle2D &);
auto initial_state = random_state(side_length, particle_num);
auto proposal_function =
unif_proposal_function(proposal_stddev, side_length);
// Simulate samples of the canonical ensemble using the Random-Walk
// Metropolis-Algorithm
Ensemble ensemble(initial_state, beta,
static_cast<PotentialPtr>(coulomb_core),
proposal_function);
std::vector<Ensemble::State> samples;
samples.reserve(sample_num);
std::size_t accepted_cnt = 0;
for (std::size_t i = 0; i < sample_num; ++i) {
if (ensemble.step()) {
++accepted_cnt;
}
samples.push_back(ensemble.get_state());
}
std::cout << "Acceptance probability: "
<< static_cast<double>(accepted_cnt) / sample_num << std::endl;
// Save output to .tsv
if (save_output) {
std::ofstream os(filename);
for (auto it = samples.cbegin(), end = samples.cend(); os && it != end;
++it) {
for (const auto &particle : *it) {
os << particle.q << "\t" << particle.x << "\t" << particle.y
<< "\t";
}
os << "\n";
}
}
return 0;
}
void vsx_statelist::random_state() {
if (0 == statelist.size()) return;
if ((*state_iter).engine != vxe) return;
int steps = rand() % statelist.size();
while (steps) {
++state_iter;
if (state_iter == statelist.end()) state_iter = statelist.begin();
--steps;
}
if ((*state_iter).engine == vxe) { random_state(); return; }
init_current((*state_iter).engine, &(*state_iter));
transition_time = 2.0f;
}
void vsx_statelist::render()
{
if (render_first)
{
glewInit();
GLint viewport[4];
glGetIntegerv(GL_VIEWPORT, viewport);
if (tex1.has_buffer_support())
{
tex1.init_feedback_buffer(viewport[2], viewport[3]);
tex_to.init_feedback_buffer(viewport[2], viewport[3]);
get_files_recursive(own_path+"visuals_faders", &fader_file_list,"",".svn CVS");
for (std::list<vsx_string>::iterator it = fader_file_list.begin(); it != fader_file_list.end(); ++it)
{
#ifdef VSXU_DEBUG
printf("initializing fader %s\n", (*it).c_str());
#endif
vsx_engine* lvxe = new vsx_engine();
lvxe->set_module_list( module_list );
lvxe->start();
lvxe->load_state(*it);
faders.push_back(lvxe);
fade_id = 0;
}
}
transitioning = false;
render_first = false;
if ( state_iter == statelist.end() ) return;
// mark all state_info instances volatile
for (state_iter = statelist.begin(); state_iter != statelist.end(); state_iter++)
{
(*state_iter).is_volatile = true;
}
// go through statelist and load and validate every plugin
std::vector<state_info> new_statelist;
for (state_iter = statelist.begin(); state_iter != statelist.end(); state_iter++)
{
if (init_current((*state_iter).engine, &(*state_iter)) > 0)
{
continue;
}
new_statelist.push_back(*state_iter);
if (option_preload_all == true)
{
while ( (*state_iter).engine->get_modules_left_to_load() )
{
(*state_iter).engine->process_message_queue( &(*state_iter).cmd_in, cmd_out = &(*state_iter).cmd_out,false, true);
(*state_iter).engine->render();
}
}
}
statelist = new_statelist;
// mark all state_info instances non-volatile (engine will be deleted when state_iter will be deleted)
for (state_iter = statelist.begin(); state_iter != statelist.end(); state_iter++)
{
(*state_iter).is_volatile = true;
}
// reset state_iter to a random state
state_iter = statelist.begin();
int steps = rand() % statelist.size();
while (steps) {
++state_iter;
if (state_iter == statelist.end()) state_iter = statelist.begin();
--steps;
}
vxe = (*state_iter).engine;
cmd_in = &(*state_iter).cmd_in;
cmd_out = &(*state_iter).cmd_out;
} // render first
// prevent from rendering by mistake
if ( !statelist.size() ) return;
if ((*state_iter).engine != vxe) // change is on the way
{
if ( tex_to.has_buffer_support() )
{
tex_to.begin_capture_to_buffer();
if ((*state_iter).engine)
{
(*state_iter).engine->process_message_queue(&(*state_iter).cmd_in,&(*state_iter).cmd_out);
(*state_iter).engine->render();
}
glColorMask(false, false, false, true);
glClearColor(0,0,0,1);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glColorMask(true, true, true, true);
tex_to.end_capture_to_buffer();
if (
(*state_iter).engine->get_modules_left_to_load() == 0 &&
(*state_iter).engine->get_commands_internal_count() &&
transition_time > 1.0f
)
{
transition_time = 1.0f;
timer.start();
fade_id = rand() % (faders.size());
}
} else
{
transition_time = -1.0f;
}
if (transition_time <= 0.0)
{
vxe = (*state_iter).engine;
cmd_in = &(*state_iter).cmd_in;
cmd_out = &(*state_iter).cmd_out;
transitioning = false;
transition_time = 2.0f;
if (cmd_out && cmd_in)
{
if (vxe)
{
vxe->process_message_queue(cmd_in,cmd_out);
}
cmd_out->clear(true);
}
if (vxe)
{
vxe->render();
}
} else
{
if (cmd_out && cmd_in)
{
if (vxe)
{
vxe->process_message_queue(cmd_in,cmd_out);
}
cmd_out->clear(true);
}
// begin capture
if (tex1.has_buffer_support())
{
tex1.begin_capture_to_buffer();
}
// render
if (vxe)
{
vxe->render();
}
glColorMask(false, false, false, true);
glClearColor(0,0,0,1);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glColorMask(true, true, true, true);
// end capture and send to fader
if (tex1.has_buffer_support())
{
tex1.end_capture_to_buffer();
vsx_module_param_texture* param_t_a = (vsx_module_param_texture*)faders[fade_id]->get_in_param_by_name("visual_fader", "texture_a_in");
vsx_module_param_texture* param_t_b = (vsx_module_param_texture*)faders[fade_id]->get_in_param_by_name("visual_fader", "texture_b_in");
vsx_module_param_float* param_pos = (vsx_module_param_float*)faders[fade_id]->get_in_param_by_name("visual_fader", "fade_pos_in");
vsx_module_param_float* fade_pos_from_engine = (vsx_module_param_float*)faders[fade_id]->get_in_param_by_name("visual_fader", "fade_pos_from_engine");
faders[fade_id]->process_message_queue(&l_cmd_in, &l_cmd_out);
l_cmd_out.clear();
if (param_t_a && param_t_b && param_pos && fade_pos_from_engine)
{
param_t_a->set(&tex1);
param_t_b->set(&tex_to);
fade_pos_from_engine->set(1.0f);
float t = transition_time;
if (t > 1.0f) t = 1.0f;
if (t < 0.0f) t = 0.0f;
param_pos->set(1.0-t);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
faders[fade_id]->render();
}
}
if (transition_time <= 1.0f)
{
transition_time -= timer.dtime();
}
}
} else
{
if (cmd_out && cmd_in)
{
vxe->process_message_queue(cmd_in, cmd_out);
cmd_out->clear();
}
vxe->render();
if (randomizer)
{
randomizer_time -= vxe->get_engine_info()->real_dtime;
if (randomizer_time < 0.0f)
{
random_state();
randomizer_time = (float)(rand()%1000)*0.001f*15.0f+10.0f;
}
}
}
}
