diff --git a/app/conf/visuoTactileRF.ini b/app/conf/visuoTactileRF.ini index b3ba633..2fe33c0 100644 --- a/app/conf/visuoTactileRF.ini +++ b/app/conf/visuoTactileRF.ini @@ -1,5 +1,5 @@ name visuoTactileRF robot icub -rate 50 +rate 20 verbosity 0 modality 1D diff --git a/app/scripts/PPS_OptFlow.xml b/app/scripts/PPS_OptFlow.xml index 17301f2..2ec5dd7 100644 --- a/app/scripts/PPS_OptFlow.xml +++ b/app/scripts/PPS_OptFlow.xml @@ -162,7 +162,7 @@ tcp - /visuoTactileRF/skin_events:o + /visuoTactileRF/pps_events_aggreg:o /avoidance/data:i tcp diff --git a/app/scripts/ppsAndReactControl_LyonIntegrated.xml b/app/scripts/ppsAndReactControl_LyonIntegrated.xml new file mode 100644 index 0000000..b931c3f --- /dev/null +++ b/app/scripts/ppsAndReactControl_LyonIntegrated.xml @@ -0,0 +1,270 @@ + + PPSReactCtrl_LyonIntegrated + + + + + + reactController + + icub-b3 + + reactController + + + + skinEventsAggregator + general::robot icub + console1 + + skinEventsAggreg + + + + ppsAggregEventsForiCubGui + + console1 + + aggregEvForiCubGui + + + + pf3dTracker + --from pf3dTracker.ini + icub-b1 + PF3DTracker + + + + visuoTactileWrapper + --noDoubleTouch + icub-b2 + + + + + visuoTactileRF + --taxelsFile taxels1D_learnedAll.ini --rate 20 + icub-b2 + + + + + + yarpview + --name /PF3DT_viewer --RefreshTime 33 --x 2340 --y 275 --w 320 --h 260 --compact + rococo + PF3DTracker_viewer + + + + + skinManagerGui + --from skinManGui.ini --context skinGui --xpos 1020 --ypos 350 + console1 + skinManGui + + + + iCubSkinGui + + --from right_arm.ini --useCalibration --xpos 1305 --ypos 390 --width 300 --height 230 + rococo + skinGuiRA + + + iCubSkinGui + + --from right_forearm.ini --useCalibration --xpos 1305 --ypos 620 --width 300 --height 230 + rococo + skinGuiRF + + + + + + + + /reactController/gui:o + /iCubGui/objects + udp + + + + /skinManager/skin_events:o + /skinEventsAggregator/skin_events:i + udp + + + + /skinEventsAggregator/skin_events_aggreg:o + /reactController/skin_events_aggreg:i + udp + + + + /visuoTactileRF/pps_events_aggreg:o + /reactController/pps_events_aggreg:i + udp + + + + + /skinEventsAggregator/skin_events_aggreg:o + /ppsAggregEventsForiCubGui/skin_events_aggreg:i + udp + + + + /visuoTactileRF/pps_events_aggreg:o + /ppsAggregEventsForiCubGui/pps_events_aggreg:i + udp + + + + /ppsAggregEventsForiCubGui/contacts:o + /iCubGui/forces + udp + + + + /skinManager/skin_events:o + /visuoTactileRF/skin_events:i + tcp + + + /icub/camcalib/left/out + /pf3dTracker/video:i + tcp + + + /pf3dTracker/video:o + /PF3DT_viewer + tcp + + + /pf3dTracker/data:o + /visuoTactileWrapper/pf3dTracker:i + tcp + + + + /visuoTactileWrapper/gui:o + /iCubGui/objects + tcp + + + + /icub/camcalib/left/out + /visuoTactileRF/imageL:i + udp + + + /icub/camcalib/right/out + /visuoTactileRF/imageR:i + udp + + + + /visuoTactileWrapper/events:o + /visuoTactileRF/events:i + tcp + + + + /visuoTactileRF/skinGuiForearmL:o + /skinGui/left_forearm_virtual:i + tcp + + + /visuoTactileRF/skinGuiForearmR:o + /skinGui/right_forearm_virtual:i + tcp + + + /visuoTactileRF/skinGuiHandL:o + /skinGui/left_hand_virtual:i + tcp + + + /visuoTactileRF/skinGuiHandR:o + /skinGui/right_hand_virtual:i + tcp + + + + + /skinManager/skin_events:o + /wholeBodyDynamics/skin_contacts:i + udp + + + /skinManagerGui/rpc:o + /skinManager/rpc + tcp + + + /skinManager/monitor:o + /skinManagerGui/monitor:i + udp + + + /skinManager/info:o + /skinManagerGui/info:i + tcp + + + + /icub/skin/right_hand_comp + /skinGui/right_hand:i + udp + + + /icub/skin/right_forearm_comp + /skinGui/right_forearm:i + udp + + + /icub/skin/right_arm_comp + /skinGui/right_arm:i + udp + + + + /icub/head/state:o + /iCubGui/head:i + udp + + + + /icub/left_arm/state:o + /iCubGui/left_arm:i + udp + + + /icub/right_arm/state:o + /iCubGui/right_arm:i + udp + + + /icub/torso/state:o + /iCubGui/torso:i + udp + + + + + + + + diff --git a/app/scripts/ppsAndReactControl_icub.xml b/app/scripts/ppsAndReactControl_icub.xml new file mode 100644 index 0000000..0f172d7 --- /dev/null +++ b/app/scripts/ppsAndReactControl_icub.xml @@ -0,0 +1,202 @@ + + PPS_and_Reactcontrol + + + + + + reactController + + localhost + + reactController + + + + skinEventsAggregator + general::robot icub + localhost + + skinEventsAggreg + + + + ppsAggregEventsForiCubGui + + localhost + + aggregEvForiCubGui + + + + pf3dTracker + --from pf3dTracker.ini + localhost + PF3DTracker + + + + visuoTactileWrapper + --noDoubleTouch + localhost + + + + + visuoTactileRF + --taxelsFile taxels1D_learnedAll.ini --rate 20 + localhost + + + + + yarpview + --name /vtRF/left --RefreshTime 33 --x 0 --y 0 --w 400 --h 400 --compact + localhost + + + + yarpview + --name /vtRF/right --RefreshTime 33 --x 420 --y 0 --w 400 --h 400 --compact + localhost + + + + yarpview + --name /PF3DT_viewer --RefreshTime 33 --x 980 --y 620 --w 320 --h 260 --compact + localhost + PF3DTracker_viewer + + + + + + /reactController/gui:o + /iCubGui/objects + udp + + + + /skinManager/skin_events:o + /skinEventsAggregator/skin_events:i + udp + + + + /skinEventsAggregator/skin_events_aggreg:o + /reactController/skin_events_aggreg:i + udp + + + + /visuoTactileRF/pps_events_aggreg:o + /reactController/pps_events_aggreg:i + udp + + + + + /skinEventsAggregator/skin_events_aggreg:o + /ppsAggregEventsForiCubGui/skin_events_aggreg:i + udp + + + + /visuoTactileRF/pps_events_aggreg:o + /ppsAggregEventsForiCubGui/pps_events_aggreg:i + udp + + + + /ppsAggregEventsForiCubGui/contacts:o + /iCubGui/forces + udp + + + + /skinManager/skin_events:o + /visuoTactileRF/skin_events:i + tcp + + + /icub/camcalib/left/out + /pf3dTracker/video:i + tcp + + + /pf3dTracker/video:o + /PF3DT_viewer + tcp + + + /pf3dTracker/data:o + /visuoTactileWrapper/pf3dTracker:i + tcp + + + + /visuoTactileWrapper/gui:o + /iCubGui/objects + tcp + + + + /icub/camcalib/left/out + /visuoTactileRF/imageL:i + udp + + + /icub/camcalib/right/out + /visuoTactileRF/imageR:i + udp + + + + /visuoTactileWrapper/events:o + /visuoTactileRF/events:i + tcp + + + + /visuoTactileRF/skinGuiForearmL:o + /skinGui/left_forearm_virtual:i + tcp + + + /visuoTactileRF/skinGuiForearmR:o + /skinGui/right_forearm_virtual:i + tcp + + + /visuoTactileRF/skinGuiHandL:o + /skinGui/left_hand_virtual:i + tcp + + + /visuoTactileRF/skinGuiHandR:o + /skinGui/right_hand_virtual:i + tcp + + + /visuoTactileRF/imageL:o + /vtRF/left + udp + + + /visuoTactileRF/imageR:o + /vtRF/right + udp + + + + diff --git a/app/scripts/ppsAndReactControl_icubSim.xml b/app/scripts/ppsAndReactControl_icubSim.xml new file mode 100644 index 0000000..6be02a0 --- /dev/null +++ b/app/scripts/ppsAndReactControl_icubSim.xml @@ -0,0 +1,101 @@ + + PPS_and_Reactcontrol_icubSim + + + + + + iCub_SIM + + localhost + + iCub_SIM + + + + iCubGui + + localhost + + iCubGui + + + + reactController + + localhost + + reactController + + + + virtualContactGeneration + + localhost + + virtContactGen + + + + skinEventsAggregator + + localhost + + skinEventsAggreg + + + + ppsAggregEventsForiCubGui + + localhost + + aggregEvForiCubGui + + + + + + + /virtualContactGeneration/virtualContacts:o + /skinEventsAggregator/skin_events:i + udp + + + + /skinEventsAggregator/skin_events_aggreg:o + /reactController/skin_events_aggreg:i + udp + + + + + /skinEventsAggregator/skin_events_aggreg:o + /ppsAggregEventsForiCubGui/skin_events_aggreg:i + udp + + + + /visuoTactileRF/pps_events_aggreg:o + /ppsAggregEventsForiCubGui/pps_events_aggreg:i + udp + + + + /ppsAggregEventsForiCubGui/contacts:o + /iCubGui/forces + udp + + + + + diff --git a/app/scripts/ppsAndReactControl_icub_Lyon.xml b/app/scripts/ppsAndReactControl_icub_Lyon.xml new file mode 100644 index 0000000..2fd8525 --- /dev/null +++ b/app/scripts/ppsAndReactControl_icub_Lyon.xml @@ -0,0 +1,185 @@ + + PPS_and_Reactcontrol_Lyon + + + + + + reactController + + console1 + + reactController + + + + skinEventsAggregator + general::robot icub + console1 + + skinEventsAggreg + + + + ppsAggregEventsForiCubGui + + console1 + + aggregEvForiCubGui + + + + pf3dTracker + --from pf3dTracker.ini + icub-b1 + PF3DTracker + + + + visuoTactileWrapper + --noDoubleTouch + icub-b2 + + + + + visuoTactileRF + --taxelsFile taxels1D_learnedAll.ini --rate 20 + icub-b2 + + + + + + yarpview + --name /PF3DT_viewer --RefreshTime 33 --x 2000 --y 750 --w 320 --h 260 --compact + rococo + PF3DTracker_viewer + + + + + + /reactController/gui:o + /iCubGui/objects + udp + + + + /skinManager/skin_events:o + /skinEventsAggregator/skin_events:i + udp + + + + /skinEventsAggregator/skin_events_aggreg:o + /reactController/skin_events_aggreg:i + udp + + + + /visuoTactileRF/pps_events_aggreg:o + /reactController/pps_events_aggreg:i + udp + + + + + /skinEventsAggregator/skin_events_aggreg:o + /ppsAggregEventsForiCubGui/skin_events_aggreg:i + udp + + + + /visuoTactileRF/pps_events_aggreg:o + /ppsAggregEventsForiCubGui/pps_events_aggreg:i + udp + + + + /ppsAggregEventsForiCubGui/contacts:o + /iCubGui/forces + udp + + + + /skinManager/skin_events:o + /visuoTactileRF/skin_events:i + tcp + + + /icub/camcalib/left/out + /pf3dTracker/video:i + tcp + + + /pf3dTracker/video:o + /PF3DT_viewer + tcp + + + /pf3dTracker/data:o + /visuoTactileWrapper/pf3dTracker:i + tcp + + + + /visuoTactileWrapper/gui:o + /iCubGui/objects + tcp + + + + /icub/camcalib/left/out + /visuoTactileRF/imageL:i + udp + + + /icub/camcalib/right/out + /visuoTactileRF/imageR:i + udp + + + + /visuoTactileWrapper/events:o + /visuoTactileRF/events:i + tcp + + + + /visuoTactileRF/skinGuiForearmL:o + /skinGui/left_forearm_virtual:i + tcp + + + /visuoTactileRF/skinGuiForearmR:o + /skinGui/right_forearm_virtual:i + tcp + + + /visuoTactileRF/skinGuiHandL:o + /skinGui/left_hand_virtual:i + tcp + + + /visuoTactileRF/skinGuiHandR:o + /skinGui/right_hand_virtual:i + tcp + + + + + + + + diff --git a/app/scripts/ppsAndReactControl_ultimateTracker_icub.xml b/app/scripts/ppsAndReactControl_ultimateTracker_icub.xml new file mode 100644 index 0000000..5525d7f --- /dev/null +++ b/app/scripts/ppsAndReactControl_ultimateTracker_icub.xml @@ -0,0 +1,252 @@ + + PPS_and_Reactcontrol_ultimTracker + + + + + + reactController + + localhost + + reactController + + + + skinEventsAggregator + general::robot icub + localhost + + skinEventsAggreg + + + + ppsAggregEventsForiCubGui + + localhost + + aggregEvForiCubGui + + + + visuoTactileWrapper + --noDoubleTouch + localhost + + + + + visuoTactileRF + --taxelsFile taxels1D_learnedAll.ini --rate 20 + localhost + + + + + motionCUT + --recogThres 0.015 --blobMinSizeThres 20 --framesPersistence 3 --cropSize + localhost + + + + templatePFTracker + + localhost + + + + SFM + + localhost + + + + ultimateTracker + + localhost + + + + + yarpview + --name /motionCUT_viewer --RefreshTime 33 --x 980 --y 340 --w 320 --h 260 --compact + localhost + + + yarpview + --name /templatePFTracker_viewer --RefreshTime 33 --x 1280 --y 60 --w 320 --h 260 --compact + localhost + + + yarpview + --name /dispViewer --RefreshTime 33 --x 1600 --y 60 --w 320 --h 260 --compact + localhost + + + + yarpview + --name /vtRF/left --RefreshTime 33 --x 0 --y 0 --w 400 --h 400 --compact + localhost + + + + yarpview + --name /vtRF/right --RefreshTime 33 --x 420 --y 0 --w 400 --h 400 --compact + localhost + + + + + + + /reactController/gui:o + /iCubGui/objects + udp + + + + /skinManager/skin_events:o + /skinEventsAggregator/skin_events:i + udp + + + + /skinEventsAggregator/skin_events_aggreg:o + /reactController/skin_events_aggreg:i + udp + + + + /visuoTactileRF/pps_events_aggreg:o + /reactController/pps_events_aggreg:i + udp + + + + + /skinEventsAggregator/skin_events_aggreg:o + /ppsAggregEventsForiCubGui/skin_events_aggreg:i + udp + + + + /visuoTactileRF/pps_events_aggreg:o + /ppsAggregEventsForiCubGui/pps_events_aggreg:i + udp + + + + /ppsAggregEventsForiCubGui/contacts:o + /iCubGui/forces + udp + + + + /skinManager/skin_events:o + /visuoTactileRF/skin_events:i + tcp + + + + /visuoTactileWrapper/gui:o + /iCubGui/objects + tcp + + + + /icub/camcalib/left/out + /visuoTactileRF/imageL:i + udp + + + /icub/camcalib/right/out + /visuoTactileRF/imageR:i + udp + + + + /visuoTactileWrapper/events:o + /visuoTactileRF/events:i + tcp + + + + /icub/camcalib/left/out + /motionCUT/img:i + udp + + + /motionCUT/img:o + /motionCUT_viewer + udp + + + /icub/camcalib/right/out + /SFM/right:i + tcp + + + /icub/camcalib/left/out + /SFM/left:i + tcp + + + /SFM/disp:o + /dispViewer + tcp + + + /icub/camcalib/left/out + /templatePFTracker/left/image:i + udp + + + /templatePFTracker/left/image:o + /templatePFTracker_viewer + udp + + + + + /visuoTactileRF/skinGuiForearmL:o + /skinGui/left_forearm_virtual:i + tcp + + + /visuoTactileRF/skinGuiForearmR:o + /skinGui/right_forearm_virtual:i + tcp + + + /visuoTactileRF/skinGuiHandL:o + /skinGui/left_hand_virtual:i + tcp + + + /visuoTactileRF/skinGuiHandR:o + /skinGui/right_hand_virtual:i + tcp + + + /visuoTactileRF/imageL:o + /vtRF/left + udp + + + /visuoTactileRF/imageR:o + /vtRF/right + udp + + + + diff --git a/app/scripts/ppsAndReactCtrl_log_icub.xml b/app/scripts/ppsAndReactCtrl_log_icub.xml new file mode 100644 index 0000000..3a597aa --- /dev/null +++ b/app/scripts/ppsAndReactCtrl_log_icub.xml @@ -0,0 +1,380 @@ + + PPSandReactrl_Logging + + + + + + yarpdatadumper + --name /data/reactCtrl --txTime --rxTime + localhost + + dataDumperReactCtrl + + + + yarpdatadumper + --name /data/skinEvAggreg --txTime --rxTime + localhost + + dataDumperSkinEvAggreg + + + + yarpdatadumper + --name /data/iCubGuiObjects --txTime --rxTime + localhost + + dataDumperObjectsiCubGui + + + + yarpdatadumper + --name /data/ppsAggregEventsForiCubGui --txTime --rxTime + localhost + + dataDumperPPSeventsForiCubGui + + + + yarpdatadumper + --name /data/skin_events --txTime --rxTime + localhost + + dataDumperSkinContact + + + + yarpdatadumper + --name /data/skin_tactile_comp_left_hand --txTime --rxTime + localhost + + dataDumperSkinLeftHandComp + + + yarpdatadumper + --name /data/skin_tactile_comp_left_forearm --txTime --rxTime + localhost + + dataDumperSkinLeftForearmComp + + + yarpdatadumper + --name /data/skin_tactile_comp_left_arm --txTime --rxTime + localhost + + dataDumperSkinLeftArmComp + + + yarpdatadumper + --name /data/skin_tactile_comp_right_hand --txTime --rxTime + localhost + + dataDumperSkinRightHandComp + + + yarpdatadumper + --name /data/skin_tactile_comp_right_forearm --txTime --rxTime + localhost + + dataDumperSkinRightForearmComp + + + yarpdatadumper + --name /data/skin_tactile_comp_right_arm --txTime --rxTime + localhost + + dataDumperSkinRightArmComp + + + + + yarpdatadumper + --name /data/joints_head --txTime --rxTime + localhost + + dataDumperHeadJoints + + + yarpdatadumper + --name /data/joints_torso --txTime --rxTime + localhost + + dataDumperTorsoJoints + + + yarpdatadumper + --name /data/joints_leftArm --txTime --rxTime + localhost + + dataDumperLeftArmJoints + + + yarpdatadumper + --name /data/joints_rightArm --txTime --rxTime + localhost + + dataDumperRightArmJoints + + + + yarpdatadumper + --name /data/pps_dumper --txTime --rxTime + localhost + + dataDumperPPS + + + yarpdatadumper + --name /data/ppsEvAggreg --txTime --rxTime + localhost + + dataDumperPPSevAggreg + + + /visuoTactileRF/pps_events_aggreg:o + + + yarpdatadumper + --name /data/pps_skinGui_leftHand_dumper --txTime --rxTime + localhost + + dataDumperPPSlefttHandSkinGui + + + yarpdatadumper + --name /data/pps_skinGui_rightHand_dumper --txTime --rxTime + localhost + + dataDumperPPSrightHandSkinGui + + + yarpdatadumper + --name /data/pps_skinGui_leftForearm_dumper --txTime --rxTime + localhost + + dataDumperPPSleftForearmSkinGui + + + yarpdatadumper + --name /data/pps_skinGui_rightForearm_dumper --txTime --rxTime + localhost + + dataDumperPPSrightForearmSkinGui + + + + yarpdatadumper + --name /data/pps_wrapper_icubGui --txTime --rxTime + localhost + + dataDumperPPSwrapperiCubGui + + + + + + + + + + + + + + + /reactController/data:o + /data/reactCtrl + udp + + + + /skinEventsAggregator/skin_events_aggreg:o + /data/skinEvAggreg + udp + + + + /reactController/gui:o + /data/iCubGuiObjects + udp + + + + /ppsAggregEventsForiCubGui/contacts:o + /data/ppsAggregEventsForiCubGui + udp + + + + /skinManager/skin_events:o + /data/skin_events + udp + + + /icub/skin/left_hand_comp + /data/skin_tactile_comp_left_hand + udp + + + /icub/skin/left_forearm_comp + /data/skin_tactile_comp_left_forearm + udp + + + /icub/skin/left_arm_comp + /data/skin_tactile_comp_left_arm + udp + + + /icub/skin/right_hand_comp + /data/skin_tactile_comp_right_hand + udp + + + /icub/skin/right_forearm_comp + /data/skin_tactile_comp_right_forearm + udp + + + /icub/skin/right_arm_comp + /data/skin_tactile_comp_right_arm + udp + + + + + /icub/head/state:o + /data/joints_head + udp + + + /icub/torso/state:o + /data/joints_torso + udp + + + /icub/left_arm/state:o + /data/joints_leftArm + udp + + + /icub/right_arm/state:o + /data/joints_rightArm + udp + + + + /visuoTactileRF/dataDumper:o + /data/pps_dumper + udp + + + /visuoTactileRF/pps_events_aggreg:o + /data/ppsEvAggreg + udp + + + + + /visuoTactileRF/skinGuiHandL:o + /data/pps_skinGui_leftHand_dumper + udp + + + /visuoTactileRF/skinGuiHandR:o + /data/pps_skinGui_rightHand_dumper + udp + + + /visuoTactileRF/skinGuiForearmL:o + /data/pps_skinGui_leftForearm_dumper + udp + + + /visuoTactileRF/skinGuiForearmR:o + /data/pps_skinGui_rightForearm_dumper + udp + + + /visuoTactileWrapper/gui:o + /data/pps_wrapper_icubGui + udp + + + + + + + + + + + + + + diff --git a/app/scripts/pps_skinAndiCubGuis_Lyon.xml b/app/scripts/pps_skinAndiCubGuis_Lyon.xml new file mode 100644 index 0000000..5d51ea2 --- /dev/null +++ b/app/scripts/pps_skinAndiCubGuis_Lyon.xml @@ -0,0 +1,123 @@ + + PPS_skiunAndiCubGuis_Lyon + + + + + + + skinManager + --from skinManAll.ini --context skinGui + console1 + skinMan + + + + skinManagerGui + --from skinManGui.ini --context skinGui --xpos 1020 --ypos 350 + console1 + skinManGui + + + + iCubSkinGui + + --from right_arm.ini --useCalibration --xpos 1750 --ypos 0 --width 250 --height 250 + rococo + skinGuiRA + + + iCubSkinGui + + --from right_forearm.ini --useCalibration --xpos 1750 --ypos 250 --width 250 --height 250 + rococo + skinGuiRF + + + iCubSkinGui + + --from right_hand_V2_1.ini --useCalibration --xpos 1750 --ypos 500 --width 250 --height 250 + rococo + skinGuiRH + + + + iCubGui + --xpos 2000 --width 500 --height 700 + rococo + + iCubGui + + + + + + + + /skinManager/skin_events:o + /wholeBodyDynamics/skin_contacts:i + udp + + + /skinManagerGui/rpc:o + /skinManager/rpc + tcp + + + /skinManager/monitor:o + /skinManagerGui/monitor:i + udp + + + /skinManager/info:o + /skinManagerGui/info:i + tcp + + + + /icub/skin/right_hand_comp + /skinGui/right_hand:i + udp + + + /icub/skin/right_forearm_comp + /skinGui/right_forearm:i + udp + + + /icub/skin/right_arm_comp + /skinGui/right_arm:i + udp + + + + + + + /icub/head/state:o + /iCubGui/head:i + udp + + + + /icub/left_arm/state:o + /iCubGui/left_arm:i + udp + + + /icub/right_arm/state:o + /iCubGui/right_arm:i + udp + + + /icub/torso/state:o + /iCubGui/torso:i + udp + + + + diff --git a/modules/CMakeLists.txt b/modules/CMakeLists.txt index 3032bec..df43927 100644 --- a/modules/CMakeLists.txt +++ b/modules/CMakeLists.txt @@ -12,6 +12,8 @@ add_subdirectory(ultimateTracker) #add_subdirectory(tactileServoing) #add_subdirectory(fingertipTracker) add_subdirectory(virtualContactGeneration) +add_subdirectory(skinEventsAggregator) +add_subdirectory(ppsAggregEventsForiCubGui) #add_subdirectory(selfCalibration) #add_subdirectory(alignEyes) add_subdirectory(testSkinDynLib) diff --git a/modules/demoAvoidance/demoAvoidance.cpp b/modules/demoAvoidance/demoAvoidance.cpp index a2bbefb..3daa4b8 100644 --- a/modules/demoAvoidance/demoAvoidance.cpp +++ b/modules/demoAvoidance/demoAvoidance.cpp @@ -28,6 +28,7 @@ #include #include +#include #define PPS_AVOIDANCE_RADIUS 0.2 // [m] #define PPS_AVOIDANCE_VELOCITY 0.10 // [m/s] @@ -51,7 +52,10 @@ class Avoidance: public RFModule, PolyDriver driverJointL,driverJointR; int contextL,contextR; Port dataPort; - int motionGain; + double motionGain; + + bool useLeftArm; + bool useRightArm; RpcServer rpcSrvr; @@ -70,28 +74,47 @@ class Avoidance: public RFModule, //******************************************** bool read(ConnectionReader &connection) { - Bottle input; - input.read(connection); - if (input.size()>=7) + Bottle in; + in.read(connection); + + if (in.size()>=1) { - string arm=input.get(0).asString().c_str(); - transform(arm.begin(),arm.end(),arm.begin(),::tolower); - mutex.lock(); - map::iterator it=data.find(arm); - if (it!=data.end()) + Bottle input; + input=*(in.get(0).asList()); + + if (input.size()>=7) { - Data &d=it->second; - d.point[0]=input.get(1).asDouble(); - d.point[1]=input.get(2).asDouble(); - d.point[2]=input.get(3).asDouble(); - d.dir[0]=input.get(4).asDouble(); - d.dir[1]=input.get(5).asDouble(); - d.dir[2]=input.get(6).asDouble(); - d.persistence=PPS_AVOIDANCE_PERSISTENCE; - d.timeout=PPS_AVOIDANCE_TIMEOUT; + yDebug("[demoAvoidance] got command (%s)",input.toString().c_str()); + string arm=iCub::skinDynLib::SkinPart_s[input.get(0).asInt()]; + + if (arm == "r_hand" || arm == "r_forearm") + { + arm = "right"; + } + else if (arm == "l_hand" || arm == "l_forearm") + { + arm = "left"; + } + + transform(arm.begin(),arm.end(),arm.begin(),::tolower); + mutex.lock(); + map::iterator it=data.find(arm); + if (it!=data.end()) + { + Data &d=it->second; + d.point[0]=input.get(7).asDouble(); + d.point[1]=input.get(8).asDouble(); + d.point[2]=input.get(9).asDouble(); + d.dir[0]=input.get(10).asDouble(); + d.dir[1]=input.get(11).asDouble(); + d.dir[2]=input.get(12).asDouble(); + d.persistence=PPS_AVOIDANCE_PERSISTENCE; + d.timeout=PPS_AVOIDANCE_TIMEOUT; + } + mutex.unlock(); } - mutex.unlock(); } + return true; } @@ -182,17 +205,14 @@ class Avoidance: public RFModule, //******************************************** bool configure(ResourceFinder &rf) { - data["left"]=Data(); - data["left"].home_x[0]=-0.30; - data["left"].home_x[1]=-0.20; - data["left"].home_x[2]=+0.05; - - Matrix R(4,4); - R(0,0)=-1.0; R(2,1)=-1.0; R(1,2)=-1.0; R(3,3)=1.0; - data["left"].home_o=dcm2axis(R); + if (rf.check("noLeftArm") && rf.check("noRightArm")) + { + yError("[demoAvoidance] no arm has been selected. Closing."); + return false; + } - data["right"]=data["left"]; - data["right"].home_x[1]=-data["right"].home_x[1]; + useLeftArm = false; + useRightArm = false; string name=rf.check("name",Value("avoidance")).asString().c_str(); string robot=rf.check("robot",Value("icub")).asString().c_str(); @@ -203,82 +223,125 @@ class Avoidance: public RFModule, } if (motionGain!=-1.0) { - yWarning("motionGain set to catching"); + yWarning("[demoAvoidance] motionGain set to catching"); } bool autoConnect=rf.check("autoConnect"); if (autoConnect) { - yWarning("Autoconnect mode set to ON"); + yWarning("[demoAvoidance] Autoconnect mode set to ON"); } bool stiff=rf.check("stiff"); if (stiff) { - yInfo("Stiff Mode enabled."); + yInfo("[demoAvoidance] Stiff Mode enabled."); } - Property optionCartL; - optionCartL.put("device","cartesiancontrollerclient"); - optionCartL.put("remote","/"+robot+"/cartesianController/left_arm"); - optionCartL.put("local",("/"+name+"/cart/left_arm").c_str()); - if (!driverCartL.open(optionCartL)) - { - close(); - return false; - } + Matrix R(4,4); + R(0,0)=-1.0; R(2,1)=-1.0; R(1,2)=-1.0; R(3,3)=1.0; - Property optionCartR; - optionCartR.put("device","cartesiancontrollerclient"); - optionCartR.put("remote","/"+robot+"/cartesianController/right_arm"); - optionCartR.put("local",("/"+name+"/cart/right_arm").c_str()); - if (!driverCartR.open(optionCartR)) + if (!rf.check("noLeftArm")) { - close(); - return false; - } + useLeftArm=true; + + data["left"]=Data(); + data["left"].home_x[0]=-0.30; + data["left"].home_x[1]=-0.20; + data["left"].home_x[2]=+0.05; + data["left"].home_o=dcm2axis(R); + + Property optionCartL; + optionCartL.put("device","cartesiancontrollerclient"); + optionCartL.put("remote","/"+robot+"/cartesianController/left_arm"); + optionCartL.put("local",("/"+name+"/cart/left_arm").c_str()); + if (!driverCartL.open(optionCartL)) + { + close(); + return false; + } - Property optionJointL; - optionJointL.put("device","remote_controlboard"); - optionJointL.put("remote","/"+robot+"/left_arm"); - optionJointL.put("local",("/"+name+"/joint/left_arm").c_str()); - if (!driverJointL.open(optionJointL)) - { - close(); - return false; + Property optionJointL; + optionJointL.put("device","remote_controlboard"); + optionJointL.put("remote","/"+robot+"/left_arm"); + optionJointL.put("local",("/"+name+"/joint/left_arm").c_str()); + if (!driverJointL.open(optionJointL)) + { + close(); + return false; + } + + driverCartL.view(data["left"].iarm); + data["left"].iarm->storeContext(&contextL); + + Vector dof; + data["left"].iarm->getDOF(dof); + dof=0.0; dof[3]=dof[4]=dof[5]=dof[6]=1.0; + data["left"].iarm->setDOF(dof,dof); + data["left"].iarm->setTrajTime(0.9); + + data["left"].iarm->goToPoseSync(data["left"].home_x,data["left"].home_o); + data["left"].iarm->waitMotionDone(); + + IInteractionMode *imode; driverJointL.view(imode); + IImpedanceControl *iimp; driverJointL.view(iimp); + + if (!stiff) + { + imode->setInteractionMode(0,VOCAB_IM_COMPLIANT); iimp->setImpedance(0,0.4,0.03); + imode->setInteractionMode(1,VOCAB_IM_COMPLIANT); iimp->setImpedance(1,0.4,0.03); + imode->setInteractionMode(2,VOCAB_IM_COMPLIANT); iimp->setImpedance(2,0.4,0.03); + imode->setInteractionMode(3,VOCAB_IM_COMPLIANT); iimp->setImpedance(3,0.2,0.01); + imode->setInteractionMode(4,VOCAB_IM_COMPLIANT); iimp->setImpedance(4,0.2,0.0); + } } - Property optionJointR; - optionJointR.put("device","remote_controlboard"); - optionJointR.put("remote","/"+robot+"/right_arm"); - optionJointR.put("local",("/"+name+"/joint/right_arm").c_str()); - if (!driverJointR.open(optionJointR)) + if (!rf.check("noRightArm")) { - close(); - return false; - } + useRightArm = true; + + data["right"]=Data(); + data["right"].home_x[0]=-0.30; + data["right"].home_x[1]=+0.20; + data["right"].home_x[2]=+0.05; + data["right"].home_o=dcm2axis(R); + + Property optionCartR; + optionCartR.put("device","cartesiancontrollerclient"); + optionCartR.put("remote","/"+robot+"/cartesianController/right_arm"); + optionCartR.put("local",("/"+name+"/cart/right_arm").c_str()); + if (!driverCartR.open(optionCartR)) + { + close(); + return false; + } - driverCartL.view(data["left"].iarm); - driverCartR.view(data["right"].iarm); + Property optionJointR; + optionJointR.put("device","remote_controlboard"); + optionJointR.put("remote","/"+robot+"/right_arm"); + optionJointR.put("local",("/"+name+"/joint/right_arm").c_str()); + if (!driverJointR.open(optionJointR)) + { + close(); + return false; + } - data["left"].iarm->storeContext(&contextL); - data["right"].iarm->storeContext(&contextR); + driverCartR.view(data["right"].iarm); + data["right"].iarm->storeContext(&contextR); - Vector dof; - data["left"].iarm->getDOF(dof); - dof=0.0; dof[3]=dof[4]=dof[5]=dof[6]=1.0; - data["left"].iarm->setDOF(dof,dof); data["right"].iarm->setDOF(dof,dof); - data["left"].iarm->setTrajTime(0.9); data["right"].iarm->setTrajTime(0.9); + Vector dof; + data["right"].iarm->getDOF(dof); + dof=0.0; dof[3]=dof[4]=dof[5]=dof[6]=1.0; + data["right"].iarm->setDOF(dof,dof); + data["right"].iarm->setTrajTime(0.9); - data["left"].iarm->goToPoseSync(data["left"].home_x,data["left"].home_o); - data["right"].iarm->goToPoseSync(data["right"].home_x,data["right"].home_o); - data["left"].iarm->waitMotionDone(); - data["right"].iarm->waitMotionDone(); + data["right"].iarm->goToPoseSync(data["right"].home_x,data["right"].home_o); + data["right"].iarm->waitMotionDone(); + + + IInteractionMode *imode; driverJointR.view(imode); + IImpedanceControl *iimp; driverJointR.view(iimp); - IInteractionMode *imode; driverJointL.view(imode); - IImpedanceControl *iimp; driverJointL.view(iimp); - for (int i=0; i<2; i++) - { if (!stiff) { imode->setInteractionMode(0,VOCAB_IM_COMPLIANT); iimp->setImpedance(0,0.4,0.03); @@ -287,9 +350,6 @@ class Avoidance: public RFModule, imode->setInteractionMode(3,VOCAB_IM_COMPLIANT); iimp->setImpedance(3,0.2,0.01); imode->setInteractionMode(4,VOCAB_IM_COMPLIANT); iimp->setImpedance(4,0.2,0.0); } - - driverJointR.view(imode); - driverJointR.view(iimp); } dataPort.open(("/"+name+"/data:i").c_str()); @@ -309,8 +369,15 @@ class Avoidance: public RFModule, bool updateModule() { mutex.lock(); - manageArm(data["left"]); - manageArm(data["right"]); + if (useLeftArm) + { + manageArm(data["left"]); + } + + if (useRightArm) + { + manageArm(data["right"]); + } mutex.unlock(); return true; } @@ -324,40 +391,48 @@ class Avoidance: public RFModule, //******************************************** bool close() { - dataPort.close(); + yInfo("[demoAvoidance] Closing module.."); - if (driverCartL.isValid()) - { - data["left"].iarm->stopControl(); - data["left"].iarm->restoreContext(contextL); - driverCartL.close(); - } + dataPort.close(); - if (driverCartR.isValid()) + if (useLeftArm) { - data["right"].iarm->stopControl(); - data["right"].iarm->restoreContext(contextR); - driverCartR.close(); - } + if (driverCartL.isValid()) + { + data["left"].iarm->stopControl(); + data["left"].iarm->restoreContext(contextL); + driverCartL.close(); + } - if (driverJointL.isValid()) - { - IInteractionMode *imode; - driverJointL.view(imode); - for (int j=0; j<5; j++) - imode->setInteractionMode(j,VOCAB_IM_STIFF); + if (driverJointL.isValid()) + { + IInteractionMode *imode; + driverJointL.view(imode); + for (int j=0; j<5; j++) + imode->setInteractionMode(j,VOCAB_IM_STIFF); - driverJointL.close(); + driverJointL.close(); + } } - if (driverJointR.isValid()) + if (useRightArm) { - IInteractionMode *imode; - driverJointR.view(imode); - for (int j=0; j<5; j++) - imode->setInteractionMode(j,VOCAB_IM_STIFF); + if (driverCartR.isValid()) + { + data["right"].iarm->stopControl(); + data["right"].iarm->restoreContext(contextR); + driverCartR.close(); + } + + if (driverJointR.isValid()) + { + IInteractionMode *imode; + driverJointR.view(imode); + for (int j=0; j<5; j++) + imode->setInteractionMode(j,VOCAB_IM_STIFF); - driverJointR.close(); + driverJointR.close(); + } } return true; @@ -386,6 +461,8 @@ int main(int argc, char * argv[]) yInfo(" --autoConnect flag: if to auto connect the ports or not. Default no."); yInfo(" --catching flag: if enabled, the robot will catch the target instead of avoiding it."); yInfo(" --stiff flag: if enabled, the robot will perform movements in stiff mode instead of compliant."); + yInfo(" --noLeftArm flag: if enabled, the robot will perform movements without the left arm."); + yInfo(" --noRightArm flag: if enabled, the robot will perform movements without the rihgt arm."); yInfo(""); return 0; } diff --git a/modules/doubleTouch/iCubDblTchThrd.cpp b/modules/doubleTouch/iCubDblTchThrd.cpp index 42bc9d8..8b1a6d2 100644 --- a/modules/doubleTouch/iCubDblTchThrd.cpp +++ b/modules/doubleTouch/iCubDblTchThrd.cpp @@ -705,7 +705,7 @@ bool doubleTouchThread::detectContact(skinContactList *_sCL) if( it -> getPressure() > 25 && norm(it-> getNormalDir()) != 0.0) { - for (int i = 0; i < skinParts.size(); i++) + for (size_t i = 0; i < skinParts.size(); i++) { if (skinParts[i] == it -> getSkinPart()) { diff --git a/modules/doubleTouch/iCubDoubleTouch.cpp b/modules/doubleTouch/iCubDoubleTouch.cpp index a31af50..10b6c5c 100644 --- a/modules/doubleTouch/iCubDoubleTouch.cpp +++ b/modules/doubleTouch/iCubDoubleTouch.cpp @@ -145,7 +145,7 @@ class doubleTouch: public RFModule name = "doubleTouch"; type = "LtoR"; filename = ".txt"; - color = ""; + color = " "; verbosity = 0; // verbosity rate = 100; // rate of the doubleTouchThread @@ -496,9 +496,9 @@ int main(int argc, char * argv[]) if (rf.check("help")) { - yInfo(""); + yInfo(" "); yInfo("Options:"); - yInfo(""); + yInfo(" "); yInfo(" --context path: where to find the called resource"); yInfo(" --from from: the name of the .ini file."); yInfo(" --name name: the name of the module (default doubleTouch)."); @@ -522,7 +522,7 @@ int main(int argc, char * argv[]) yInfo(" --autoconnect flag: if or not to autoconnect to the skinManager"); yInfo(" --jnt_vels double: specify the joint level speed during the double touch. Default 4[deg/s]."); yInfo(" --alignEyes flag: if or not to use the rpc-thing and sync with alignEyes module."); - yInfo(""); + yInfo(" "); return 0; } diff --git a/modules/fingertipTracker/fgtThread.cpp b/modules/fingertipTracker/fgtThread.cpp index 4e01bf8..9b80c17 100644 --- a/modules/fingertipTracker/fgtThread.cpp +++ b/modules/fingertipTracker/fgtThread.cpp @@ -1,3 +1,4 @@ +#include #include "fgtThread.h" fgtThread::fgtThread(int _rate, const string &_name, const string &_robot, int _v, @@ -139,16 +140,16 @@ bool fgtThread::processImages(ImageOf &_oL, ImageOf &_oR) ImageOf imageOL=*imageInL; ImageOf imageOR=*imageInR; - cv::Mat imgL((IplImage*)imageOL.getIplImage()); - cv::Mat imgR((IplImage*)imageOR.getIplImage()); - cv::Mat mskL((IplImage*)maskL.getIplImage()); - cv::Mat mskR((IplImage*)maskR.getIplImage()); + cv::Mat imgL=cv::cvarrToMat(imageOL.getIplImage()); + cv::Mat imgR=cv::cvarrToMat(imageOR.getIplImage()); + cv::Mat mskL=cv::cvarrToMat(maskL.getIplImage()); + cv::Mat mskR=cv::cvarrToMat(maskR.getIplImage()); cv::Mat imgLHSV; cv::Mat imgRHSV; yTrace("I'm converting the images to in HSV"); - cvtColor(imgL,imgLHSV,CV_RGB2HSV); - cvtColor(imgR,imgRHSV,CV_BGR2HSV); + cv::cvtColor(imgL,imgLHSV,CV_RGB2HSV); + cv::cvtColor(imgR,imgRHSV,CV_RGB2HSV); yTrace("I'm filtering according to the HSV"); cv::inRange(imgLHSV, cv::Scalar(HSVmin[0],HSVmin[1],HSVmin[2]), cv::Scalar(HSVmax[0],HSVmax[1],HSVmax[2]),mskL); @@ -177,7 +178,7 @@ bool fgtThread::processImages(ImageOf &_oL, ImageOf &_oR) area=cv::contourArea(contours[i]); if (area>largestArea) { - largestArea=area; + largestArea=(int)area; idx=i; } } @@ -208,7 +209,7 @@ bool fgtThread::processImages(ImageOf &_oL, ImageOf &_oR) area=cv::contourArea(contours[i]); if (area>largestArea) { - largestArea=area; + largestArea=(int)area; idx=i; } } diff --git a/modules/fingertipTracker/fgtThread.h b/modules/fingertipTracker/fgtThread.h index 9c79494..c92a392 100644 --- a/modules/fingertipTracker/fgtThread.h +++ b/modules/fingertipTracker/fgtThread.h @@ -41,8 +41,8 @@ #include #include -#include -#include +#include + #include diff --git a/modules/ppsAggregEventsForiCubGui/CMakeLists.txt b/modules/ppsAggregEventsForiCubGui/CMakeLists.txt new file mode 100644 index 0000000..f5ca785 --- /dev/null +++ b/modules/ppsAggregEventsForiCubGui/CMakeLists.txt @@ -0,0 +1,22 @@ +# Copyright: (C) 2015 iCub Facility - Istituto Italiano di Tecnologia +# Author: Matej Hoffmann +# CopyPolicy: Released under the terms of the GNU GPL v2.0. + +cmake_minimum_required(VERSION 2.8) +project(ppsAggregEventsForiCubGui) + +file(GLOB source *.cpp) +file(GLOB header *.h) + +include_directories(${YARP_INCLUDE_DIRS}) +include_directories(${periPersonalSpace_INCLUDE_DIRS}) + +source_group("Source Files" FILES ${source}) +source_group("Header Files" FILES ${header}) + +add_executable(ppsAggregEventsForiCubGui ${source} ${header} ) + +target_link_libraries(ppsAggregEventsForiCubGui periPersonalSpace ${YARP_LIBRARIES}) + +install(TARGETS ppsAggregEventsForiCubGui DESTINATION bin) + diff --git a/modules/ppsAggregEventsForiCubGui/ppsAggregEventsForiCubGui.cpp b/modules/ppsAggregEventsForiCubGui/ppsAggregEventsForiCubGui.cpp new file mode 100644 index 0000000..62db1d2 --- /dev/null +++ b/modules/ppsAggregEventsForiCubGui/ppsAggregEventsForiCubGui.cpp @@ -0,0 +1,325 @@ +/* + * Copyright (C) 2016 iCub Facility - Istituto Italiano di Tecnologia + * Author: Matej Hoffmann + * email: matej.hoffmann@iit.it + * Permission is granted to copy, distribute, and/or modify this program + * under the terms of the GNU General Public License, version 2 or any + * later version published by the Free Software Foundation. + * + * A copy of the license can be found at + * http://www.robotcub.org/icub/license/gpl.txt + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details +*/ + +/** +\defgroup ppsAggregEventsForiCubGui ppsAggregEventsForiCubGui + +Transforms aggregated tactile/peripersonal space events, as generated by skinEventsAggegator or visuoTactileRF, into a dynContact output that can be sent to/iCubGui/forces and visualized. + +\section intro_sec Description + +Transforms aggregated tactile/peripersonal space events, as generated by skinEventsAggegator or visuoTactileRF, into a dynContact output that can be sent to/iCubGui/forces and visualized. +The input has the following format: aggregated output per skin part with average location, normal and magnitude as extracted from the skin positions files. +Maximum 1 vector per skin part; format: (SkinPart_enum x y z o1 o2 o3 magnitude SkinPart_string) (...) - for a maximum of the number of skin parts active. + +\section lib_sec Libraries +- YARP. +- skinDynLib + +\section parameters_sec Parameters + +--context \e path +- Where to find the called resource. + +--from \e from +- The name of the .ini file with the configuration parameters. + +--name \e name +- The name of the module. + +--verbosity \e verb +- Verbosity level (default 0). The higher is the verbosity, the more + information is printed out. + +--autoconnect \e aut +- If to connect automatically to ports. Default not. + +--tactile \e tac +- if enabled, the tactile aggreg events will be prepared for iCubGui visualization. + +--pps \e pps +- if enabled, the peripersonal space aggreg events will be prepared for iCubGui visualization. + +--gain \e gain +- the multiplication vector for the visualization of normalized event magnitude. + +\section portsc_sec Ports Created + +- //skin_events_aggreg:i gets the aggregated skin events as produced by the skinEventsAggegator +- //pps_events_aggreg:i gets the aggregated pps events as produced by the visuoTactileRF + +- //contacts:o prepares a dynContact that can be visualized using /iCubGui/forces +\section tested_os_sec Tested OS +Linux (Ubuntu 12.04) + +\author Matej Hoffmann +*/ + +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +using namespace std; +using namespace yarp::os; +using namespace yarp::sig; +using namespace yarp::math; +using namespace iCub::skinDynLib; + +/** +* \ingroup ppsAggregEventsForiCubGuiModule +* +* The module that transforms aggregated tactile/peripersonal space events, +* as generated by skinEventsAggegator/visuoTactileRF, into a skinContactList +* output that can be sent to/iCubGui/forces and visualized. +* +*/ +class ppsAggregEventsForiCubGui: public RFModule +{ +private: + //EXTERNAL VARIABLES + string context; + string from; + string name; + int verbosity; + bool autoconnect; // on | off + bool tactile; // on | off + bool pps; // on | off + double gain; + + //INTERNAL VARIABLES + BufferedPort aggregSkinEventsInPort; //coming from /skinEventsAggregator/skin_events_aggreg:o + BufferedPort aggregPPSeventsInPort; //coming from visuoTactileRF/pps_activations_aggreg:o + //expected format for both: (skinPart_s x y z o1 o2 o3 magnitude), with position x,y,z and normal o1 o2 o3 in link FoR + BufferedPort aggregEventsForiCubGuiPort; + yarp::os::Stamp ts; + + skinContactList mySkinContactList; + + // reading modified from react-control reactCtrlThread::getCollisionPointsFromPort + // writing adapted from iCub_Sim.cpp OdeSdlSimulation::inspectWholeBodyContactsAndSendTouch() + bool fillSkinContactFromAggregPort(BufferedPort &inPort, const double amplification, skinContactList &sCL) + { + SkinPart sp = SKIN_PART_UNKNOWN; + //all in the link FoR + Vector geocenter(3,0.0); //geocenter from skin / average activation locus from the pps + Vector normal(3,0.0); + Vector force(3,0.0); + Vector moment(3,0.0); //will remain zero + double normalized_activation = 0.0; + std::vector taxel_list; + taxel_list.clear(); //we will be always passing empty list + + Bottle* collPointsMultiBottle = inPort.read(false); + + if(collPointsMultiBottle != NULL) + { + //printf("fillSkinContactFromAggregPort(): There were %d bottles on the port.\n",collPointsMultiBottle->size()); + for(int i=0; i< collPointsMultiBottle->size();i++) + { + sp = SKIN_PART_UNKNOWN; + geocenter.zero(); normal.zero(); normalized_activation = 0.0; + Bottle* collPointBottle = collPointsMultiBottle->get(i).asList(); + //printf("Bottle %d contains %s \n", i,collPointBottle->toString().c_str()); + sp = (SkinPart)(collPointBottle->get(0).asInt()); + geocenter(0) = collPointBottle->get(1).asDouble(); + geocenter(1) = collPointBottle->get(2).asDouble(); + geocenter(2) = collPointBottle->get(3).asDouble(); + normal(0) = collPointBottle->get(4).asDouble(); + normal(1) = collPointBottle->get(5).asDouble(); + normal(2) = collPointBottle->get(6).asDouble(); + normalized_activation = collPointBottle->get(13).asDouble(); + + //hack - in current iCubGui version, in bvhnode.h, ForceArrow, + //there is an atan that gives a wrong direction for force vectors of the form (0 0 1) + force(0)=0.000001; force(1)=0.000001; + force(2) = -1.0*amplification*normalized_activation*normal(2); + + //see iCubGui/src/objectsthread.h ObjectsManager::manage(iCub::skinDynLib::skinContactList &forces) + //printf("fillDynContactFromAggregPort: setting dynContact: Body part: %s Linknum: %d CoP: %s F: %s M: %s\n", + //BodyPart_s[SkinPart_2_BodyPart[sp].body].c_str(),getLinkNum(sp),geoCenter.toString(3,3).c_str(), + // (-1.0*normal).toString(3,3).c_str(),moment.toString(3,3).c_str()); + + skinContact sc(SkinPart_2_BodyPart[sp].body,sp,getLinkNum(sp),geocenter,geocenter,taxel_list, + amplification*normalized_activation,normal,force,moment); + // In skinManager/src/compensator.cpp, Compensator::getContacts(): + // set an estimate of the force that is with normal direction and intensity equal to the pressure + // d.setForceModule(-0.05*activeTaxels*pressure*normal); + sCL.push_back(sc); + } + return true; + } + else{ + //printf("fillDynContactFromAggregPort(): no tactile/pps vectors on the port.\n") ; + return false; + }; + } + + void sendContacts(BufferedPort &outPort, const skinContactList &sCL) + { + ts.update(); + skinContactList &sCLout = outPort.prepare(); + sCLout.clear(); + + sCLout = sCL; + + outPort.setEnvelope(ts); + outPort.write(); + } + +public: + ppsAggregEventsForiCubGui() + { + } + + bool configure(ResourceFinder &rf) + { + context=rf.check("context",Value("periPersonalSpace")).asString(); + from=rf.check("from",Value("ppsAggregEventsForiCubGui.ini")).asString(); + name=rf.check("name",Value("ppsAggregEventsForiCubGui")).asString(); + verbosity = rf.check("verbosity",Value(0)).asInt(); + autoconnect=rf.check("autoconnect",Value("off")).asString()=="on"?true:false; // on | off + tactile=rf.check("tactile",Value("on")).asString()=="on"?true:false; // on | off + pps=rf.check("pps",Value("on")).asString()=="on"?true:false; // on | off + gain=rf.check("gain",Value(50.0)).asDouble(); + + yInfo("[ppsAggregEventsForiCubGui] Initial Parameters:"); + yInfo("Context: %s \t From: %s \t Name: %s \t Verbosity: %d", + context.c_str(),from.c_str(),name.c_str(),verbosity); + yInfo("Autoconnect : %d \n tactile: %d \n pps: %d \n gain: %f \n", + autoconnect,tactile,pps,gain); + + //open ports + if(tactile) + { + aggregSkinEventsInPort.open("/"+name+"/skin_events_aggreg:i"); + } + if(pps) + { + aggregPPSeventsInPort.open("/"+name+"/pps_events_aggreg:i"); + } + aggregEventsForiCubGuiPort.open("/"+name+"/contacts:o"); + + if (autoconnect) + { + Network::connect("/skinEventsAggregator/skin_events_aggreg:o",("/"+name+"/skin_events_aggreg:i").c_str()); + Network::connect("/visuoTactileRF/pps_events_aggreg:o",("/"+name+"/pps_events_aggreg:i").c_str()); + Network::connect(("/"+name+"/contacts:o").c_str(),"/iCubGui/forces"); + } + + + return true; + } + + bool close() + { + yInfo("Stopping ppsAggregEventsForiCubGui module.."); + + mySkinContactList.clear(); + + yInfo("Closing ports..\n"); + if (tactile) + { + aggregSkinEventsInPort.interrupt(); + aggregSkinEventsInPort.close(); + yInfo("aggregSkinEventsInPort successfully closed"); + } + if (pps) + { + aggregPPSeventsInPort.interrupt(); + aggregPPSeventsInPort.close(); + yInfo("aggregPPSeventsInPort successfully closed"); + } + //if (aggregEventsForiCubGuiPort.isOpen()) + //{ + aggregEventsForiCubGuiPort.interrupt(); + aggregEventsForiCubGuiPort.close(); + yInfo("aggregEventsForiCubGuiPort successfully closed"); + //} + + + return true; + } + + double getPeriod() + { + return 0.03; + } + + bool updateModule() + { + + mySkinContactList.clear(); + + if(tactile) + fillSkinContactFromAggregPort(aggregSkinEventsInPort,gain,mySkinContactList); + if(pps) + fillSkinContactFromAggregPort(aggregPPSeventsInPort,gain,mySkinContactList); + + // if (! (mySkinContactList.)) + sendContacts(aggregEventsForiCubGuiPort,mySkinContactList); + + return true; + } + +}; + +//******************************************** +int main(int argc, char * argv[]) +{ + Network yarp; + + ResourceFinder rf; + rf.setVerbose(false); + rf.setDefaultContext("periPersonalSpace"); + rf.setDefaultConfigFile("ppsAggregEventsForiCubGui.ini"); + rf.configure(argc,argv); + + if (rf.check("help")) + { + yInfo(" "); + yInfo("Options:"); + yInfo(" --context path: where to find the called resource (default periPersonalSpace)."); + yInfo(" --from from: the name of the .ini file (default ppsAggregEventsForiCubGui.ini)."); + yInfo(" --name name: the name of the module (default ppsAggregEventsForiCubGui)."); + yInfo(" --verbosity verbosity: verbosity level."); + yInfo(" --autoConnect flag: if to auto connect the ports or not. Default no."); + yInfo(" --tactile flag: if enabled, the tactile aggreg events will be prepared for iCubGui visualization."); + yInfo(" --pps flag: if enabled, the peripersonal space aggreg events will be prepared for iCubGui visualization."); + yInfo(" --gain gain: the multiplication vector for the visualization of normalized event magnitude."); + yInfo(" "); + return 0; + } + + if (!yarp.checkNetwork()) + { + yError("No Network!!!"); + return -1; + } + + ppsAggregEventsForiCubGui module; + return module.runModule(rf); +} + diff --git a/modules/skinEventsAggregator/CMakeLists.txt b/modules/skinEventsAggregator/CMakeLists.txt new file mode 100644 index 0000000..c6cdea8 --- /dev/null +++ b/modules/skinEventsAggregator/CMakeLists.txt @@ -0,0 +1,22 @@ +# Copyright: (C) 2015 iCub Facility - Istituto Italiano di Tecnologia +# Author: Matej Hoffmann +# CopyPolicy: Released under the terms of the GNU GPL v2.0. + +cmake_minimum_required(VERSION 2.8) +project(skinEventsAggregator) + +file(GLOB source *.cpp) +file(GLOB header *.h) + +include_directories(${YARP_INCLUDE_DIRS}) +include_directories(${periPersonalSpace_INCLUDE_DIRS}) + +source_group("Source Files" FILES ${source}) +source_group("Header Files" FILES ${header}) + +add_executable(skinEventsAggregator ${source} ${header} ) + +target_link_libraries(skinEventsAggregator periPersonalSpace ${YARP_LIBRARIES}) + +install(TARGETS skinEventsAggregator DESTINATION bin) + diff --git a/modules/skinEventsAggregator/skinEventsAggregThread.cpp b/modules/skinEventsAggregator/skinEventsAggregThread.cpp new file mode 100644 index 0000000..36e6fe9 --- /dev/null +++ b/modules/skinEventsAggregator/skinEventsAggregThread.cpp @@ -0,0 +1,133 @@ +#include +#include "skinEventsAggregThread.h" + +#define MIN_NUMBER_OF_TAXELS_ACTIVATED 3 //to filter out phantoms +#define SKIN_ACTIVATION_MAX_ICUB_SIM 100 +#define SKIN_ACTIVATION_MAX_ICUB 30 + +using namespace yarp; +using namespace yarp::os; +using namespace yarp::sig; + +using namespace iCub::skinDynLib; + + +/*************** public methods section ********************************************/ + +skinEventsAggregThread::skinEventsAggregThread(int _rate, const string &_name, const string &_robot, int _v) : + RateThread(_rate),name(_name), robot(_robot), verbosity(_v) +{ + +} + +bool skinEventsAggregThread::threadInit() +{ + ts.update(); + skinEventsPortIn.open(("/"+name+"/skin_events:i").c_str()); + skinEvAggregPortOut.open(("/"+name+"/skin_events_aggreg:o").c_str()); + + if (robot == "icub") + SKIN_ACTIVATION_MAX = SKIN_ACTIVATION_MAX_ICUB; + else if (robot == "icubSim") + SKIN_ACTIVATION_MAX = SKIN_ACTIVATION_MAX_ICUB_SIM; + else + yError("skinEventsAggregThread::threadInit(): unknown robot type"); + + return true; +} + +void skinEventsAggregThread::run() +{ + int indexOfBiggestContact = -1; + skinContact biggestContactInSkinPart; + Vector geoCenter(3,0.0), normalDir(3,0.0); + double activation = 0.0; + Bottle & out = skinEvAggregPortOut.prepare(); out.clear(); + Bottle b; + b.clear(); + + ts.update(); + + skinContactList *scl = skinEventsPortIn.read(false); + + if(scl) + { + if(!(scl->empty())) + { + //Probably source of crazy inefficiencies, here just to reach a working state as soon as possible \todo TODO + map contactsPerSkinPart = scl->splitPerSkinPart(); + + for(map::iterator it=contactsPerSkinPart.begin(); it!=contactsPerSkinPart.end(); it++) + { + indexOfBiggestContact = getIndexOfBiggestContactInList(it->second); + + if (indexOfBiggestContact != -1) + { + b.clear(); + biggestContactInSkinPart = (it->second)[indexOfBiggestContact]; + //the output prepared should have identical format to the one prepared in void vtRFThread::manageSkinEvents() + b.addInt(biggestContactInSkinPart.getSkinPart()); + vectorIntoBottle(biggestContactInSkinPart.getGeoCenter(),b); + vectorIntoBottle(biggestContactInSkinPart.getNormalDir(),b); + //we add dummy geoCenter and normalDir in Root frame to keep same format as vtRFThread manageSkinEvents + b.addDouble(0.0); b.addDouble(0.0); b.addDouble(0.0); + b.addDouble(0.0); b.addDouble(0.0); b.addDouble(0.0); + b.addDouble(std::max(1.0,(biggestContactInSkinPart.getPressure()/SKIN_ACTIVATION_MAX))); // % pressure "normalized" with ad hoc constant + b.addString(biggestContactInSkinPart.getSkinPartName()); //this one just for readability + out.addList().read(b); + } + } + skinEvAggregPortOut.setEnvelope(ts); + skinEvAggregPortOut.write(); // send something anyway (if there is no contact the bottle is empty) + } + } +} + + +void skinEventsAggregThread::threadRelease() +{ + printMessage(0,"Closing ports..\n"); + skinEventsPortIn.interrupt(); + skinEventsPortIn.close(); + printMessage(1,"skinEventPortIn successfully closed!\n"); + skinEvAggregPortOut.interrupt(); + skinEvAggregPortOut.close(); + printMessage(1,"skinEvAggregPortOut successfully closed!\n"); +} + +int skinEventsAggregThread::getIndexOfBiggestContactInList(iCub::skinDynLib::skinContactList &sCL) +{ + int index = -1; + unsigned int maxActivatedTaxels = MIN_NUMBER_OF_TAXELS_ACTIVATED; + if (! sCL.empty()) + { + for(skinContactList::iterator c=sCL.begin(); c!=sCL.end(); c++) + { + if( c->getActiveTaxels() >= maxActivatedTaxels) + { + maxActivatedTaxels = c->getActiveTaxels(); + index = std::distance( sCL.begin(), c); + } + } + } + //if (index == -1) + // yError("skinEventsAggregThread::getIndexOfBiggestContactInList: returning index -1"); + + return index; +} + +int skinEventsAggregThread::printMessage(const int l, const char *f, ...) const +{ + if (verbosity>=l) + { + fprintf(stdout,"[%s] ",name.c_str()); + + va_list ap; + va_start(ap,f); + int ret=vfprintf(stdout,f,ap); + va_end(ap); + return ret; + } + else + return -1; +} diff --git a/modules/skinEventsAggregator/skinEventsAggregThread.h b/modules/skinEventsAggregator/skinEventsAggregThread.h new file mode 100644 index 0000000..4605dd1 --- /dev/null +++ b/modules/skinEventsAggregator/skinEventsAggregThread.h @@ -0,0 +1,92 @@ +/* + * Copyright: (C) 2014 iCub Facility - Istituto Italiano di Tecnologia + * Author: Matej Hoffmann + * email: matej.hoffmann@iit.it + * Permission is granted to copy, distribute, and/or modify this program + * under the terms of the GNU General Public License, version 2 or any + * later version published by the Free Software Foundation. + * + * A copy of the license can be found at + * http://www.robotcub.org/icub/license/gpl.txt + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details +*/ + +#ifndef __SKINEVENTSAGGREGTHREAD_H__ +#define __SKINEVENTSAGGREGTHREAD_H__ + +#include /* srand, rand */ +#include /* time */ +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +using namespace std; + +class skinEventsAggregThread: public yarp::os::RateThread +{ +private: + /***************************************************************************/ + // EXTERNAL VARIABLES: change them from command line or through .ini file + // Flag that manages verbosity (v=1 -> more text printed out; v=2 -> even more text): + int verbosity; + // Name of the module (to change port names accordingly): + string name; + // Name of the robot (to address the module toward icub or icubSim): + string robot; + // Resource finder used to find for files and configurations: + yarp::os::ResourceFinder* rf; + //the period used by the thread. + int threadPeriod; + + /***************************************************************************/ + // INTERNAL VARIABLES + + // Stamp for the setEnvelope for the ports + yarp::os::Stamp ts; + + yarp::os::BufferedPort skinEventsPortIn; // input from the skinManager - skin_events port + yarp::os::BufferedPort skinEvAggregPortOut; // output for the transformed skin events + + double SKIN_ACTIVATION_MAX; + + int getIndexOfBiggestContactInList(iCub::skinDynLib::skinContactList &sCL); + + /** + * Prints a message according to the verbosity level: + * @param l will be checked against the global var verbosity: if verbosity >= l, something is printed + * @param f is the text. Please use c standard (like printf) + */ + int printMessage(const int l, const char *f, ...) const; + +public: + // CONSTRUCTOR + skinEventsAggregThread(int _rate, const string &_name, const string &_robot, int _v); + // INIT + virtual bool threadInit(); + // RUN + virtual void run(); + // RELEASE + virtual void threadRelease(); + + +}; + +#endif diff --git a/modules/skinEventsAggregator/skinEventsAggregator.cpp b/modules/skinEventsAggregator/skinEventsAggregator.cpp new file mode 100644 index 0000000..7181dcb --- /dev/null +++ b/modules/skinEventsAggregator/skinEventsAggregator.cpp @@ -0,0 +1,244 @@ +/* + * Copyright: (C) 2014 iCub Facility - Istituto Italiano di Tecnologia + * Author: Matej Hoffmann + * email: matej.hoffmann@iit.it + * Permission is granted to copy, distribute, and/or modify this program + * under the terms of the GNU General Public License, version 2 or any + * later version published by the Free Software Foundation. + * + * A copy of the license can be found at + * http://www.robotcub.org/icub/license/gpl.txt + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details +*/ +/** +\defgroup skinEventsAggregationModule skinEventsAggregationModule + +@ingroup periPersonalSpace + +Aggregates skinContact events (aka skin_events) - into one aggregated output per skin part with average location, normal and magnitude. + +Date first release: 01/02/2016 + +CopyPolicy: Released under the terms of the GNU GPL v2.0. + +\section intro_sec Description +Aggregates skinContact events (aka skin_events) - into one aggregated output per skin part with average location, normal and magnitude as extracted from the skin positions files. Maximum 1 vector per skin part; format: (SkinPart_enum x y z n1 n2 n3 0.0 0.0 0.0 0.0 0.0 0.0 magnitude SkinPart_string) (...) - for a maximum of the number of skin parts active. We add dummy geoCenter and normalDir in Root frame to keep same format as vtRFThread manageSkinEvents(). + +\section lib_sec Libraries +YARP, ICUB libraries + +\section parameters_sec Parameters + +--context \e path +- Where to find the called resource. + +--from \e from +- The name of the .ini file with the configuration parameters. + +--name \e name +- The name of the module (default skinEventsAggregation). + +--robot \e rob +- The name of the robot (either "icub" or "icub"). Default icubSim. + +--rate \e rate +- The period used by the thread. Default 100ms, i.e. 10 Hz. + +--verbosity \e verb +- Verbosity level (default 0). The higher is the verbosity, the more + information is printed out. + +--type \e type +- Type of selection of contacts - e.g. random. + + +\section portsc_sec Ports Created +- //skin_events:i gets the skin events either from icub or icubSim + +- //skin_events:o it sends out the aggegated skin events. + +\section in_files_sec Input Data Files +None. + +\section out_data_sec Output Data Files +None. + +\section tested_os_sec Tested OS +Linux (Ubuntu 12.04). + +\author: Matej Hoffmann +*/ + +#include +#include + +#include +#include + +#include "skinEventsAggregThread.h" + +using namespace yarp; +using namespace yarp::os; +using namespace std; + +using namespace iCub::skinDynLib; + +/** +* \ingroup skinEventsAggregationModule +* +* The module that achieves the skin events aggregation. +* +*/ +class skinEventsAggregator: public RFModule +{ +private: + skinEventsAggregThread *skinEvAggThrd; + + string robot; + string name; + int verbosity; + int threadPeriod; + string type; + + vector activeSkinPartsNamesVector; + map skinPartsPositionsFilePaths; + + +public: + skinEventsAggregator() + { + skinEvAggThrd = 0; + } + + bool configure(ResourceFinder &rf) + { + + name = "skinEventsAggregator"; + robot = "icubSim"; + threadPeriod = 20; //period of the virtContactGenThread in ms + verbosity = 0; + + //****************************************************** + //********************** CONFIGS *********************** + + //******************* GENERAL GROUP ****************** + Bottle &bGeneral=rf.findGroup("general"); + bGeneral.setMonitor(rf.getMonitor()); + + //******************* NAME ****************** + if (bGeneral.check("name")) + { + name = bGeneral.find("name").asString(); + yInfo("Module name set to %s", name.c_str()); + } + else yInfo("Module name set to default, i.e. %s", name.c_str()); + setName(name.c_str()); + + //******************* ROBOT ****************** + if (bGeneral.check("robot")) + { + robot = bGeneral.find("robot").asString(); + yInfo("Robot is: %s", robot.c_str()); + } + else yInfo("Could not find robot option in the config file; using %s as default",robot.c_str()); + + //****************** rate ****************** + if (bGeneral.check("rate")) + { + threadPeriod = bGeneral.find("rate").asInt(); + yInfo("skinEventsAggregThread rateThread working at %i ms.",threadPeriod); + } + else yInfo("Could not find rate in the config file; using %i ms as default period",threadPeriod); + + //******************* VERBOSE ****************** + if (bGeneral.check("verbosity")) + { + verbosity = bGeneral.find("verbosity").asInt(); + yInfo("verbosity set to %i", verbosity); + } + else yInfo("Could not find verbosity option in the config file; using %i as default",verbosity); + + + + //****************************************************** + //*********************** THREAD ********************** + skinEvAggThrd = new skinEventsAggregThread(threadPeriod,name,robot,verbosity); + if (!skinEvAggThrd -> start()) + { + delete skinEvAggThrd; + skinEvAggThrd = 0; + yError("skinEventsAggregThread wasn't instantiated!!"); + return false; + } + yInfo("skinEventsAggregThread instantiated..."); + + return true; + } + + bool close() + { + yInfo("skinEventsAggregation: Stopping thread.."); + if (skinEvAggThrd) + { + skinEvAggThrd -> stop(); + delete skinEvAggThrd; + skinEvAggThrd = 0; + } + + return true; + } + + double getPeriod() + { + return 1.0; + } + + bool updateModule() + { + return true; + } +}; + + +/** +* Main function. +*/ +int main(int argc, char * argv[]) +{ + Network yarp; + + ResourceFinder rf; + rf.setVerbose(true); + rf.setDefaultContext("periPersonalSpace"); + rf.setDefaultConfigFile("skinEventsAggregation.ini"); + rf.configure(argc,argv); + + if (rf.check("help")) + { + yInfo(" "); + yInfo("Options:"); + yInfo(" "); + yInfo(" --context path: where to find the called resource"); + yInfo(" --from from: the name of the .ini file."); + yInfo(" --general::name name: the name of the module (default virtualContactGeneration)."); + yInfo(" --general::robot robot: the name of the robot. Default icubSim."); + yInfo(" --general::rate rate: the period used by the thread. Default 100ms."); + yInfo(" --general::verbosity int: verbosity level (default 0)."); + yInfo(" "); + return 0; + } + + if (!yarp.checkNetwork()) + { + printf("No Network!!!\n"); + return -1; + } + + skinEventsAggregator sEA; + return sEA.runModule(rf); +} +// empty line to make gcc happy diff --git a/modules/testModule/CMakeLists.txt b/modules/testModule/CMakeLists.txt new file mode 100644 index 0000000..185de72 --- /dev/null +++ b/modules/testModule/CMakeLists.txt @@ -0,0 +1,17 @@ +# Copyright: (C) 2013 iCub Facility - Istituto Italiano di Tecnologia +# Author: Alessandro Roncone +# CopyPolicy: Released under the terms of the GNU GPL v2.0. + +cmake_minimum_required(VERSION 2.8) +project(testModule) + +include_directories(${YARP_INCLUDE_DIRS}) +include_directories(${ICUB_INCLUDE_DIRS}) + +set(sources testModule.cpp) +source_group("Source Files" FILES ${sources}) + +add_executable(testModule ${sources}) +target_link_libraries(testModule ${YARP_LIBRARIES} ctrlLib) +install(TARGETS testModule DESTINATION bin) + diff --git a/modules/testModule/testModule.cpp b/modules/testModule/testModule.cpp new file mode 100644 index 0000000..48235f9 --- /dev/null +++ b/modules/testModule/testModule.cpp @@ -0,0 +1,54 @@ +// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- + +/* + * Copyright (C) 2011 Department of Robotics Brain and Cognitive Sciences - Istituto Italiano di Tecnologia + * Authors: Paul Fitzpatrick + * CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT + * + */ + +#include +#include + +// #include +#include + +using namespace yarp::os; +using namespace yarp::sig; +using namespace yarp::mjpeg; + +int main(int argc, char *argv[]) { + Network yarp; + + Property options; + options.fromCommand(argc,argv); + + ConstString inName = options.check("in",Value("/mjpeg/in")).asString(); + ConstString outName = options.check("out",Value("/mjpeg/out")).asString(); + + BufferedPort in; + // BufferedPort would also work fine, but less efficient + + BufferedPort > out; + + if (!in.open(inName)) return 1; + if (!out.open(outName)) return 1; + + MjpegDecompression decompression; + if (decompression.isAutomatic()) { + fprintf(stderr,"For this test, turn MJPEG_AUTOCOMPRESS off\n"); + return 1; + } + while (true) { + ManagedBytes *data = in.read(); + if (!data) continue; + if (!decompression.decompress(data->bytes(),out.prepare())) { + fprintf(stderr,"Decompression failed!\n"); + return 1; + } + out.write(); + } + + return 0; +} + diff --git a/modules/ultimateTracker/kalmanThread.cpp b/modules/ultimateTracker/kalmanThread.cpp index ff4eab0..eb3ae62 100644 --- a/modules/ultimateTracker/kalmanThread.cpp +++ b/modules/ultimateTracker/kalmanThread.cpp @@ -78,7 +78,7 @@ bool kalmanThread::generateMatrices() for (size_t i = 0; i < kalOrder; i++) { int k = 0; - for (int j = i+1; j < kalOrder; j++) + for (size_t j = i+1; j < kalOrder; j++) { kalA(i,j) = el(0,k); k += 1; diff --git a/modules/ultimateTracker/ultimateTracker.cpp b/modules/ultimateTracker/ultimateTracker.cpp index f4adaef..0774333 100644 --- a/modules/ultimateTracker/ultimateTracker.cpp +++ b/modules/ultimateTracker/ultimateTracker.cpp @@ -182,7 +182,7 @@ class ultimateTracker: public RFModule //******************* VERBOSE ****************** if (rf.check("useNearBlobber")) { - useNearBlobber = rf.find("useNearBlobber").asInt(); + useNearBlobber = rf.find("useNearBlobber").asInt()!=0; cout << "utManagerThread useNearBlobber set to " << useNearBlobber << endl; } else cout << "Could not find useNearBlobber option in " << diff --git a/modules/virtualContactGeneration/virtContactGenThread.cpp b/modules/virtualContactGeneration/virtContactGenThread.cpp index bea215c..3bd1508 100644 --- a/modules/virtualContactGeneration/virtContactGenThread.cpp +++ b/modules/virtualContactGeneration/virtContactGenThread.cpp @@ -6,6 +6,7 @@ int virtContactGenerationThread::initSkinParts() { SkinPart skin_part_name; + int OFFSET = 4; //the taxel pos files have as of May 2015 first 4 lines with metainfo string line; ifstream posFile; @@ -14,7 +15,8 @@ int virtContactGenerationThread::initSkinParts() string filename; //go through skin parts and initialize them - for (std::vector::const_iterator it = activeSkinPartsNames.begin() ; it != activeSkinPartsNames.end(); ++it){ + for (std::vector::const_iterator it = activeSkinPartsNames.begin() ; it != activeSkinPartsNames.end(); ++it) + { skin_part_name = *it; iCub::skinDynLib::skinPart skinPartWithTaxels; @@ -25,17 +27,19 @@ int virtContactGenerationThread::initSkinParts() yWarning("[virtContactGenerationThread] File %s has not been opened!",skinPartPosFilePaths[skin_part_name].c_str()); return false; } - printMessage(4,"Initializing %s from %s.\n",SkinPart_s[skin_part_name].c_str(),skinPartPosFilePaths[skin_part_name].c_str()); + printMessage(4,"[virtContactGenerationThread] Initializing %s from %s.\n", + SkinPart_s[skin_part_name].c_str(),skinPartPosFilePaths[skin_part_name].c_str()); posFile.clear(); posFile.seekg(0, std::ios::beg);//rewind iterator - switch(skin_part_name){ + switch(skin_part_name) + { case SKIN_LEFT_HAND: case SKIN_RIGHT_HAND: - for(unsigned int i= 0; getline(posFile,line); i++) + for(int i= 0; getline(posFile,line); i++) { line.erase(line.find_last_not_of(" \n\r\t")+1); - if(line.empty()) + if((line.empty()) || (i=96) && (i<=143) && (i!=107) && (i!=119) && (i!=131) && (i!=139)) //all palm taxels, without thermal pads + //all palm taxels, without thermal pads + if((i>=96+OFFSET) && (i<=143+OFFSET) && (i!=107+OFFSET) && (i!=119+OFFSET) && (i!=131+OFFSET) && (i!=139+OFFSET)) { - skinPartWithTaxels.taxels.push_back(new Taxel(taxelPos,taxelNorm,i)); - printMessage(10,"Pushing taxel ID:%d, pos:%f %f %f; norm:%f %f %f.\n",i,taxelPos[0],taxelPos[1],taxelPos[2],taxelNorm[0],taxelNorm[1],taxelNorm[2]); + skinPartWithTaxels.taxels.push_back(new Taxel(taxelPos,taxelNorm,i-OFFSET)); + printMessage(10,"[virtContactGenerationThread] Pushing taxel ID:%d, pos:%f %f %f; norm:%f %f %f.\n", + i-OFFSET,taxelPos[0],taxelPos[1],taxelPos[2],taxelNorm[0],taxelNorm[1],taxelNorm[2]); } } - if(skinPartWithTaxels.getSize() != 192){ - yWarning("[virtContactGenerationThread]::initSkinParts():initalizing %s from file, 192 positions expected, but %d present.\n",SkinPart_s[skin_part_name].c_str(),skinPartWithTaxels.getSize()); + if(skinPartWithTaxels.getSize() != 192) + { + yWarning("[virtContactGenerationThread]::initSkinParts():initalizing %s from file, 192 positions expected, but %d present.\n", + SkinPart_s[skin_part_name].c_str(),skinPartWithTaxels.getSize()); } skinPartWithTaxels.name = skin_part_name; - if (skin_part_name == SKIN_LEFT_HAND){ + if (skin_part_name == SKIN_LEFT_HAND) + { activeSkinParts[SKIN_LEFT_HAND] = skinPartWithTaxels; - printMessage(4,"Adding SKIN_LEFT_HAND to activeSkinParts, it now has %d members.\n",activeSkinParts.size()); + printMessage(4,"[virtContactGenerationThread]Adding SKIN_LEFT_HAND to activeSkinParts, it now has %d members.\n",activeSkinParts.size()); } - else{ // skin_part_name == SKIN_RIGHT_HAND + else // skin_part_name == SKIN_RIGHT_HAND + { activeSkinParts[SKIN_RIGHT_HAND] = skinPartWithTaxels; - printMessage(4,"Adding SKIN_RIGHT_HAND to activeSkinParts, it now has %d members.\n",activeSkinParts.size()); + printMessage(4,"[virtContactGenerationThread]Adding SKIN_RIGHT_HAND to activeSkinParts, it now has %d members.\n",activeSkinParts.size()); } break; case SKIN_LEFT_FOREARM: case SKIN_RIGHT_FOREARM: - for(unsigned int i= 0; getline(posFile,line); i++) + + for(int i= 0; getline(posFile,line); i++) { line.erase(line.find_last_not_of(" \n\r\t")+1); - if(line.empty()) + if(line.empty() || (i=0) && (i<=191)) //first patch - full one, 16 triangles, lower part of forearm (though in Marco's files, it is called upper) + + if((i>=0+OFFSET) && (i<=191+OFFSET)) //first patch - full one, 16 triangles, lower part of forearm (though in Marco's files, it is called upper) { - if( ((i % 12) != 6) && ((i % 12) != 10)){ //every 7th and 11th taxel of a triangle are thermal pads - skinPartWithTaxels.taxels.push_back(new Taxel(taxelPos,taxelNorm,i)); - printMessage(10,"Pushing taxel ID:%d, pos:%f %f %f; norm:%f %f %f.\n",i,taxelPos[0],taxelPos[1],taxelPos[2],taxelNorm[0],taxelNorm[1],taxelNorm[2]); + if( (((i-OFFSET) % 12) != 6) && (((i-OFFSET) % 12) != 10)) + { //every 7th and 11th taxel of a triangle are thermal pads + skinPartWithTaxels.taxels.push_back(new Taxel(taxelPos,taxelNorm,i-OFFSET)); + printMessage(10,"[virtContactGenerationThread]Pushing taxel ID:%d, pos:%f %f %f; norm:%f %f %f.\n", + i-OFFSET,taxelPos[0],taxelPos[1],taxelPos[2],taxelNorm[0],taxelNorm[1],taxelNorm[2]); } } - else if((i>=192) && (i<=383)){ //second patch - 7 triangles in skin V1, upper part of forearm (though in Marco's files, it is called lower) - if( ((i>=204) && (i<=215)) && (i!=204+6) && (i!=204+10)){ //first triangle without thermal pads - skinPartWithTaxels.taxels.push_back(new Taxel(taxelPos,taxelNorm,i)); - printMessage(10,"Pushing taxel ID:%d, pos:%f %f %f; norm:%f %f %f.\n",i,taxelPos[0],taxelPos[1],taxelPos[2],taxelNorm[0],taxelNorm[1],taxelNorm[2]); + else if((i>=192+OFFSET) && (i<=383+OFFSET)) + { //second patch - 7 triangles in skin V1, upper part of forearm (though in Marco's files, it is called lower) + if( ((i>=204+OFFSET) && (i<=215+OFFSET)) && (i!=204+6+OFFSET) && (i!=204+10+OFFSET)) + { //first triangle without thermal pads + skinPartWithTaxels.taxels.push_back(new Taxel(taxelPos,taxelNorm,i-OFFSET)); + printMessage(10,"[virtContactGenerationThread]Pushing taxel ID:%d, pos:%f %f %f; norm:%f %f %f.\n", + i-OFFSET,taxelPos[0],taxelPos[1],taxelPos[2],taxelNorm[0],taxelNorm[1],taxelNorm[2]); } - else if( ((i>=252) && (i<=263)) && (i!=252+6) && (i!=252+10)){ //2nd triangle without thermal pads - skinPartWithTaxels.taxels.push_back(new Taxel(taxelPos,taxelNorm,i)); - printMessage(10,"Pushing taxel ID:%d, pos:%f %f %f; norm:%f %f %f.\n",i,taxelPos[0],taxelPos[1],taxelPos[2],taxelNorm[0],taxelNorm[1],taxelNorm[2]); + else if( ((i>=252+OFFSET) && (i<=263+OFFSET)) && (i!=252+6+OFFSET) && (i!=252+10+OFFSET)) + { //2nd triangle without thermal pads + skinPartWithTaxels.taxels.push_back(new Taxel(taxelPos,taxelNorm,i-OFFSET)); + printMessage(10,"[virtContactGenerationThread]Pushing taxel ID:%d, pos:%f %f %f; norm:%f %f %f.\n", + i-OFFSET,taxelPos[0],taxelPos[1],taxelPos[2],taxelNorm[0],taxelNorm[1],taxelNorm[2]); } - else if( ((i>=288) && (i<=299)) && (i!=288+6) && (i!=288+10)){ //3rd triangle without thermal pads - skinPartWithTaxels.taxels.push_back(new Taxel(taxelPos,taxelNorm,i)); - printMessage(10,"Pushing taxel ID:%d, pos:%f %f %f; norm:%f %f %f.\n",i,taxelPos[0],taxelPos[1],taxelPos[2],taxelNorm[0],taxelNorm[1],taxelNorm[2]); + else if( ((i>=288+OFFSET) && (i<=299+OFFSET)) && (i!=288+6+OFFSET) && (i!=288+10+OFFSET)) + { //3rd triangle without thermal pads + skinPartWithTaxels.taxels.push_back(new Taxel(taxelPos,taxelNorm,i-OFFSET)); + printMessage(10,"[virtContactGenerationThread]Pushing taxel ID:%d, pos:%f %f %f; norm:%f %f %f.\n", + i-OFFSET,taxelPos[0],taxelPos[1],taxelPos[2],taxelNorm[0],taxelNorm[1],taxelNorm[2]); } - else if( ((i>=300) && (i<=311)) && (i!=300+6) && (i!=300+10)){ //4th triangle without thermal pads - skinPartWithTaxels.taxels.push_back(new Taxel(taxelPos,taxelNorm,i)); - printMessage(10,"Pushing taxel ID:%d, pos:%f %f %f; norm:%f %f %f.\n",i,taxelPos[0],taxelPos[1],taxelPos[2],taxelNorm[0],taxelNorm[1],taxelNorm[2]); + else if( ((i>=300+OFFSET) && (i<=311+OFFSET)) && (i!=300+6+OFFSET) && (i!=300+10+OFFSET)) + { //4th triangle without thermal pads + skinPartWithTaxels.taxels.push_back(new Taxel(taxelPos,taxelNorm,i-OFFSET)); + printMessage(10,"[virtContactGenerationThread]Pushing taxel ID:%d, pos:%f %f %f; norm:%f %f %f.\n", + i-OFFSET,taxelPos[0],taxelPos[1],taxelPos[2],taxelNorm[0],taxelNorm[1],taxelNorm[2]); } - else if( ((i>=312) && (i<=323)) && (i!=312+6) && (i!=312+10)){ //5th triangle without thermal pads - skinPartWithTaxels.taxels.push_back(new Taxel(taxelPos,taxelNorm,i)); - printMessage(10,"Pushing taxel ID:%d, pos:%f %f %f; norm:%f %f %f.\n",i,taxelPos[0],taxelPos[1],taxelPos[2],taxelNorm[0],taxelNorm[1],taxelNorm[2]); + else if( ((i>=312+OFFSET) && (i<=323+OFFSET)) && (i!=312+6+OFFSET) && (i!=312+10+OFFSET)) + { //5th triangle without thermal pads + skinPartWithTaxels.taxels.push_back(new Taxel(taxelPos,taxelNorm,i-OFFSET)); + printMessage(10,"[virtContactGenerationThread]Pushing taxel ID:%d, pos:%f %f %f; norm:%f %f %f.\n", + i-OFFSET,taxelPos[0],taxelPos[1],taxelPos[2],taxelNorm[0],taxelNorm[1],taxelNorm[2]); } - else if( ((i>=336) && (i<=347)) && (i!=336+6) && (i!=336+10)){ //6th triangle without thermal pads - skinPartWithTaxels.taxels.push_back(new Taxel(taxelPos,taxelNorm,i)); - printMessage(10,"Pushing taxel ID:%d, pos:%f %f %f; norm:%f %f %f.\n",i,taxelPos[0],taxelPos[1],taxelPos[2],taxelNorm[0],taxelNorm[1],taxelNorm[2]); + else if( ((i>=336+OFFSET) && (i<=347+OFFSET)) && (i!=336+6+OFFSET) && (i!=336+10+OFFSET)) + { //6th triangle without thermal pads + skinPartWithTaxels.taxels.push_back(new Taxel(taxelPos,taxelNorm,i-OFFSET)); + printMessage(10,"[virtContactGenerationThread]Pushing taxel ID:%d, pos:%f %f %f; norm:%f %f %f.\n", + i-OFFSET,taxelPos[0],taxelPos[1],taxelPos[2],taxelNorm[0],taxelNorm[1],taxelNorm[2]); } - else if( ((i>=348) && (i<=359)) && (i!=348+6) && (i!=348+10)){ //7th triangle without thermal pads - skinPartWithTaxels.taxels.push_back(new Taxel(taxelPos,taxelNorm,i)); - printMessage(10,"Pushing taxel ID:%d, pos:%f %f %f; norm:%f %f %f.\n",i,taxelPos[0],taxelPos[1],taxelPos[2],taxelNorm[0],taxelNorm[1],taxelNorm[2]); + else if( ((i>=348+OFFSET) && (i<=359+OFFSET)) && (i!=348+6+OFFSET) && (i!=348+10+OFFSET)) + { //7th triangle without thermal pads + skinPartWithTaxels.taxels.push_back(new Taxel(taxelPos,taxelNorm,i-OFFSET)); + printMessage(10,"[virtContactGenerationThread]Pushing taxel ID:%d, pos:%f %f %f; norm:%f %f %f.\n", + i-OFFSET,taxelPos[0],taxelPos[1],taxelPos[2],taxelNorm[0],taxelNorm[1],taxelNorm[2]); } } } - if(skinPartWithTaxels.getSize() != 384){ - yWarning("[virtContactGenerationThread]::initSkinParts():initalizing %s from file, 384 positions expected, but %d present.\n",SkinPart_s[skin_part_name].c_str(),skinPartWithTaxels.getSize()); + if(skinPartWithTaxels.getSize() != 384) + { + yWarning("[virtContactGenerationThread]::initSkinParts():initalizing %s from file, 384 positions expected, but %d present.\n", + SkinPart_s[skin_part_name].c_str(),skinPartWithTaxels.getSize()); } skinPartWithTaxels.name = skin_part_name; - if (skin_part_name == SKIN_LEFT_FOREARM){ + if (skin_part_name == SKIN_LEFT_FOREARM) + { activeSkinParts[SKIN_LEFT_FOREARM] = skinPartWithTaxels; - printMessage(4,"Adding SKIN_LEFT_FOREARM to activeSkinParts, it now has %d members.\n",activeSkinParts.size()); + + printMessage(4,"[virtContactGenerationThread]Adding SKIN_LEFT_FOREARM to activeSkinParts, it now has %d members.\n", + activeSkinParts.size()); } - else{ // skin_part_name == SKIN_RIGHT_FOREARM + else + { // skin_part_name == SKIN_RIGHT_FOREARM activeSkinParts[SKIN_RIGHT_FOREARM] = skinPartWithTaxels; - printMessage(4,"Adding SKIN_RIGHT_FOREARM to activeSkinParts, it now has %d members.\n",activeSkinParts.size()); + printMessage(4,"[virtContactGenerationThread]Adding SKIN_RIGHT_FOREARM to activeSkinParts, it now has %d members.\n", + activeSkinParts.size()); } break; default: - yError("[virtContactGenerationThread] Asked to initialize skinDynLib::SkinPart:: %d, but that skin part is not implemented yet.\n",skin_part_name); + yError("[virtContactGenerationThread] Asked to initialize skinDynLib::SkinPart:: %d, but it is not implemented yet.\n", + skin_part_name); return -1; } posFile.close(); - - } return 0; @@ -155,10 +190,12 @@ int virtContactGenerationThread::initSkinParts() void virtContactGenerationThread::printInitializedSkinParts() { - for (std::map::const_iterator it = activeSkinParts.begin() ; it != activeSkinParts.end(); ++it){ + for (std::map::const_iterator it = activeSkinParts.begin() ; it != activeSkinParts.end(); ++it) + { iCub::skinDynLib::skinPart locSkinPartTaxel = it->second; vector taxels = locSkinPartTaxel.taxels; - printMessage(6,"Iterating through activeSkinParts (%d members), now: it->first: %d, locSkinPartTaxel.name: %s.\n",activeSkinParts.size(),it->first,locSkinPartTaxel.name.c_str()); + printMessage(6,"Iterating through activeSkinParts (%d members), now: it->first: %d, locSkinPartTaxel.name: %s.\n", + activeSkinParts.size(),it->first,locSkinPartTaxel.name.c_str()); ofstream outFile; outFile.open(locSkinPartTaxel.name.c_str()); if (outFile.fail()) // Check for file creation and return error. @@ -166,8 +203,13 @@ void virtContactGenerationThread::printInitializedSkinParts() printMessage(6,"Error opening %s for output.\n",it->second.name.c_str()); continue; } - for (vector::const_iterator it_taxel = taxels.begin(); it_taxel!= taxels.end(); ++it_taxel){ - outFile << (**it_taxel).getPosition()[0] << " " <<(**it_taxel).getPosition()[1] << " " <<(**it_taxel).getPosition()[2] << " " <<(**it_taxel).getNormal()[0] << " " <<(**it_taxel).getNormal()[1] << " " <<(**it_taxel).getNormal()[2] << " " <<(**it_taxel).getID() <::const_iterator it_taxel = taxels.begin(); it_taxel!= taxels.end(); ++it_taxel) + { + outFile << (**it_taxel).getPosition()[0] << " " <<(**it_taxel).getPosition()[1] + << " " <<(**it_taxel).getPosition()[2] << " " <<(**it_taxel).getNormal()[0] + << " " <<(**it_taxel).getNormal()[1] << " " <<(**it_taxel).getNormal()[2] + << " " <<(**it_taxel).getID() < &_activeSkinPartsNames, const map &_skinPartPosFilePaths): -RateThread(_rate),name(_name), robot(_robot), verbosity(_v), type(_type), activeSkinPartsNames(_activeSkinPartsNames), skinPartPosFilePaths(_skinPartPosFilePaths) +virtContactGenerationThread::virtContactGenerationThread(int _rate, const string &_name, const string &_robot, int _v, + const string &_type, const vector &_activeSkinPartsNames, + const map &_skinPartPosFilePaths) : + RateThread(_rate),name(_name), robot(_robot), verbosity(_v), type(_type), + activeSkinPartsNames(_activeSkinPartsNames), skinPartPosFilePaths(_skinPartPosFilePaths) { skinEventsOutPort = new BufferedPort; @@ -186,57 +231,77 @@ RateThread(_rate),name(_name), robot(_robot), verbosity(_v), type(_type), active bool virtContactGenerationThread::threadInit() { - ts.update(); skinEventsOutPort->open(("/"+name+"/virtualContacts:o").c_str()); /* initialize random seed: */ - srand (time(NULL)); + srand ((unsigned int)time(NULL)); int returnValue = initSkinParts(); - if(returnValue == -1){ + if(returnValue == -1) + { yError("[virtContactGenerationThread] Could not initialize skin parts.\n"); return false; } - if(verbosity > 5){ + if(verbosity > 5) + { printInitializedSkinParts(); } - if (activeSkinPartsNames.size() != activeSkinParts.size()){ - yError("[virtContactGenerationThread] activeSkinPartsNames and activeSkinParts have different size (%lu vs. %lu).\n",activeSkinPartsNames.size(),activeSkinParts.size()); + if (activeSkinPartsNames.size() != activeSkinParts.size()) + { + yError("[virtContactGenerationThread] activeSkinPartsNames and activeSkinParts have different size (%lu vs. %lu).\n", + activeSkinPartsNames.size(),activeSkinParts.size()); return false; } - + + taxelIDinList.clear(); + skinPartIndexInVector = rand() % activeSkinPartsNames.size(); //so e.g. for size 3, this should give 0, 1, or 2, which is right + skinPartPickedName = activeSkinPartsNames[skinPartIndexInVector]; + skinPartPicked = activeSkinParts[skinPartPickedName]; + taxelPickedIndex = rand() % skinPartPicked.taxels.size(); + taxelPicked = *(skinPartPicked.taxels[taxelPickedIndex]); + taxelIDinList.push_back(taxelPicked.getID()); //there will be only a single taxel in the list, but we want to keep the information which taxel it was + printMessage(3,"Randomly selecting taxel ID: %d, from %s. Pose in local FoR (pos,norm): %f %f %f; norm:%f %f %f.\n", + taxelPicked.getID(),SkinPart_s[skinPartPickedName].c_str(),taxelPicked.getPosition()[0],taxelPicked.getPosition()[1], + taxelPicked.getPosition()[2],taxelPicked.getNormal()[0],taxelPicked.getNormal()[1],taxelPicked.getNormal()[2]); + return true; } void virtContactGenerationThread::run() { ts.update(); - taxelIDinList.clear(); - if (type == "random"){ + if (type == "random") + { + taxelIDinList.clear(); skinPartIndexInVector = rand() % activeSkinPartsNames.size(); //so e.g. for size 3, this should give 0, 1, or 2, which is right skinPartPickedName = activeSkinPartsNames[skinPartIndexInVector]; skinPartPicked = activeSkinParts[skinPartPickedName]; taxelPickedIndex = rand() % skinPartPicked.taxels.size(); taxelPicked = *(skinPartPicked.taxels[taxelPickedIndex]); taxelIDinList.push_back(taxelPicked.getID()); //there will be only a single taxel in the list, but we want to keep the information which taxel it was - printMessage(3,"Randomly selecting taxel ID: %d, from %s. Pose in local FoR (pos,norm): %f %f %f; norm:%f %f %f.\n",taxelPicked.getID(),SkinPart_s[skinPartPickedName].c_str(),taxelPicked.getPosition()[0],taxelPicked.getPosition()[1],taxelPicked.getPosition()[2],taxelPicked.getNormal()[0],taxelPicked.getNormal()[1],taxelPicked.getNormal()[2]); - - skinContact c(getBodyPart(skinPartPickedName), skinPartPickedName, getLinkNum(skinPartPickedName), taxelPicked.getPosition(), taxelPicked.getPosition(),taxelIDinList,VIRT_CONTACT_PRESSURE,taxelPicked.getNormal()); - // skinContact(const BodyPart &_bodyPart, const SkinPart &_skinPart, unsigned int _linkNumber, const yarp::sig::Vector &_CoP, - // const yarp::sig::Vector &_geoCenter, std::vector _taxelList, double _pressure, const yarp::sig::Vector &_normalDir); - printMessage(3,"Creating skin contact as follows: %s.\n",c.toString().c_str()); - - //see also void SimulatorModule::sendSkinEvents(iCub::skinDynLib::skinContactList& skinContactListReport) - //and compensationThread.cpp void CompensationThread::sendSkinEvents() - skinContactList &listWithPickedSkinContact = skinEventsOutPort->prepare(); - listWithPickedSkinContact.clear();; - listWithPickedSkinContact.push_back(c); - skinEventsOutPort->setEnvelope(ts); - skinEventsOutPort->write(); + printMessage(3,"Randomly selecting taxel ID: %d, from %s. Pose in local FoR (pos,norm): %f %f %f; norm:%f %f %f.\n", + taxelPicked.getID(),SkinPart_s[skinPartPickedName].c_str(),taxelPicked.getPosition()[0], + taxelPicked.getPosition()[1],taxelPicked.getPosition()[2],taxelPicked.getNormal()[0], + taxelPicked.getNormal()[1],taxelPicked.getNormal()[2]); } + + skinContact c(getBodyPart(skinPartPickedName), skinPartPickedName, getLinkNum(skinPartPickedName), taxelPicked.getPosition(), + taxelPicked.getPosition(),taxelIDinList,VIRT_CONTACT_PRESSURE,taxelPicked.getNormal()); + // skinContact(const BodyPart &_bodyPart, const SkinPart &_skinPart, unsigned int _linkNumber, const yarp::sig::Vector &_CoP, + // const yarp::sig::Vector &_geoCenter, std::vector _taxelList, double _pressure, const yarp::sig::Vector &_normalDir); + printMessage(3,"Creating skin contact as follows: %s.\n",c.toString().c_str()); + + //see also void SimulatorModule::sendSkinEvents(iCub::skinDynLib::skinContactList& skinContactListReport) + //and compensationThread.cpp void CompensationThread::sendSkinEvents() + skinContactList &listWithPickedSkinContact = skinEventsOutPort->prepare(); + listWithPickedSkinContact.clear();; + listWithPickedSkinContact.push_back(c); + skinEventsOutPort->setEnvelope(ts); + skinEventsOutPort->write(); + } int virtContactGenerationThread::printMessage(const int l, const char *f, ...) const @@ -257,6 +322,11 @@ int virtContactGenerationThread::printMessage(const int l, const char *f, ...) c void virtContactGenerationThread::threadRelease() { + activeSkinPartsNames.clear(); + skinPartPosFilePaths.clear(); + activeSkinParts.clear(); + taxelIDinList.clear(); + printMessage(0,"Closing ports..\n"); closePort(skinEventsOutPort); printMessage(1,"skinEventsOutPort successfully closed!\n"); diff --git a/modules/virtualContactGeneration/virtContactGenThread.h b/modules/virtualContactGeneration/virtContactGenThread.h index 3c6d392..7aabd66 100644 --- a/modules/virtualContactGeneration/virtContactGenThread.h +++ b/modules/virtualContactGeneration/virtContactGenThread.h @@ -101,7 +101,7 @@ class virtContactGenerationThread: public RateThread /** * Prints a message according to the verbosity level: - * @param l is the level of verbosity: if level > verbosity, something is printed + * @param l will be checked against the global var verbosity: if verbosity >= l, something is printed * @param f is the text. Please use c standard (like printf) */ int printMessage(const int l, const char *f, ...) const; @@ -110,7 +110,8 @@ class virtContactGenerationThread: public RateThread public: // CONSTRUCTOR virtContactGenerationThread(int _rate, const string &_name, const string &_robot, - int _v, const string &_type, const vector &_activeSkinPartsNames, const map &_skinPartPosFilePaths); + int _v, const string &_type, const vector &_activeSkinPartsNames, + const map &_skinPartPosFilePaths); // INIT virtual bool threadInit(); // RUN diff --git a/modules/virtualContactGeneration/virtualContactGeneration.cpp b/modules/virtualContactGeneration/virtualContactGeneration.cpp index f66ac71..177aa1a 100644 --- a/modules/virtualContactGeneration/virtualContactGeneration.cpp +++ b/modules/virtualContactGeneration/virtualContactGeneration.cpp @@ -173,12 +173,14 @@ class virtualContactGeneration: public RFModule //*************** ACTIVE SKIN PARTS GROUP **************** Bottle &bSkinParts=rf.findGroup("skin_parts"); - if (!bSkinParts.isNull()){ + if (!bSkinParts.isNull()) + { bSkinParts.setMonitor(rf.getMonitor()); if (bSkinParts.check("SKIN_LEFT_HAND")) { - if(bSkinParts.find("SKIN_LEFT_HAND").asString()=="on"){ + if(bSkinParts.find("SKIN_LEFT_HAND").asString()=="on") + { partOfSkin = SKIN_LEFT_HAND; activeSkinPartsNamesVector.push_back(partOfSkin); yInfo("Adding SKIN_LEFT_HAND to active skin parts."); @@ -189,7 +191,8 @@ class virtualContactGeneration: public RFModule if (bSkinParts.check("SKIN_LEFT_FOREARM")) { - if(bSkinParts.find("SKIN_LEFT_FOREARM").asString()=="on"){ + if(bSkinParts.find("SKIN_LEFT_FOREARM").asString()=="on") + { partOfSkin = SKIN_LEFT_FOREARM; activeSkinPartsNamesVector.push_back(partOfSkin); yInfo("Adding SKIN_LEFT_FOREARM to active skin parts."); @@ -199,7 +202,8 @@ class virtualContactGeneration: public RFModule if (bSkinParts.check("SKIN_LEFT_UPPER_ARM")) { - if(bSkinParts.find("SKIN_LEFT_UPPER_ARM").asString()=="on"){ + if(bSkinParts.find("SKIN_LEFT_UPPER_ARM").asString()=="on") + { partOfSkin = SKIN_LEFT_UPPER_ARM; activeSkinPartsNamesVector.push_back(partOfSkin); yInfo("Adding SKIN_LEFT_UPPER_ARM to active skin parts."); @@ -209,7 +213,8 @@ class virtualContactGeneration: public RFModule if (bSkinParts.check("SKIN_RIGHT_HAND")) { - if(bSkinParts.find("SKIN_RIGHT_HAND").asString()=="on"){ + if(bSkinParts.find("SKIN_RIGHT_HAND").asString()=="on") + { partOfSkin = SKIN_RIGHT_HAND; activeSkinPartsNamesVector.push_back(partOfSkin); yInfo("Adding SKIN_RIGHT_HAND to active skin parts."); @@ -220,7 +225,8 @@ class virtualContactGeneration: public RFModule if (bSkinParts.check("SKIN_RIGHT_FOREARM")) { - if(bSkinParts.find("SKIN_RIGHT_FOREARM").asString()=="on"){ + if(bSkinParts.find("SKIN_RIGHT_FOREARM").asString()=="on") + { partOfSkin = SKIN_RIGHT_FOREARM; activeSkinPartsNamesVector.push_back(partOfSkin); yInfo("Adding SKIN_RIGHT_FOREARM to active skin parts."); @@ -231,7 +237,8 @@ class virtualContactGeneration: public RFModule if (bSkinParts.check("SKIN_RIGHT_UPPER_ARM")) { - if(bSkinParts.find("SKIN_RIGHT_UPPER_ARM").asString()=="on"){ + if(bSkinParts.find("SKIN_RIGHT_UPPER_ARM").asString()=="on") + { partOfSkin = SKIN_RIGHT_UPPER_ARM; activeSkinPartsNamesVector.push_back(partOfSkin); yInfo("Adding SKIN_RIGHT_UPPER_ARM to active skin parts."); @@ -251,8 +258,9 @@ class virtualContactGeneration: public RFModule skinRF.setVerbose(false); skinRF.setDefaultContext("skinGui"); //overridden by --context parameter skinRF.setDefaultConfigFile("skinManAll.ini"); //overridden by --from parameter + skinRF.setVerbose(true); skinRF.configure(0,NULL); - + Bottle &skinEventsConf = skinRF.findGroup("SKIN_EVENTS"); if(!skinEventsConf.isNull()) { @@ -317,6 +325,8 @@ class virtualContactGeneration: public RFModule delete virtContactGenThrd; virtContactGenThrd = 0; } + activeSkinPartsNamesVector.clear(); + skinPartsPositionsFilePaths.clear(); return true; } diff --git a/modules/visuoTactileRF/visuoTactileRF.cpp b/modules/visuoTactileRF/visuoTactileRF.cpp index d738494..65cb777 100644 --- a/modules/visuoTactileRF/visuoTactileRF.cpp +++ b/modules/visuoTactileRF/visuoTactileRF.cpp @@ -195,7 +195,7 @@ class visuoTactileRF: public RFModule modality = "1D"; verbosity = 0; // verbosity - rate = 50; // rate of the vtRFThread + rate = 20; // rate of the vtRFThread //****************************************************** //********************** CONFIGS *********************** @@ -274,7 +274,7 @@ class visuoTactileRF: public RFModule string filePath(skinRF.findFile(taxelPosFile.c_str())); if (filePath!="") { - yInfo("[skin_event] filePath [%i] %s\n",0,filePath.c_str()); + yInfo("[visuoTactileRF] filePath [%i] %s\n",0,filePath.c_str()); filenames.push_back(filePath); } } @@ -284,7 +284,7 @@ class visuoTactileRF: public RFModule string filePath(skinRF.findFile(taxelPosFile.c_str())); if (filePath!="") { - yInfo("[skin_event] filePath [%i] %s\n",1,filePath.c_str()); + yInfo("[visuoTactileRF] filePath [%i] %s\n",1,filePath.c_str()); filenames.push_back(filePath); } } @@ -294,7 +294,7 @@ class visuoTactileRF: public RFModule string filePath(skinRF.findFile(taxelPosFile.c_str())); if (filePath!="") { - yInfo("[skin_event] filePath [%i] %s\n",2,filePath.c_str()); + yInfo("[visuoTactileRF] filePath [%i] %s\n",2,filePath.c_str()); filenames.push_back(filePath); } } @@ -304,7 +304,7 @@ class visuoTactileRF: public RFModule string filePath(skinRF.findFile(taxelPosFile.c_str())); if (filePath!="") { - yInfo("[skin_event] filePath [%i] %s\n",3,filePath.c_str()); + yInfo("[visuoTactileRF] filePath [%i] %s\n",3,filePath.c_str()); filenames.push_back(filePath); } } @@ -317,7 +317,7 @@ class visuoTactileRF: public RFModule string filePath(skinRF.findFile(taxelPosFile.c_str())); if (filePath!="") { - yInfo("[skin_event] filePath [%i] %s\n",i,filePath.c_str()); + yInfo("[visuoTactileRF] filePath [%i] %s\n",i,filePath.c_str()); filenames.push_back(filePath); } } @@ -332,7 +332,7 @@ class visuoTactileRF: public RFModule //*************** eyes' Resource finder **************** ResourceFinder gazeRF; - gazeRF.setVerbose(bool(verbosity)); + gazeRF.setVerbose(verbosity!=0); gazeRF.setDefaultContext("iKinGazeCtrl"); robot=="icub"?gazeRF.setDefaultConfigFile("config.ini"):gazeRF.setDefaultConfigFile("configSim.ini"); gazeRF.configure(0,NULL); @@ -344,7 +344,7 @@ class visuoTactileRF: public RFModule if(!camerasGroup.isNull()) { - eyeAlignRF.setVerbose(bool(verbosity)); + eyeAlignRF.setVerbose(verbosity!=0); camerasGroup.check("context")? eyeAlignRF.setDefaultContext(camerasGroup.find("context").asString().c_str()): eyeAlignRF.setDefaultContext(gazeRF.getContext().c_str()); diff --git a/modules/visuoTactileRF/vtRFThread.cpp b/modules/visuoTactileRF/vtRFThread.cpp index 0169009..1b35f3d 100644 --- a/modules/visuoTactileRF/vtRFThread.cpp +++ b/modules/visuoTactileRF/vtRFThread.cpp @@ -70,41 +70,41 @@ bool vtRFThread::threadInit() skinGuiPortHandL.open(("/"+name+"/skinGuiHandL:o").c_str()); skinGuiPortHandR.open(("/"+name+"/skinGuiHandR:o").c_str()); skinPortIn -> open(("/"+name+"/skin_events:i").c_str()); - skinPortOut.open(("/"+name+"/skin_events:o").c_str()); + ppsEventsPortOut.open(("/"+name+"/pps_events_aggreg:o").c_str()); dataDumperPortOut.open(("/"+name+"/dataDumper:o").c_str()); /** * It is not recommended but it is fast practice as well **/ - if (robot=="icub") - { - Network::connect("/icub/camcalib/left/out",("/"+name+"/imageL:i").c_str()); - Network::connect("/icub/camcalib/right/out",("/"+name+"/imageR:i").c_str()); - } - else - { - Network::connect("/icubSim/cam/left",("/"+name+"/imageL:i").c_str()); - Network::connect("/icubSim/cam/right",("/"+name+"/imageR:i").c_str()); - } - - Network::connect(("/"+name+"/imageL:o").c_str(),"/vtRF/left"); - Network::connect(("/"+name+"/imageR:o").c_str(),"/vtRF/right"); - - Network::connect("/doubleTouch/status:o",("/"+name+"/input:i").c_str()); - Network::connect("/visuoTactileWrapper/events:o",("/"+name+"/events:i").c_str()); - - Network::connect(("/"+name+"/skinGuiForearmL:o").c_str(),"/skinGui/left_forearm_virtual:i"); - Network::connect(("/"+name+"/skinGuiForearmR:o").c_str(),"/skinGui/right_forearm_virtual:i"); - Network::connect(("/"+name+"/skinGuiHandL:o").c_str(),"/skinGui/left_hand_virtual:i"); - Network::connect(("/"+name+"/skinGuiHandR:o").c_str(),"/skinGui/right_hand_virtual:i"); + // if (robot=="icub") + // { + // Network::connect("/icub/camcalib/left/out",("/"+name+"/imageL:i").c_str()); + // Network::connect("/icub/camcalib/right/out",("/"+name+"/imageR:i").c_str()); + // } + // else + // { + // Network::connect("/icubSim/cam/left",("/"+name+"/imageL:i").c_str()); + // Network::connect("/icubSim/cam/right",("/"+name+"/imageR:i").c_str()); + // } + + // Network::connect(("/"+name+"/imageL:o").c_str(),"/vtRF/left"); + // Network::connect(("/"+name+"/imageR:o").c_str(),"/vtRF/right"); + + // Network::connect("/doubleTouch/status:o",("/"+name+"/input:i").c_str()); + // Network::connect("/visuoTactileWrapper/events:o",("/"+name+"/events:i").c_str()); + + // Network::connect(("/"+name+"/skinGuiForearmL:o").c_str(),"/skinGui/left_forearm_virtual:i"); + // Network::connect(("/"+name+"/skinGuiForearmR:o").c_str(),"/skinGui/right_forearm_virtual:i"); + // Network::connect(("/"+name+"/skinGuiHandL:o").c_str(),"/skinGui/left_hand_virtual:i"); + // Network::connect(("/"+name+"/skinGuiHandR:o").c_str(),"/skinGui/right_hand_virtual:i"); - Network::connect("/skinManager/skin_events:o",("/"+name+"/skin_events:i").c_str()); - - ts.update(); + // Network::connect("/skinManager/skin_events:o",("/"+name+"/skin_events:i").c_str()); + + ts.update(); /**************************/ if (rf->check("rightHand") || rf->check("rightForeArm") || - (!rf->check("rightHand") && !rf->check("rightForeArm") && !rf->check("leftHand") && !rf->check("leftForeArm"))) + (!rf->check("rightHand") && !rf->check("rightForeArm") && !rf->check("leftHand") && !rf->check("leftForeArm"))) { Property OptR; OptR.put("robot", robot.c_str()); @@ -134,7 +134,7 @@ bool vtRFThread::threadInit() /**************************/ if (rf->check("leftHand") || rf->check("leftForeArm") || - (!rf->check("rightHand") && !rf->check("rightForeArm") && !rf->check("leftHand") && !rf->check("leftForeArm"))) + (!rf->check("rightHand") && !rf->check("rightForeArm") && !rf->check("leftHand") && !rf->check("leftForeArm"))) { Property OptL; OptL.put("robot", robot.c_str()); @@ -290,10 +290,10 @@ void vtRFThread::run() if (inputEvents.size() != 0) { // read the events - for (size_t i = 0; i < inputEvents.size(); i++) + for (int i = 0; i < inputEvents.size(); i++) { incomingEvents.push_back(IncomingEvent(*(inputEvents.get(i).asList()))); - printMessage(3,"[EVENT] %s", incomingEvents.back().toString().c_str()); + printMessage(3,"[EVENT] %s\n", incomingEvents.back().toString().c_str()); } // manage the buffer @@ -415,76 +415,87 @@ void vtRFThread::manageSkinEvents() int iCubSkinID=-1; bool isThereAnEvent = false; + Bottle & out = ppsEventsPortOut.prepare(); out.clear(); + Bottle b; b.clear(); + if (incomingEvents.size()>0) // if there's an event { - for (size_t i = 0; i < iCubSkinSize; i++) // cycle through the skinparts + for (int i = 0; i < iCubSkinSize; i++) // cycle through the skinparts { - if (!isThereAnEvent) // process only one contact at a time + b.clear(); //so there will be one bottle per skin part (if there was a significant event) + taxelsIDs.clear(); + isThereAnEvent = false; + + //take only highly activated "taxels" + for (size_t j = 0; j < iCubSkin[i].taxels.size(); j++) // cycle through the taxels { - for (size_t j = 0; j < iCubSkin[i].taxels.size(); j++) // cycle through the taxels - { - if (dynamic_cast(iCubSkin[i].taxels[j])->Resp > 50) - { - taxelsIDs.push_back(iCubSkin[i].taxels[j]->getID()); - isThereAnEvent = true; - } - } - if (isThereAnEvent) + if (dynamic_cast(iCubSkin[i].taxels[j])->Resp > 50) + { - part = iCubSkin[i].name; - iCubSkinID = i; + taxelsIDs.push_back(iCubSkin[i].taxels[j]->getID()); + isThereAnEvent = true; } - else - taxelsIDs.clear(); } - } - } - - if (isThereAnEvent && taxelsIDs.size()>0) - { - Vector geoCenter(3,0.0), normalDir(3,0.0); - int w = 0, w_sum = 0; - Bottle b; - b.clear(); - - if (part == SkinPart_s[SKIN_LEFT_FOREARM] || part == SkinPart_s[SKIN_LEFT_HAND]) - { - b.addString("left"); - } - else if (part == SkinPart_s[SKIN_RIGHT_FOREARM] || part == SkinPart_s[SKIN_RIGHT_HAND]) - { - b.addString("right"); - } - - for (size_t i = 0; i < taxelsIDs.size(); ++i) - { - for (size_t j = 0; j < iCubSkin[iCubSkinID].taxels.size(); j++) + if (isThereAnEvent && taxelsIDs.size()>0) { - if (iCubSkin[iCubSkinID].taxels[j]->getID() == taxelsIDs[i]) + Vector geoCenter(3,0.0), normalDir(3,0.0); + Vector geoCenterWRF(3,0.0), normalDirWRF(3,0.0); //in world reference frame + int w = 0; + int w_max = 0; + int w_sum = 0; + part = iCubSkin[i].name; + + //the output format on the port will be: + //(SkinPart_enum x_linkFoR y_linkFoR z_linkFoR n1_linkFoR n2_linkFoR n3_linkFoR x_RootFoR y_RootFoR z_RootFoR n1_RootFoR n2_RootFoR n3_RootFoR magnitude SkinPart_string) + //paralleling the one produced in skinEventsAggregator skinEventsAggregThread::run() + + b.addInt(getSkinPartFromString(iCubSkin[i].name)); + + for (size_t k = 0; k < taxelsIDs.size(); k++) { - w = dynamic_cast(iCubSkin[iCubSkinID].taxels[j])->Resp; - geoCenter += iCubSkin[iCubSkinID].taxels[j]->getWRFPosition()*w; - normalDir += locateTaxel(iCubSkin[iCubSkinID].taxels[j]->getNormal(),part)*w; - w_sum += w; + for (size_t p = 0; p < iCubSkin[i].taxels.size(); p++) //these two loops are not an efficient implementation + { + if (iCubSkin[i].taxels[p]->getID() == taxelsIDs[k]) + { + w = dynamic_cast(iCubSkin[i].taxels[p])->Resp; + printMessage(4,"part %s: pps taxel ID %d, pos (%s), activation: %d\n",part.c_str(),taxelsIDs[k],iCubSkin[i].taxels[p]->getPosition().toString().c_str(),w); + //The final geoCenter and normalDir will be a weighted average of the activations + geoCenter += iCubSkin[i].taxels[p]->getPosition()*w; //Matej, 24.2., changing convention - link not Root FoR + normalDir += iCubSkin[i].taxels[p]->getNormal()*w; + geoCenterWRF += iCubSkin[i].taxels[p]->getWRFPosition()*w; //original code + normalDirWRF += locateTaxel(iCubSkin[i].taxels[p]->getNormal(),part)*w; + w_sum += w; + if (w>w_max) + w_max = w; + } + } } + + geoCenter /= w_sum; + normalDir /= w_sum; + geoCenterWRF /= w_sum; + normalDirWRF /= w_sum; + vectorIntoBottle(geoCenter,b); + vectorIntoBottle(normalDir,b); + vectorIntoBottle(geoCenterWRF,b); + vectorIntoBottle(normalDirWRF,b); + b.addDouble(w_max/255.0); //scaling - will be normalized in the end + b.addString(part); + out.addList().read(b); } } - - geoCenter /= w_sum; - normalDir /= w_sum; - vectorIntoBottle(geoCenter,b); - vectorIntoBottle(normalDir,b); - skinPortOut.setEnvelope(ts); - skinPortOut.write(b); // send something anyway (if there is no contact the bottle is empty) - } + + ppsEventsPortOut.setEnvelope(ts); + ppsEventsPortOut.write(); // let's send only if there was en event + } } void vtRFThread::sendContactsToSkinGui() { Vector respToSkin; - for(size_t i=0; igetID()); + yWarning("skinPart %d Taxel %d : no list of represented taxels is available, even if repr2TaxelList is not empty",i,iCubSkin[i].taxels[j]->getID()); respToSkin[iCubSkin[i].taxels[j]->getID()] = dynamic_cast(iCubSkin[i].taxels[j])->Resp; } else @@ -571,7 +582,7 @@ bool vtRFThread::detectContact(iCub::skinDynLib::skinContactList *_sCL, int &idx idv.clear(); if( it -> getPressure() > SKIN_THRES && (it -> getTaxelList()).size() > 2 ) { - for (size_t i = 0; i < iCubSkinSize; i++) + for (int i = 0; i < iCubSkinSize; i++) { if (SkinPart_s[it -> getSkinPart()] == iCubSkin[i].name) { @@ -624,7 +635,7 @@ string vtRFThread::load() Bottle b; b.read(data); yDebug("[vtRF::load] iCubSkinSize %i",iCubSkinSize); - for (size_t i = 0; i < iCubSkinSize; i++) + for (int i = 0; i < iCubSkinSize; i++) { Bottle bb = b.findGroup(iCubSkin[i].name.c_str()); @@ -660,7 +671,7 @@ string vtRFThread::load() yDebug("[vtRF::load][%s] size %i\tnTaxels %i\text %s\tbinsNum %i %i",iCubSkin[i].name.c_str(),size, nTaxels,toVector(ext).toString(3,3).c_str(),bNum[0],bNum[1]); printMessage(3,"Mapping\n"); - for (size_t j = 0; j < size; j++) + for (int j = 0; j < size; j++) { mapp.push_back(bbb->get(j).asInt()); if (verbosity>=3) @@ -671,13 +682,13 @@ string vtRFThread::load() if (verbosity>=3) printf("\n"); iCubSkin[i].taxel2Repr = mapp; - for (size_t j = 0; j < nTaxels; j++) + for (int j = 0; j < nTaxels; j++) { // 7 are the number of lines in the skinpart group that are not taxels bbb = bb.get(j+7).asList(); printMessage(3,"Reading taxel %s\n",bbb->toString().c_str()); - for (int k = 0; k < iCubSkin[i].taxels.size(); k++) + for (size_t k = 0; k < iCubSkin[i].taxels.size(); k++) { if (iCubSkin[i].taxels[k]->getID() == bbb->get(0).asInt()) { @@ -707,7 +718,7 @@ string vtRFThread::save() if (myfile.is_open()) { - for (size_t i = 0; i < iCubSkinSize; i++) + for (int i = 0; i < iCubSkinSize; i++) { Bottle data; data.clear(); @@ -805,38 +816,40 @@ bool vtRFThread::trainTaxels(const std::vector IDv, const int IDx) bool vtRFThread::projectIncomingEvent() { - for (size_t i = 0; i < iCubSkinSize; i++) + for (size_t k = 0; k < incomingEvents.size(); k++) { - Matrix T_a = eye(4); // transform matrix relative to the arm - if ((iCubSkin[i].name == SkinPart_s[SKIN_LEFT_FOREARM]) || (iCubSkin[i].name == SkinPart_s[SKIN_LEFT_HAND])) - { - iencsL->getEncoders(encsL->data()); - yarp::sig::Vector qL=encsL->subVector(0,6); - armL -> setAng(qL*CTRL_DEG2RAD); - if (iCubSkin[i].name == SkinPart_s[SKIN_LEFT_FOREARM]) - T_a = armL -> getH(3+4, true); - else //(iCubSkin[i].name == SkinPart_s[SKIN_LEFT_HAND]) - T_a = armL -> getH(3+6, true); - } - else if ((iCubSkin[i].name == SkinPart_s[SKIN_RIGHT_FOREARM]) || (iCubSkin[i].name == SkinPart_s[SKIN_RIGHT_HAND])) - { - iencsR->getEncoders(encsR->data()); - yarp::sig::Vector qR=encsR->subVector(0,6); - armR -> setAng(qR*CTRL_DEG2RAD); - if (iCubSkin[i].name == SkinPart_s[SKIN_RIGHT_FOREARM]) - T_a = armR -> getH(3+4, true); - else //(iCubSkin[i].name == SkinPart_s[SKIN_RIGHT_HAND]) - T_a = armR -> getH(3+6, true); - } - else - yError("[vtRFThread] in projectIncomingEvent!\n"); + for (int i = 0; i < iCubSkinSize; i++) + { + Matrix T_a = eye(4); // transform matrix relative to the arm + if ((iCubSkin[i].name == SkinPart_s[SKIN_LEFT_FOREARM]) || (iCubSkin[i].name == SkinPart_s[SKIN_LEFT_HAND])) + { + iencsL->getEncoders(encsL->data()); + yarp::sig::Vector qL=encsL->subVector(0,6); + armL -> setAng(qL*CTRL_DEG2RAD); + if (iCubSkin[i].name == SkinPart_s[SKIN_LEFT_FOREARM]) + T_a = armL -> getH(3+4, true); + else //(iCubSkin[i].name == SkinPart_s[SKIN_LEFT_HAND]) + T_a = armL -> getH(3+6, true); + } + else if ((iCubSkin[i].name == SkinPart_s[SKIN_RIGHT_FOREARM]) || (iCubSkin[i].name == SkinPart_s[SKIN_RIGHT_HAND])) + { + iencsR->getEncoders(encsR->data()); + yarp::sig::Vector qR=encsR->subVector(0,6); + armR -> setAng(qR*CTRL_DEG2RAD); + if (iCubSkin[i].name == SkinPart_s[SKIN_RIGHT_FOREARM]) + T_a = armR -> getH(3+4, true); + else //(iCubSkin[i].name == SkinPart_s[SKIN_RIGHT_HAND]) + T_a = armR -> getH(3+6, true); + } + else + yError("[vtRFThread] in projectIncomingEvent!\n"); - // yInfo("T_A:\n%s",T_a.toString().c_str()); + // yInfo("T_A:\n%s",T_a.toString().c_str()); - for (size_t j = 0; j < iCubSkin[i].taxels.size(); j++) - { - dynamic_cast(iCubSkin[i].taxels[j])->Evnt=projectIntoTaxelRF(iCubSkin[i].taxels[j]->getFoR(),T_a, - incomingEvents[incomingEvents.size()-1]); + for (size_t j = 0; j < iCubSkin[i].taxels.size(); j++) + { + dynamic_cast(iCubSkin[i].taxels[j])->Evnt=projectIntoTaxelRF(iCubSkin[i].taxels[j]->getFoR(),T_a, + incomingEvents[k]); // There's a reason behind this choice dumpedVector.push_back(dynamic_cast(iCubSkin[i].taxels[j])->Evnt.Pos[0]); @@ -844,6 +857,7 @@ bool vtRFThread::projectIncomingEvent() dumpedVector.push_back(dynamic_cast(iCubSkin[i].taxels[j])->Evnt.Pos[2]); printMessage(5,"Projection -> i: %i\tID %i\tEvent: %s\n",i,j,dynamic_cast(iCubSkin[i].taxels[j])->Evnt.toString().c_str()); + } } } return true; @@ -873,7 +887,7 @@ IncomingEvent4TaxelPWE vtRFThread::projectIntoTaxelRF(const Matrix &RF,const Mat void vtRFThread::resetParzenWindows() { - for (size_t i = 0; i < iCubSkinSize; i++) + for (int i = 0; i < iCubSkinSize; i++) { for (size_t j = 0; j < iCubSkin[i].taxels.size(); j++) { @@ -884,7 +898,7 @@ void vtRFThread::resetParzenWindows() bool vtRFThread::computeResponse() { - for (size_t i = 0; i < iCubSkinSize; i++) + for (int i = 0; i < iCubSkinSize; i++) { for (size_t j = 0; j < iCubSkin[i].taxels.size(); j++) { @@ -944,7 +958,7 @@ void vtRFThread::drawTaxels(string _eye) return; } - for (size_t i = 0; i < iCubSkinSize; i++) + for (int i = 0; i < iCubSkinSize; i++) { for (size_t j = 0; j < iCubSkin[i].taxels.size(); j++) { @@ -969,9 +983,9 @@ void vtRFThread::drawTaxel(ImageOf &Im, const yarp::sig::Vector &px, if ((u >= r) && (u <= 320 - r) && (v >= r) && (v <= 240 - r)) { - for (size_t x=0; x<2*r; x++) + for (int x=0; x<2*r; x++) { - for (size_t y=0; y<2*r; y++) + for (int y=0; y<2*r; y++) { if (part == SkinPart_s[SKIN_LEFT_FOREARM] || part == SkinPart_s[SKIN_RIGHT_FOREARM] || part == SkinPart_s[SKIN_LEFT_HAND] || part == SkinPart_s[SKIN_RIGHT_HAND]) @@ -1148,9 +1162,10 @@ bool vtRFThread::setTaxelPosesFromFile(const string filePath, skinPartPWE &sP) yarp::os::Bottle &calibration = rf.findGroup("calibration"); if (calibration.isNull()) { - yError("[skinPart::setTaxelPosesFromFile] No calibration group found!"); + yError("[vtRFThread::setTaxelPosesFromFile] No calibration group found!"); return false; } + printMessage(6,"[vtRFThread::setTaxelPosesFromFile] found %i taxels (not all of them are valid taxels).\n", calibration.size()-1); // First item of the bottle is "calibration", so we should not use it for (int i = 1; i < calibration.size()-1; i++) @@ -1247,6 +1262,7 @@ bool vtRFThread::setTaxelPosesFromFile(const string filePath, skinPartPWE &sP) void vtRFThread::initRepresentativeTaxels(skinPart &sP) { + printMessage(6,"[vtRFThread::initRepresentativeTaxels] Initializing representative taxels for %s\n",sP.name.c_str()); int i=0; list taxels_list; if (sP.name == SkinPart_s[SKIN_LEFT_FOREARM] || sP.name == SkinPart_s[SKIN_RIGHT_FOREARM]) @@ -1780,6 +1796,10 @@ void vtRFThread::threadRelease() closePort(skinPortIn); yDebug(" skinPortIn successfully closed!\n"); + ppsEventsPortOut.interrupt(); + ppsEventsPortOut.close(); + yDebug("ppsEventsPortOut successfully closed!\n"); + // closePort(skinGuiPortForearmL); skinGuiPortForearmL.interrupt(); skinGuiPortForearmL.close(); diff --git a/modules/visuoTactileRF/vtRFThread.h b/modules/visuoTactileRF/vtRFThread.h index 4814af4..22a12d8 100644 --- a/modules/visuoTactileRF/vtRFThread.h +++ b/modules/visuoTactileRF/vtRFThread.h @@ -104,7 +104,7 @@ class vtRFThread: public RateThread BufferedPort skinGuiPortHandR; BufferedPort *skinPortIn; // input from the skinManager - Port skinPortOut; // output for the events + BufferedPort ppsEventsPortOut; // output for the events Port dataDumperPortOut; // output for the dataDumper (quick thing yarp::sig::Vector dumpedVector; diff --git a/modules/visuoTactileWrapper/vtWThread.cpp b/modules/visuoTactileWrapper/vtWThread.cpp index 5087073..c7a0602 100644 --- a/modules/visuoTactileWrapper/vtWThread.cpp +++ b/modules/visuoTactileWrapper/vtWThread.cpp @@ -11,16 +11,16 @@ vtWThread::vtWThread(int _rate, const string &_name, const string &_robot, int _ RateThread(_rate), name(_name), robot(_robot), verbosity(_v) { optFlowPos.resize(3,0.0); - optFlowVelEstimate.resize(3,0.0); + optFlowVel.resize(3,0.0); pf3dTrackerPos.resize(3,0.0); - pf3dTrackerVelEstimate.resize(3,0.0); + pf3dTrackerVel.resize(3,0.0); doubleTouchPos.resize(3,0.0); - doubleTouchVelEstimate.resize(3,0.0); + doubleTouchVel.resize(3,0.0); fgtTrackerPos.resize(3,0.0); - fgtTrackerVelEstimate.resize(3,0.0); + fgtTrackerVel.resize(3,0.0); armR = new iCubArm("right"); armL = new iCubArm("left"); @@ -36,6 +36,7 @@ bool vtWThread::threadInit() pf3dTrackerPort.open(("/"+name+"/pf3dTracker:i").c_str()); doubleTouchPort.open(("/"+name+"/doubleTouch:i").c_str()); fgtTrackerPort.open(("/"+name+"/fingertipTracker:i").c_str()); + genericObjectsPort.open(("/"+name+"/genericObjects:i").c_str()); outPortGui.open(("/"+name+"/gui:o").c_str()); eventsPort.open(("/"+name+"/events:o").c_str()); depth2kinPort.open(("/"+name+"/depth2kin:o").c_str()); @@ -44,6 +45,7 @@ bool vtWThread::threadInit() Network::connect("/pf3dTracker/data:o",("/"+name+"/pf3dTracker:i").c_str()); Network::connect("/doubleTouch/status:o",("/"+name+"/doubleTouch:i").c_str()); Network::connect("/fingertipTracker/out:o",("/"+name+"/fingertipTracker:i").c_str()); + Network::connect("/objectGeneratorSim/obstacles:o",("/"+name+"/genericObjects:i").c_str()); Network::connect(("/"+name+"/events:o").c_str(),"/visuoTactileRF/events:i"); Network::connect(("/"+name+"/gui:o").c_str(),"/iCubGui/objects"); @@ -128,7 +130,6 @@ bool vtWThread::threadInit() void vtWThread::run() { - optFlowPos.resize(3,0.0); pf3dTrackerPos.resize(3,0.0); doubleTouchPos.resize(3,0.0); @@ -137,6 +138,28 @@ void vtWThread::run() bool isTarget = false; events.clear(); + // process the generiObject port + if (genericObjectsBottle = genericObjectsPort.read(false)) + { + for (int i = 0; i < genericObjectsBottle->size(); i++) + { + Bottle b = *(genericObjectsBottle->get(i).asList()); + if (b.size()>=4) + { + Vector p(3,0.0); + double r=0; + + p[0] = b.get(0).asDouble(); + p[1] = b.get(1).asDouble(); + p[2] = b.get(2).asDouble(); + r = b.get(3).asDouble(); + + events.push_back(IncomingEvent(p,Vector(3,0.0),r,"genericObjects")); + isTarget=true; + } + } + } + // process the optFlow if (optFlowBottle = optFlowPort.read(false)) { @@ -150,9 +173,9 @@ void vtWThread::run() optFlowPos[2]=optFlowBottle->get(2).asDouble(); AWPolyElement el(optFlowPos,Time::now()); - optFlowVelEstimate=linEst_optFlow->estimate(el); + optFlowVel=linEst_optFlow->estimate(el); - events.push_back(IncomingEvent(optFlowPos,optFlowVelEstimate,0.05,"optFlow")); + events.push_back(IncomingEvent(optFlowPos,optFlowVel,0.05,"optFlow")); isTarget=true; } } @@ -185,9 +208,9 @@ void vtWThread::run() pf3dTrackerPos.pop_back(); AWPolyElement el(pf3dTrackerPos,Time::now()); - pf3dTrackerVelEstimate=linEst_pf3dTracker->estimate(el); + pf3dTrackerVel=linEst_pf3dTracker->estimate(el); - events.push_back(IncomingEvent(pf3dTrackerPos,pf3dTrackerVelEstimate,0.05,"pf3dTracker")); + events.push_back(IncomingEvent(pf3dTrackerPos,pf3dTrackerVel,0.05,"pf3dTracker")); isTarget=true; } } @@ -211,7 +234,7 @@ void vtWThread::run() fgtTrackerPos[1] = fgtTrackerBottle->get(2).asDouble(); fgtTrackerPos[2] = fgtTrackerBottle->get(3).asDouble(); AWPolyElement el2(fgtTrackerPos,Time::now()); - fgtTrackerVelEstimate=linEst_fgtTracker->estimate(el2); + fgtTrackerVel=linEst_fgtTracker->estimate(el2); if(doubleTouchStep<=1) { @@ -221,7 +244,7 @@ void vtWThread::run() else if(doubleTouchStep>1 && doubleTouchStep<8) { events.clear(); - events.push_back(IncomingEvent(fgtTrackerPos,fgtTrackerVelEstimate,-1.0,"fingertipTracker")); + events.push_back(IncomingEvent(fgtTrackerPos,fgtTrackerVel,-1.0,"fingertipTracker")); isTarget=true; } } @@ -281,8 +304,8 @@ void vtWThread::run() yDebug("Computing data from the doubleTouch %g\n",getEstUsed()); AWPolyElement el2(doubleTouchPos,Time::now()); - doubleTouchVelEstimate=linEst_doubleTouch->estimate(el2); - events.push_back(IncomingEvent(doubleTouchPos,doubleTouchVelEstimate,-1.0,"doubleTouch")); + doubleTouchVel=linEst_doubleTouch->estimate(el2); + events.push_back(IncomingEvent(doubleTouchPos,doubleTouchVel,-1.0,"doubleTouch")); isTarget=true; } } @@ -328,7 +351,7 @@ void vtWThread::sendGuiTarget() { Bottle obj; obj.addString("object"); - obj.addString("Target"); + obj.addString("obstacle"); // size obj.addDouble(50.0); @@ -347,8 +370,8 @@ void vtWThread::sendGuiTarget() // color obj.addInt(255); - obj.addInt(125); - obj.addInt(125); + obj.addInt(0); + obj.addInt(0); // transparency obj.addDouble(0.9); diff --git a/modules/visuoTactileWrapper/vtWThread.h b/modules/visuoTactileWrapper/vtWThread.h index 372fec3..7a52660 100644 --- a/modules/visuoTactileWrapper/vtWThread.h +++ b/modules/visuoTactileWrapper/vtWThread.h @@ -92,17 +92,17 @@ class vtWThread: public RateThread BufferedPort pf3dTrackerPort; Bottle *pf3dTrackerBottle; Vector pf3dTrackerPos; - Vector pf3dTrackerVelEstimate; + Vector pf3dTrackerVel; BufferedPort optFlowPort; Bottle *optFlowBottle; Vector optFlowPos; - Vector optFlowVelEstimate; + Vector optFlowVel; BufferedPort doubleTouchPort; Bottle *doubleTouchBottle; Vector doubleTouchPos; - Vector doubleTouchVelEstimate; + Vector doubleTouchVel; int doubleTouchStep; // the step the doubleTouch is in string currentTask; @@ -110,7 +110,10 @@ class vtWThread: public RateThread BufferedPort fgtTrackerPort; Bottle *fgtTrackerBottle; Vector fgtTrackerPos; - Vector fgtTrackerVelEstimate; + Vector fgtTrackerVel; + + BufferedPort genericObjectsPort; + Bottle *genericObjectsBottle; // Velocity Estimators (using adaptive window linear fitting) AWLinEstimator *linEst_optFlow;