workspace/calibOptim at master · hou-zp/workspace · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
#!/bin/bash
## To calibrate, init the robot, start walk and start arucoCalibration on rhio.
## After the set number of frames, the file arucoCalibration.csv will be available on the robot's env
## This scripts copies that file, starts the calibration and sends the results to the robot.

echo "A new day dawns"

host=$(ssh rhoban@10.0.0.1 hostname)
robot_home=$(ssh rhoban@10.0.0.1 pwd)
echo "Robot home is "$robot_home
echo "Going to perform a calibration for $host"

ARUCO_DIR="$robot_home/env/$host/arucoCalibration.csv"
MODEL_DIR="$HOME/workspace/devel_release/lib/model/"
LOCAL_DIR="$HOME/workspace/devel_release/lib/model/$host"
ENV_DIR="$HOME/workspace/env/$host"

#echo "Changing the file $ENV_DIR/rhal.json, setting pitch to 0"
#python $HOME/workspace/env/setOffset.py $ENV_DIR/rhal.json head_pitch 0 0

echo "*** Deploy-enving..."
bash deploy-env

echo "Start RhobanServer now and press enter to init"
read input

rhio 10.0.0.1 init
echo "Press enter to Ned Stark"
read input
rhio 10.0.0.1 /lowlevel/head_pitch/torqueEnable=0

echo "Set by hand the maximum neck angle (the robot must be looking its feet) and press enter..."
read input

IDEAL_PITCH=110.7
value=$(rhio 10.0.0.1 /lowlevel/head_pitch/position)
pitch=$(echo $value| cut -d'=' -f 2)
echo "Pitch is $pitch"
echo "Ideal pitch is $IDEAL_PITCH"
echo "Changing the file $ENV_DIR/rhal.json"
python $HOME/workspace/env/setOffset.py $ENV_DIR/rhal.json head_pitch $pitch $IDEAL_PITCH

echo "*** Deploy-enving..."
bash deploy-env

echo "Setting the calibration pipeline on the robot..."
## Sad fix
ssh rhoban@10.0.0.1 rm $robot_home/env/$host/LocalisationTest.xml
ssh rhoban@10.0.0.1 rm $robot_home/env/$host/LocalisationTest.xml
ssh rhoban@10.0.0.1 rm $robot_home/env/$host/LocalisationTest.xml
ssh rhoban@10.0.0.1 ln -sf $robot_home/env/common/arucoCalibEmbedded.xml $robot_home/env/$host/LocalisationTest.xml
ssh rhoban@10.0.0.1 ln -sf $robot_home/env/common/arucoCalibEmbedded.xml $robot_home/env/$host/LocalisationTest.xml
ssh rhoban@10.0.0.1 ln -sf $robot_home/env/common/arucoCalibEmbedded.xml $robot_home/env/$host/LocalisationTest.xml


echo "*** Restart RhobanServer, when the lowlevel is ready press enter..."
read input

echo "Press enter to walk"
read input
rhio 10.0.0.1 init
rhio 10.0.0.1 walk

echo "Hold me in the air for the pressure tare..."
read input
rhio 10.0.0.1 tare

echo "Starting the gyroTare. Make sure the robot is perfectly idle, then press enter..."
read input
rhio 10.0.0.1 gyroTare

echo "Set the robot on the calibration arena and press enter to start recording frames..."
read input
rhio 10.0.0.1 arucoCalibration

#result=""
#while true ; do
#    nbFrames=$(rhio 10.0.0.1 /moves/arucoCalibration/currentNbFrames)
#    echo -ne "Wait until the robot stops moving and press enter ($nbFrames)"\\r
#    if $(read -t 0.1); then
#        break
#    fi
#done
echo "Wait until the robot stops moving and press enter..."
read input

echo "*** Creating $LOCAL_DIR..."
mkdir $LOCAL_DIR


echo "*** Retrieving the calibration data from the robot"
if scp rhoban@10.0.0.1:$ARUCO_DIR $LOCAL_DIR
then
    echo "scp succeeded."
else
    echo "scp failed, find the closest monkey and blame him"
    exit
fi
#echo "*** Deleting the calibration data on the robot"
#ssh rhoban@10.0.0.1 rm -rf $ARUCO_DIR

echo "*** Copying the urdf for $host..."
rm $LOCAL_DIR/sigmaban.urdf
if cp $ENV_DIR/sigmaban.urdf $LOCAL_DIR
then
    echo "urdf copied."
else
    echo "cp failed, check that $ENV_DIR/sigmaban.urdf exists"
    exit
fi

echo "*** Starting the optimization"
cd $LOCAL_DIR
parent="$(dirname "$LOCAL_DIR")"
$parent/appCameraModelLearning ./arucoCalibration.csv $host

echo "*** Moving the result to the local robot's environment (on your pc: $ENV_DIR)..."
if cp ./cameraModel.params $ENV_DIR
then
    echo "mv succeeded..."
else
    echo "mv failed, find the closest ape and blame him"
    exit
fi

echo "*** deploy-enving..."
cd $HOME/workspace/
bash deploy-env
#echo "*** Moving the result to the distant robot's environment (on the robot: $robot_home/env/$host/)..."
#if scp ./cameraModel.params rhoban@10.0.0.1:$robot_home/env/$host/
#then
#    echo "mv succeeded..."
#else
#    echo "mv failed, find the closest monkey and blame him"
#    exitenvironment
#fi

echo "*** All set. Good luck buddy"

echo ""