void operator () () const { try { V v1 (N), v2 (N); M m1 (N, N); (*this) (v1, v2, m1); ublas::matrix_row<M> mr1 (m1, N - 1), mr2 (m1, N - 1); (*this) (mr1, mr2, m1); ublas::matrix_column<M> mc1 (m1, 0), mc2 (m1, 0); (*this) (mc1, mc2, m1); #ifdef USE_RANGE_AND_SLICE ublas::matrix_vector_range<M> mvr1 (m1, ublas::range (0, N), ublas::range (0, N)), mvr2 (m1, ublas::range (0, N), ublas::range (0, N)); (*this) (mvr1, mvr2, m1); ublas::matrix_vector_slice<M> mvs1 (m1, ublas::slice (0, 1, N), ublas::slice (0, 1, N)), mvs2 (m1, ublas::slice (0, 1, N), ublas::slice (0, 1, N)); (*this) (mvs1, mvs2, m1); #endif } catch (std::exception &e) { std::cout << e.what () << std::endl; } catch (...) { std::cout << "unknown exception" << std::endl; } }
void operator () () const { { #ifdef USE_BANDED M m1 (N, N, 1, 1), m2 (N, N, 1, 1), m3 (N, N, 1, 1); #endif #ifdef USE_DIAGONAL M m1 (N, N), m2 (N, N), m3 (N, N); #endif test_with (m1, m2, m3); #ifdef USE_RANGE ublas::matrix_range<M> mr1 (m1, ublas::range (0, N), ublas::range (0, N)), mr2 (m2, ublas::range (0, N), ublas::range (0, N)), mr3 (m3, ublas::range (0, N), ublas::range (0, N)); test_with (mr1, mr2, mr3); #endif #ifdef USE_SLICE ublas::matrix_slice<M> ms1 (m1, ublas::slice (0, 1, N), ublas::slice (0, 1, N)), ms2 (m2, ublas::slice (0, 1, N), ublas::slice (0, 1, N)), ms3 (m3, ublas::slice (0, 1, N), ublas::slice (0, 1, N)); test_with (ms1, ms2, ms3); #endif } }
void operator () (int) const { #ifdef USE_ADAPTOR { V v1 (N), v2 (N); M m1 (N, N); ublas::symmetric_adaptor<M> tam1 (m1); test_with (v1, v2, tam1); ublas::matrix_row<ublas::symmetric_adaptor<M> > mr1 (tam1, N - 1), mr2 (tam1, N - 1); test_with (mr1, mr2, tam1); ublas::matrix_column<ublas::symmetric_adaptor<M> > mc1 (tam1, 0), mc2 (tam1, 0); test_with (mc1, mc2, tam1); #ifdef USE_RANGE ublas::matrix_vector_range<ublas::symmetric_adaptor<M> > mvr1 (tam1, ublas::range (0, N), ublas::range (0, N)), mvr2 (tam1, ublas::range (0, N), ublas::range (0, N)); test_with (mvr1, mvr2, tam1); #endif #ifdef USE_SLICE ublas::matrix_vector_slice<ublas::symmetric_adaptor<M> > mvs1 (tam1, ublas::slice (0, 1, N), ublas::slice (0, 1, N)), mvs2 (tam1, ublas::slice (0, 1, N), ublas::slice (0, 1, N)); test_with (mvs1, mvs2, tam1); #endif } #endif }
void operator () () const { try { M m1 (N, N), m2 (N, N), m3 (N, N); (*this) (m1, m2, m3); #ifdef USE_RANGE ublas::matrix_range<M> mr1 (m1, ublas::range (0, N), ublas::range (0, N)), mr2 (m2, ublas::range (0, N), ublas::range (0, N)), mr3 (m3, ublas::range (0, N), ublas::range (0, N)); (*this) (mr1, mr2, mr3); #endif #ifdef USE_SLICE ublas::matrix_slice<M> ms1 (m1, ublas::slice (0, 1, N), ublas::slice (0, 1, N)), ms2 (m2, ublas::slice (0, 1, N), ublas::slice (0, 1, N)), ms3 (m3, ublas::slice (0, 1, N), ublas::slice (0, 1, N)); (*this) (ms1, ms2, ms3); #endif } catch (std::exception &e) { std::cout << e.what () << std::endl; } catch (...) { std::cout << "unknown exception" << std::endl; } }
void operator () (int) const { #ifdef USE_ADAPTOR try { M m1 (N, N), m2 (N, N), m3 (N, N); ublas::triangular_adaptor<M> tam1 (m1), tam2 (m2), tam3 (m3); (*this) (tam1, tam2, tam3); #ifdef USE_RANGE ublas::matrix_range<ublas::triangular_adaptor<M> > mr1 (tam1, ublas::range (0, N), ublas::range (0, N)), mr2 (tam2, ublas::range (0, N), ublas::range (0, N)), mr3 (tam3, ublas::range (0, N), ublas::range (0, N)); (*this) (mr1, mr2, mr3); #endif #ifdef USE_SLICE ublas::matrix_slice<ublas::triangular_adaptor<M> > ms1 (tam1, ublas::slice (0, 1, N), ublas::slice (0, 1, N)), ms2 (tam2, ublas::slice (0, 1, N), ublas::slice (0, 1, N)), ms3 (tam3, ublas::slice (0, 1, N), ublas::slice (0, 1, N)); (*this) (ms1, ms2, ms3); #endif } catch (std::exception &e) { std::cout << e.what () << std::endl; } catch (...) { std::cout << "unknown exception" << std::endl; } #endif }
void operator () () const { { V v1 (N), v2 (N); M m1 (N, N); test_with (v1, v2, m1); ublas::matrix_row<M> mr1 (m1, 0), mr2 (m1, 1); test_with (mr1, mr2, m1); ublas::matrix_column<M> mc1 (m1, 0), mc2 (m1, 1); test_with (mc1, mc2, m1); #ifdef USE_RANGE ublas::matrix_vector_range<M> mvr1 (m1, ublas::range (0, N), ublas::range (0, N)), mvr2 (m1, ublas::range (0, N), ublas::range (0, N)); test_with (mvr1, mvr2, m1); #endif #ifdef USE_SLICE ublas::matrix_vector_slice<M> mvs1 (m1, ublas::slice (0, 1, N), ublas::slice (0, 1, N)), mvs2 (m1, ublas::slice (0, 1, N), ublas::slice (0, 1, N)); test_with (mvs1, mvs2, m1); #endif } }
inline void ModUnionClosurePar::do_MemRegion(MemRegion mr) { // Align the end of mr so it's at a card boundary. // This is superfluous except at the end of the space; // we should do better than this XXX MemRegion mr2(mr.start(), (HeapWord*)round_to((intptr_t)mr.end(), CardTableModRefBS::card_size /* bytes */)); _t->par_mark_range(mr2); }
void operator () (int) const { #ifdef USE_ADAPTOR { #ifdef USE_BANDED M m1 (N, N, 1, 1), m2 (N, N, 1, 1), m3 (N, N, 1, 1); ublas::banded_adaptor<M> bam1 (m1, 1, 1), bam2 (m2, 1, 1), bam3 (m3, 1, 1); test_with (bam1, bam2, bam3); #ifdef USE_RANGE ublas::matrix_range<ublas::banded_adaptor<M> > mr1 (bam1, ublas::range (0, N), ublas::range (0, N)), mr2 (bam2, ublas::range (0, N), ublas::range (0, N)), mr3 (bam3, ublas::range (0, N), ublas::range (0, N)); test_with (mr1, mr2, mr3); #endif #ifdef USE_SLICE ublas::matrix_slice<ublas::banded_adaptor<M> > ms1 (bam1, ublas::slice (0, 1, N), ublas::slice (0, 1, N)), ms2 (bam2, ublas::slice (0, 1, N), ublas::slice (0, 1, N)), ms3 (bam3, ublas::slice (0, 1, N), ublas::slice (0, 1, N)); test_with (ms1, ms2, ms3); #endif #endif #ifdef USE_DIAGONAL M m1 (N, N), m2 (N, N), m3 (N, N); ublas::diagonal_adaptor<M> dam1 (m1), dam2 (m2), dam3 (m3); test_with (dam1, dam2, dam3); #ifdef USE_RANGE ublas::matrix_range<ublas::diagonal_adaptor<M> > mr1 (dam1, ublas::range (0, N), ublas::range (0, N)), mr2 (dam2, ublas::range (0, N), ublas::range (0, N)), mr3 (dam3, ublas::range (0, N), ublas::range (0, N)); test_with (mr1, mr2, mr3); #endif #ifdef USE_SLICE ublas::matrix_slice<ublas::diagonal_adaptor<M> > ms1 (dam1, ublas::slice (0, 1, N), ublas::slice (0, 1, N)), ms2 (dam2, ublas::slice (0, 1, N), ublas::slice (0, 1, N)), ms3 (dam3, ublas::slice (0, 1, N), ublas::slice (0, 1, N)); test_with (ms1, ms2, ms3); #endif #endif } #endif }
void operator () () const { { M m1 (N, N), m2 (N, N), m3 (N, N); test_with (m1, m2, m3); #ifdef USE_RANGE ublas::matrix_range<M> mr1 (m1, ublas::range (0, N), ublas::range (0, N)), mr2 (m2, ublas::range (0, N), ublas::range (0, N)), mr3 (m3, ublas::range (0, N), ublas::range (0, N)); test_with (mr1, mr2, mr3); #endif #ifdef USE_SLICE ublas::matrix_slice<M> ms1 (m1, ublas::slice (0, 1, N), ublas::slice (0, 1, N)), ms2 (m2, ublas::slice (0, 1, N), ublas::slice (0, 1, N)), ms3 (m3, ublas::slice (0, 1, N), ublas::slice (0, 1, N)); test_with (ms1, ms2, ms3); #endif } #ifdef USE_ADAPTOR { M m1 (N, N), m2 (N, N), m3 (N, N); ublas::triangular_adaptor<M> tam1 (m1), tam2 (m2), tam3 (m3); test_with (tam1, tam2, tam3); #ifdef USE_RANGE ublas::matrix_range<ublas::triangular_adaptor<M> > mr1 (tam1, ublas::range (0, N), ublas::range (0, N)), mr2 (tam2, ublas::range (0, N), ublas::range (0, N)), mr3 (tam3, ublas::range (0, N), ublas::range (0, N)); test_with (mr1, mr2, mr3); #endif #ifdef USE_SLICE ublas::matrix_slice<ublas::triangular_adaptor<M> > ms1 (tam1, ublas::slice (0, 1, N), ublas::slice (0, 1, N)), ms2 (tam2, ublas::slice (0, 1, N), ublas::slice (0, 1, N)), ms3 (tam3, ublas::slice (0, 1, N), ublas::slice (0, 1, N)); test_with (ms1, ms2, ms3); #endif } #endif }
void do_MemRegion(MemRegion mr) { // We start at the high end of "mr", walking backwards // while accumulating a contiguous dirty range of cards in // [start_of_non_clean, end_of_non_clean) which we then // process en masse. HeapWord* end_of_non_clean = mr.end(); HeapWord* start_of_non_clean = end_of_non_clean; jbyte* entry = _ct->byte_for(mr.last()); const jbyte* first_entry = _ct->byte_for(mr.start()); while (entry >= first_entry) { HeapWord* cur = _ct->addr_for(entry); if (!clear_card(entry)) { // We hit a clean card; process any non-empty // dirty range accumulated so far. if (start_of_non_clean < end_of_non_clean) { MemRegion mr2(start_of_non_clean, end_of_non_clean); _dirty_card_closure->do_MemRegion(mr2); } // Reset the dirty window while continuing to // look for the next dirty window to process. end_of_non_clean = cur; start_of_non_clean = end_of_non_clean; } // Open the left end of the window one card to the left. start_of_non_clean = cur; // Note that "entry" leads "start_of_non_clean" in // its leftward excursion after this point // in the loop and, when we hit the left end of "mr", // will point off of the left end of the card-table // for "mr". entry--; } // If the first card of "mr" was dirty, we will have // been left with a dirty window, co-initial with "mr", // which we now process. if (start_of_non_clean < end_of_non_clean) { MemRegion mr2(start_of_non_clean, end_of_non_clean); _dirty_card_closure->do_MemRegion(mr2); } }
// @requires UseG1GC TEST_VM(FreeRegionList, length) { if (!UseG1GC) { return; } FreeRegionList l("test"); const uint num_regions_in_test = 5; // Create a fake heap. It does not need to be valid, as the HeapRegion constructor // does not access it. MemRegion heap(NULL, num_regions_in_test * HeapRegion::GrainWords); // Allocate a fake BOT because the HeapRegion constructor initializes // the BOT. size_t bot_size = G1BlockOffsetTable::compute_size(heap.word_size()); HeapWord* bot_data = NEW_C_HEAP_ARRAY(HeapWord, bot_size, mtGC); ReservedSpace bot_rs(G1BlockOffsetTable::compute_size(heap.word_size())); G1RegionToSpaceMapper* bot_storage = G1RegionToSpaceMapper::create_mapper(bot_rs, bot_rs.size(), os::vm_page_size(), HeapRegion::GrainBytes, BOTConstants::N_bytes, mtGC); G1BlockOffsetTable bot(heap, bot_storage); bot_storage->commit_regions(0, num_regions_in_test); // Set up memory regions for the heap regions. MemRegion mr0(heap.start(), HeapRegion::GrainWords); MemRegion mr1(mr0.end(), HeapRegion::GrainWords); MemRegion mr2(mr1.end(), HeapRegion::GrainWords); MemRegion mr3(mr2.end(), HeapRegion::GrainWords); MemRegion mr4(mr3.end(), HeapRegion::GrainWords); HeapRegion hr0(0, &bot, mr0); HeapRegion hr1(1, &bot, mr1); HeapRegion hr2(2, &bot, mr2); HeapRegion hr3(3, &bot, mr3); HeapRegion hr4(4, &bot, mr4); l.add_ordered(&hr1); l.add_ordered(&hr0); l.add_ordered(&hr3); l.add_ordered(&hr4); l.add_ordered(&hr2); EXPECT_EQ(l.length(), num_regions_in_test) << "Wrong free region list length"; l.verify_list(); bot_storage->uncommit_regions(0, num_regions_in_test); delete bot_storage; FREE_C_HEAP_ARRAY(HeapWord, bot_data); }
void do_MemRegion(MemRegion mr) { HeapWord* end_of_non_clean = mr.end(); HeapWord* start_of_non_clean = end_of_non_clean; jbyte* entry = _ct->byte_for(mr.last()); HeapWord* cur = _ct->addr_for(entry); while (mr.contains(cur)) { jbyte entry_val = *entry; if (!clear_card(entry)) { if (start_of_non_clean < end_of_non_clean) { MemRegion mr2(start_of_non_clean, end_of_non_clean); _dirty_card_closure->do_MemRegion(mr2); } end_of_non_clean = cur; start_of_non_clean = end_of_non_clean; } entry--; start_of_non_clean = cur; cur = _ct->addr_for(entry); } if (start_of_non_clean < end_of_non_clean) { MemRegion mr2(start_of_non_clean, end_of_non_clean); _dirty_card_closure->do_MemRegion(mr2); } }
void operator () () const { M m1 (N, N), m2 (N, N), m3 (N, N); test_expression_with (m1, m2, m3); test_container_with (m1); #ifdef USE_RANGE ublas::matrix_range<M> mr1 (m1, ublas::range (0, N), ublas::range (0, N)), mr2 (m2, ublas::range (0, N), ublas::range (0, N)), mr3 (m3, ublas::range (0, N), ublas::range (0, N)); test_expression_with (mr1, mr2, mr3); #endif #ifdef USE_SLICE ublas::matrix_slice<M> ms1 (m1, ublas::slice (0, 1, N), ublas::slice (0, 1, N)), ms2 (m2, ublas::slice (0, 1, N), ublas::slice (0, 1, N)), ms3 (m3, ublas::slice (0, 1, N), ublas::slice (0, 1, N)); test_expression_with (ms1, ms2, ms3); #endif }
void operator () (int runs, fast_tag) const { try { static M m1 (N, N), m2 (N, N), m3 (N, N); ublas::matrix_range<M> mr1 (m1, ublas::range (0, N), ublas::range (0, N)), mr2 (m2, ublas::range (0, N), ublas::range (0, N)), mr3 (m3, ublas::range (0, N), ublas::range (0, N)); initialize_matrix (mr1); initialize_matrix (mr2); boost::timer t; for (int i = 0; i < runs; ++ i) { mr3.assign (ublas::prod (mr1, mr2)); // sink_matrix (mr3); } footer<value_type> () (N * N * N, N * N * (N - 1), runs, t.elapsed ()); } catch (std::exception &e) { std::cout << e.what () << std::endl; } catch (...) { std::cout << "unknown exception" << std::endl; } }
void operator () (int) const { #ifdef USE_ADAPTOR { M m1 (N, N), m2 (N, N), m3 (N, N); ublas::symmetric_adaptor<M> sam1 (m1), sam2 (m2), sam3 (m3); test_with (sam1, sam2, sam3); #ifdef USE_RANGE ublas::matrix_range<ublas::symmetric_adaptor<M> > mr1 (sam1, ublas::range (0, N), ublas::range (0, N)), mr2 (sam2, ublas::range (0, N), ublas::range (0, N)), mr3 (sam3, ublas::range (0, N), ublas::range (0, N)); test_with (mr1, mr2, mr3); #endif #ifdef USE_SLICE ublas::matrix_slice<ublas::symmetric_adaptor<M> > ms1 (sam1, ublas::slice (0, 1, N), ublas::slice (0, 1, N)), ms2 (sam2, ublas::slice (0, 1, N), ublas::slice (0, 1, N)), ms3 (sam3, ublas::slice (0, 1, N), ublas::slice (0, 1, N)); test_with (ms1, ms2, ms3); #endif } #endif }
com() { int i, kuusho, hukasa; int x, y; char **a; char *bp; CELL *p; joseki(); if(te) return; kuusho = 0; a = aki; for (i = 0; i < 60; ) { bp = ban + yuusen[i++]; if(*bp == BLANK) { *(a++) = bp; kuusho++; } } *a = 0; sekisa = 0; for(y = 1; y < 9; y++) { for(x = 1; x < 9; x++) { switch(ban[y*9+x]) { case BLACK: sekisa++; break; case WHITE: sekisa--; } } } init_cell(); p = get_cell(); p->child = NULL; p->cdr = NULL; if(kuusho <= lvtbl[level-1].kanzen) { p->h = -64; mr2(p, kuusho, 64); } else if(kuusho <= lvtbl[level-1].hishou) { p->h = -1; mr2(p, kuusho, 1); } else { hukasa = lvtbl[level-1].hukasa; if(kuusho <= lvtbl[level-1].kyoka) hukasa += 2; p->h = -10000; mr1(p, hukasa, 10000); } te = p->t; }
int main(int argc, char * argv []) { ulxr::intializeLog4J(argv[0]); int success = 0; #ifdef STRESS_IT for (int i= 0; i < 1000; ++i) { #endif try { ULXR_COUT << ULXR_PCHAR("Testing patterns\n"); ULXR_COUT << ULXR_PCHAR("boolPattern\n") << std::flush; testPattern(boolPattern, sizeof(boolPattern)); ULXR_COUT << ULXR_PCHAR("int1Pattern\n"); testPattern(int1Pattern, sizeof(int1Pattern)); ULXR_COUT << ULXR_PCHAR("int2Pattern\n"); testPattern(int2Pattern, sizeof(int2Pattern)); ULXR_COUT << ULXR_PCHAR("doublePattern\n"); testPattern(doublePattern, sizeof(doublePattern)); ULXR_COUT << ULXR_PCHAR("stringPattern\n"); testPattern(stringPattern, sizeof(stringPattern)); ULXR_COUT << ULXR_PCHAR("base64Pattern\n"); testPattern(base64Pattern, sizeof(base64Pattern)); ULXR_COUT << ULXR_PCHAR("datePattern\n"); testPattern(datePattern, sizeof(datePattern)); ULXR_COUT << ULXR_PCHAR("struct1Pattern\n"); testPattern(struct1Pattern, sizeof(struct1Pattern)); ULXR_COUT << ULXR_PCHAR("struct2Pattern\n"); testPattern(struct2Pattern, sizeof(struct2Pattern)); ULXR_COUT << ULXR_PCHAR("arrayPattern\n"); testPattern(arrayPattern, sizeof(arrayPattern)); ULXR_COUT << ULXR_PCHAR("callPattern\n"); testCallPattern(callPattern, sizeof(callPattern)); ULXR_COUT << ULXR_PCHAR("emptyCallPattern\n"); testCallPattern(emptyCallPattern, sizeof(emptyCallPattern)); ULXR_COUT << ULXR_PCHAR("respPattern\n"); testRespPattern(respPattern, sizeof(respPattern)); ULXR_COUT << ULXR_PCHAR("implPattern\n"); testPattern(implPattern, sizeof(implPattern)); ULXR_COUT << ULXR_PCHAR("emptyArrayPattern\n"); testRespPattern(emptyArrayPattern, sizeof(emptyArrayPattern)); ULXR_COUT << ULXR_PCHAR("emptyRespPattern\n"); testRespPattern(emptyRespPattern, sizeof(emptyRespPattern)); ULXR_COUT << ULXR_PCHAR("emptyStructPattern\n"); testRespPattern(emptyStructPattern, sizeof(emptyStructPattern)); ULXR_COUT << ULXR_PCHAR("----------------------------------------------------\n"); ULXR_COUT << ULXR_PCHAR("MethodResponse abc\n"); ulxr::MethodResponse mr1(123, ULXR_PCHAR("faultstr_m")); ULXR_COUT << ulxr::binaryDebugOutput(mr1.getWbXml()); ULXR_COUT << std::endl << mr1.getXml(0) << std::endl; ulxr::MethodResponse mr2(ulxr::Integer(1)); ULXR_COUT << ulxr::binaryDebugOutput(mr2.getWbXml()); ULXR_COUT << std::endl << mr2.getXml(0) << std::endl; ulxr::MethodResponse mr3; ULXR_COUT << ulxr::binaryDebugOutput(mr3.getWbXml()); ULXR_COUT << std::endl << mr3.getXml(0) << std::endl; ULXR_COUT << ULXR_PCHAR("----------------------------------------------------\n"); ulxr::MethodCall mc (ULXR_PCHAR("test.call")); ULXR_COUT << ulxr::binaryDebugOutput(mc.getWbXml()); ULXR_COUT << std::endl << mc.getXml(0) << std::endl; ULXR_COUT << ULXR_PCHAR("====================================================\n"); ulxr::Boolean b(true); ulxr::Integer i(123); ulxr::Double d(123.456); ulxr::RpcString s("<>&\"\'string<>&\"\'"); ulxr::DateTime dt(ULXR_PCHAR("20020310T10:23:45")); ulxr::Base64 b64(ULXR_PCHAR("ABASrt466a90")); ulxr::Struct st; ulxr::Array ar; ULXR_COUT << ulxr::binaryDebugOutput(b.getWbXml()) << std::endl; ULXR_COUT << b.getXml(0) << std::endl; ULXR_COUT << ULXR_PCHAR("----------------------------------------------------\n"); ULXR_COUT << ulxr::binaryDebugOutput(i.getWbXml()) << std::endl; ULXR_COUT << i.getXml(0) << std::endl; ULXR_COUT << ULXR_PCHAR("----------------------------------------------------\n"); ULXR_COUT << ulxr::binaryDebugOutput(d.getWbXml()) << std::endl; ULXR_COUT << d.getXml(0) << std::endl; ULXR_COUT << ULXR_PCHAR("----------------------------------------------------\n"); ULXR_COUT << ulxr::binaryDebugOutput(s.getWbXml()) << std::endl; ULXR_COUT << s.getXml(0) << std::endl; ULXR_COUT << ULXR_PCHAR("----------------------------------------------------\n"); ULXR_COUT << ulxr::binaryDebugOutput(dt.getWbXml()) << std::endl; ULXR_COUT << dt.getXml(0) << std::endl; ULXR_COUT << ULXR_PCHAR("----------------------------------------------------\n"); ULXR_COUT << ulxr::binaryDebugOutput(b64.getWbXml()) << std::endl; ULXR_COUT << b64.getXml(0) << std::endl; ULXR_COUT << ULXR_PCHAR("----------------------------------------------------\n"); ULXR_COUT << ulxr::binaryDebugOutput(st.getWbXml()) << std::endl; ULXR_COUT << st.getXml(0) << std::endl; ULXR_COUT << ULXR_PCHAR("----------------------------------------------------\n"); ULXR_COUT << ulxr::binaryDebugOutput(ar.getWbXml()) << std::endl; ULXR_COUT << ar.getXml(0) << std::endl; ULXR_COUT << ULXR_PCHAR("====================================================\n"); ULXR_COUT << "wbToken_Value " << ulxr::HtmlFormHandler::makeHexNumber((unsigned char)ulxr::ValueParserWb::wbToken_Value) << std::endl; ULXR_COUT << "wbToken_Array " << ulxr::HtmlFormHandler::makeHexNumber((unsigned char)ulxr::ValueParserWb::wbToken_Array) << std::endl; ULXR_COUT << "wbToken_Data " << ulxr::HtmlFormHandler::makeHexNumber((unsigned char)ulxr::ValueParserWb::wbToken_Data) << std::endl; ULXR_COUT << "wbToken_Struct " << ulxr::HtmlFormHandler::makeHexNumber((unsigned char)ulxr::ValueParserWb::wbToken_Struct) << std::endl; ULXR_COUT << "wbToken_Member " << ulxr::HtmlFormHandler::makeHexNumber((unsigned char)ulxr::ValueParserWb::wbToken_Member) << std::endl; ULXR_COUT << "wbToken_Name " << ulxr::HtmlFormHandler::makeHexNumber((unsigned char)ulxr::ValueParserWb::wbToken_Name) << std::endl; ULXR_COUT << "wbToken_Boolean " << ulxr::HtmlFormHandler::makeHexNumber((unsigned char)ulxr::ValueParserWb::wbToken_Boolean) << std::endl; ULXR_COUT << "wbToken_Int " << ulxr::HtmlFormHandler::makeHexNumber((unsigned char)ulxr::ValueParserWb::wbToken_Int) << std::endl; ULXR_COUT << "wbToken_I4 " << ulxr::HtmlFormHandler::makeHexNumber((unsigned char)ulxr::ValueParserWb::wbToken_I4) << std::endl; ULXR_COUT << "wbToken_Double " << ulxr::HtmlFormHandler::makeHexNumber((unsigned char)ulxr::ValueParserWb::wbToken_Double) << std::endl; ULXR_COUT << "wbToken_String " << ulxr::HtmlFormHandler::makeHexNumber((unsigned char)ulxr::ValueParserWb::wbToken_String) << std::endl; ULXR_COUT << "wbToken_Base64 " << ulxr::HtmlFormHandler::makeHexNumber((unsigned char)ulxr::ValueParserWb::wbToken_Base64) << std::endl; ULXR_COUT << "wbToken_Date " << ulxr::HtmlFormHandler::makeHexNumber((unsigned char)ulxr::ValueParserWb::wbToken_Date) << std::endl; ULXR_COUT << std::endl; ULXR_COUT << "wbToken_MethodCall " << ulxr::HtmlFormHandler::makeHexNumber((unsigned char)ulxr::MethodCallParserWb::wbToken_MethodCall) << std::endl; ULXR_COUT << "wbToken_MethodName " << ulxr::HtmlFormHandler::makeHexNumber((unsigned char)ulxr::MethodCallParserWb::wbToken_MethodName) << std::endl; ULXR_COUT << "wbToken_Params " << ulxr::HtmlFormHandler::makeHexNumber((unsigned char)ulxr::MethodCallParserWb::wbToken_Params) << std::endl; ULXR_COUT << "wbToken_Param " << ulxr::HtmlFormHandler::makeHexNumber((unsigned char)ulxr::MethodCallParserWb::wbToken_Param) << std::endl; ULXR_COUT << std::endl; ULXR_COUT << "wbToken_MethodResponse " << ulxr::HtmlFormHandler::makeHexNumber((unsigned char)ulxr::MethodResponseParserWb::wbToken_MethodResponse) << std::endl; ULXR_COUT << "wbToken_Fault " << ulxr::HtmlFormHandler::makeHexNumber((unsigned char)ulxr::MethodResponseParserWb::wbToken_Fault) << std::endl; ULXR_COUT << "wbToken_Params " << ulxr::HtmlFormHandler::makeHexNumber((unsigned char)ulxr::MethodResponseParserWb::wbToken_Params) << std::endl; ULXR_COUT << "wbToken_Param " << ulxr::HtmlFormHandler::makeHexNumber((unsigned char)ulxr::MethodResponseParserWb::wbToken_Param) << std::endl; ULXR_COUT << std::endl; ULXR_COUT << "wbToken_ValueParserLast " << ulxr::HtmlFormHandler::makeHexNumber((unsigned char)ulxr::ValueParserWb::wbToken_ValueParserLast) << std::endl; ULXR_COUT << "wbToken_CallParserLast " << ulxr::HtmlFormHandler::makeHexNumber((unsigned char)ulxr::MethodCallParserWb::wbToken_CallParserLast) << std::endl; ULXR_COUT << "wbToken_ResponseParserLast " << ulxr::HtmlFormHandler::makeHexNumber((unsigned char)ulxr::MethodResponseParserWb::wbToken_ResponseParserLast) << std::endl; } catch(ulxr::Exception &ex) { ULXR_COUT << ULXR_PCHAR("Error occured: ") << ULXR_GET_STRING(ex.why()) << std::endl; success= 1; } #ifdef STRESS_IT } #endif return success; }
void controller::legController(int leg_id, int phase) { cout << endl << "Leg Controller = " << leg_id << endl; if(swingStart[leg_id] == phase) { cout << "Starting swing phase"<< endl; computePlacementLocation(leg_id, lfHeight[0]); setFootLocation(leg_id, phase); } else if(isInSwing(leg_id)) { cout << "Swing phase"<< endl; swingFlag[leg_id] = true; //Get target position setFootLocation(leg_id, phase); vector<float> targetPosition = getTargetPosition(leg_id); //Get link lengths vector<float> lengths(2); for(int i = 0; i < 2; i++) { lengths[i] = body_bag->getFootLinkLength(leg_id, i); } //Set axis perpendicular to the kinematic chain plane Eigen::MatrixXf axis = Eigen::MatrixXf::Random(4, 1); axis(0, 0) = 0; axis(1, 0) = 0; if(leg_id % 2 == 0) { axis(2, 0) = 1; } else { axis(2, 0) = -1; } axis(3, 0) = 1; //Get transformation matrices vector<Eigen::MatrixXf> transformationMatrices(2); Eigen::MatrixXf alignment = Eigen::MatrixXf::Identity(4, 4); alignment(1, 1) = 0; alignment(1, 2) = 1; alignment(2, 1) = -1; alignment(2, 2) = 0; Eigen::MatrixXf mr = Eigen::MatrixXf::Identity(4, 4); const dReal *rotation_matrix_ode = dBodyGetRotation(body_bag->getFootLinkBody(leg_id, 3)); for(int i = 0; i < 3; i++) { for(int j = 0; j < 4; j++) { mr(i, j) = rotation_matrix_ode[i*4+j]; } } mr(3, 0) = 0; mr(3, 1) = 0; mr(3, 2) = 0; mr(3, 3) = 1; Eigen::MatrixXf mr2 = Eigen::MatrixXf::Identity(4, 4); const dReal *rotation_matrix_ode2 = dBodyGetRotation(body_bag->getFootLinkBody(leg_id, 3)); for(int i = 0; i < 3; i++) { for(int j = 0; j < 4; j++) { mr2(i, j) = rotation_matrix_ode2[i*4+j]; } } mr2(3, 0) = 0; mr2(3, 1) = 0; mr2(3, 2) = 0; mr2(3, 3) = 1; transformationMatrices[0] = mr*alignment.inverse(); transformationMatrices[1] = mr2*alignment.inverse(); //Get translation matrix const dReal *center_location = dBodyGetPosition(body_bag->getFootLinkBody(leg_id,0)); //get location of the center of the link Eigen::MatrixXf mt = Eigen::MatrixXf::Identity(4, 4); //translate to the start of the link mt(1, 3) = -body_bag->getFootLinkLength(leg_id, 0)/2; mr = Eigen::MatrixXf::Identity(4, 4); //get orientation rotation_matrix_ode = dBodyGetRotation(body_bag->getFootLinkBody(leg_id, 0)); for(int k = 0; k < 3; k++) { for(int j = 0; j < 4; j++) { mr(k, j) = rotation_matrix_ode[k*4+j]; } } mr(3, 0) = 0; mr(3, 1) = 0; mr(3, 2) = 0; mr(3, 3) = 1; Eigen::MatrixXf origin = Eigen::MatrixXf::Random(4, 1); origin(0, 0) = 0; origin(1, 0) = 0; origin(2, 0) = 0; origin(3, 0) = 1; Eigen::MatrixXf addition = alignment.inverse()*mr.inverse()*mt*origin; //part to add to the center location Eigen::MatrixXf translationMatrix = Eigen::MatrixXf::Identity(4, 4); translationMatrix(0, 3) = center_location[0] + addition(0, 0); translationMatrix(1, 3) = center_location[1] + addition(1, 0); translationMatrix(2, 3) = center_location[2] + addition(2, 0); vector<float> angles = body_bag->getAngles(0); Eigen::MatrixXf jointAngleChange = applyIK(lengths, transformationMatrices, angles, targetPosition, translationMatrix, axis); //Use PD controllers to get torque //link 1 float torque = foot_link_gain_kp[leg_id][0]*jointAngleChange(0,0) + foot_link_gain_kd[leg_id][0]*(jointAngleChange(0,0) - foot_link_pd_error[leg_id][0]); dBodyAddTorque(body_bag->getFootLinkBody(leg_id,0), axis(0, 0)*torque, axis(1, 0)*torque, axis(2, 0)*torque); for(int i = 0; i < 3; i++) { swing_torque[leg_id][i] = axis(i, 0)*torque; } foot_link_pd_error[leg_id][0] = jointAngleChange(0,0); //link 2 torque = foot_link_gain_kp[leg_id][1]*jointAngleChange(1,0) + foot_link_gain_kd[leg_id][1]*(jointAngleChange(1,0) - foot_link_pd_error[leg_id][1]); dBodyAddTorque(body_bag->getFootLinkBody(leg_id,1), axis(0, 0)*torque, axis(1, 0)*torque, axis(2, 0)*torque); foot_link_pd_error[leg_id][1] = jointAngleChange(1,0); //link 3 rotation_matrix_ode = dBodyGetRotation(body_bag->getFootLinkBody(leg_id, 2)); for(int k = 0; k < 3; k++) { for(int j = 0; j < 4; j++) { mr(k, j) = rotation_matrix_ode[k*4+j]; } } mr(3, 0) = 0; mr(3, 1) = 0; mr(3, 2) = 0; mr(3, 3) = 1; float swing_phase = computeSwingPhase(leg_id, phase); float error = ((30*M_PI)/180)*(1 - swing_phase) - ((60*M_PI)/180)*swing_phase - acos(mr(0, 0)); torque = foot_link_gain_kp[leg_id][2]*error + foot_link_gain_kd[leg_id][2]*(error - foot_link_pd_error[leg_id][2]); dBodyAddTorque(body_bag->getFootLinkBody(leg_id,2), axis(0, 0)*torque, axis(1, 0)*torque, axis(2, 0)*torque); foot_link_pd_error[leg_id][2] = error; //link 4 rotation_matrix_ode = dBodyGetRotation(body_bag->getFootLinkBody(leg_id, 3)); for(int k = 0; k < 3; k++) { for(int j = 0; j < 4; j++) { mr(k, j) = rotation_matrix_ode[k*4+j]; } } mr(3, 0) = 0; mr(3, 1) = 0; mr(3, 2) = 0; mr(3, 3) = 1; swing_phase = computeSwingPhase(leg_id, phase); error = ((90*M_PI)/180)*(1 - swing_phase) - ((30*M_PI)/180)*swing_phase - acos(mr(0, 0)); torque = foot_link_gain_kp[leg_id][3]*error + foot_link_gain_kd[leg_id][3]*(error - foot_link_pd_error[leg_id][3]); dBodyAddTorque(body_bag->getFootLinkBody(leg_id,3), axis(0, 0)*torque, axis(1, 0)*torque, axis(2, 0)*torque); foot_link_pd_error[leg_id][3] = error; //Apply gravity compensation float force = 9.810*body_bag->getDensity()*(body_bag->getFootLinkLength(leg_id, 0) + body_bag->getFootLinkLength(leg_id, 1) + body_bag->getFootLinkLength(leg_id, 2) + body_bag->getFootLinkLength(leg_id, 3)); dBodyAddForce(body_bag->getFootLinkBody(leg_id, 2), 0.0, 9.810*body_bag->getDensity()*body_bag->getFootLinkLength(leg_id, 2), 0.0); if(leg_id < 2) { dBodyAddForce(body_bag->getBackLink1Body(), 0.0, force, 0.0); } else { dBodyAddForce(body_bag->getBackLink6Body(), 0.0, force, 0.0); } } else { cout << "Stance phase"<< endl; swingFlag[leg_id] = false; /*for(int i = 0; i < 3; i++){ swing_torque[leg_id][i] = 0; }*/ stanceLegTreatment(leg_id); } }