/* LICENSE: ========================================================================= CMPack'04 Source Code Release for OPEN-R SDK 1.1.5-r2 for ERS7 Copyright (C) 2004 Multirobot Lab [Project Head: Manuela Veloso] School of Computer Science, Carnegie Mellon University All rights reserved. ========================================================================= */ #include #include #include #include #include #include "../headers/CircBufPacket.h" #include "../headers/Dictionary.h" #include "../headers/Geometry.h" #include "../headers/Reporting.h" #include "../headers/Sensors.h" #include "../headers/Util.h" #include "../headers/Utility.h" #include "../Motion/MotionObject.h" static const int OffJointLFJoint = 0; // Left front static const int OffJointLFShoulder = 1; static const int OffJointLFKnee = 2; static const int OffJointRFJoint = 3; // Right front static const int OffJointRFShoulder = 4; static const int OffJointRFKnee = 5; static const int OffJointLHJoint = 6; // Left hind static const int OffJointLHShoulder = 7; static const int OffJointLHKnee = 8; static const int OffJointRHJoint = 9; // Right hind static const int OffJointRHShoulder = 10; static const int OffJointRHKnee = 11; static const int OffJointNeckTilt = 12; // Head static const int OffJointNeckPan = 13; static const int OffJointNeckRoll = 14; static const int OffJointTailPan = 15; // Tail static const int OffJointTailTilt = 16; static const int OffJointMouth = 17; static const int OffSensorHeadFrontPressure = 18; static const int OffSensorHeadBackPressure = 19; static const int OffSensorBackSwitch = 20; static const int OffSensorChinSwitch = 21; static const int OffSensorLFPaw = 22; static const int OffSensorRFPaw = 24; static const int OffSensorLHPaw = 23; static const int OffSensorRHPaw = 25; static const int OffSensorPSD = 26; static const int OffSensorAccelFB = 27; // Front-back (front positive) static const int OffSensorAccelLR = 28; // Left-right (right positive) static const int OffSensorAccelUD = 29; // Up-down (up positive) //static const int OffSensorThermoSensor = 30; static const int OffSensorWlan = 30; static const int OffSensorChestPSD = 31; static const int OffSensorBackRearSwitch = 32; static const int OffSensorHeadFarPSD = 33; // ERS-7 reuses some of the offsets for different things than the 210 static const int OffSensorBackFrontSwitch = 21; // frontmost back switch on ERS-7 static const int OffSensorHeadNearPSD = 26; const int CPCTranslate_ERS210[] = { 8 , // 00 left front rotate PRM:/r2/c1-Joint2:j1, 9 , // 01 left front shoulder PRM:/r2/c1/c2-Joint2:j2, 10, // 02 left front knee PRM:/r2/c1/c2/c3-Joint2:j3, 16, // 03 right front rotate PRM:/r4/c1-Joint2:j1, 17, // 04 right front shoulder PRM:/r4/c1/c2-Joint2:j2, 18, // 05 right front knee PRM:/r4/c1/c2/c3-Joint2:j3, 12, // 06 left hind rotate PRM:/r3/c1-Joint2:j1, 13, // 07 left hind shoulder PRM:/r3/c1/c2-Joint2:j2, 14, // 08 left hind knee PRM:/r3/c1/c2/c3-Joint2:j3, 20, // 09 right hind rotate PRM:/r5/c1-Joint2:j1, 21, // 10 right hind shoulder PRM:/r5/c1/c2-Joint2:j2, 22, // 11 right hind knee PRM:/r5/c1/c2/c3-Joint2:j3, 0 , // 12 neck tilt PRM:/r1/c1-Joint2:j1, 1 , // 13 neck pan PRM:/r1/c1/c2-Joint2:j2, 2 , // 14 neck roll PRM:/r1/c1/c2/c3-Joint2:j3, 24, // 15 tail pan PRM:/r6/c1-Joint2:j1, 25, // 16 tail tilt PRM:/r6/c2-Joint2:j2, 6 , // 17 mouth PRM:/r1/c1/c2/c3/c4-Joint2:j4, 4 , // 18 head button front PRM:/r1/c1/c2/c3/f1-Sensor:f1, 3 , // 19 head button back PRM:/r1/c1/c2/c3/f2-Sensor:f2, 27, // 20 back button PRM:/r6/s1-Sensor:s1, 7 , // 21 chin/front back button PRM:/r1/c1/c2/c3/c4/s5-Sensor:s5, 11, // 22 left front paw sw PRM:/r2/c1/c2/c3/c4-Sensor:s4, 19, // 24 right front paw sw PRM:/r4/c1/c2/c3/c4-Sensor:s4, 15, // 23 left hind paw sw PRM:/r3/c1/c2/c3/c4-Sensor:s4, 23, // 25 right hind paw sw PRM:/r5/c1/c2/c3/c4-Sensor:s4, 5 , // 26 PSD PRM:/r1/c1/c2/c3/p1-Sensor:p1, 28, // 27 x accel PRM:/a1-Sensor:a1, 29, // 28 -y accel PRM:/a2-Sensor:a2, 30, // 29 z accel PRM:/a3-Sensor:a3, //26, // thermo sensor PRM:/r6/t1-Sensor:t1, }; const int CPCTranslate_ERS7[] = { 11, // 00 left front rotate PRM:/r1/c1-Joint2:11, 10, // 01 left front shoulder PRM:/r1/c1/c2-Joint2:12, 9 , // 02 left front knee PRM:/r1/c1/c2/c3-Joint2:13, 19, // 03 right front rotate PRM:/r1/c1/c2/c3/c4-Joint2:14, 18, // 04 right front shoulder PRM:/r2/c1-Joint2:21, 17, // 05 right front knee PRM:/r2/c1/c2-Joint2:22, 15, // 06 left hind rotate PRM:/r2/c1/c2/c3-Joint2:23, 14, // 07 left hind shoulder PRM:/r4/c1-Joint2:41, 13, // 08 left hind knee PRM:/r4/c1/c2-Joint2:42, 23, // 09 right hind rotate PRM:/r4/c1/c2/c3-Joint2:43, 22, // 10 right hind shoulder PRM:/r3/c1-Joint2:31, 21, // 11 right hind knee PRM:/r3/c1/c2-Joint2:32, 7 , // 12 neck tilt PRM:/r3/c1/c2/c3-Joint2:33, 6 , // 13 neck pan PRM:/r5/c1-Joint2:51, 2 , // 14 neck roll PRM:/r5/c1/c2-Joint2:52, 25, // 15 tail pan PRM:/r5/c1/c2/c3-Joint2:53, 24, // 16 tail tilt PRM:/ 0 , // 17 mouth PRM:/ 3 , // 18 head button front PRM:/ (head button) 1 , // 19 head button back PRM:/ (chin button) 32, // 20 back button PRM:/ (back 2) 33, // 21 chin button PRM:/ (back 3) 8 , // 22 left front paw sw PRM:/ 16, // 24 right front paw sw PRM:/ 12, // 23 left hind paw sw PRM:/ 20, // 25 right hind paw sw PRM:/ 4 , // 26 PSD PRM:/ (head psd near) 26, // 27 x accel PRM:/ 27, // 28 -y accel PRM:/ 28, // 29 z accel PRM:/ 30, // 30 wavelan switch PRM:/b1-Sensor:b1 29, // 31 (chest distance sensor) 31, // 32 (back 1) 5, // 33 (head psd far) }; const int *CPCTranslate=NULL; //static const char * PrimLocs[] = { // "PRM:/r1/c1-Joint2:11" , // Neck tilt1 // "PRM:/r1/c1/c2-Joint2:12" , // Neck pan // "PRM:/r1/c1/c2/c3-Joint2:13" , // Neck tilt2 // "PRM:/r1/c1/c2/c3/c4-Joint2:14" , // Mouth // "PRM:/r1/c1/c2/c3/e5-Joint4:15" , // Left ear // "PRM:/r1/c1/c2/c3/e6-Joint4:16" , // Right ear // "PRM:/r1/c1/c2/c3/t1-Sensor:t1" , // Head sensor // "PRM:/r1/c1/c2/c3/p1-Sensor:p1" , // Head distance sensor(near) // "PRM:/r1/c1/c2/c3/p2-Sensor:p2" , // Head distance sensor(far) // "PRM:/r1/c1/c2/c3/c4/s5-Sensor:s5" , // Chin sensor // "PRM:/r1/c1/c2/c3/l1-LED2:l1" , // Head light(color) // "PRM:/r1/c1/c2/c3/l2-LED2:l2" , // Head light(white) // "PRM:/r1/c1/c2/c3/l3-LED2:l3" , // Mode Indicator(red) // "PRM:/r1/c1/c2/c3/l4-LED2:l4" , // Mode Indicator(green) // "PRM:/r1/c1/c2/c3/l5-LED2:l5" , // Mode Indicator(blue) // "PRM:/r1/c1/c2/c3/l6-LED2:l6" , // Wireless light // "PRM:/r1/c1/c2/c3/la-LED3:la" , // Face light1 // "PRM:/r1/c1/c2/c3/lb-LED3:lb" , // Face light2 // "PRM:/r1/c1/c2/c3/lc-LED3:lc" , // Face light3 // "PRM:/r1/c1/c2/c3/ld-LED3:ld" , // Face light4 // "PRM:/r1/c1/c2/c3/le-LED3:le" , // Face light5 // "PRM:/r1/c1/c2/c3/lf-LED3:lf" , // Face light6 // "PRM:/r1/c1/c2/c3/lg-LED3:lg" , // Face light7 // "PRM:/r1/c1/c2/c3/lh-LED3:lh" , // Face light8 // "PRM:/r1/c1/c2/c3/li-LED3:li" , // Face light9 // "PRM:/r1/c1/c2/c3/lj-LED3:lj" , // Face light10 // "PRM:/r1/c1/c2/c3/lk-LED3:lk" , // Face light11 // "PRM:/r1/c1/c2/c3/ll-LED3:ll" , // Face light12 // "PRM:/r1/c1/c2/c3/lm-LED3:lm" , // Face light13 // "PRM:/r1/c1/c2/c3/ln-LED3:ln" , // Face light14 // "PRM:/r1/c1/c2/c3/i1-FbkImageSensor:F1", // Color camera // "PRM:/r2/c1-Joint2:21" , // Left fore legJ1 // "PRM:/r2/c1/c2-Joint2:22" , // Left fore legJ2 // "PRM:/r2/c1/c2/c3-Joint2:23" , // Left fore legJ3 // "PRM:/r2/c1/c2/c3/c4-Sensor:24" , // Left fore leg, paw sensor // "PRM:/r3/c1-Joint2:31" , // Left hind legJ1 // "PRM:/r3/c1/c2-Joint2:32" , // Left hind legJ2 // "PRM:/r3/c1/c2/c3-Joint2:33" , // Left hind legJ3 // "PRM:/r3/c1/c2/c3/c4-Sensor:34" , // Left hind leg, paw sensor // "PRM:/r4/c1-Joint2:41" , // Right fore legJ1 // "PRM:/r4/c1/c2-Joint2:42" , // Right fore legJ2 // "PRM:/r4/c1/c2/c3-Joint2:43" , // Right fore legJ3 // "PRM:/r4/c1/c2/c3/c4-Sensor:44" , // Right fore leg, paw sensor // "PRM:/r5/c1-Joint2:51" , // Right hind legJ1 // "PRM:/r5/c1/c2-Joint2:52" , // Right hind legJ2 // "PRM:/r5/c1/c2/c3-Joint2:53" , // Right hind legJ3 // "PRM:/r5/c1/c2/c3/c4-Sensor:54" , // Right hind leg, paw sensor // "PRM:/r6/c1-Joint2:61" , // Tail tilt // "PRM:/r6/c2-Joint2:62" , // Tail pan // "PRM:/r1/c1/c2/c3/m1-Mic:M1" , // Stereo microphones // "PRM:/s1-Speaker:S1" , // Speaker // "PRM:/a1-Sensor:a1" , // Acceleration sensor(front-back) // "PRM:/a2-Sensor:a2" , // Acceleration sensor(right-left) // "PRM:/a3-Sensor:a3" , // Acceleration sensor(up-down) // "PRM:/p1-Sensor:p1" , // Chest distance sensor // "PRM:/b1-Sensor:b1" , // Wireless LAN switch // "PRM:/t2-Sensor:t2" , // Back sensor(front) // "PRM:/t3-Sensor:t3" , // Back sensor(middle) // "PRM:/t4-Sensor:t4" , // Back sensor(rear) // "PRM:/lu-LED3:lu" , // Back light(front, color) // "PRM:/lv-LED3:lv" , // Back light(front, white) // "PRM:/lw-LED3:lw" , // Back light(middle, color) // "PRM:/lx-LED3:lx" , // Back light(middle, white) // "PRM:/ly-LED3:ly" , // Back light(rear, color) // "PRM:/lz-LED3:lz" , // Back light(rear, white) //}; // //const uint NumPrimLocs = 65;//sizeof(PrimLocs)/sizeof(PrimLocs[0]); // //ulong PrimIds[NumPrimLocs]; //#define OGETD(idx,cpc) (((double)sensor[0].GetData(cpc)->frame[idx].value) / 1.0E6) //#define OGETB(idx,cpc) ((bool)sensor[0].GetData(cpc)->frame[idx].value) //#define OGETPWM(idx,cpc) (((OJointValue &)sensor[0].GetData(cpc)->frame[idx]).pwmDuty) #define GET_VALUE(idx,cpc) (sensor[0].GetData(CPCTranslate[cpc])->frame[idx].value) // get milli double #define GET_MDBL(idx,cpc) (((double)sensor[0].GetData(CPCTranslate[cpc])->frame[idx].value) / 1.0E3) // get micro double #define GET_UDBL(idx,cpc) (((double)sensor[0].GetData(CPCTranslate[cpc])->frame[idx].value) / 1.0E6) // get bool #define GET_BOOL(idx,cpc) ((bool)sensor[0].GetData(CPCTranslate[cpc])->frame[idx].value) // get integer as bool #define GET_IBOOL(idx,cpc) (sensor[0].GetData(CPCTranslate[cpc])->frame[idx].value!=0) // get switch bool #define GET_SBOOL(idx,cpc) ((OSwitchValue)sensor[0].GetData(CPCTranslate[cpc])->frame[src_idx].value == oswitchON); // get pwm duty #define GET_PWM(idx,cpc) (((OJointValue &)sensor[0].GetData(CPCTranslate[cpc])->frame[idx]).pwmDuty) std::string SensorData::name("SensorData"); unsigned int SensorData::ButtonTranslations[SensorData::NumButtonTranslations]; SensorData::SensorData() : mostRecentFrame(0) { mzero(sensorFrames); InitializeSensors(); char design[256]; OPENR::GetRobotDesign(design); if(strcmp(design,"ERS-210")==0){ CPCTranslate = CPCTranslate_ERS210; }else{ CPCTranslate = CPCTranslate_ERS7; } //for(uint i=0; ilegAngles[0*3 + 0] = GET_UDBL(src_idx,OffJointLFJoint); frame->legAngles[0*3 + 1] = GET_UDBL(src_idx,OffJointLFShoulder); frame->legAngles[0*3 + 2] = GET_UDBL(src_idx,OffJointLFKnee); frame->legAngles[1*3 + 0] = GET_UDBL(src_idx,OffJointRFJoint); frame->legAngles[1*3 + 1] = GET_UDBL(src_idx,OffJointRFShoulder); frame->legAngles[1*3 + 2] = GET_UDBL(src_idx,OffJointRFKnee); frame->legAngles[2*3 + 0] = GET_UDBL(src_idx,OffJointLHJoint); frame->legAngles[2*3 + 1] = GET_UDBL(src_idx,OffJointLHShoulder); frame->legAngles[2*3 + 2] = GET_UDBL(src_idx,OffJointLHKnee); frame->legAngles[3*3 + 0] = GET_UDBL(src_idx,OffJointRHJoint); frame->legAngles[3*3 + 1] = GET_UDBL(src_idx,OffJointRHShoulder); frame->legAngles[3*3 + 2] = GET_UDBL(src_idx,OffJointRHKnee); //get joint duty cycles frame->legDutyCycles[0*3 + 0] = GET_PWM(src_idx,OffJointLFJoint); frame->legDutyCycles[0*3 + 1] = GET_PWM(src_idx,OffJointLFShoulder); frame->legDutyCycles[0*3 + 2] = GET_PWM(src_idx,OffJointLFKnee); frame->legDutyCycles[1*3 + 0] = GET_PWM(src_idx,OffJointRFJoint); frame->legDutyCycles[1*3 + 1] = GET_PWM(src_idx,OffJointRFShoulder); frame->legDutyCycles[1*3 + 2] = GET_PWM(src_idx,OffJointRFKnee); frame->legDutyCycles[2*3 + 0] = GET_PWM(src_idx,OffJointLHJoint); frame->legDutyCycles[2*3 + 1] = GET_PWM(src_idx,OffJointLHShoulder); frame->legDutyCycles[2*3 + 2] = GET_PWM(src_idx,OffJointLHKnee); frame->legDutyCycles[3*3 + 0] = GET_PWM(src_idx,OffJointRHJoint); frame->legDutyCycles[3*3 + 1] = GET_PWM(src_idx,OffJointRHShoulder); frame->legDutyCycles[3*3 + 2] = GET_PWM(src_idx,OffJointRHKnee); // Get head tilt,pan,roll joint anglea frame->headAngles[0] = GET_UDBL(src_idx,OffJointNeckTilt); frame->headAngles[1] = GET_UDBL(src_idx,OffJointNeckPan); frame->headAngles[2] = GET_UDBL(src_idx,OffJointNeckRoll); // Get tail angles frame->tailAngles[0] = GET_UDBL(src_idx,OffJointTailPan ); frame->tailAngles[1] = GET_UDBL(src_idx,OffJointTailTilt); // Get IR distance sensor frame->IRDistance = GET_MDBL(src_idx,OffSensorPSD); frame->IRDistChest = -1.0; // we'll set this only for ERS-7 frame->IRDistHeadNear = frame->IRDistance; frame->IRDistHeadFar = frame->IRDistance; // set again below on ERS-7. // Get foot switches frame->paw[0] = GET_BOOL(src_idx,OffSensorLFPaw); frame->paw[1] = GET_BOOL(src_idx,OffSensorRFPaw); frame->paw[2] = GET_BOOL(src_idx,OffSensorLHPaw); frame->paw[3] = GET_BOOL(src_idx,OffSensorRHPaw); // Get acceleration sensors frame->accel.x = GET_UDBL(src_idx,OffSensorAccelFB) / g; frame->accel.y = -GET_UDBL(src_idx,OffSensorAccelLR) / g; frame->accel.z = GET_UDBL(src_idx,OffSensorAccelUD) / g; frame->timestamp = time; frame->sensor_frame = frame_num; char design[256]; OPENR::GetRobotDesign(design); if(strcmp(design,"ERS-210")==0){ chin = GET_SBOOL(src_idx,OffSensorChinSwitch); back = GET_SBOOL(src_idx,OffSensorBackSwitch); head_f = (GET_UDBL(src_idx,OffSensorHeadFrontPressure) > 0.50); head_b = (GET_UDBL(src_idx,OffSensorHeadBackPressure ) > 0.75); }else{ chin = GET_IBOOL(src_idx,OffSensorChinSwitch); back = GET_IBOOL(src_idx,OffSensorBackSwitch); head_f = GET_IBOOL(src_idx,OffSensorHeadFrontPressure); head_b = GET_IBOOL(src_idx,OffSensorHeadBackPressure); back_f = GET_IBOOL(src_idx,OffSensorBackFrontSwitch); back_b = GET_IBOOL(src_idx,OffSensorBackRearSwitch); // Get wlan sensor value frame->wlan = GET_BOOL(src_idx,OffSensorWlan); // we have a chest sensor frame->IRDistChest = GET_MDBL(src_idx,OffSensorChestPSD); // near distance is already set correctly; just update far. frame->IRDistHeadFar = GET_MDBL(src_idx,OffSensorHeadFarPSD); } frame->button = 0; if(chin) frame->button |= ChinButton | ButtonTranslations[ChinButtonIdx ]; if(back) frame->button |= BackButton | ButtonTranslations[BackButtonIdx ]; if(head_f) frame->button |= HeadFrontButton | ButtonTranslations[HeadFrontButtonIdx]; if(head_b) frame->button |= HeadBackButton | ButtonTranslations[HeadBackButtonIdx ]; if(back_f) frame->button |= BackFrontButton | ButtonTranslations[BackFrontButtonIdx]; if(back_b) frame->button |= BackRearButton | ButtonTranslations[BackRearButtonIdx ]; } void SensorData::read(OSensorFrameVectorData *sensor, ulong time, ulong frame_num) { for(int src_idx=0; src_idx<4; src_idx++) { mostRecentFrame = (mostRecentFrame + 1) % NumSavedSensorFrames; read(&sensorFrames[mostRecentFrame],src_idx,sensor, time, frame_num+src_idx); } button = sensorFrames[mostRecentFrame].button; } const SensorDataFrame *SensorData::getFrame(int frame_rel_time) { int frame_time; frame_rel_time = bound(frame_rel_time,-(NumSavedSensorFrames-1),0); frame_time = mostRecentFrame + frame_rel_time; frame_time += NumSavedSensorFrames; frame_time = frame_time % NumSavedSensorFrames; return &sensorFrames[frame_time]; } const SensorDataFrame *SensorData::getByFrameNumber(ulong frame_number) { int delta = frame_number - sensorFrames[mostRecentFrame].sensor_frame; return getFrame(delta); // this does clipping for us } int SensorData::ButtonStringToNumber(const char *button_str) { if(strcmp(button_str,"HeadFrontButton")==0) return HeadFrontButtonIdx; else if(strcmp(button_str,"HeadBackButton")==0) return HeadBackButtonIdx; else if(strcmp(button_str,"ChinButton")==0) return ChinButtonIdx; else if(strcmp(button_str,"BackButton")==0) return BackButtonIdx; else if(strcmp(button_str,"BackFrontButton")==0) return BackFrontButtonIdx; else if(strcmp(button_str,"BackRearButton")==0) return BackRearButtonIdx; else return -1; } void SensorData::InitializeSensors() { ResetButtonTranslations(); Dictionary button_dict; button_dict.read("/MS/config/button.cfg"); const char *physical_button_str=""; int physical_button_num; if(button_dict.getValueString("LogButton",physical_button_str) && (physical_button_num=ButtonStringToNumber(physical_button_str))!=-1) { AddButtonTranslation(physical_button_num,LogButtonIdx); } if(button_dict.getValueString("VisionObjPlusButton",physical_button_str) && (physical_button_num=ButtonStringToNumber(physical_button_str))!=-1) { AddButtonTranslation(physical_button_num,VisionObjPlusButtonIdx); } if(button_dict.getValueString("VisionObjMinusButton",physical_button_str) && (physical_button_num=ButtonStringToNumber(physical_button_str))!=-1) { AddButtonTranslation(physical_button_num,VisionObjMinusButtonIdx); } if(button_dict.getValueString("ResetModelsButton",physical_button_str) && (physical_button_num=ButtonStringToNumber(physical_button_str))!=-1) { AddButtonTranslation(physical_button_num,ResetModelsButtonIdx); } if(button_dict.getValueString("SelNextButton",physical_button_str) && (physical_button_num=ButtonStringToNumber(physical_button_str))!=-1) { AddButtonTranslation(physical_button_num,SelNextButtonIdx); } if(button_dict.getValueString("SelPrevButton",physical_button_str) && (physical_button_num=ButtonStringToNumber(physical_button_str))!=-1) { AddButtonTranslation(physical_button_num,SelPrevButtonIdx); } } void SensorData::ResetButtonTranslations() { for(int but_idx=0; but_idx