24 #include <yarp/os/LogStream.h>
30 using namespace yarp::sig;
36 static double dstep = 10.0/1000.0;
113 #define MAX_DJOINT_FEEDBACKSTRUCTS 500
125 void OdeSdlSimulation::draw() {
131 void OdeSdlSimulation::printStats() {
141 static double starting_time_stamp = 0;
146 if(
duration - starting_time_stamp >= 1){
154 void OdeSdlSimulation::handle_key_down(SDL_keysym* keysym) {
167 yInfo(
"SPACEBAR pressed! Press spacebar again to disable/enable drawing.\n");
175 void OdeSdlSimulation::handle_mouse_motion(SDL_MouseMotionEvent* mousemotion) {
176 if (SDL_GetMouseState(NULL, NULL) & SDL_BUTTON(1)){
210 if (SDL_GetMouseState(NULL, NULL) & SDL_BUTTON(3)){
217 void OdeSdlSimulation::process_events(
void) {
221 Uint8 * keystate = SDL_GetKeyState(NULL);
222 if(keystate[SDLK_q]){
xrot += 1 * 0.4f;
if (
xrot >360)
xrot -= 360 * 0.1f;}
223 if(keystate[SDLK_z]){
xrot -= 1 * 0.4f;
if (
xrot < -360)
xrot += 360 * 0.1f;}
230 if(keystate[SDLK_e]){
zrot += 1 * 0.4f;
if (
zrot >360)
zrot -= 360 * 0.4f;}
231 if(keystate[SDLK_c]){
zrot -= 1 * 0.4f;
if (
zrot < -360)
zrot += 360 * 0.4f;}
233 if (keystate[SDLK_f]){
ypos +=1 *0.005f;}
234 if (keystate[SDLK_v]){
ypos -=1 *0.005f;}
236 if(keystate[SDLK_1]){initViewpoint();}
238 if (keystate[SDLK_5]){
243 if (keystate[SDLK_6]){
247 if (keystate[SDLK_h])
252 while (SDL_PollEvent(&event)){
255 case SDL_VIDEORESIZE:
256 width =
event.resize.w;
258 SDL_SetVideoMode(
width,
height,16,SDL_OPENGL | SDL_RESIZABLE);
270 handle_key_down(&event.key.keysym);
274 case SDL_MOUSEMOTION:
275 handle_mouse_motion(&event.motion);
284 case SDL_MOUSEBUTTONDOWN:
285 handle_mouse_motion(&event.motion);
286 switch (event.button.button)
288 case SDL_BUTTON_LEFT:
293 case SDL_BUTTON_MIDDLE:
295 case SDL_BUTTON_RIGHT:
303 case SDL_MOUSEBUTTONUP:
304 switch (event.button.button)
306 case SDL_BUTTON_LEFT:
310 case SDL_BUTTON_MIDDLE:
313 case SDL_BUTTON_RIGHT:
325 void OdeSdlSimulation::nearCallback (
void *
data, dGeomID o1, dGeomID o2) {
327 const double CONTACT_HEIGHT_TRESHOLD_METERS = 0.1;
335 dSpaceID space1,space2;
336 dSpaceID superSpace1,superSpace2;
337 std::string geom1className(
"");
338 std::string geom2ClassName(
"");
339 std::string geom1name(
"");
340 std::string geom2name(
"");
341 bool geom1isiCubPart =
false;
342 bool geom2isiCubPart =
false;
343 bool geom1isTorsoOrArm =
false;
344 bool geom2isTorsoOrArm =
false;
347 string indentString(
"");
348 std::map<dGeomID,string>::iterator geom1namesIt;
349 std::map<dGeomID,string>::iterator geom2namesIt;
351 if (dGeomIsSpace(o1)){
352 space1 = (dSpaceID)o1;
354 space1 = dGeomGetSpace(o1);
355 indentString = indentString +
" --- ";
357 subLevel1 = dSpaceGetSublevel(space1);
358 for (
int i=1;i<=subLevel1;i++){
359 indentString = indentString +
" --- ";
362 if (odeinit.
verbosity > 3) yDebug(
"%s nearCallback()\n",indentString.c_str());
364 if (dGeomIsSpace(o1)){
365 space1 = (dSpaceID)o1;
367 yDebug(
"%s Object nr. 1: %s, sublevel: %d, contained within: %s, nr. geoms: %d. \n",indentString.c_str(),odeinit.
_iCub->
dSpaceNames[space1].c_str(),dSpaceGetSublevel(space1),odeinit.
_iCub->
dSpaceNames[dGeomGetSpace(o1)].c_str(),dSpaceGetNumGeoms(space1));
371 getGeomClassName(dGeomGetClass(o1),geom1className);
372 superSpace1 = dGeomGetSpace(o1);
375 geom1name = geom1namesIt->second;
376 if (odeinit.
verbosity > 3) yDebug(
"%s Object nr. 1: geom: %s, class: %s, contained within %s (sublevel %d).\n",indentString.c_str(),geom1name.c_str(),geom1className.c_str(),odeinit.
_iCub->
dSpaceNames[superSpace1].c_str(),dSpaceGetSublevel(superSpace1));
379 if (odeinit.
verbosity > 3) yDebug(
"%s Object nr. 1: A geom, ID: %p, class: %s, contained within %s (sublevel %d).\n",indentString.c_str(),o1,geom1className.c_str(),odeinit.
_iCub->
dSpaceNames[superSpace1].c_str(),dSpaceGetSublevel(superSpace1));
383 if (dGeomIsSpace(o2)){
384 space2 = (dSpaceID)o2;
386 yDebug(
"%s Object nr. 2: %s, sublevel: %d, contained within: %s, nr. geoms: %d. \n",indentString.c_str(),odeinit.
_iCub->
dSpaceNames[space2].c_str(),dSpaceGetSublevel(space2),odeinit.
_iCub->
dSpaceNames[dGeomGetSpace(o2)].c_str(),dSpaceGetNumGeoms(space2));
389 getGeomClassName(dGeomGetClass(o2),geom2ClassName);
390 superSpace2 = dGeomGetSpace(o2);
393 geom2name = geom2namesIt->second;
394 if (odeinit.
verbosity > 3) yDebug(
"%s Object nr. 2: geom: %s, class: %s, contained within %s (sublevel %d).\n",indentString.c_str(),geom2name.c_str(),geom2ClassName.c_str(),odeinit.
_iCub->
dSpaceNames[superSpace2].c_str(),dSpaceGetSublevel(superSpace2));
397 if (odeinit.
verbosity > 3) yDebug(
"%s Object nr. 2: A geom, ID: %p, class: %s, contained within %s (sublevel %d).\n",indentString.c_str(),o2,geom2ClassName.c_str(),odeinit.
_iCub->
dSpaceNames[superSpace2].c_str(),dSpaceGetSublevel(superSpace2));
402 if (dGeomIsSpace(o1) || dGeomIsSpace(o2)){
403 if (dGeomIsSpace(o1) && dGeomIsSpace(o2)){
405 if (odeinit.
verbosity > 3) yDebug(
"%s Ignoring head vs. torso collision space checking.\n",indentString.c_str());
409 if (odeinit.
verbosity > 3) yDebug(
"%s Ignoring legs vs. torso collision space checking.\n",indentString.c_str());
413 dSpaceCollide2(o1,o2,
data,&nearCallback);
417 dSpaceCollide2(o1,o2,
data,&nearCallback);
430 dBodyID b1 = dGeomGetBody(o1);
431 dBodyID b2 = dGeomGetBody(o2);
432 if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)){
433 if (odeinit.
verbosity > 3) yDebug(
"%s Collision ignored: the bodies of o1 and o2 are connected by a joint.\n",indentString.c_str());
437 if (selfCollisionOnIgnoreList(geom1name,geom2name)){
439 yDebug(
"%s geom: %s (class: %s, contained within %s) AND geom: %s (class: %s, contained within %s).\n",indentString.c_str(),geom1name.c_str(),geom1className.c_str(),odeinit.
_iCub->
dSpaceNames[superSpace1].c_str(),geom2name.c_str(),geom2ClassName.c_str(),odeinit.
_iCub->
dSpaceNames[superSpace2].c_str());
440 yDebug(
"%s Collision ignored (ignore list).\n",indentString.c_str());
445 if (odeinit.
verbosity > 3) yDebug(
"%s Collision candidate. Preparing contact joints.\n",indentString.c_str());
449 contact[i].surface.mode = dContactSlip1| dContactSlip2| dContactBounce | dContactSoftCFM;
452 contact[i].surface.bounce = 0.01;
453 contact[i].surface.bounce_vel = 0.01;
454 contact[i].surface.slip1 = (dReal)0.000001;
455 contact[i].surface.slip2 = (dReal)0.000001;
456 contact[i].surface.soft_cfm = 0.0001;
458 int numc = dCollide (o1,o2,
MAX_CONTACTS,&contact[0].geom,
sizeof(dContact));
460 if (odeinit.
verbosity > 3) yDebug(
"%s Collision suspect confirmed. There are %d contacts - creating joints.\n",indentString.c_str(),numc);
463 if(contact[0].geom.pos[1]>CONTACT_HEIGHT_TRESHOLD_METERS){
465 yDebug(
"%s ****** non-ground COLLISION, %d contacts - creating joints************************************************************\n",indentString.c_str(),numc);
466 yDebug(
"%s geom: %s (%s, contained within %s) AND geom: %s (%s, contained within %s)\n",indentString.c_str(),geom1name.c_str(),geom1className.c_str(),odeinit.
_iCub->
dSpaceNames[superSpace1].c_str(),geom2name.c_str(),geom2ClassName.c_str(),odeinit.
_iCub->
dSpaceNames[superSpace2].c_str());
469 for (i=0; i<numc; i++) {
470 if (odeinit.
verbosity > 4) yDebug(
"%s Contact joint nr. %d (index:%d): at (%f,%f,%f), depth: %f \n",indentString.c_str(),i+1,i,contact[i].geom.pos[0],contact[i].geom.pos[1],contact[i].geom.pos[2],contact[i].geom.depth);
472 dJointAttach (c,b1,b2);
476 bool b1isTouchSensitive = isBodyTouchSensitive (b1);
477 bool b2isTouchSensitive = isBodyTouchSensitive (b2);
479 if (b1isTouchSensitive || b2isTouchSensitive) {
481 if (odeinit.
verbosity > 2) yDebug(
"%s Adding tactile feedback for emulating finger/palm skin to this one (ODE joint feedback counter: %d).\n",indentString.c_str(),
nFeedbackStructs);
491 geom1isiCubPart =
true;
494 geom1isiCubPart =
true;
495 geom1isTorsoOrArm =
true;
500 geom2isiCubPart =
true;
503 geom2isiCubPart =
true;
504 geom2isTorsoOrArm =
true;
508 if ( geom1isTorsoOrArm || geom2isTorsoOrArm){
509 if (odeinit.
verbosity > 3) yDebug(
"%s Adding tactile feedback for whole-body skinContact to this contact (ODE joint feedback counter: %d).\n",indentString.c_str(),
nFeedbackStructs);
511 yWarning(
"out of contact joint feedback structures for ODE (exceeded %d) - some contact joints will not have info about forces stored\n.",
MAX_DJOINT_FEEDBACKSTRUCTS);
518 if (geom1isiCubPart){
526 if (geom2isiCubPart){
536 if (odeinit.
verbosity > 3) yDebug(
"%s Ignoring skin contact - so far only arms and torso are implemented.\n",indentString.c_str());
543 if (odeinit.
verbosity > 3) yDebug(
"%s Collision suspect NOT confirmed. There were %d contacts.\n",indentString.c_str(),numc);
548 bool OdeSdlSimulation::selfCollisionOnIgnoreList(
string geom1_string,
string geom2_string)
551 if ( ( (geom1_string.compare(
"upper left arm cover")==0) && (geom2_string.compare(
"torsoGeom[4]")==0) ) || ( (geom2_string.compare(
"upper left arm cover")==0) && (geom1_string.compare(
"torsoGeom[4]")==0) ) ){
554 if ( ( (geom1_string.compare(
"upper left arm cover")==0) && (geom2_string.compare(
"torso cover")==0) ) || ( (geom2_string.compare(
"upper left arm cover")==0) && (geom1_string.compare(
"torso cover")==0) ) ){
557 if ( ( (geom1_string.compare(
"geom[2]")==0) && (geom2_string.compare(
"torso cover")==0) ) || ( (geom2_string.compare(
"geom[2]")==0) && (geom1_string.compare(
"torso cover")==0) ) ){
561 if ( ( (geom1_string.compare(
"geom[4]")==0) && (geom2_string.compare(
"torso cover")==0) ) || ( (geom2_string.compare(
"geom[4]")==0) && (geom1_string.compare(
"torso cover")==0) ) ){
564 if ( ( (geom1_string.compare(
"geom[4]")==0) && (geom2_string.compare(
"torsoGeom[5]")==0) ) || ( (geom2_string.compare(
"geom[4]")==0) && (geom1_string.compare(
"torsoGeom[5]")==0) ) ){
569 if ( ( (geom1_string.compare(
"upper right arm cover")==0) && (geom2_string.compare(
"torsoGeom[5]")==0) ) || ( (geom2_string.compare(
"upper right arm cover")==0) && (geom1_string.compare(
"torsoGeom[5]")==0) ) ){
572 if ( ( (geom1_string.compare(
"upper right arm cover")==0) && (geom2_string.compare(
"torso cover")==0) ) || ( (geom2_string.compare(
"upper right arm cover")==0) && (geom1_string.compare(
"torso cover")==0) ) ){
575 if ( ( (geom1_string.compare(
"geom[3]")==0) && (geom2_string.compare(
"torso cover")==0) ) || ( (geom2_string.compare(
"geom[3]")==0) && (geom1_string.compare(
"torso cover")==0) ) ){
578 if ( ( (geom1_string.compare(
"geom[5]")==0) && (geom2_string.compare(
"torso cover")==0) ) || ( (geom2_string.compare(
"geom[5]")==0) && (geom1_string.compare(
"torso cover")==0) ) ){
581 if ( ( (geom1_string.compare(
"geom[5]")==0) && (geom2_string.compare(
"torsoGeom[5]")==0) ) || ( (geom2_string.compare(
"geom[5]")==0) && (geom1_string.compare(
"torsoGeom[5]")==0) ) ){
586 if ( ( (geom1_string.compare(
"upper left arm cover")==0) && (geom2_string.compare(
"torsoGeom[4]")==0) ) || ( (geom2_string.compare(
"upper left arm cover")==0) && (geom1_string.compare(
"torsoGeom[4]")==0) ) ){
589 if ( ( (geom1_string.compare(
"upper left arm cover")==0) && (geom2_string.compare(
"torso cover")==0) ) || ( (geom2_string.compare(
"upper left arm cover")==0) && (geom1_string.compare(
"torso cover")==0) ) ){
592 if ( ( (geom1_string.compare(
"geom[2]")==0) && (geom2_string.compare(
"torso cover")==0) ) || ( (geom2_string.compare(
"geom[2]")==0) && (geom1_string.compare(
"torso cover")==0) ) ){
596 if ( ( (geom1_string.compare(
"geom[4]")==0) && (geom2_string.compare(
"torso cover")==0) ) || ( (geom2_string.compare(
"geom[4]")==0) && (geom1_string.compare(
"torso cover")==0) ) ){
599 if ( ( (geom1_string.compare(
"geom[4]")==0) && (geom2_string.compare(
"torsoGeom[5]")==0) ) || ( (geom2_string.compare(
"geom[4]")==0) && (geom1_string.compare(
"torsoGeom[5]")==0) ) ){
608 bool OdeSdlSimulation::isBodyTouchSensitive (dBodyID bodyID) {
615 }
else if (bodyID == odeinit.
_iCub->
body[30]) {
617 }
else if (bodyID == odeinit.
_iCub->
body[24]) {
619 }
else if (bodyID == odeinit.
_iCub->
body[25]) {
634 }
else if (bodyID == odeinit.
_iCub->
body[49]) {
636 }
else if (bodyID == odeinit.
_iCub->
body[43]) {
638 }
else if (bodyID == odeinit.
_iCub->
body[44]) {
654 void OdeSdlSimulation::inspectHandTouch_icubSensors(Bottle& reportLeft, Bottle& reportRight,
bool boolean) {
658 int indicesLeft[6] = {24, 25, 26, 27, 30, 10};
659 int indicesRight[6] = {43, 44, 45, 46, 49, 11};
662 double resultLeft=0, resultRight = 0;
663 for (
int x = 0;
x < 6;
x++){
674 for (
int i = 0; i < 12; i++){
675 reportLeft.addFloat64(resultLeft * 255);
676 reportRight.addFloat64(resultRight * 255);
680 for (
int y = 0;
y<3;
y++){
681 for (
int i = 0; i < 12; i++){
682 reportLeft.addFloat64(0.0);
683 reportRight.addFloat64(0.0);
687 for (
int y = 0;
y<4;
y++){
688 for (
int i = 0; i < 12; i++){
689 reportLeft.addFloat64(resultLeft * 255);
690 reportRight.addFloat64(resultRight * 255);
693 for (
int y = 0;
y<4;
y++){
694 for (
int i = 0; i < 12; i++){
695 reportLeft.addFloat64(0.0);
696 reportRight.addFloat64(0.0);
703 double resultLeft=0, resultRight = 0;
704 for (
int x = 0;
x < 6;
x++){
714 for (
int i = 0; i < 12; i++){
715 reportLeft.addFloat64(resultLeft * 255);
716 reportRight.addFloat64(resultRight * 255);
720 for (
int y = 0;
y<3;
y++){
721 for (
int i = 0; i < 12; i++){
722 reportLeft.addFloat64(0.0);
723 reportRight.addFloat64(0.0);
726 for (
int y = 0;
y<4;
y++){
727 for (
int i = 0; i < 12; i++){
728 reportLeft.addFloat64(resultLeft * 255);
729 reportRight.addFloat64(resultRight * 255);
732 for (
int y = 0;
y<4;
y++){
733 for (
int i = 0; i < 12; i++){
734 reportLeft.addFloat64(0.0);
735 reportRight.addFloat64(0.0);
742 double resultLeft=0, resultRight = 0;
743 for (
int x = 0;
x < 6;
x++){
754 for (
int i = 0; i < 12; i++){
755 reportLeft.addFloat64(resultLeft * 255);
756 reportRight.addFloat64(resultRight * 255);
760 for (
int y = 0;
y<3;
y++){
761 for (
int i = 0; i < 12; i++){
762 reportLeft.addFloat64(0.0);
763 reportRight.addFloat64(0.0);
766 for (
int y = 0;
y<4;
y++){
767 for (
int i = 0; i < 12; i++){
768 reportLeft.addFloat64(resultLeft * 255);
769 reportRight.addFloat64(resultRight * 255);
772 for (
int y = 0;
y<4;
y++){
773 for (
int i = 0; i < 12; i++){
774 reportLeft.addFloat64(0.0);
775 reportRight.addFloat64(0.0);
782 for (
int x = 0;
x < 6;
x++){
783 double resultLeft=0, resultRight = 0;
794 for (
int i = 0; i < 12; i++){
795 reportLeft.addFloat64(resultLeft * 255);
796 reportRight.addFloat64(resultRight * 255);
800 for (
int y = 0;
y<3;
y++){
801 for (
int i = 0; i < 12; i++){
802 reportLeft.addFloat64(0.0);
803 reportRight.addFloat64(0.0);
806 for (
int y = 0;
y<4;
y++){
807 for (
int i = 0; i < 12; i++){
808 reportLeft.addFloat64(resultLeft * 255);
809 reportRight.addFloat64(resultRight * 255);
812 for (
int y = 0;
y<4;
y++){
813 for (
int i = 0; i < 12; i++){
814 reportLeft.addFloat64(0.0);
815 reportRight.addFloat64(0.0);
824 void OdeSdlSimulation::getAngles(
const dReal *m,
float&
z,
float&
y,
float&
x) {
825 const dReal eps = 0.00001;
831 x = atan2(-m[6]/c,m[10]/c);
832 z = atan2(-m[1]/c,m[0]/c);
835 z = -atan2(m[4],m[5]);
842 void OdeSdlSimulation::initViewpoint() {
851 void OdeSdlSimulation::mouseMovement(
float x,
float y) {
856 xrot += (float) diffy;
857 yrot += (float) diffx;
860 void OdeSdlSimulation::draw_screen() {
863 glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT);
870 glMatrixMode (GL_PROJECTION);
872 gluPerspective( 75, (
float)
width/
height, 0.01, 100.0 );
873 glMatrixMode (GL_MODELVIEW);
876 glRotatef (
xrot, 1,0,0);
877 glRotatef (
yrot, 0,1,0);
878 glRotatef (
zrot, 0,0,1);
887 glColor3d(0.5,0.5,1);
889 glRotatef(90.0,1,0,0);
890 glRotatef(180.0,0,1,0);
893 glDisable(GL_TEXTURE_2D);
895 glEnable(GL_TEXTURE_2D);
897 SDL_GL_SwapBuffers();
902 void OdeSdlSimulation::retreiveInertialData(Bottle& inertialReport) {
904 static dReal OldLinearVel[3], LinearVel[3], LinearAccel[3];
905 inertialReport.clear();
920 float roll, pitch, yaw;
922 double unit = sqx + sqy + sqz + sqw;
924 if (
test > 0.499*unit) {
925 roll = 2 * atan2(
x,w);
930 if (
test < -0.499*unit) {
931 roll = -2 * atan2(
x,w);
936 roll =(float) ( atan2(2.0*
y*w-2*
x*
z , sqx - sqy - sqz + sqw) );
937 pitch = (float) (atan2(2.0*
x*w-2*
y*
z , -sqx + sqy - sqz + sqw) );
938 yaw = asin(2*
test/unit);
945 inertialReport.addFloat64( -yaw * 180/
M_PI);
946 inertialReport.addFloat64( -pitch * 180/
M_PI);
947 inertialReport.addFloat64( roll * 180/
M_PI);
962 Vector grav,grav1,grav2,grav3;
974 grav1[0]=grav[0]*cos(theta)+grav[2]*sin(theta);
976 grav1[2]=grav[0]*(-sin(theta))+grav[2]*cos(theta);
980 grav2[1]=grav1[1]*cos(theta)+grav1[2]*(-sin(theta));
981 grav2[2]=grav1[1]*sin(theta)+grav1[2]*cos(theta);
984 grav3[0]=grav2[0]*cos(theta)+grav2[1]*(-sin(theta));
985 grav3[1]=grav2[0]*sin(theta)+grav2[1]*cos(theta);
988 inertialReport.addFloat64( grav3[0] );
989 inertialReport.addFloat64( grav3[1] );
990 inertialReport.addFloat64( grav3[2] );
998 inertialReport.addFloat64(0.0);
999 inertialReport.addFloat64(0.0);
1000 inertialReport.addFloat64(0.0);
1003 int OdeSdlSimulation::thread_ode(
void *unused) {
1005 double cpms = 1e3 / CLOCKS_PER_SEC;
1006 long lastOdeProcess = (long) (clock()*cpms);
1007 double avg_ode_step_length = 0.0;
1011 long lastTimeCacheUpdate = (long) (clock()*cpms);
1012 double alpha = 0.99;
1015 bool realTime =
true;
1019 temp = (long) (clock()*cpms);
1020 timeCache += temp - lastTimeCacheUpdate;
1021 lastTimeCacheUpdate = temp;
1024 temp = (long) (clock()*cpms);
1025 timeCache += temp - lastTimeCacheUpdate;
1026 lastTimeCacheUpdate = temp;
1034 lastOdeProcess = (long) (clock()*cpms);
1035 ODE_process(1, (
void*)1);
1036 avg_ode_step_length = alpha*avg_ode_step_length + (1.0-alpha)*((
long) (clock()*cpms) -lastOdeProcess);
1046 yWarning(
"the simulation is too slow to run in real-time, you should increase the timestep in ode_params.ini (current value: %ld, suggested value: %.0f)\n",
1049 yWarning(
"you could get a more accurate dynamics simulation by decreasing the timestep in ode_params.ini (current value: %ld, suggested value: %.0f)\n",
1057 Uint32 OdeSdlSimulation::ODE_process(Uint32 interval,
void *param) {
1065 if (odeinit.
verbosity > 3) yDebug(
"\n ***info code collision detection ***");
1066 if (odeinit.
verbosity > 3) yDebug(
"OdeSdlSimulation::ODE_process: dSpaceCollide(odeinit.space,0,&nearCallback): will test iCub space against the rest of the world (e.g. ground).\n");
1067 dSpaceCollide(odeinit.
space,0,&nearCallback);
1071 yDebug(
"OdeSdlSimulation::ODE_process: dSpaceCollide(odeinit._iCub->iCub,0,&nearCallback): will test iCub subspaces against each other.");
1073 dSpaceCollide(odeinit.
_iCub->
iCub,0,&nearCallback);
1076 if (odeinit.
verbosity > 3) yDebug(
"***END OF info code collision detection\n ***");
1080 for (
int ipart = 0; ipart<
MAX_PART; ipart++) {
1090 retreiveInertialData(inertialBot);
1095 odeinit.
sync =
true;
1096 odeinit.
mtx.unlock();
1102 bool boolean =
true;
1106 inspectHandTouch_icubSensors(reportLeft, reportRight,
boolean);
1121 if (odeinit.
verbosity > 2) yDebug(
"OdeSdlSimulation::ODE_process():There were %lu iCub collisions to process.", odeinit.
listOfSkinContactInfos.size());
1122 inspectWholeBodyContactsAndSendTouch();
1127 emptySkinContactList.clear();
1162 yDebug(
"contactICubSkinEmulMap before resetting:");
1163 printContactICubSkinEmulMap();
1165 resetContactICubSkinEmulMap();
1167 yDebug(
"contactICubSkinEmulMap after resetting:");
1168 printContactICubSkinEmulMap();
1175 Bottle inertialReport;
1176 retreiveInertialData(inertialReport);
1223 void OdeSdlSimulation::sighandler(
int sig) {
1225 odeinit.
stop =
true;
1226 yInfo() <<
"\nCAUGHT Ctrl-c";
1230 yDebug(
"***** OdeSdlSimulation::simLoop \n");
1233 SDL_Init(SDL_INIT_TIMER | SDL_GL_ACCELERATED_VISUAL);
1234 SDL_SetVideoMode(
h,w,32,SDL_OPENGL | SDL_RESIZABLE);
1236 dAllocateODEDataForThread(dAllocateMaskAll);
1240 SDL_WM_SetIcon(
image,0);
1241 SDL_FreeSurface(
image);
1242 SDL_WM_SetCaption(
"iCub Simulator",
"image");
1245 SDL_Thread *ode_thread = SDL_CreateThread(thread_ode, NULL);
1248 if ( ode_thread == NULL ) {
1249 yError(
"Unable to create thread: %s\n", SDL_GetError());
1257 odeinit.
stop =
false;
1278 yWarning(
"the robot is not starting from HomePos and self-collision mode is on. The initial posture is already self-colliding.\n");
1283 while(!odeinit.
stop) {
1297 SDL_Delay(timeLeft);
1317 yInfo(
"Stopping SDL and ODE threads...");
1324 SDL_WaitThread( ode_thread, NULL );
1333 glMatrixMode (GL_PROJECTION);
1340 glMatrixMode (GL_MODELVIEW);
1358 glMatrixMode (GL_MODELVIEW);
1374 glMatrixMode (GL_MODELVIEW);
1377 glRotatef (
xrot, 1,0,0);
1378 glRotatef (
yrot, 0,1,0);
1383 glColor3d(0.5,0.5,1);
1384 glEnable(GL_TEXTURE_2D);
1386 glRotatef(90.0,1,0,0);
1387 glRotatef(180.0,0,1,0);
1390 glDisable(GL_TEXTURE_2D);
1392 glEnable(GL_TEXTURE_2D);
1397 glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT);
1407 yError(
"Only one Simulation object allowed\n");
1417 string moduleName = odeinit.
getName();
1424 Value(
"cameraCalibration")).asString().c_str();
1426 Value(
"icubSimEyes.ini")).asString().c_str();
1428 ResourceFinder rf_camcalib;
1429 rf_camcalib.setDefaultContext(camcalib_context.c_str());
1430 rf_camcalib.setDefaultConfigFile(camcalib_file.c_str());
1431 rf_camcalib.configure(0,NULL);
1434 Bottle &bCalibLeft=rf_camcalib.findGroup(
"CAMERA_CALIBRATION_LEFT");
1435 width_left=bCalibLeft.check(
"w",Value(320)).asInt32();
1436 height_left=bCalibLeft.check(
"h",Value(240)).asInt32();
1441 double focal_length_left=bCalibLeft.check(
"fy",Value(257.34)).asFloat64();
1445 Bottle &bCalibRight=rf_camcalib.findGroup(
"CAMERA_CALIBRATION_RIGHT");
1446 width_right=bCalibRight.check(
"w",Value(320)).asInt32();
1447 height_right=bCalibRight.check(
"h",Value(240)).asInt32();
1449 double focal_length_right=bCalibRight.check(
"fy",Value(257.34)).asFloat64();
1455 options.fromConfigFile(videoconf.c_str());
1457 Bottle textures = *options.find(
"textures").asList();
1458 for (
int i=0; i<textures.size(); i++) {
1459 string name = textures.get(i).asString();
1460 yInfo(
"Adding video texture %s\n", name.c_str());
1461 video->
add(options.findGroup(name.c_str()));
1464 initContactICubSkinEmulMap();
1468 void OdeSdlSimulation::initContactICubSkinEmulMap(
void)
1541 void OdeSdlSimulation::resetContactICubSkinEmulMap(
void)
1597 void OdeSdlSimulation::printContactICubSkinEmulMap(
void)
1599 std::set<unsigned int> taxels_touched;
1600 yDebug(
"OdeSdlSimulation::printContactICubSkinEmulMap");
1602 yDebug(
"key: %d, %s,cover touched: %d, indivTaxelResolution: %d, list of taxel IDs:", it->first,
SkinPart_s[it->first].c_str(),it->second.coverTouched,it->second.indivTaxelResolution);
1603 taxels_touched = it->second.taxelsTouched;
1604 for (std::set<unsigned int>::const_iterator taxel_it = taxels_touched.begin(); taxel_it!=taxels_touched.end(); ++taxel_it){
1605 yDebug(
"%d ",*taxel_it);
1619 odeinit.
sync =
false;
1621 return odeinit.
sync;
1627 for (
int s=0; s<
data.size(); s++){
1641 char *buf=
new char[w *
h *
p];
1642 glReadPixels( 0, 0, w,
h, GL_RGB, GL_UNSIGNED_BYTE, buf);
1643 ImageOf<PixelRgb> img;
1645 img.setExternal(buf,w,
h);
1649 int ww = img.width();
1650 int hh = img.height();
1651 for (
int x=0;
x<ww;
x++) {
1652 for (
int y=0;
y<hh;
y++) {
1653 target(
x,
y) = img(
x,hh-1-
y);
1656 glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
1661 void OdeSdlSimulation::inspectWholeBodyContactsAndSendTouch()
1669 Vector geoCenter_SIM_FoR_forHomo(4,0.0), normal_SIM_FoR_forHomo(4,0.0);
1670 Vector force_SIM_FoR_forHomo(4,0.0), moment_SIM_FoR_forHomo(4,0.0);
1672 Vector geoCenter_link_FoR(3,0.0), normal_link_FoR(3,0.0);
1673 Vector force_link_FoR(3,0.0), moment_link_FoR(3,0.0);
1674 double forceOnBody_magnitude;
1675 double left_arm_encoders[16], right_arm_encoders[16], torso_encoders[3], head_encoders[6];
1676 Vector left_arm_for_iKin(10,0.0), right_arm_for_iKin(10,0.0), inertial_for_iKin(6,0.0);
1679 std::vector<unsigned int> taxel_list;
1680 bool upper_body_transforms_available =
false;
1682 bool skinCoverFlag =
false;
1683 bool fingertipFlag =
true;
1686 mySkinContactList.clear();
1689 upper_body_transforms_available =
false;
1690 yWarning(
"With self-collisions on but head/torso/left_arm/right_arm off, the upper body transforms are unavailable and skinContactList can't be created.");
1693 upper_body_transforms_available =
true;
1700 left_arm_for_iKin(j)=torso_encoders[j];
1701 right_arm_for_iKin(j)=torso_encoders[j];
1702 inertial_for_iKin(j)=torso_encoders[j];
1704 for (
int l=0;l<7;l++){
1705 left_arm_for_iKin(l+
TORSO_DOF) = left_arm_encoders[l];
1706 right_arm_for_iKin(l+
TORSO_DOF) = right_arm_encoders[l];
1708 for (
int m=0;m<3;m++){
1709 inertial_for_iKin(m+
TORSO_DOF) = head_encoders[m];
1716 if (odeinit.
verbosity > 4) yDebug(
"OdeSdlSimulation::inspectWholeBodyContactsAndSendTouch:There were %lu iCub collisions to process.", odeinit.
listOfSkinContactInfos.size());
1722 if(upper_body_transforms_available){
1723 geoCenter_SIM_FoR_forHomo.zero(); geoCenter_SIM_FoR_forHomo(3)=1.0;
1724 normal_SIM_FoR_forHomo.zero(); normal_SIM_FoR_forHomo(3)=1.0;
1725 force_SIM_FoR_forHomo.zero(); force_SIM_FoR_forHomo(3)=1.0;
1726 moment_SIM_FoR_forHomo.zero(); moment_SIM_FoR_forHomo(3)=1.0;
1727 geoCenter_link_FoR.zero();normal_link_FoR.zero();
1728 moment_link_FoR.zero();force_link_FoR.zero();
1729 forceOnBody_magnitude=0.0;
1730 T_root_to_link.zero(); T_link_to_root.zero();
1731 for (
int i=0;i<3;i++){
1732 geoCenter_SIM_FoR_forHomo(i)= (*it).contact_geom.pos[i];
1733 normal_SIM_FoR_forHomo(i) = (*it).contact_geom.normal[i];
1735 dJointFeedback * fb = dJointGetFeedback ((*it).contact_joint);
1737 yDebug(
"Warning:OdeSdlSimulation::inspectWholeBodyContactsAndSendTouch: This joint (at %d skin part) has no feedback structure defined - contact force not available: setting to -1.",
skinPart);
1738 forceOnBody_magnitude = -1;
1745 for(
int k=0;k<3;k++){
1746 if((*it).body_index == 1){
1747 force_SIM_FoR_forHomo(k)=fb->f1[k];
1748 moment_SIM_FoR_forHomo(k)=fb->t1[k];
1750 else if((*it).body_index == 2){
1751 force_SIM_FoR_forHomo(k)=fb->f2[k];
1752 moment_SIM_FoR_forHomo(k)=fb->t2[k];
1755 yError(
"OdeSdlSimulation::inspectWholeBodyContactsAndSendTouch: unexpected body_index for colliding body: %d.\n",(*it).body_index);
1758 forceOnBody_magnitude=sqrt(force_SIM_FoR_forHomo(0)*force_SIM_FoR_forHomo(0) + force_SIM_FoR_forHomo(1)*force_SIM_FoR_forHomo(1)
1759 + force_SIM_FoR_forHomo(2)*force_SIM_FoR_forHomo(2));
1781 if (odeinit.
verbosity > 0) yDebug(
"OdeSdlSimulation::processWholeBodyCollisions: FoR transforms to BODY PART %d not implemented yet\n",bodyPart);
1784 T_link_to_root = SE3inv(T_root_to_link);
1787 v1 = T_link_to_root * (odeinit.
_iCub->
H_r2w) * geoCenter_SIM_FoR_forHomo;
1788 geoCenter_link_FoR = v1.subVector(0,2);
1791 v1 = T_link_to_root * (odeinit.
_iCub->
H_r2w) * normal_SIM_FoR_forHomo;
1792 normal_link_FoR = v1.subVector(0,2);
1795 v1 = T_link_to_root * (odeinit.
_iCub->
H_r2w) * force_SIM_FoR_forHomo;
1796 force_link_FoR = v1.subVector(0,2);
1799 v1 = T_link_to_root * (odeinit.
_iCub->
H_r2w) * moment_SIM_FoR_forHomo;
1800 moment_link_FoR = v1.subVector(0,2);
1809 Vector temp_v4(4,0.0);
1810 temp_v4 = (odeinit.
_iCub->
H_r2w) * geoCenter_SIM_FoR_forHomo;
1820 mapPositionIntoTaxelList(
skinPart,geoCenter_link_FoR,taxel_list);
1822 else if(fingertipFlag){
1823 mapFingertipIntoTaxelList(handPart,taxel_list);
1832 if (odeinit.
verbosity > 4) yDebug(
"Creating skin contact as follows: %s.\n",c.toString().c_str());
1833 mySkinContactList.push_back(c);
1836 if(skinCoverFlag || fingertipFlag){
1840 if (!taxel_list.empty()){
1841 unsigned int first_taxel_in_list = taxel_list[0];
1843 for (std::vector<unsigned int>::const_iterator it = taxel_list.begin() ; it != taxel_list.end(); ++it){
1865 Bottle bottleLeftHand;
1870 for (
y = 0;
y<=59;
y++){
1872 bottleLeftHand.addFloat64(255.0);
1875 bottleLeftHand.addFloat64(0.0);
1880 for (
y = 0;
y<=59;
y++){
1881 bottleLeftHand.addFloat64(255);
1885 for (
y = 60;
y<=95;
y++){
1886 bottleLeftHand.addFloat64(0.0);
1891 for (
y = 96;
y<=143;
y++){
1893 bottleLeftHand.addFloat64(255.0);
1896 bottleLeftHand.addFloat64(0.0);
1901 for (
int y = 96;
y<=143;
y++){
1902 bottleLeftHand.addFloat64(255.0);
1906 for (
int y = 144;
y<=191;
y++){
1907 bottleLeftHand.addFloat64(0.0);
1918 Bottle bottleRightHand;
1923 for (
y = 0;
y<=59;
y++){
1925 bottleRightHand.addFloat64(255.0);
1928 bottleRightHand.addFloat64(0.0);
1933 for (
y = 0;
y<=59;
y++){
1934 bottleRightHand.addFloat64(255);
1938 for (
y = 60;
y<=95;
y++){
1939 bottleRightHand.addFloat64(0.0);
1944 for (
y = 96;
y<=143;
y++){
1946 bottleRightHand.addFloat64(255.0);
1949 bottleRightHand.addFloat64(0.0);
1954 for (
int y = 96;
y<=143;
y++){
1955 bottleRightHand.addFloat64(255.0);
1959 for (
int y = 144;
y<=191;
y++){
1960 bottleRightHand.addFloat64(0.0);
1973 Bottle bottleLeftArm;
1976 for (
int y = 0;
y<=767;
y++){
1978 bottleLeftArm.addFloat64(255.0);
1981 bottleLeftArm.addFloat64(0.0);
1995 Bottle bottleLeftForearm;
1998 for (
int y = 0;
y<=383;
y++){
2000 bottleLeftForearm.addFloat64(255.0);
2003 bottleLeftForearm.addFloat64(0.0);
2017 Bottle bottleRightArm;
2020 for (
int y = 0;
y<=767;
y++){
2022 bottleRightArm.addFloat64(255.0);
2025 bottleRightArm.addFloat64(0.0);
2039 Bottle bottleRightForearm;
2042 for (
int y = 0;
y<=383;
y++){
2044 bottleRightForearm.addFloat64(255.0);
2047 bottleRightForearm.addFloat64(0.0);
2064 for (
int y = 0;
y<=767;
y++){
2066 bottleTorso.addFloat64(255.0);
2069 bottleTorso.addFloat64(0.0);
2085 void OdeSdlSimulation::mapPositionIntoTaxelList(
const SkinPart skin_part,
const Vector geo_center_link_FoR,std::vector<unsigned int>& list_of_taxels){
2094 list_of_taxels.push_back(121);list_of_taxels.push_back(122);list_of_taxels.push_back(123);
2095 list_of_taxels.push_back(124);list_of_taxels.push_back(125);list_of_taxels.push_back(126);
2096 list_of_taxels.push_back(127);list_of_taxels.push_back(128);
2099 else if ((geo_center_link_FoR[0]<=0.003+
MORE_EXTRA_MARGIN_FOR_TAXEL_POSITION_M) && (geo_center_link_FoR[0]>=-0.014) && (geo_center_link_FoR[1]>=-0.0055) && (geo_center_link_FoR[1]<=0.01)){
2100 list_of_taxels.push_back(96);list_of_taxels.push_back(97);list_of_taxels.push_back(98);
2101 list_of_taxels.push_back(99);list_of_taxels.push_back(102);list_of_taxels.push_back(103);
2102 list_of_taxels.push_back(120);list_of_taxels.push_back(129);list_of_taxels.push_back(130);
2105 list_of_taxels.push_back(100);list_of_taxels.push_back(101);list_of_taxels.push_back(104);
2106 list_of_taxels.push_back(105);list_of_taxels.push_back(106);list_of_taxels.push_back(113);
2107 list_of_taxels.push_back(116);list_of_taxels.push_back(117);
2110 list_of_taxels.push_back(108);list_of_taxels.push_back(109);list_of_taxels.push_back(110);
2111 list_of_taxels.push_back(111);list_of_taxels.push_back(112);list_of_taxels.push_back(114);
2112 list_of_taxels.push_back(115);list_of_taxels.push_back(118); list_of_taxels.push_back(142);
2113 list_of_taxels.push_back(143);
2116 list_of_taxels.push_back(132);list_of_taxels.push_back(133);list_of_taxels.push_back(134);
2117 list_of_taxels.push_back(135);list_of_taxels.push_back(136);list_of_taxels.push_back(137);
2118 list_of_taxels.push_back(138);list_of_taxels.push_back(140); list_of_taxels.push_back(141);
2121 yWarning(
"OdeSdlSimulation::mapPositionIntoTaxelList: WARNING: contact at part: %d, coordinates: %f %f %f, but no taxels asigned to this position. \n",skin_part,geo_center_link_FoR[0],geo_center_link_FoR[1],geo_center_link_FoR[2]);
2126 list_of_taxels.push_back(120);list_of_taxels.push_back(121);list_of_taxels.push_back(122);
2127 list_of_taxels.push_back(123);list_of_taxels.push_back(124);list_of_taxels.push_back(125);
2128 list_of_taxels.push_back(126);list_of_taxels.push_back(128);
2131 else if ((geo_center_link_FoR[0]<=0.003+
MORE_EXTRA_MARGIN_FOR_TAXEL_POSITION_M) && (geo_center_link_FoR[0]>=-0.014) && (geo_center_link_FoR[1]>=-0.0055) && (geo_center_link_FoR[1]<=0.01)){
2132 list_of_taxels.push_back(99);list_of_taxels.push_back(102);list_of_taxels.push_back(103);
2133 list_of_taxels.push_back(104);list_of_taxels.push_back(105);list_of_taxels.push_back(106);
2134 list_of_taxels.push_back(127);list_of_taxels.push_back(129);list_of_taxels.push_back(130);
2137 list_of_taxels.push_back(96);list_of_taxels.push_back(97);list_of_taxels.push_back(98);
2138 list_of_taxels.push_back(100);list_of_taxels.push_back(101);list_of_taxels.push_back(110);
2139 list_of_taxels.push_back(111);list_of_taxels.push_back(112);
2142 list_of_taxels.push_back(108);list_of_taxels.push_back(109);list_of_taxels.push_back(113);
2143 list_of_taxels.push_back(114);list_of_taxels.push_back(115);list_of_taxels.push_back(116);
2144 list_of_taxels.push_back(117);list_of_taxels.push_back(118); list_of_taxels.push_back(142);
2145 list_of_taxels.push_back(143);
2148 list_of_taxels.push_back(132);list_of_taxels.push_back(133);list_of_taxels.push_back(134);
2149 list_of_taxels.push_back(135);list_of_taxels.push_back(136);list_of_taxels.push_back(137);
2150 list_of_taxels.push_back(138);list_of_taxels.push_back(140); list_of_taxels.push_back(141);
2153 yWarning(
"OdeSdlSimulation::mapPositionIntoTaxelList: WARNING: contact at part: %d, coordinates: %f %f %f, but no taxels asigned to this position. \n",skin_part,geo_center_link_FoR[0],geo_center_link_FoR[1],geo_center_link_FoR[2]);
2158 if((geo_center_link_FoR[0]>=-0.0326) && (geo_center_link_FoR[0]<=0.0326) && (geo_center_link_FoR[1]>=-0.0528) && (geo_center_link_FoR[1]<=0.0039) && (geo_center_link_FoR[2]>=-0.0538) && (geo_center_link_FoR[2]<=0.0)){
2160 pushTriangleToTaxelList(288,list_of_taxels);
2162 pushTriangleToTaxelList(300,list_of_taxels);
2164 pushTriangleToTaxelList(348,list_of_taxels);
2166 else if((geo_center_link_FoR[0]>=-0.0545) && (geo_center_link_FoR[0]<=0.0) && (geo_center_link_FoR[1]>=-0.1288) && (geo_center_link_FoR[1]<=-0.0528) && (geo_center_link_FoR[2]>=-0.0569) && (geo_center_link_FoR[2]<=0.0)){
2168 pushTriangleToTaxelList(204,list_of_taxels);
2170 pushTriangleToTaxelList(336,list_of_taxels);
2172 else if((geo_center_link_FoR[0]>=0.0) && (geo_center_link_FoR[0]<=0.0545) && (geo_center_link_FoR[1]>=-0.1288) && (geo_center_link_FoR[1]<=-0.0528) && (geo_center_link_FoR[2]>=-0.0569) && (geo_center_link_FoR[2]<=0.0)){
2174 pushTriangleToTaxelList(252,list_of_taxels);
2176 pushTriangleToTaxelList(312,list_of_taxels);
2182 else if((geo_center_link_FoR[0]>=-0.0375) && (geo_center_link_FoR[0]<=0.0) && (geo_center_link_FoR[1]>=-0.0716) && (geo_center_link_FoR[1]<=0.0) && (geo_center_link_FoR[2]>=0.0281) && (geo_center_link_FoR[2]<=0.0484)){
2184 pushTriangleToTaxelList(132,list_of_taxels);
2186 pushTriangleToTaxelList(168,list_of_taxels);
2188 else if((geo_center_link_FoR[0]>=-0.0375) && (geo_center_link_FoR[0]<=0.0) && (geo_center_link_FoR[1]>=-0.1281) && (geo_center_link_FoR[1]<=-0.0716) && (geo_center_link_FoR[2]>=0.0343) && (geo_center_link_FoR[2]<=0.0526)){
2190 pushTriangleToTaxelList(156,list_of_taxels);
2192 pushTriangleToTaxelList(144,list_of_taxels);
2194 else if((geo_center_link_FoR[0]>=-0.0375) && (geo_center_link_FoR[0]<=0.0) && (geo_center_link_FoR[1]>=-0.1333) && (geo_center_link_FoR[1]<=-0.0716) && (geo_center_link_FoR[2]>=0.0) && (geo_center_link_FoR[2]<=0.0343)){
2196 pushTriangleToTaxelList(24,list_of_taxels);
2198 pushTriangleToTaxelList(12,list_of_taxels);
2200 else if((geo_center_link_FoR[0]>=-0.0375) && (geo_center_link_FoR[0]<=0.0) && (geo_center_link_FoR[1]>=-0.0716) && (geo_center_link_FoR[1]<=0.0) && (geo_center_link_FoR[2]>=0.0) && (geo_center_link_FoR[2]<=0.0281)){
2202 pushTriangleToTaxelList(0,list_of_taxels);
2204 pushTriangleToTaxelList(180,list_of_taxels);
2206 else if((geo_center_link_FoR[0]>=0.0) && (geo_center_link_FoR[0]<=0.0375) && (geo_center_link_FoR[1]>=-0.0716) && (geo_center_link_FoR[1]<=0.0) && (geo_center_link_FoR[2]>=0.0281) && (geo_center_link_FoR[2]<=0.0484)){
2208 pushTriangleToTaxelList(120,list_of_taxels);
2210 pushTriangleToTaxelList(60,list_of_taxels);
2212 else if((geo_center_link_FoR[0]>=0.0) && (geo_center_link_FoR[0]<=0.0375) && (geo_center_link_FoR[1]>=-0.1281) && (geo_center_link_FoR[1]<=-0.0716) && (geo_center_link_FoR[2]>=0.0343) && (geo_center_link_FoR[2]<=0.0526)){
2214 pushTriangleToTaxelList(96,list_of_taxels);
2216 pushTriangleToTaxelList(108,list_of_taxels);
2218 else if((geo_center_link_FoR[0]>=0.0) && (geo_center_link_FoR[0]<=0.0375) && (geo_center_link_FoR[1]>=-0.1333) && (geo_center_link_FoR[1]<=-0.0716) && (geo_center_link_FoR[2]>=0.0) && (geo_center_link_FoR[2]<=0.0343)){
2220 pushTriangleToTaxelList(84,list_of_taxels);
2222 pushTriangleToTaxelList(72,list_of_taxels);
2224 else if((geo_center_link_FoR[0]>=0.0) && (geo_center_link_FoR[0]<=0.0375) && (geo_center_link_FoR[1]>=-0.0716) && (geo_center_link_FoR[1]<=0.0) && (geo_center_link_FoR[2]>=0.0) && (geo_center_link_FoR[2]<=0.0281)){
2226 pushTriangleToTaxelList(36,list_of_taxels);
2228 pushTriangleToTaxelList(48,list_of_taxels);
2231 yWarning(
"OdeSdlSimulation::mapPositionIntoTaxelList: WARNING: contact at part: %d, coordinates: %f %f %f, but no taxels asigned to this position. \n",skin_part,geo_center_link_FoR[0],geo_center_link_FoR[1],geo_center_link_FoR[2]);
2236 if((geo_center_link_FoR[0]>=-0.0326) && (geo_center_link_FoR[0]<=0.0326) && (geo_center_link_FoR[1]>=-0.0039) && (geo_center_link_FoR[1]<=0.0528) && (geo_center_link_FoR[2]>=0.0) && (geo_center_link_FoR[2]<=0.0538)){
2238 pushTriangleToTaxelList(288,list_of_taxels);
2240 pushTriangleToTaxelList(300,list_of_taxels);
2242 pushTriangleToTaxelList(348,list_of_taxels);
2244 else if((geo_center_link_FoR[0]>=-0.0545) && (geo_center_link_FoR[0]<=0.0) && (geo_center_link_FoR[1]>=0.0528) && (geo_center_link_FoR[1]<=0.1288) && (geo_center_link_FoR[2]>=0.0) && (geo_center_link_FoR[2]<=0.0569)){
2246 pushTriangleToTaxelList(204,list_of_taxels);
2248 pushTriangleToTaxelList(336,list_of_taxels);
2250 else if((geo_center_link_FoR[0]>=0.0) && (geo_center_link_FoR[0]<=0.0545) && (geo_center_link_FoR[1]>=0.0528) && (geo_center_link_FoR[1]<=0.1288) && (geo_center_link_FoR[2]>=0.0) && (geo_center_link_FoR[2]<=0.0569)){
2252 pushTriangleToTaxelList(252,list_of_taxels);
2254 pushTriangleToTaxelList(312,list_of_taxels);
2260 else if((geo_center_link_FoR[0]>=-0.0375) && (geo_center_link_FoR[0]<=0.0) && (geo_center_link_FoR[1]>=0.0) && (geo_center_link_FoR[1]<=0.0716) && (geo_center_link_FoR[2]>=-0.0484) && (geo_center_link_FoR[2]<=-0.0281)){
2262 pushTriangleToTaxelList(132,list_of_taxels);
2264 pushTriangleToTaxelList(168,list_of_taxels);
2266 else if((geo_center_link_FoR[0]>=-0.0375) && (geo_center_link_FoR[0]<=0.0) && (geo_center_link_FoR[1]>=0.0716) && (geo_center_link_FoR[1]<=0.1281) && (geo_center_link_FoR[2]>=-0.0526) && (geo_center_link_FoR[2]<=-0.0343)){
2268 pushTriangleToTaxelList(156,list_of_taxels);
2270 pushTriangleToTaxelList(144,list_of_taxels);
2272 else if((geo_center_link_FoR[0]>=-0.0375) && (geo_center_link_FoR[0]<=0.0) && (geo_center_link_FoR[1]>=0.0716) && (geo_center_link_FoR[1]<=0.1333) && (geo_center_link_FoR[2]>=-0.0343) && (geo_center_link_FoR[2]<=0.0)){
2274 pushTriangleToTaxelList(24,list_of_taxels);
2276 pushTriangleToTaxelList(12,list_of_taxels);
2278 else if((geo_center_link_FoR[0]>=-0.0375) && (geo_center_link_FoR[0]<=0.0) && (geo_center_link_FoR[1]>=0.0) && (geo_center_link_FoR[1]<=0.0716) && (geo_center_link_FoR[2]>=-0.0281) && (geo_center_link_FoR[2]<=0.0)){
2280 pushTriangleToTaxelList(0,list_of_taxels);
2282 pushTriangleToTaxelList(180,list_of_taxels);
2284 else if((geo_center_link_FoR[0]>=0.0) && (geo_center_link_FoR[0]<=0.0375) && (geo_center_link_FoR[1]>=0.0) && (geo_center_link_FoR[1]<=0.0716) && (geo_center_link_FoR[2]>=-0.0484) && (geo_center_link_FoR[2]<=-0.0281)){
2286 pushTriangleToTaxelList(120,list_of_taxels);
2288 pushTriangleToTaxelList(60,list_of_taxels);
2290 else if((geo_center_link_FoR[0]>=0.0) && (geo_center_link_FoR[0]<=0.0375) && (geo_center_link_FoR[1]>=0.0716) && (geo_center_link_FoR[1]<=0.1281) && (geo_center_link_FoR[2]>=-0.0526) && (geo_center_link_FoR[2]<=-0.0343)){
2292 pushTriangleToTaxelList(96,list_of_taxels);
2294 pushTriangleToTaxelList(108,list_of_taxels);
2296 else if((geo_center_link_FoR[0]>=0.0) && (geo_center_link_FoR[0]<=0.0375) && (geo_center_link_FoR[1]>=0.0716) && (geo_center_link_FoR[1]<=0.1333) && (geo_center_link_FoR[2]>=-0.0343) && (geo_center_link_FoR[2]<=0.0)){
2298 pushTriangleToTaxelList(84,list_of_taxels);
2300 pushTriangleToTaxelList(72,list_of_taxels);
2302 else if((geo_center_link_FoR[0]>=0.0) && (geo_center_link_FoR[0]<=0.0375) && (geo_center_link_FoR[1]>=0.0) && (geo_center_link_FoR[1]<=0.0716) && (geo_center_link_FoR[2]>=-0.0281) && (geo_center_link_FoR[2]<=0.0)){
2304 pushTriangleToTaxelList(36,list_of_taxels);
2306 pushTriangleToTaxelList(48,list_of_taxels);
2309 yWarning(
"OdeSdlSimulation::mapPositionIntoTaxelList: WARNING: contact at part: %d, coordinates: %f %f %f, but no taxels asigned to this position. \n",skin_part,geo_center_link_FoR[0],geo_center_link_FoR[1],geo_center_link_FoR[2]);
2314 yWarning(
"OdeSdlSimulation::mapPositionIntoTaxelList: WARNING: contact at part: %d, but no taxel resolution implemented for this skin part. \n",skin_part);
2329 void OdeSdlSimulation::pushTriangleToTaxelList(
const int startingTaxelID,std::vector<unsigned int>& list_of_taxels)
2331 int i = startingTaxelID;
2332 for (i=startingTaxelID;i<startingTaxelID+6;i++){
2333 list_of_taxels.push_back(i);
2336 for (i = startingTaxelID + 7; i < startingTaxelID + 10; i++){
2337 list_of_taxels.push_back(i);
2339 list_of_taxels.push_back(startingTaxelID+11);
2342 void OdeSdlSimulation::mapFingertipIntoTaxelList(
const HandPart hand_part,std::vector<unsigned int>& list_of_taxels)
2348 for(i=0; i<=11; i++){
2349 list_of_taxels.push_back(i);
2353 for(i=12; i<=23; i++){
2354 list_of_taxels.push_back(i);
2358 for(i=24; i<=35; i++){
2359 list_of_taxels.push_back(i);
2363 for(i=36; i<=47; i++){
2364 list_of_taxels.push_back(i);
2368 for(i=48; i<=59; i++){
2369 list_of_taxels.push_back(i);
2373 printf(
"Warning: OdeSdlSimulation::mapFingertipIntoTaxelList: unexpected HandPart: %d. Pushing fake taxel ID \n",hand_part);
2382 std::string OdeSdlSimulation::getGeomClassName(
const int geom_class,std::string & s)
This file is responsible for the initialisation of the world parameters that are controlled by ODE.
double checkTouchSensor_continuousValued(int bodyToCheck)
bool checkTouchSensor(int bodyToCheck)
iCub::iKin::iCubInertialSensor iKinInertialSensor
Bottle fullSkinActivationTorso
Bottle fullSkinActivationForearm
Bottle emptySkinActivationUpperArm
Bottle emptySkinActivationForearm
Bottle emptySkinActivationHand
iCub::iKin::iCubArm iKinRightArm
iCub::iKin::iCubArm iKinLeftArm
void getSkinAndBodyPartFromSpaceAndGeomID(const dSpaceID geomSpaceID, const dGeomID geomID, SkinPart &skinPart, BodyPart &bodyPart, HandPart &handPart, bool &skinCoverFlag, bool &fingertipFlag)
void setJointControlAction()
Set the control action for all the joints, that can be either a velocity command or a torque command,...
Bottle emptySkinActivationTorso
std::map< dSpaceID, string > dSpaceNames
dSpaceID iCubLeftArmSpace
dSpaceID iCubRightArmSpace
Bottle fullSkinActivationUpperArm
std::map< dGeomID, string > dGeomNames
list< contactOnSkin_t > listOfSkinContactInfos
dJointGroupID contactgroup
iCubSimulationControl ** _controls
double contactFrictionCoefficient
virtual bool getImage(yarp::sig::ImageOf< yarp::sig::PixelRgb > &target)
void clearBuffer()
Signal that we're done with a view.
void init(RobotStreamer *streamer, RobotConfig *config)
Initialization.
OdeSdlSimulation()
Constructor.
~OdeSdlSimulation()
Destructor.
bool checkSync(bool reset=false)
virtual bool getTrqData(Bottle data)
void drawView(bool left, bool right, bool wide)
Render the requested view.
void simLoop(int h, int w)
Run the simulation.
virtual yarp::os::ResourceFinder & getFinder()=0
virtual int getWorldTimestep()=0
virtual void sendTouchRightHand(yarp::os::Bottle &report)=0
virtual bool shouldSendTouchLeftArm()=0
virtual void sendTouchRightForearm(yarp::os::Bottle &report)=0
virtual bool shouldSendTouchRightHand()=0
virtual bool shouldSendTouchLeftHand()=0
virtual void checkTorques()=0
virtual void sendTouchLeftHand(yarp::os::Bottle &report)=0
virtual void sendTouchLeftArm(yarp::os::Bottle &report)=0
virtual void sendInertial(yarp::os::Bottle &report)=0
virtual void sendTouchTorso(yarp::os::Bottle &report)=0
virtual bool shouldSendTouchRightForearm()=0
virtual bool shouldSendInertial()=0
virtual bool shouldSendTouchRightArm()=0
virtual void sendVision()=0
virtual void sendTouchLeftForearm(yarp::os::Bottle &report)=0
virtual bool shouldSendTouchLeftForearm()=0
virtual void sendTouchRightArm(yarp::os::Bottle &report)=0
virtual bool shouldSendSkinEvents()=0
virtual bool shouldSendTouchTorso()=0
virtual void sendSkinEvents(iCub::skinDynLib::skinContactList &skinContactListReport)=0
bool add(const char *port, int textureIndex)
void setName(string module)
void updateIMUData(const yarp::os::Bottle &imuData)
yarp::sig::Matrix getH(const unsigned int i, const bool allLink=false)
Returns the rigid roto-translation matrix from the root reference frame to the ith frame in Denavit-H...
yarp::sig::Vector setAng(const yarp::sig::Vector &q)
Sets the free joint angles to values of q[i].
Class that encloses everything relate to a skinPart.
void loadTexture(std::string texture, int numTexture)
virtual bool getEncodersRaw(double *encs) override
struct timeval prevTime[BOARD_NUM]
int getLinkNum(SkinPart s)
Get the link number associated to the specified skin part.
static float view2_xyz[3]
static VideoTexture * video
static const double MORE_EXTRA_MARGIN_FOR_TAXEL_POSITION_M
static long ode_step_length
static bool START_SELF_COLLISION_DETECTION
static long gl_frame_length
static float view2_hpr[3]
static const double EXTRA_MARGIN_FOR_TAXEL_POSITION_M
static bool extractImages
static RobotConfig * robot_config
static RobotStreamer * robot_streamer
static SDL_Surface * image
static int cameraSizeWidth
static dJointFeedback touchSensorFeedbacks[MAX_DJOINT_FEEDBACKSTRUCTS]
static std::map< SkinPart, contactICubSkinEmul_t > contactICubSkinEmulMap
static double TimestepManager
static int cameraSizeHeight
#define MAX_DJOINT_FEEDBACKSTRUCTS
static int nFeedbackStructs
static const GLfloat light_position[]
This class controls the simulation speed using dWorldstep for "exact" calculations,...
static void sighandler(int _signum)
const std::string SkinPart_s[]
const Skin_2_Link SkinPart_2_LinkNum[SKIN_PART_SIZE]
static const int TORSO_DOF
void DrawVideo(VideoTexture *video)
void DrawGround(bool wireframe)
void drawSkyDome(float x, float y, float z, float width, float height, float length)
bool setup_opengl(ResourceFinder &finder)