void dependency_test1() {
std::cout << "Starting dependency test 1" << std::endl;
enum { kTestRuns = 1 };
for (int test = 0; test < kTestRuns; ++test) {
enum { kChildren = 1000 };
child1_context ctx = { kChildren + 1, 0 };
task_manager jq(next_power_of_two(kChildren + 2), 1);
task_id parentid = jq.begin_add(child1, &ctx);
for (int i = 0; i < kChildren; ++i) {
task_id childid = jq.begin_add(child1, &ctx);
jq.add_child(parentid, childid);
jq.end_add(childid);
}
task_id dependentid = jq.begin_add(dependent1, &ctx);
jq.add_dependency(parentid, dependentid);
jq.end_add(dependentid);
jq.end_add(parentid);
jq.wait(dependentid);
}
std::cout << "Dependency test 1 succedded!\nEnding dependency test 1\n\n";
}
void dependency_test3() {
std::cout << "Starting dependency test 3" << std::endl;
enum { kTestRuns = 5 };
for (int test = 0; test < kTestRuns; ++test) {
task_manager jq(8);
// Create the parent
dependency_test2_global_context global_ctx;
dependency_test2_local_context parent_ctx = { &global_ctx, 0 };
task_id parentid = jq.begin_add(dependency_test2_func, &parent_ctx);
// Create sub-parent
dependency_test2_local_context subparent_ctx = { &global_ctx, 1 };
task_id subparentid = jq.begin_add(dependency_test2_func, &subparent_ctx);
jq.add_child(parentid, subparentid);
// Create first child
dependency_test2_local_context child1_ctx = { &global_ctx, 2 };
task_id child1id = jq.begin_add(dependency_test2_func, &child1_ctx);
jq.add_child(subparentid, child1id);
jq.end_add(child1id);
// Create the second child
dependency_test2_local_context child2_ctx = { &global_ctx, 3 };
task_id child2id = jq.begin_add(dependency_test2_func, &child2_ctx);
jq.add_child(subparentid, child2id);
jq.end_add(child2id);
// Create children's dependent task
dependency_test2_local_context child_dependent_ctx = { &global_ctx, 4 };
task_id child_dependentid = jq.begin_add(dependency_test2_func, &child_dependent_ctx);
jq.add_dependency(subparentid, child_dependentid);
jq.add_child(parentid, child_dependentid);
jq.end_add(child_dependentid);
jq.end_add(subparentid);
// Add parent's dependent task
dependency_test2_local_context parent_dependent_ctx = { &global_ctx, 5 };
task_id parent_dependentid = jq.begin_add(dependency_test2_func, &parent_dependent_ctx);
jq.add_dependency(parentid, parent_dependentid);
jq.end_add(parent_dependentid);
jq.end_add(parentid);
jq.wait(parent_dependentid);
int i = 1;
mpsc_queue< int >::node* n = 0;
while ((n = global_ctx.executionOrder.pop()) != 0) {
std::cout << i << ".) " << n->value << std::endl;
++i;
}
std::cout << std::endl;
}
std::cout << "Ending dependency test 3\n\n";
}
bool pathPlanning::singularity()
{
Eigen::MatrixXd jq(this->device.get()->baseJframe(this->toolFrame,this->state).e().cols(), this->device.get()->baseJframe(this->toolFrame,this->state).e().rows());
jq = this->device.get()->baseJframe(this->toolFrame,this->state).e();
if(rw::math::LinearAlgebra::det(jq)> 0.01)
{
return true;
}
else
{
//cout << "Determinant: " <<rw::math::LinearAlgebra::det(jq) << endl;
return false;
}
}
void mandelbrot_test() {
std::cout << "Starting mandelbrot test." << std::endl;
// Mandelbrot fractal setup
enum { kNumBlocks = kNumHorizontalBlocks * kNumVerticalBlocks };
enum { kNumFractals = 4 };
#if 0
// Single threaded profiling
{
timeval t1, t2;
double mandelbrot_x = -2.0f;
double mandelbrot_y = -1.0f;
double mandelbrot_width = 3.0f;
double mandelbrot_height = 2.0f;
mandelbrot_y += mandelbrot_height;
uint8_t* image_mt = (uint8_t*)malloc(kImageWidth*kImageHeight);
mandelbrot_block* blocks = (mandelbrot_block*)malloc(kNumBlocks*sizeof(mandelbrot_block));
// setup image blocks and add jobs for multi-threaded test
double elapsed = 0.0f;
{
for(unsigned i = 0; i < kNumFractals; ++i)
{
printf("Calculating fractal %i/%i...\n", i+1, kNumFractals);
gettimeofday(&t1, 0);
g_delta_cr = mandelbrot_width/kImageWidth;
g_delta_ci = mandelbrot_height/kImageWidth;
unsigned bi = 0;
for(unsigned by = 0; by < kNumVerticalBlocks; ++by)
for(unsigned bx = 0; bx < kNumHorizontalBlocks; ++bx)
{
mandelbrot_block &block = blocks[bi++];
block.start_cr = mandelbrot_x + double(bx) * mandelbrot_width / kNumHorizontalBlocks;
block.start_ci = mandelbrot_y - double(by) * mandelbrot_height / kNumVerticalBlocks;
block.result = image_mt + bx * kBlockWidth + by * kBlockHeight * kImageWidth;
calculate_mandelbrot_block(&block);
}
gettimeofday(&t2, 0);
double e = elapsed_time_ms(t1, t2);
std::cout << e << std::endl;
elapsed += e;
mandelbrot_x += mandelbrot_width * 0.05f;
mandelbrot_y -= mandelbrot_height * 0.025f;
mandelbrot_width *= 0.9f;
mandelbrot_height *= 0.9f;
}
}
free(blocks);
free(image_mt);
std::cout << "Serial time (ms): " << elapsed << std::endl;
}
#endif
// Multi-threaded profiling
{
task_manager jq(next_power_of_two(kNumBlocks));
timeval t1, t2;
double mandelbrot_x = -2.0f;
double mandelbrot_y = -1.0f;
double mandelbrot_width = 3.0f;
double mandelbrot_height = 2.0f;
mandelbrot_y += mandelbrot_height;
uint8_t* image_mt = (uint8_t*)malloc(kImageWidth*kImageHeight);
mandelbrot_block* blocks = (mandelbrot_block*)malloc(kNumBlocks*sizeof(mandelbrot_block));
// setup image blocks and add jobs for multi-threaded test
double elapsed = 0.0f;
{
for(unsigned i = 0; i < kNumFractals; ++i)
{
task_id parent = jq.begin_add(0, 0);
printf("Calculating fractal %i/%i...\n", i+1, kNumFractals);
gettimeofday(&t1, 0);
g_delta_cr = mandelbrot_width/kImageWidth;
g_delta_ci = mandelbrot_height/kImageWidth;
unsigned bi = 0;
for(unsigned by = 0; by < kNumVerticalBlocks; ++by)
for(unsigned bx = 0; bx < kNumHorizontalBlocks; ++bx)
{
mandelbrot_block &block = blocks[bi++];
block.start_cr = mandelbrot_x + double(bx) * mandelbrot_width / kNumHorizontalBlocks;
block.start_ci = mandelbrot_y - double(by) * mandelbrot_height / kNumVerticalBlocks;
block.result = image_mt + bx * kBlockWidth + by * kBlockHeight * kImageWidth;
task_id id = jq.begin_add(calculate_mandelbrot_block, &block);
jq.add_child(parent, id);
jq.end_add(id);
}
jq.end_add(parent);
jq.wait(parent);
gettimeofday(&t2, 0);
double e = elapsed_time_ms(t1, t2);
std::cout << e << std::endl;
elapsed += e;
mandelbrot_x += mandelbrot_width * 0.05f;
mandelbrot_y -= mandelbrot_height * 0.025f;
mandelbrot_width *= 0.9f;
mandelbrot_height *= 0.9f;
}
}
free(blocks);
free(image_mt);
std::cout << "Parallel time (ms): " << elapsed << std::endl;
}
std::cout << "Ending mandelbrot test.\n\n";
}
rw::math::Q pathPlanning::invKin(double dx, double dy , double dz)
{
rw::kinematics::State state = this->state;
rw::math::Transform3D<> t_tool_base = this->device.get()->baseTframe(this->toolFrame,state);
Eigen::MatrixXd jq(this->device.get()->baseJframe(this->toolFrame,state).e().cols(), this->device.get()->baseJframe(this->toolFrame,state).e().rows());
jq = this->device.get()->baseJframe(this->toolFrame,state).e();
//cout << "Determinant: " <<rw::math::LinearAlgebra::det(jq) << endl;
//Least square solver - dq = [j(q)]T (j(q)[j(q)]T)⁻1 du <=> dq = A*du
Eigen::MatrixXd A (6,6);
//A = jq.transpose()*(jq*jq.transpose()).inverse();
A = (jq*jq.transpose()).inverse()*jq.transpose();
std::vector<rw::math::Transform3D<> > out = sphere(dx,dy,dz);
std::ofstream outfile;
outfile.open("q_conf.txt", std::ios_base::app);
cout << "invKin" << endl;
for(unsigned int i = 0; i <= out.size() ; ++i )
{
cout << "\r"+to_string(i);
rw::math::Vector3D<> dif_p = out[i].P()-t_tool_base.P();
Eigen::Matrix3d dif = out[i].R().e()- t_tool_base.R().e();
rw::math::Rotation3D<> dif_r(dif);
rw::math::RPY<> dif_rot(dif_r);
Eigen::VectorXd du(6);
du(0) = dif_p[0];
du(1) = dif_p[1];
du(2) = dif_p[2];
du(3) = dif_rot[0];
du(4) = dif_rot[1];
du(5) = dif_rot[2];
Eigen::VectorXd q(6);
q = A*du;
rw::math::Q q_current;
q_current = this->device->getQ(this->state);
rw::math::Q dq(q);
rw::math::Q q_new = q_current+ dq;
if(!collision(q_new) && !within_bound(q_new))
{
std::string text = "setQ{" + to_string(q_new[0]) + ", " + to_string(q_new[1]) + ", " + to_string(q_new[2]) + ", " + to_string(q_new[3]) + ", " + to_string(q_new[4]) + ", " + to_string(q_new[5]) + "}";
//cout << text << endl;
outfile << text << endl;
}
else
{
cout << endl;
//cout << "\rfalse";
//cout << "collision" << endl;
}
}
rw::math::Q bla(6); //Just used the text file for debugging purposes, Which why I just return a random Q config.
return bla;
}
