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