int main(int argc, char** argv){
ros::init(argc, argv, "superchick_node");
const std::string& package_name("superchick");
boost::filesystem::path chick_path;
if(!pathfinder::getROSPackagePath(package_name, chick_path)){
ROS_INFO("Could not find the package %s. Please make sure you clone the %s o package",
package_name.c_str(), package_name.c_str());
return EXIT_FAILURE;
}
std::stringstream ss;
ss << chick_path.c_str() << "/models/superchick.mjcf";
std::string model_filename_c = ss.str();
mjModel* model;
char *model_filename = new char[model_filename_c.length() + 1];
// char* model_filename = std::make_shared<char> (new char[model_filename_c.length() + 1]);// no_except;
strcpy(model_filename, model_filename_c.c_str());
// activate software
boost::filesystem::path mjkey_file;
if(!pathfinder::getMujocoFile(mjkey_file))
printf("Ouch, I had problem reconciling the path variable with your dir structure. %s",
"please ensure you have a Linux system. Darwin architectures are yet unsupported.");
mj_activate(mjkey_file.c_str());
NewModelFromXML(model_filename, model);
mjData* data = mj_makeData(model);
delete[] model_filename;
if(!model)
ROS_FATAL("Ouch! I had problem loading model %s there", model_filename);
MujocoOSGViewer viewer;
viewer.SetModel(model);
viewer.SetData(data);
viewer.Idle();
while(!ros::ok()){
mj_step(model, data);
viewer.SetData(data);
viewer.RenderOnce();
OpenThreads::Thread::microSleep(30000);
}
//deallocate existing model
mj_deleteModel(model);
//deallocate existing mjData
mj_deleteData(data);
mj_deactivate();
return EXIT_SUCCESS;
}
int main(int argc, const char** argv) {
printf("start\n");
//string model_name = argv[1];
mj_activate("mjkey.txt");
m = mj_loadXML(argv[1], 0, 0, 0);
d = mj_makeData(m);
printf("Loaded model and made data\n");
d->qpos[0] = 1;
d->qpos[1] = 1;
EKF* ekf = new EKF(m, d, 1.0, 1.0, 1.0, 1e-3);
mjData* d2 = mj_makeData(m);
std::vector<mjData*> frame;
MatrixXd deriv = ekf->get_deriv(m, d, d2);
printf("Calc deriv done\n");
printf("nu: %d\n", m->nu);
printf("L: %d\n", m->nq+m->nv);
IOFormat CleanFmt(3, 0, ", ", "\n", "[", "]");
std::cout << deriv.rows() << "\t" << deriv.cols() << std::endl;
std::cout << deriv.block(0, 0, m->nq+m->nv, m->nq+m->nv).format(CleanFmt) << std::endl;
//MatrixXd sigma = MatrixXd::Identity(m->nq+m->nv+m->nv, m->nq+m->nv+m->nv);
//printf("%f\n", d->qpos[10]);
//double dt = m->opt.timestep;
//ekf->predict_correct(d->ctrl, dt, d->sensordata, conf);
}
