This application performs reaching and gazing while learning the head and arm body schema, (i.e. More...
This application performs reaching and gazing while learning the head and arm body schema, (i.e.
something equivalent to the D.H parameters of the robot)
The following modules are instantiated:
Here is how they are interconnected
One file describes the initial body schema. This is ./conf/icub_head_right_arm_tree.xml or ./conf/icub_head_right_arm_tree_safe.xml for more restrictive joint ranges This filename is given as a parameters to all the modules.
Each module apart from the kinematic_simulator has its own configuration file So
The application can be run on with various level of complexity, so it is advise to proceed in an incremental manner and do some preliminary checking.
1. Check the reaching controller and the simulator.
This can be done by launching the sim_launch script, which launches the reaching module and the kinematic simulation module. For this you need to go to $ICUB_DIR/src/lasaBodySchema/conf/config_reaching_right_arm.ini and uncomment the "simulation parameter"
If you don't see an openGL window with an arm reaching to random targets, then something is wrong and it's a good starting point for debugging. If it works, comment simulation parameter.
2. Check the stereovision. This involves the calibration of the cameras intrinsics and extrinsic parameters as well as the color tracking. This is quite tedious, but it should only be done once. If you change the camera lenses, you have to do it again.
For this you will need a -- a color patch, that will be tracked by the vision system. -- a chessboard for camera calibration
a. go into the stereoVisualTracker directory
b. write a configuration file conf/my_conf_file.ini according to the doc in icub_stereoVisualTracker
c. make sure that the required directory (the "directory" parameter exists in ./params/
d. launch the icub_stereoVisualTracker on a machine that has opencv installed.
./stereoVisualTracker --name /my_name --file conf/my_conf_file.ini;
e. If the module finds no calibration file, it will launch the calibration. Put the checkerboard in the field of view of the camera and press n, making sure the opencv window is active. Repeat this as long as you are asked to (10 times), each time putting the checker board in a different position. Then do the same for the other camera
f. Make sure that the right camera image is seen on the window 0, and the left camera image on the window 1
g. Press 't' meaning that you want to track a color blob. The word 'tracking' should appear in the terminal
h. Approach your color marker from the cameras and in each window delimit an imaginary rectangle with the mouse (left button) on the color blob. This indicate the color to track. You can add additional rectangles with the same button or reset the color by clicking in the window with the right button. The pixels corresponding to a this color are highlighted in the window and an ellipse is drawn at the estimated position of the marker, so you can evaluate if you marker is well tracked. You can save the color parameters by pressing 's' when the opencv window is active. In the terminal type a name for the color you are tracking, when asked. You will then be able to load this color with the command 'g').
i. Make sure that all the robot torso, head and eye joints are in a zero position (using controlMotorGui for example)
h. Press 'l' while having an opencv window active, for locating the marker in 3d
i. If the module cannot find the extrinsic parameters file, it will launch the 3d calibration. Hold the chessboard vertically straight in front of the robot. When it finds it, it calibrates and prints "calibration done" in the terminal.
j. Press 'h' while having an opencv window active to center the frame of reference for the localization between the two eyes.
h. Press 'u' to open a port and stream out the position of the marker.
i. Launch the kinematic_simulator and the head_transfo modules and connect the ouput of the stereovision to it.
./simulator conf/icub_head_right_arm_tree.xml conf/icub_head_right_arm_shape.xml ./head_transfo_module --file conf/config_head2body.ini --name /transfo sleep 1 yarp connect /icub/head/state:o /simulator/head:i yarp connect /icub/head/state:o /transfo/proprio_head:i yarp connect /my_name/vision0:o /transfo/position:i yarp connect /transfo/position:o /simulator/target:i yarp connect /simulator/cmd:o /transfo/cmd:i
You should see you color blob represented by a sphere at approximately the right position. Make sure that when you move it up and dow, left and right, back and forth, the same appends in the simulator. This may well not be the case. If it is not, you need to recalibrate the extrinsic parameters of the cameras. Put the robot head and torso in the zero position. Press '3' in the opencv window. Approach the checkerboard covering some squares with the hand. Hold it straight vertically. Make sure the lower corner of the checkerboard is closer from the cameras than the upper corner. And the same thing for the right and side (I think, but maybe it is the other way around). Then uncover the squares. If it is still not good you can try again or go in the code of the icub_head_transfo module (src/Head2BodyCentered.cpp) to change the signs. If you do this, you should do the same change in the body_schema_module (src/BodySchemaLearning.cpp)
When the vision seems ok, you can try the real thing. Press 'Esc' in simulator window to stop the icub_kinematic_simulator and the icub_head_transfo modules.
3. Calibrate the second color marker
a. In the opencv window press 'a' to add and object to track. b. Approach a color marker from the two cameras and select a rectangle in each window as done previously. c. When the tracking seems ok put the color marker on the robot thumb so as to avoid occlusions as much as possible. d. You can save and load color files by pressing 's' and 'g' respectively. You can swtich between colors for color calibration by pressing 'n'. The number in the terminal indicates which color marker is ready for calibration or saving or loading
4. Launch the icub_visualRotation modules on machine that has opencv installed
5. Launch the icub_velocityControl for the right arm and the head on the PC104
./velocityControl --part head
And in another terminal
./velocityControl --part right_arm
6. The you can try to launch the whole application.
See the details of this script to see what are the possible options. In the full operation mode, the robot should try to reach to the target (first color blob) if it is within its reach. It should also adapt its body schema if it sees its hand (second color blob). The simulator should show both the original (on the left) and the adapted (on the right) body schemas. You can change the view of the simulator with the mouse.
7. To stop the application, press the "Esc" button in the kinematic simulator window. The icub_velocityControl modules must be stopped separately.
8. To relaunch the application go back to 5 and 6. (It could be possible to keep the the icub_velocityControl modules running and just go to 6, but for security reasons I didn't do it.)
9. To stop the stereoVisualTracker, press 'q' when the opencv window is active
10. To stop the icub_visualRotation use Ctl-C. For some reason it doesn't shut down very cleanly
CopyPolicy: Released under the terms of the GNU GPL v2.0.
This file can be edited at src/lasaBodySchema/dox/doc.dox