bool RagGraphPlanner::findLocalTrajectory(const Controller::State &cbegin, GenWorkspaceChainState::Seq::const_iterator wbegin, GenWorkspaceChainState::Seq::const_iterator wend, Controller::Trajectory &trajectory, Controller::Trajectory::iterator iter, MSecTmU32 timeOut) {
CriticalSectionWrapper csw(csCommand);
#ifdef _HBGRAPHPLANNER_PERFMON
PerfTimer t;
#ifdef _HBHEURISTIC_PERFMON
HBHeuristic::resetLog();
HBCollision::resetLog();
#endif
#endif
HBHeuristic *heuristic = getHBHeuristic();
if (heuristic/* && heuristic->enableUnc*/)
enableHandPlanning();
// context.write("findLocalTrajectory(): %s\n", hbplannerDebug(*this).c_str());
//context.write("RagGraphPlanner::findLocalTrajectory: %s\n", hbplannerConfigspaceDebug(*this).c_str());
//context.write("RagGraphPlanner::findLocalTrajectory: %s\n", hbplannerWorkspaceDebug(*this).c_str());
// trajectory size
const size_t size = 1 + (size_t)(wend - wbegin);
// check initial size
if (size < 2) {
context.error("GraphPlanner::findLocalTrajectory(): Invalid workspace sequence size\n");
return false;
}
// time out
const MSecTmU32 segTimeOut = timeOut == MSEC_TM_U32_INF ? MSEC_TM_U32_INF : timeOut / MSecTmU32(size - 1);
// fill trajectory with cbegin
const Controller::State cinit = cbegin; // backup
Controller::Trajectory::iterator end = ++trajectory.insert(iter, cinit);
for (GenWorkspaceChainState::Seq::const_iterator i = wbegin; i != wend; ++i)
end = ++trajectory.insert(end, cinit);
Controller::Trajectory::iterator begin = end - size;
getCallbackDataSync()->syncCollisionBounds();
optimisedPath.resize(size - 1);
population.assign(1, cbegin.cpos); // always keep initial solution in case no transformation is required
pKinematics->setPopulation(&population);
pKinematics->setDistRootFac(pKinematics->getDesc().distRootLocalFac);
// find configspace trajectory
PARAMETER_GUARD(Heuristic, GenCoordConfigspace, Min, *pHeuristic);
PARAMETER_GUARD(Heuristic, GenCoordConfigspace, Max, *pHeuristic);
for (size_t i = 1; i < size; ++i) {
// pointers
const Controller::Trajectory::iterator c[2] = { begin + i - 1, begin + i };
const GenWorkspaceChainState::Seq::const_iterator w = wbegin + i - 1;
// setup search limits
GenCoordConfigspace min = pHeuristic->getMin();
GenCoordConfigspace max = pHeuristic->getMin();
for (Configspace::Index j = stateInfo.getJoints().begin(); j < stateInfo.getJoints().end(); ++j) {
const idx_t k = j - stateInfo.getJoints().begin();
min[j].pos = c[0]->cpos[j] - localFinderDesc.range[k];
max[j].pos = c[0]->cpos[j] + localFinderDesc.range[k];
}
pHeuristic->setMin(min);
pHeuristic->setMax(max);
// and search for a solution
if (!pKinematics->findGoal(*c[0], *w, *c[1], segTimeOut)) {
context.error("GraphPlanner::findLocalTrajectory(): unable to solve inverse kinematics\n");
return false;
}
// visualisation
optimisedPath[i - 1].cpos = c[1]->cpos;
optimisedPath[i - 1].wpos = w->wpos;
}
// profile configspace trajectory
pProfile->profile(trajectory, begin, end);
getCallbackDataSync()->syncFindTrajectory(begin, end, &*(wend - 1));
#ifdef _HBGRAPHPLANNER_PERFMON
context.write("GraphPlanner::findLocalTrajectory(): time_elapsed = %f [sec], len = %d\n", t.elapsed(), size);
#ifdef _HBHEURISTIC_PERFMON
if (heuristic) {
context.write("Enabled Uncertainty %s\n", heuristic->enableUnc ? "ON" : "OFF");
//heuristic->writeLog(context, "GraphPlanner::findTarget()");
heuristic->getCollision()->writeLog(context, "GraphPlanner::findTarget()");;
}
#endif
#endif
if (heuristic/* && heuristic->enableUnc*/)
disableHandPlanning();
return true;
}
bool RagGraphPlanner::findGlobalTrajectory(const Controller::State &begin, const Controller::State &end, Controller::Trajectory &trajectory, Controller::Trajectory::iterator iter, const GenWorkspaceChainState* wend) {
CriticalSectionWrapper csw(csCommand);
#ifdef _HBGRAPHPLANNER_PERFMON
PerfTimer t;
#endif
#ifdef _HBGRAPHPLANNER_PERFMON
#ifdef _HBHEURISTIC_PERFMON
HBHeuristic::resetLog();
HBCollision::resetLog();
#endif
#ifdef _BOUNDS_PERFMON
Bounds::resetLog();
#endif
#endif
getCallbackDataSync()->syncCollisionBounds();
#ifdef _HBGRAPHPLANNER_PERFMON
t.reset();
#endif
// generate global graph only for the arm
context.debug("GraphPlanner::findGlobalTrajectory(): Enabled Uncertainty %s. disable hand planning...\n", getHBHeuristic()->enableUnc ? "ON" : "OFF");
disableHandPlanning();
// context.write("findGlobalTrajectory(): %s\n", hbplannerDebug(*this).c_str());
//context.write("GraphPlanner::findGlobalTrajectory(): %s\n", hbplannerConfigspaceDebug(*this).c_str());
//context.write("GraphPlanner::findGlobalTrajectory(): %s\n", hbplannerWorkspaceDebug(*this).c_str());
// generate global graph
pGlobalPathFinder->generateOnlineGraph(begin.cpos, end.cpos);
// find node path on global graph
globalPath.clear();
if (!pGlobalPathFinder->findPath(end.cpos, globalPath, globalPath.begin())) {
context.error("GlobalPathFinder::findPath(): unable to find global path\n");
return false;
}
#ifdef _HBGRAPHPLANNER_PERFMON
context.write(
"GlobalPathFinder::findPath(): time_elapsed = %f [sec], len = %d\n",
t.elapsed(), globalPath.size()
);
#endif
if (pLocalPathFinder != NULL) {
#ifdef _HBGRAPHPLANNER_PERFMON
t.reset();
#endif
PARAMETER_GUARD(Heuristic, Real, Scale, *pHeuristic);
for (U32 i = 0;;) {
localPath = globalPath;
if (localFind(begin.cpos, end.cpos, localPath))
break;
else if (++i > pathFinderDesc.numOfTrials) {
context.error("LocalPathFinder::findPath(): unable to find local path\n");
return false;
}
}
#ifdef _HBGRAPHPLANNER_PERFMON
context.write(
"LocalPathFinder::findPath(): time_elapsed = %f [sec], len = %d\n",
t.elapsed(), localPath.size()
);
#endif
// copy localPath
optimisedPath = localPath;
}
else {
// copy globalPath
optimisedPath = globalPath;
}
#ifdef _HBGRAPHPLANNER_PERFMON
#ifdef _HBHEURISTIC_PERFMON
// context.debug("Enabled Uncertainty %s\n", getHBHeuristic()->enableUnc ? "ON" : "OFF");
//HBHeuristic::writeLog(context, "PathFinder::find()");
HBCollision::writeLog(context, "PathFinder::find()");
#endif
#ifdef _BOUNDS_PERFMON
Bounds::writeLog(context, "PathFinder::find()");
#endif
#endif
#ifdef _HBGRAPHPLANNER_PERFMON
#ifdef _HBHEURISTIC_PERFMON
HBHeuristic::resetLog();
HBCollision::resetLog();
#endif
#ifdef _BOUNDS_PERFMON
Bounds::resetLog();
#endif
t.reset();
#endif
optimize(optimisedPath, begin.cacc, end.cacc);
#ifdef _HBGRAPHPLANNER_PERFMON
context.write(
"GraphPlanner::optimize(): time_elapsed = %f [sec], len = %d\n", t.elapsed(), optimisedPath.size()
);
#ifdef _HBHEURISTIC_PERFMON
HBHeuristic::writeLog(context, "GraphPlanner::optimize()");
HBCollision::writeLog(context, "GraphPlanner::optimize()");
#endif
#ifdef _BOUNDS_PERFMON
Bounds::writeLog(context, "GraphPlanner::optimize()");
#endif
#endif
#ifdef _HBGRAPHPLANNER_PERFMON
t.reset();
#endif
Controller::Trajectory::iterator iend = iter;
pProfile->create(optimisedPath.begin(), optimisedPath.end(), begin, end, trajectory, iter, iend);
pProfile->profile(trajectory, iter, iend);
#ifdef _HBGRAPHPLANNER_PERFMON
context.write(
"GraphPlanner::profile(): time_elapsed = %f [sec], len = %d\n",
t.elapsed(), trajectory.size()
);
#endif
getCallbackDataSync()->syncFindTrajectory(trajectory.begin(), trajectory.end(), wend);
return true;
}
bool RagGraphPlanner::findTarget(const GenConfigspaceState &begin, const GenWorkspaceChainState& wend, GenConfigspaceState &cend) {
CriticalSectionWrapper csw(csCommand);
HBHeuristic *heuristic = getHBHeuristic();
if (heuristic) {
enableUnc = heuristic->enableUnc;
heuristic->enableUnc = false;
context.debug("RagGraphPlanner::findTarget(): enable unc %s\n", heuristic->enableUnc ? "ON" : "OFF");
}
// TODO: Find why the pre-grasp pose returns with close fingers
disableHandPlanning();
// context.debug("findTarget: %s\n", hbplannerDebug(*this).c_str());
//context.write("RagGraphPlanner::findTarget: %s\n", hbplannerConfigspaceDebug(*this).c_str());
//context.write("RagGraphPlanner::findTarget: %s\n", hbplannerWorkspaceDebug(*this).c_str());
#ifdef _HBHEURISTIC_PERFMON
heuristic->resetLog();
heuristic->getCollision()->resetLog();
#endif
#ifdef _HBGRAPHPLANNER_PERFMON
PerfTimer t;
#endif
getCallbackDataSync()->syncCollisionBounds();
// generate graph
pGlobalPathFinder->generateOnlineGraph(begin.cpos, wend.wpos);
// create waypoints pointers
const Waypoint::Seq& graph = pGlobalPathFinder->getGraph();
WaypointPtr::Seq waypointPtrGraph;
waypointPtrGraph.reserve(graph.size());
for (U32 i = 0; i < graph.size(); ++i)
waypointPtrGraph.push_back(WaypointPtr(&graph[i]));
// Create waypoint population
const U32 populationSize = std::min(U32(pKinematics->getDesc().populationSize), (U32)waypointPtrGraph.size());
// sort waypoints (pointers) from the lowest to the highest cost
std::partial_sort(waypointPtrGraph.begin(), waypointPtrGraph.begin() + populationSize, waypointPtrGraph.end(), WaypointPtr::cost_less());
// create initial population for kinematics solver
ConfigspaceCoord::Seq population;
population.reserve(populationSize);
for (WaypointPtr::Seq::const_iterator i = waypointPtrGraph.begin(); population.size() < populationSize && i != waypointPtrGraph.end(); ++i)
population.push_back((*i)->cpos);
pKinematics->setPopulation(&population);
// set global root distance factor
pKinematics->setDistRootFac(pKinematics->getDesc().distRootGlobalFac);
GenConfigspaceState root;
root.setToDefault(controller.getStateInfo().getJoints().begin(), controller.getStateInfo().getJoints().end());
root.cpos = graph[Node::IDX_ROOT].cpos;
// find the goal state
if (!pKinematics->findGoal(root, wend, cend)) {
context.error("GraphPlanner::findTarget(): unable to find target\n");
return false;
}
cend.t = wend.t;
cend.cvel.fill(REAL_ZERO);
cend.cacc.fill(REAL_ZERO);
#ifdef _HBGRAPHPLANNER_PERFMON
context.write(
"GraphPlanner::findTarget(): time_elapsed = %f [sec]\n", t.elapsed()
);
#endif
#ifdef _HBHEURISTIC_PERFMON
heuristic->writeLog(context, "GraphPlanner::findTarget()");
heuristic->getCollision()->writeLog(context, "GraphPlanner::findTarget()");;
#endif
if (heuristic)
heuristic->enableUnc = enableUnc;
//enableHandPlanning();
// context.write("RagGraphPlanner::findTarget(): done.\n");
return true;
}
int main(int argc, char *argv[]) {
if (argc == 2) {
push_to_graphite = std::string(argv[1]) == "graphite";
}
const char* pattern = ":1234567\r\n+3.14159\r\n";
std::string input;
for (int i = 0; i < TEST_ITERATIONS/2; i++) {
input += pattern;
}
std::vector<double> timedeltas;
uint64_t intvalue;
Byte buffer[RESPStream::STRING_LENGTH_MAX];
for (int i = N_TESTS; i --> 0;) {
PerfTimer tm;
MemStreamReader stream(input.data(), input.size());
RESPStream protocol(&stream);
for (int j = TEST_ITERATIONS; j --> 0;) {
auto type = protocol.next_type();
switch(type) {
case RESPStream::INTEGER:
intvalue = protocol.read_int();
if (intvalue != 1234567) {
std::cerr << "Bad int value at " << j << std::endl;
return -1;
}
break;
case RESPStream::STRING: {
int len = protocol.read_string(buffer, sizeof(buffer));
if (len != 7) {
std::cerr << "Bad string value at " << j << std::endl;
return -1;
}
char *p = buffer;
double res = strtod(buffer, &p);
if (abs(res - 3.14159) > 0.0001) {
std::cerr << "Can't parse float at " << j << std::endl;
return -1;
}
}
break;
case RESPStream::ARRAY:
case RESPStream::BAD:
case RESPStream::BULK_STR:
case RESPStream::ERROR:
default:
std::cerr << "Error at " << j << std::endl;
return -1;
};
}
timedeltas.push_back(tm.elapsed());
}
double min = std::numeric_limits<double>::max();
for (auto t: timedeltas) {
min = std::min(min, t);
}
std::cout << "Parsing " << TEST_ITERATIONS << " messages in " << min << " sec." << std::endl;
if (push_to_graphite) {
push_metric_to_graphite("respstream", 1000.0*min);
}
return 0;
}
int main(int argc, char** argv) {
const uint64_t N_TIMESTAMPS = 1000;
const uint64_t N_PARAMS = 100;
UncompressedChunk header;
std::cout << "Testing timestamp sequence" << std::endl;
int c = 100;
std::vector<aku_ParamId> ids;
for (uint64_t id = 0; id < N_PARAMS; id++) { ids.push_back(id); }
RandomWalk rwalk(10.0, 0.0, 0.01, N_PARAMS);
for (uint64_t id = 0; id < N_PARAMS; id++) {
for (uint64_t ts = 0; ts < N_TIMESTAMPS; ts++) {
header.paramids.push_back(ids[id]);
int k = rand() % 2;
if (k) {
c++;
} else if (c > 0) {
c--;
}
header.timestamps.push_back((ts + c) << 8);
header.values.push_back(rwalk.generate(0));
}
}
ByteVector out;
out.resize(N_PARAMS*N_TIMESTAMPS*24);
const size_t UNCOMPRESSED_SIZE = header.paramids.size()*8 // Didn't count lengths and offsets
+ header.timestamps.size()*8 // because because this arrays contains
+ header.values.size()*8; // no information and should be compressed
// to a few bytes
struct Writer : ChunkWriter {
ByteVector *out;
Writer(ByteVector *out) : out(out) {}
virtual aku_MemRange allocate() {
aku_MemRange range = {
out->data(),
static_cast<uint32_t>(out->size())
};
return range;
}
//! Commit changes
virtual aku_Status commit(size_t bytes_written) {
out->resize(bytes_written);
return AKU_SUCCESS;
}
};
Writer writer(&out);
aku_Timestamp tsbegin, tsend;
uint32_t n;
auto status = CompressionUtil::encode_chunk(&n, &tsbegin, &tsend, &writer, header);
if (status != AKU_SUCCESS) {
std::cout << "Encoding error" << std::endl;
return 1;
}
// Compress using zlib
// Ids copy (zlib need all input data to be aligned because it uses SSE2 internally)
Bytef* pgz_ids = (Bytef*)aligned_alloc(64, header.paramids.size()*8);
memcpy(pgz_ids, header.paramids.data(), header.paramids.size()*8);
// Timestamps copy
Bytef* pgz_ts = (Bytef*)aligned_alloc(64, header.timestamps.size()*8);
memcpy(pgz_ts, header.timestamps.data(), header.timestamps.size()*8);
// Values copy
Bytef* pgz_val = (Bytef*)aligned_alloc(64, header.values.size()*8);
memcpy(pgz_val, header.values.data(), header.values.size()*8);
const auto gz_max_size = N_PARAMS*N_TIMESTAMPS*24;
Bytef* pgzout = (Bytef*)aligned_alloc(64, gz_max_size);
uLongf gzoutlen = gz_max_size;
size_t total_gz_size = 0, id_gz_size = 0, ts_gz_size = 0, float_gz_size = 0;
// compress param ids
auto zstatus = compress(pgzout, &gzoutlen, pgz_ids, header.paramids.size()*8);
if (zstatus != Z_OK) {
std::cout << "GZip error" << std::endl;
exit(zstatus);
}
total_gz_size += gzoutlen;
id_gz_size = gzoutlen;
gzoutlen = gz_max_size;
// compress timestamps
zstatus = compress(pgzout, &gzoutlen, pgz_ts, header.timestamps.size()*8);
if (zstatus != Z_OK) {
std::cout << "GZip error" << std::endl;
exit(zstatus);
}
total_gz_size += gzoutlen;
ts_gz_size = gzoutlen;
gzoutlen = gz_max_size;
// compress floats
zstatus = compress(pgzout, &gzoutlen, pgz_val, header.values.size()*8);
if (zstatus != Z_OK) {
std::cout << "GZip error" << std::endl;
exit(zstatus);
}
total_gz_size += gzoutlen;
float_gz_size = gzoutlen;
const float GZ_BPE = float(total_gz_size)/header.paramids.size();
const float GZ_RATIO = float(UNCOMPRESSED_SIZE)/float(total_gz_size);
const size_t COMPRESSED_SIZE = out.size();
const float BYTES_PER_EL = float(COMPRESSED_SIZE)/header.paramids.size();
const float COMPRESSION_RATIO = float(UNCOMPRESSED_SIZE)/COMPRESSED_SIZE;
std::cout << "Uncompressed: " << UNCOMPRESSED_SIZE << std::endl
<< " compressed: " << COMPRESSED_SIZE << std::endl
<< " elements: " << header.paramids.size() << std::endl
<< " bytes/elem: " << BYTES_PER_EL << std::endl
<< " ratio: " << COMPRESSION_RATIO << std::endl
;
std::cout << "Gzip stats: " << std::endl
<< "bytes/elem: " << GZ_BPE << std::endl
<< " ratio: " << GZ_RATIO << std::endl
<< " id bytes: " << id_gz_size << std::endl
<< " ts bytes: " << ts_gz_size << std::endl
<< " val bytes: " << float_gz_size << std::endl;
// Try to decompress
UncompressedChunk decomp;
const unsigned char* pbegin = out.data();
const unsigned char* pend = pbegin + out.size();
CompressionUtil::decode_chunk(&decomp, pbegin, pend, header.timestamps.size());
bool first_error = true;
for (auto i = 0u; i < header.timestamps.size(); i++) {
if (header.timestamps.at(i) != decomp.timestamps.at(i) && first_error) {
std::cout << "Error, bad timestamp at " << i << std::endl;
first_error = false;
}
if (header.paramids.at(i) != decomp.paramids.at(i) && first_error) {
std::cout << "Error, bad paramid at " << i << std::endl;
first_error = false;
}
double origvalue = header.values.at(i);
double decvalue = decomp.values.at(i);
if (origvalue != decvalue && first_error) {
std::cout << "Error, bad value at " << i << std::endl;
std::cout << "Expected: " << origvalue << std::endl;
std::cout << "Actual: " << decvalue << std::endl;
first_error = false;
}
}
if (argc == 2 && std::string(argv[1]) == "benchmark") {
// Bench compression process
const int NRUNS = 1000;
PerfTimer tm;
aku_Status tstatus;
volatile uint32_t vn;
ByteVector vec;
for (int i = 0; i < NRUNS; i++) {
vec.resize(N_PARAMS*N_TIMESTAMPS*24);
Writer w(&vec);
aku_Timestamp ts;
uint32_t n;
tstatus = CompressionUtil::encode_chunk(&n, &ts, &ts, &w, header);
if (tstatus != AKU_SUCCESS) {
std::cout << "Encoding error" << std::endl;
return 1;
}
vn = n;
}
double elapsed = tm.elapsed();
std::cout << "Elapsed (akumuli): " << elapsed << " " << vn << std::endl;
tm.restart();
for (int i = 0; i < NRUNS; i++) {
uLongf offset = 0;
// compress param ids
auto zstatus = compress(pgzout, &gzoutlen, pgz_ids, header.paramids.size()*8);
if (zstatus != Z_OK) {
std::cout << "GZip error" << std::endl;
exit(zstatus);
}
offset += gzoutlen;
gzoutlen = gz_max_size - offset;
// compress timestamps
zstatus = compress(pgzout + offset, &gzoutlen, pgz_ts, header.timestamps.size()*8);
if (zstatus != Z_OK) {
std::cout << "GZip error" << std::endl;
exit(zstatus);
}
offset += gzoutlen;
gzoutlen = gz_max_size - offset;
// compress floats
zstatus = compress(pgzout + offset, &gzoutlen, pgz_val, header.values.size()*8);
if (zstatus != Z_OK) {
std::cout << "GZip error" << std::endl;
exit(zstatus);
}
}
elapsed = tm.elapsed();
std::cout << "Elapsed (zlib): " << elapsed << " " << vn << std::endl;
}
}
// this callback is called every second
void callback(void*)
{
PerfTimer timer;
initGraphics();
_gl_main->redraw();
//Fl::repeat_timeout(.5, callback);
//return;
if (!_gl_main->pause_video->value()) {
Fl::repeat_timeout(_tempo, callback);
return;
}
//refresh the control bar
_wind->child(2)->redraw();
// for performance reasons...
_gl_main->_camera->setActiveSensor(_gl_main->_calibrated_camera);
// check database first
if (_gl_main->_database == NULL) {
// grab ladybug images
if (_gl_main->_ladybug->_init) {
_gl_main->_ladybug->grab(_gl_main->_camera->_frame._grab_img);
_gl_main->_camera->_frame.processFrame();
_tempo = 0.05;
} else {
_tempo = 1.0;
}
}
// save frame if recording
if ( _gl_main->_ladybug_recording ) {
_tempo = .5;
_gl_main->_ladybug->grab(_gl_main->_camera->_frame._grab_img); // grab an image on the ladybug
_gl_main->_camera->_frame.processFrame();
for (int sensorId = 0; sensorId < 6; sensorId++ ) {
std::string filename = _gl_main->_database->_dirname + "//" + _gl_main->_database->getLadybugImageFilename( sensorId, _gl_main->frameId );
cvSaveImage( filename.c_str(), _gl_main->_camera->_frame._tmp_img[sensorId] );
}
_gl_main->_ladybug_record_nimages++;
_gl_main->frameId++;
_gl_main->_database->_frameId = _gl_main->frameId;
_gl_main->redraw();
}
// compute FAST features and update the tracker
if ( _gl_main->view_flow->value() ) {
}
// refresh the main window
if ((_gl_main->_windowMode == MODE_VIDEO) || (_gl_main->_windowMode == MODE_CALIBRATION))
_gl_main->redraw();
// refresh the main window
if (_gl_main->_windowMode == MODE_SPHERE)
_gl_main->redraw();
_gl_control->_perf_time = timer.elapsed();
//Fl::repeat_timeout(MAX(0.05,1.5*_gl_control->_perf_time), callback);
Fl::repeat_timeout(_tempo, callback);
}
