alecive / gaze-stabilization

This repository deals with a complete framework for gaze stabilization in the iCub humanoid robot.

Gaze Stabilization

This repository deals with a complete framework for gaze stabilization in the iCub humanoid robot.

gaze-stabilization's Issues

Fix memory leak into gazeEstimator

This is the valgrind.txt output

Module finished
==1139== 
==1139== HEAP SUMMARY:
==1139==     in use at exit: 19,120,358 bytes in 1,142 blocks
==1139==   total heap usage: 68,295 allocs, 67,153 frees, 191,768,608 bytes allocated
==1139== 
==1139== 2,190 (176 direct, 2,014 indirect) bytes in 1 blocks are definitely lost in loss record 74 of 88
==1139==    at 0x4C286E7: operator new(unsigned long) (vg_replace_malloc.c:287)
==1139==    by 0x40D143: gazeEvaluatorThread::gazeEvaluatorThread(int, std::string, std::string, int) (in /usr/local/src/robot/iCubContrib/bin/gazeEvaluator)
==1139==    by 0x40F6DA: gazeEvaluator::configure(yarp::os::ResourceFinder&) (in /usr/local/src/robot/iCubContrib/bin/gazeEvaluator)
==1139==    by 0x4F615F9: yarp::os::RFModule::runModule(yarp::os::ResourceFinder&) (in /usr/local/src/robot/yarp/build-x86_64/lib/libYARP_OS.so.2.3.62.2)
==1139==    by 0x40AE31: main (in /usr/local/src/robot/iCubContrib/bin/gazeEvaluator)
==1139== 
==1139== 230,424 bytes in 1 blocks are possibly lost in loss record 79 of 88
==1139==    at 0x4C28BED: malloc (vg_replace_malloc.c:263)
==1139==    by 0xC0754EC: cv::fastMalloc(unsigned long) (in /usr/local/src/robot/opencv/opencv-2.4.8/build/lib/libopencv_core.so.2.4.8)
==1139==    by 0xC0806FD: cvCreateData (in /usr/local/src/robot/opencv/opencv-2.4.8/build/lib/libopencv_core.so.2.4.8)
==1139==    by 0xC08F242: cvCloneImage (in /usr/local/src/robot/opencv/opencv-2.4.8/build/lib/libopencv_core.so.2.4.8)
==1139==    by 0xC04D832: cvClone (in /usr/local/src/robot/opencv/opencv-2.4.8/build/lib/libopencv_core.so.2.4.8)
==1139==    by 0x40C877: gazeEvaluatorThread::run() (in /usr/local/src/robot/iCubContrib/bin/gazeEvaluator)
==1139==    by 0x4F54491: RateThreadCallbackAdapter::run() (in /usr/local/src/robot/yarp/build-x86_64/lib/libYARP_OS.so.2.3.62.2)
==1139==    by 0x4F8635F: theExecutiveBranch(void*) (in /usr/local/src/robot/yarp/build-x86_64/lib/libYARP_OS.so.2.3.62.2)
==1139==    by 0xF54D635: ACE_OS_Thread_Adapter::invoke() (in /usr/lib/libACE-6.0.3.so)
==1139==    by 0xDB1FB4F: start_thread (pthread_create.c:304)
==1139==    by 0xF1A10EC: clone (clone.S:112)
==1139== 
==1139== 230,592 (168 direct, 230,424 indirect) bytes in 1 blocks are definitely lost in loss record 81 of 88
==1139==    at 0x4C28BED: malloc (vg_replace_malloc.c:263)
==1139==    by 0xC0754EC: cv::fastMalloc(unsigned long) (in /usr/local/src/robot/opencv/opencv-2.4.8/build/lib/libopencv_core.so.2.4.8)
==1139==    by 0xC08F191: cvCloneImage (in /usr/local/src/robot/opencv/opencv-2.4.8/build/lib/libopencv_core.so.2.4.8)
==1139==    by 0xC04D832: cvClone (in /usr/local/src/robot/opencv/opencv-2.4.8/build/lib/libopencv_core.so.2.4.8)
==1139==    by 0x40C4C7: gazeEvaluatorThread::run() (in /usr/local/src/robot/iCubContrib/bin/gazeEvaluator)
==1139==    by 0x4F54491: RateThreadCallbackAdapter::run() (in /usr/local/src/robot/yarp/build-x86_64/lib/libYARP_OS.so.2.3.62.2)
==1139==    by 0x4F8635F: theExecutiveBranch(void*) (in /usr/local/src/robot/yarp/build-x86_64/lib/libYARP_OS.so.2.3.62.2)
==1139==    by 0xF54D635: ACE_OS_Thread_Adapter::invoke() (in /usr/lib/libACE-6.0.3.so)
==1139==    by 0xDB1FB4F: start_thread (pthread_create.c:304)
==1139==    by 0xF1A10EC: clone (clone.S:112)
==1139== 
==1139== 232,592 (72 direct, 232,520 indirect) bytes in 1 blocks are definitely lost in loss record 82 of 88
==1139==    at 0x4C286E7: operator new(unsigned long) (vg_replace_malloc.c:287)
==1139==    by 0x40D15C: gazeEvaluatorThread::gazeEvaluatorThread(int, std::string, std::string, int) (in /usr/local/src/robot/iCubContrib/bin/gazeEvaluator)
==1139==    by 0x40F6DA: gazeEvaluator::configure(yarp::os::ResourceFinder&) (in /usr/local/src/robot/iCubContrib/bin/gazeEvaluator)
==1139==    by 0x4F615F9: yarp::os::RFModule::runModule(yarp::os::ResourceFinder&) (in /usr/local/src/robot/yarp/build-x86_64/lib/libYARP_OS.so.2.3.62.2)
==1139==    by 0x40AE31: main (in /usr/local/src/robot/iCubContrib/bin/gazeEvaluator)
==1139== 
==1139== 4,842,600 (3,696 direct, 4,838,904 indirect) bytes in 22 blocks are definitely lost in loss record 84 of 88
==1139==    at 0x4C28BED: malloc (vg_replace_malloc.c:263)
==1139==    by 0xC0754EC: cv::fastMalloc(unsigned long) (in /usr/local/src/robot/opencv/opencv-2.4.8/build/lib/libopencv_core.so.2.4.8)
==1139==    by 0xC08F191: cvCloneImage (in /usr/local/src/robot/opencv/opencv-2.4.8/build/lib/libopencv_core.so.2.4.8)
==1139==    by 0xC04D832: cvClone (in /usr/local/src/robot/opencv/opencv-2.4.8/build/lib/libopencv_core.so.2.4.8)
==1139==    by 0x40C877: gazeEvaluatorThread::run() (in /usr/local/src/robot/iCubContrib/bin/gazeEvaluator)
==1139==    by 0x4F54491: RateThreadCallbackAdapter::run() (in /usr/local/src/robot/yarp/build-x86_64/lib/libYARP_OS.so.2.3.62.2)
==1139==    by 0x4F8635F: theExecutiveBranch(void*) (in /usr/local/src/robot/yarp/build-x86_64/lib/libYARP_OS.so.2.3.62.2)
==1139==    by 0xF54D635: ACE_OS_Thread_Adapter::invoke() (in /usr/lib/libACE-6.0.3.so)
==1139==    by 0xDB1FB4F: start_thread (pthread_create.c:304)
==1139==    by 0xF1A10EC: clone (clone.S:112)
==1139== 
==1139== 6,762,624 (3,696 direct, 6,758,928 indirect) bytes in 22 blocks are definitely lost in loss record 87 of 88
==1139==    at 0x4C28BED: malloc (vg_replace_malloc.c:263)
==1139==    by 0xC0754EC: cv::fastMalloc(unsigned long) (in /usr/local/src/robot/opencv/opencv-2.4.8/build/lib/libopencv_core.so.2.4.8)
==1139==    by 0xC08F191: cvCloneImage (in /usr/local/src/robot/opencv/opencv-2.4.8/build/lib/libopencv_core.so.2.4.8)
==1139==    by 0x40BD83: gazeEvaluatorThread::drawFlowModule(_IplImage*) (in /usr/local/src/robot/iCubContrib/bin/gazeEvaluator)
==1139==    by 0x40C266: gazeEvaluatorThread::sendOptFlow() (in /usr/local/src/robot/iCubContrib/bin/gazeEvaluator)
==1139==    by 0x40C8B4: gazeEvaluatorThread::run() (in /usr/local/src/robot/iCubContrib/bin/gazeEvaluator)
==1139==    by 0x4F54491: RateThreadCallbackAdapter::run() (in /usr/local/src/robot/yarp/build-x86_64/lib/libYARP_OS.so.2.3.62.2)
==1139==    by 0x4F8635F: theExecutiveBranch(void*) (in /usr/local/src/robot/yarp/build-x86_64/lib/libYARP_OS.so.2.3.62.2)
==1139==    by 0xF54D635: ACE_OS_Thread_Adapter::invoke() (in /usr/lib/libACE-6.0.3.so)
==1139==    by 0xDB1FB4F: start_thread (pthread_create.c:304)
==1139==    by 0xF1A10EC: clone (clone.S:112)
==1139== 
==1139== 6,762,624 (3,696 direct, 6,758,928 indirect) bytes in 22 blocks are definitely lost in loss record 88 of 88
==1139==    at 0x4C28BED: malloc (vg_replace_malloc.c:263)
==1139==    by 0xC0754EC: cv::fastMalloc(unsigned long) (in /usr/local/src/robot/opencv/opencv-2.4.8/build/lib/libopencv_core.so.2.4.8)
==1139==    by 0xC08F191: cvCloneImage (in /usr/local/src/robot/opencv/opencv-2.4.8/build/lib/libopencv_core.so.2.4.8)
==1139==    by 0x40BD93: gazeEvaluatorThread::drawFlowModule(_IplImage*) (in /usr/local/src/robot/iCubContrib/bin/gazeEvaluator)
==1139==    by 0x40C266: gazeEvaluatorThread::sendOptFlow() (in /usr/local/src/robot/iCubContrib/bin/gazeEvaluator)
==1139==    by 0x40C8B4: gazeEvaluatorThread::run() (in /usr/local/src/robot/iCubContrib/bin/gazeEvaluator)
==1139==    by 0x4F54491: RateThreadCallbackAdapter::run() (in /usr/local/src/robot/yarp/build-x86_64/lib/libYARP_OS.so.2.3.62.2)
==1139==    by 0x4F8635F: theExecutiveBranch(void*) (in /usr/local/src/robot/yarp/build-x86_64/lib/libYARP_OS.so.2.3.62.2)
==1139==    by 0xF54D635: ACE_OS_Thread_Adapter::invoke() (in /usr/lib/libACE-6.0.3.so)
==1139==    by 0xDB1FB4F: start_thread (pthread_create.c:304)
==1139==    by 0xF1A10EC: clone (clone.S:112)
==1139== 
==1139== LEAK SUMMARY:
==1139==    definitely lost: 11,504 bytes in 69 blocks
==1139==    indirectly lost: 18,821,718 bytes in 86 blocks
==1139==      possibly lost: 230,424 bytes in 1 blocks
==1139==    still reachable: 56,712 bytes in 986 blocks
==1139==         suppressed: 0 bytes in 0 blocks
==1139== Reachable blocks (those to which a pointer was found) are not shown.
==1139== To see them, rerun with: --leak-check=full --show-reachable=yes
==1139== 
==1139== For counts of detected and suppressed errors, rerun with: -v
==1139== ERROR SUMMARY: 7 errors from 7 contexts (suppressed: 9 from 9)

Stop motion upon failure detection

From gitlab:

@pattacini : When a sub-set of joints is detected as unhealthy, is there a mechanism to stop the movement completely, including also the healthy joints?

me: Right now, I'm simply stopping the whole module from doing anything else. Hence, I expect the real robot to stop because it doesn't receive any other command after the 100ms delay. Do I have instead to send 0 velocities to the healthy joints?

@pattacini : Yep, you have to for two main reasons:

(1) safety

(2) high-level controller should not rely on how the low-level controller is implemented, i.e. the internal timeout is done just for internal safety

Make the nicest video ever and publish it on youtube

Feed the integrator with the old measurement if there is no gyro

This repo is currently private, shall we make it public?

Put integrator after xdot_FP estimation

the title explains it all

Better handle the integrator

The integrator should be resetted if the norm of the gyro goes below a certain value and the norm of the acceleration goes below a certain value. In this way we do not shut it down if there is an abrupt change in velocity (e.g. the velocity goes from positive to negative instead going to zero).

Measurement noise on IMU data

Don't apply any dead-band on the imu measurement data because noise will interfere with the integrator. Rather, tune the imuFilter.

Remove completely the dead-band from the software.