10 #include <rtm/Manager.h>
11 #include <rtm/CorbaNaming.h>
16 #include <libxml/encoding.h>
17 #include <libxml/xmlwriter.h>
27 xmlTextWriterStartElement(writer, BAD_CAST
"property");
28 xmlTextWriterWriteAttribute(writer, BAD_CAST
"name", BAD_CAST
name.c_str());
29 xmlTextWriterWriteAttribute(writer, BAD_CAST
"value", BAD_CAST
value.c_str());
30 xmlTextWriterEndElement(writer);
34 std::string ret =
name.substr(
name.find_last_of(
'/')+1);
35 return ret.substr(0,ret.find_last_of(
'.'));
47 int numSensors = sensorInfoSeq.length();
49 for(
int i=0 ;
i < numSensors ; ++
i ) {
50 const SensorInfo& sensorInfo = sensorInfoSeq[
i];
52 int id = sensorInfo.id;
54 std::cerr <<
"Warning: sensor ID is not given to sensor " << sensorInfo.name
55 <<
"of model " << body->modelName() <<
"." << std::endl;
59 CORBA::String_var type0 = sensorInfo.type;
66 else if(
type ==
"Range") { sensorType = Sensor::RANGE; }
68 CORBA::String_var name0 = sensorInfo.name;
71 Sensor* sensor = body->createSensor(link, sensorType,
id,
name);
74 const DblArray3& p = sensorInfo.translation;
77 const Vector3 axis(sensorInfo.rotation[0], sensorInfo.rotation[1], sensorInfo.rotation[2]);
82 if ( sensorType == Sensor::RANGE ) {
84 range->
scanAngle = sensorInfo.specValues[0];
85 range->
scanStep = sensorInfo.specValues[1];
86 range->
scanRate = sensorInfo.specValues[2];
90 vision->
near = sensorInfo.specValues[0];
91 vision->
far = sensorInfo.specValues[1];
92 vision->
fovy = sensorInfo.specValues[2];
93 vision->
width = sensorInfo.specValues[4];
94 vision->
height = sensorInfo.specValues[5];
96 switch((
int)sensorInfo.specValues[3]){
102 vision->
image.resize(npixel*3);
106 vision->
image.resize(npixel);
111 case Camera::COLOR_DEPTH:
112 vision->
imageType = VisionSensor::COLOR_DEPTH;
113 vision->
image.resize(npixel*3);
115 case Camera::MONO_DEPTH:
116 vision->
imageType = VisionSensor::MONO_DEPTH;
117 vision->
image.resize(npixel);
120 vision->
frameRate = sensorInfo.specValues[6];
129 return (limitseq.length() == 0) ? defaultValue : limitseq[0];
134 const LinkInfo& linkInfo = linkInfoSeq[index];
135 int jointId = linkInfo.
jointId;
139 CORBA::String_var name0 = linkInfo.name;
140 link->
name = string( name0 );
143 Vector3 relPos(linkInfo.translation[0], linkInfo.translation[1], linkInfo.translation[2]);
144 link->
b = parentRs * relPos;
146 Vector3 rotAxis(linkInfo.rotation[0], linkInfo.rotation[1], linkInfo.rotation[2]);
148 link->
Rs = (parentRs * R);
151 CORBA::String_var jointType = linkInfo.jointType;
152 const std::string jt( jointType );
156 }
else if(jt ==
"free" ){
158 }
else if(jt ==
"rotate" ){
159 link->
jointType = Link::ROTATIONAL_JOINT;
160 }
else if(jt ==
"slide" ){
162 }
else if(jt ==
"crawler"){
170 if(link->
jointType == Link::ROTATIONAL_JOINT || link->
jointType == Link::SLIDE_JOINT){
171 std::cerr <<
"Warning: Joint ID is not given to joint " << link->
name
172 <<
" of model " << body->modelName() <<
"." << std::endl;
179 Vector3 axis( Rs *
Vector3(linkInfo.jointAxis[0], linkInfo.jointAxis[1], linkInfo.jointAxis[2]));
181 if(link->
jointType == Link::ROTATIONAL_JOINT || jt ==
"crawler"){
183 }
else if(link->
jointType == Link::SLIDE_JOINT){
187 link->
m = linkInfo.mass;
188 link->
Ir = linkInfo.rotorInertia;
197 DblSequence ulimit = linkInfo.ulimit;
198 DblSequence llimit = linkInfo.llimit;
199 DblSequence uvlimit = linkInfo.uvlimit;
200 DblSequence lvlimit = linkInfo.lvlimit;
201 DblSequence climit = linkInfo.climit;
211 link->
c = Rs *
Vector3(linkInfo.centerOfMass[0], linkInfo.centerOfMass[1], linkInfo.centerOfMass[2]);
215 link->
I = Rs * Io * Rs.transpose();
218 std::stack<Link*> children;
221 int childNum = linkInfo.childIndices.length();
222 for(
int i = 0 ;
i < childNum ;
i++) {
223 int childIndex = linkInfo.childIndices[
i];
226 children.push(childLink);
229 while(!children.empty()){
237 if(collisionDetectionModelLoading){
238 createColdetModel(link, linkInfo);
249 body->setModelName(
name);
252 int n = bodyInfo.
links()->length();
253 linkInfoSeq = bodyInfo.
links();
254 shapeInfoSeq = bodyInfo.
shapes();
259 for(
int i=0;
i <
n; ++
i){
260 if(linkInfoSeq[
i].parentIndex < 0){
275 body->setRootLink(rootLink);
276 body->setDefaultRootPosition(rootLink->
b, rootLink->
Rs);
278 body->installCustomizer();
279 body->initializeConfiguration();
295 int main (
int argc,
char** argv)
298 std::vector<std::string> inputs, filenames;
299 std::string conf_file_option, robothardware_conf_file_option, integrate(
"true"), dt(
"0.005"), timeStep(dt), joint_properties,
method(
"EULER");
300 bool use_highgain_mode(
true);
302 bool file_exist_flag =
false;
304 for (
int i = 1;
i < argc; ++
i) {
305 std::string
arg(argv[
i]);
306 coil::normalize(
arg);
307 if (
arg ==
"--output" ) {
309 }
else if (
arg ==
"--integrate" ) {
310 if (++
i < argc) integrate = argv[
i];
311 }
else if (
arg ==
"--dt" ) {
312 if (++
i < argc) dt = argv[
i];
313 }
else if (
arg ==
"--timestep" ) {
314 if (++
i < argc) timeStep = argv[
i];
315 }
else if (
arg ==
"--conf-file-option" ) {
316 if (++
i < argc) conf_file_option += std::string(
"\n") + argv[
i];
317 }
else if (
arg ==
"--robothardware-conf-file-option" ) {
318 if (++
i < argc) robothardware_conf_file_option += std::string(
"\n") + argv[
i];
319 }
else if (
arg ==
"--joint-properties" ) {
320 if (++
i < argc) joint_properties = argv[
i];
321 }
else if (
arg ==
"--use-highgain-mode" ) {
322 if (++
i < argc) use_highgain_mode = (std::string(argv[
i])==std::string(
"true")?
true:
false);
323 }
else if (
arg ==
"--method" ) {
324 if (++
i < argc)
method = std::string(argv[
i]);
325 }
else if (
arg.find(
"--gtest_output") == 0 ||
arg.find(
"--text") == 0 ||
arg.find(
"__log") == 0 ||
arg.find(
"__name") == 0 ) {
327 inputs.push_back(argv[
i]);
331 CORBA::ORB_var orb = CORBA::ORB::_nil();
335 orb = CORBA::ORB_init(argc, argv);
337 CORBA::Object_var
obj;
339 obj = orb->resolve_initial_references(
"RootPOA");
340 PortableServer::POA_var poa = PortableServer::POA::_narrow(
obj);
341 if(CORBA::is_nil(poa))
343 throw string(
"error: failed to narrow root POA.");
346 PortableServer::POAManager_var poaManager = poa->the_POAManager();
347 if(CORBA::is_nil(poaManager))
349 throw string(
"error: failed to narrow root POA manager.");
352 xmlTextWriterPtr writer;
353 if (stat(
output.c_str(), &st) == 0) {
354 file_exist_flag =
true;
356 writer = xmlNewTextWriterFilename(
output.c_str(), 0);
357 xmlTextWriterSetIndent(writer, 4);
358 xmlTextWriterStartElement(writer, BAD_CAST
"grxui");
360 xmlTextWriterStartElement(writer, BAD_CAST
"mode");
361 xmlTextWriterWriteAttribute(writer, BAD_CAST
"name", BAD_CAST
"Simulation");
363 xmlTextWriterStartElement(writer, BAD_CAST
"item");
364 xmlTextWriterWriteAttribute(writer, BAD_CAST
"class", BAD_CAST
"com.generalrobotix.ui.item.GrxSimulationItem");
365 xmlTextWriterWriteAttribute(writer, BAD_CAST
"name", BAD_CAST
"simulationItem");
372 xmlTextWriterEndElement(writer);
375 Eigen::AngleAxis<double> WAIST_offset_rot = Eigen::AngleAxis<double>(0,
hrp::Vector3(0,0,1));
376 for (std::vector<std::string>::iterator it = inputs.begin();
385 coil::vstring filename_arg_str = coil::split(*it,
",");
386 std::string
filename = filename_arg_str[0];
388 if ( filename_arg_str.size () > 1 ){
389 for (
size_t i = 0;
i < 3;
i++) {
390 coil::stringTo(WAIST_offset_pos(
i), filename_arg_str[
i+1].c_str());
391 coil::stringTo(WAIST_offset_rot.axis()(
i), filename_arg_str[
i+1+3].c_str());
393 coil::stringTo(WAIST_offset_rot.angle(), filename_arg_str[1+3+3].c_str());
402 std::string
name = body->name();
404 xmlTextWriterStartElement(writer, BAD_CAST
"item");
405 xmlTextWriterWriteAttribute(writer, BAD_CAST
"class", BAD_CAST
"com.generalrobotix.ui.item.GrxRTSItem");
406 xmlTextWriterWriteAttribute(writer, BAD_CAST
"name", BAD_CAST
name.c_str());
407 xmlTextWriterWriteAttribute(writer, BAD_CAST
"select", BAD_CAST
"true");
410 if (use_highgain_mode) {
413 if (it==inputs.begin()) {
421 if (it==inputs.begin()) {
426 xmlTextWriterEndElement(writer);
429 xmlTextWriterStartElement(writer, BAD_CAST
"item");
430 xmlTextWriterWriteAttribute(writer, BAD_CAST
"class", BAD_CAST
"com.generalrobotix.ui.item.GrxModelItem");
431 xmlTextWriterWriteAttribute(writer, BAD_CAST
"name", BAD_CAST (
basename(*it).c_str()));
432 xmlTextWriterWriteAttribute(writer, BAD_CAST
"url", BAD_CAST
filename.c_str());
435 if (use_highgain_mode) {
439 if (integrate ==
"false") {
452 if ( nforce > 0 ) std::cerr <<
"hrp::Sensor::FORCE";
453 for (
int i=0;
i<nforce;
i++){
457 std::cerr <<
" " << s->
name;
459 if ( nforce > 0 ) std::cerr << std::endl;
461 if ( ngyro > 0 ) std::cerr <<
"hrp::Sensor::GYRO";
466 std::cerr <<
" " << s->
name;
468 for (
int i=0;
i<ngyro;
i++){
470 std::stringstream str_strm;
471 str_strm << s->
name <<
i <<
":" + s->
name <<
":RATE_GYRO_SENSOR";
473 std::cerr <<
" " << s->
name;
476 if ( ngyro > 0 ) std::cerr << std::endl;
478 if ( nacc > 0 ) std::cerr <<
"hrp::Sensor::ACCELERATION";
483 std::cerr <<
" " << s->
name;
485 for (
int i=0;
i<nacc;
i++){
487 std::stringstream str_strm;
488 str_strm << s->
name <<
i <<
":" << s->
name <<
":ACCELERATION_SENSOR";
490 std::cerr <<
" " << s->
name;
493 if ( nacc > 0 ) std::cerr << std::endl;
496 std::string root_name = body->rootLink()->name;
500 std::ostringstream os;
501 os << body->rootLink()->p[0] + WAIST_offset_pos(0) <<
" "
502 << body->rootLink()->p[1] + WAIST_offset_pos(1) <<
" "
503 << body->rootLink()->p[2] + WAIST_offset_pos(2);
506 Eigen::AngleAxis<double> tmpAA = Eigen::AngleAxis<double>(
hrp::Matrix33(body->rootLink()->R * WAIST_offset_rot.toRotationMatrix()));
507 os << tmpAA.axis()(0) <<
" "
508 << tmpAA.axis()(1) <<
" "
509 << tmpAA.axis()(2) <<
" "
513 if ( ! body->isStaticModel() ) {
521 coil::vstring joint_properties_arg_str = coil::split(joint_properties,
",");
522 std::map <std::string, std::string> joint_properties_map;
523 for (
size_t i = 0;
i < joint_properties_arg_str.size()/2;
i++) {
524 joint_properties_map.insert(std::pair<std::string, std::string>(joint_properties_arg_str[
i*2], joint_properties_arg_str[
i*2+1]));
526 if ( body->numJoints() > 0 ) std::cerr <<
"hrp::Joint";
527 for(
unsigned int i = 0;
i < body->numJoints();
i++){
528 if ( body->joint(
i)->index > 0 ) {
529 std::cerr <<
" " << body->joint(
i)->name <<
"(" << body->joint(
i)->jointId <<
")";
530 std::string joint_name = body->joint(
i)->name;
531 std::string j_property = joint_name+
".angle";
533 (joint_properties_map.find(j_property) != joint_properties_map.end()) ? joint_properties_map[j_property] :
"0.0");
534 j_property = joint_name+
".mode";
536 (joint_properties_map.find(j_property) != joint_properties_map.end()) ? joint_properties_map[j_property] : (use_highgain_mode?
"HighGain":
"Torque"));
537 j_property = joint_name+
".NumOfAABB";
539 (joint_properties_map.find(j_property) != joint_properties_map.end()) ? joint_properties_map[j_property] :
"1");
542 xmlTextWriterEndElement(writer);
543 if ( body->numJoints() > 0 ) std::cerr << std::endl;
564 xmlTextWriterStartElement(writer, BAD_CAST
"view");
565 xmlTextWriterWriteAttribute(writer, BAD_CAST
"class", BAD_CAST
"com.generalrobotix.ui.view.GrxRobotHardwareClientView");
566 xmlTextWriterWriteAttribute(writer, BAD_CAST
"name", BAD_CAST
"RobotHardware RTC Client");
573 xmlTextWriterEndElement(writer);
575 xmlTextWriterStartElement(writer, BAD_CAST
"view");
576 xmlTextWriterWriteAttribute(writer, BAD_CAST
"class", BAD_CAST
"com.generalrobotix.ui.view.Grx3DView");
577 xmlTextWriterWriteAttribute(writer, BAD_CAST
"name", BAD_CAST
"3DView");
587 xmlTextWriterEndElement(writer);
591 for (std::vector<std::string>::iterator it1 = inputs.begin(); it1 != inputs.end(); it1++) {
593 for (std::vector<std::string>::iterator it2 = it1+1; it2 != inputs.end(); it2++) {
596 xmlTextWriterStartElement(writer, BAD_CAST
"item");
597 xmlTextWriterWriteAttribute(writer, BAD_CAST
"class", BAD_CAST
"com.generalrobotix.ui.item.GrxCollisionPairItem");
598 xmlTextWriterWriteAttribute(writer, BAD_CAST
"name", BAD_CAST std::string(
"CP#"+name2+
"_#"+name1+
"_").c_str());
610 xmlTextWriterEndElement(writer);
615 xmlTextWriterEndElement(writer);
617 xmlTextWriterEndElement(writer);
619 xmlFreeTextWriter(writer);
621 if (file_exist_flag) {
622 std::cerr <<
"\033[1;31mOver\033[0m";
624 std::cerr <<
"Writing project files to .... " <<
output << std::endl;
626 std::string conf_file =
output.substr(0,
output.find_last_of(
'.'))+
".conf";
627 if (stat(conf_file.c_str(), &st) == 0) {
628 std::cerr <<
"\033[1;31mOver\033[0m";
630 std::fstream s(conf_file.c_str(), std::ios::out);
632 s <<
"model: file://" << filenames[0] << std::endl;
633 s <<
"exec_cxt.periodic.rate: " <<
static_cast<size_t>(1/atof(dt.c_str())+0.5) << std::endl;
634 s <<
"dt: " << dt << std::endl;
635 s << conf_file_option << std::endl;
636 std::cerr <<
"Writing conf files to ....... " << conf_file << std::endl;
640 std::string conf_file =
output.substr(0,
output.find_last_of(
'.'))+
".RobotHardware.conf";
641 if (stat(conf_file.c_str(), &st) == 0) {
642 std::cerr <<
"\033[1;31mOver\033[0m";
644 std::fstream s(conf_file.c_str(), std::ios::out);
646 s <<
"model: file://" << filenames[0] << std::endl;
647 s <<
"exec_cxt.periodic.type: hrpExecutionContext" << std::endl;
648 s <<
"exec_cxt.periodic.rate: " <<
static_cast<size_t>(1/atof(dt.c_str())+0.5) << std::endl;
649 s << robothardware_conf_file_option << std::endl;
650 std::cerr <<
"Writing hardware files to ... " << conf_file << std::endl;
654 catch (CORBA::SystemException& ex)
656 cerr << ex._rep_id() << endl;
658 catch (
const string&
error)
660 cerr <<
error << endl;