| 1 |
1 |
#include "icub_data_receiver.h"
|
| 2 |
2 |
#include <humotion/server/joint_interface.h>
|
| 3 |
3 |
#include <yarp/os/Property.h>
|
| 4 |
|
//#include <boost/format.hpp>
|
|
4 |
#include <boost/format.hpp>
|
|
5 |
|
| 5 |
6 |
using std::cout;
|
| 6 |
7 |
using std::cerr;
|
| 7 |
8 |
using std::string;
|
| ... | ... | |
| 10 |
11 |
using yarp::sig::Vector;
|
| 11 |
12 |
|
| 12 |
13 |
#define ICUB_DATA_RECEIVER_USE_ENCODERSPEED 0
|
|
14 |
#define ICUB_DATA_RECEIVER_DUMP_DATA 0
|
| 13 |
15 |
|
| 14 |
|
iCubDataReceiver::iCubDataReceiver(int period, iCubJointInterface *icub_jointinterface)
|
| 15 |
|
: yarp::os::RateThread(period) {
|
|
16 |
//! constructor
|
|
17 |
//! \param period_ms for the yarp rate thread
|
|
18 |
//! \param icub_jointinterface
|
|
19 |
iCubDataReceiver::iCubDataReceiver(int period_ms, iCubJointInterface *icub_jointinterface)
|
|
20 |
: yarp::os::RateThread(period_ms) {
|
| 16 |
21 |
|
| 17 |
22 |
// store pointer to icub jointinterface
|
| 18 |
23 |
icub_jointinterface_ = icub_jointinterface;
|
| ... | ... | |
| 33 |
38 |
timestamps_.resize(joints);
|
| 34 |
39 |
}
|
| 35 |
40 |
|
|
41 |
//! yarp rate thread initializer
|
| 36 |
42 |
bool iCubDataReceiver::threadInit() {
|
| 37 |
43 |
return true;
|
| 38 |
44 |
}
|
| 39 |
45 |
|
|
46 |
//! yarp thread release function
|
| 40 |
47 |
void iCubDataReceiver::threadRelease() {
|
| 41 |
48 |
}
|
| 42 |
49 |
|
|
50 |
//! manully calculate joint velocities (instead of using joint encoder speeds)
|
|
51 |
//! \param positions vector with current encoder position
|
|
52 |
//! \param timestamps vector with the associated timestamps
|
|
53 |
//! \return velocities as vector
|
| 43 |
54 |
Vector iCubDataReceiver::calculate_velocities(Vector positions, Vector timestamps) {
|
| 44 |
55 |
Vector velocities;
|
| 45 |
56 |
velocities.resize(positions.size());
|
| ... | ... | |
| 64 |
75 |
return velocities;
|
| 65 |
76 |
}
|
| 66 |
77 |
|
|
78 |
//! main loop routine, called by yarp rate thread
|
| 67 |
79 |
void iCubDataReceiver::run() {
|
| 68 |
80 |
float velocity;
|
| 69 |
81 |
|
| ... | ... | |
| 89 |
101 |
//small hack to tell humotion to update the lid angle
|
| 90 |
102 |
//fixme: use real id
|
| 91 |
103 |
store_incoming_position(100, 0.0, timestamps_[0]);
|
|
104 |
|
|
105 |
#if ICUB_DATA_RECEIVER_DUMP_DATA
|
|
106 |
dump_incoming_data();
|
|
107 |
#endif
|
| 92 |
108 |
}
|
| 93 |
109 |
|
| 94 |
|
void iCubDataReceiver::store_incoming_position(int icub_id, double value, double timestamp) {
|
| 95 |
|
cout << "iCubDataReceiver::store_incoming_position(icub=" << icub_id << ", " << value << ")\n";
|
|
110 |
//! store incoming position for a given icub joint
|
|
111 |
//! \param icub _id icub joint id
|
|
112 |
//! \param position
|
|
113 |
//! \param timestamp
|
|
114 |
void iCubDataReceiver::store_incoming_position(int icub_id, double position, double timestamp) {
|
|
115 |
cout << "iCubDataReceiver::store_incoming_position(icub=" << icub_id << ", " << position << ")\n";
|
| 96 |
116 |
|
| 97 |
117 |
// store joint position in humotion backend
|
| 98 |
118 |
if ((icub_id == iCubJointInterface::ICUB_ID_EYES_PAN) ||
|
| ... | ... | |
| 101 |
121 |
// instead of using seperate left/right pan the icub uses
|
| 102 |
122 |
// a combined pan angle and vergence. therfore we have to convert this here:
|
| 103 |
123 |
if (icub_id == iCubJointInterface::ICUB_ID_EYES_PAN) {
|
| 104 |
|
target_eye_pan_ = value;
|
|
124 |
target_eye_pan_ = position;
|
| 105 |
125 |
} else {
|
| 106 |
|
target_eye_vergence_ = -value;
|
|
126 |
target_eye_vergence_ = -position;
|
| 107 |
127 |
}
|
| 108 |
128 |
|
| 109 |
129 |
float left = target_eye_pan_ + target_eye_vergence_/2.0;
|
| ... | ... | |
| 120 |
140 |
} else {
|
| 121 |
141 |
if (icub_id == iCubJointInterface::ID_NECK_PAN) {
|
| 122 |
142 |
// icub uses an inverted neck pan specification
|
| 123 |
|
value = -value;
|
|
143 |
position = -position;
|
| 124 |
144 |
}
|
| 125 |
145 |
|
| 126 |
146 |
// known configured mapping between joint ids
|
| 127 |
147 |
int humotion_id = icub_jointinterface_->convert_icub_jointid_to_humotion(icub_id);
|
| 128 |
|
icub_jointinterface_->store_incoming_position(humotion_id, value, timestamp);
|
|
148 |
icub_jointinterface_->store_incoming_position(humotion_id, position, timestamp);
|
| 129 |
149 |
}
|
| 130 |
150 |
}
|
| 131 |
151 |
|
|
152 |
//! store incoming velocity for a given icub joint
|
|
153 |
//! \param icub_id icub joint id
|
|
154 |
//! \param velocity
|
|
155 |
//! \param timestamp
|
| 132 |
156 |
void iCubDataReceiver::store_incoming_velocity(int icub_id, double velocity, double timestamp) {
|
| 133 |
157 |
cout << "iCubDataReceiver::store_incoming_velocity(icub=" << icub_id << ", " << velocity << ")\n";
|
| 134 |
158 |
|
| ... | ... | |
| 167 |
191 |
}
|
| 168 |
192 |
}
|
| 169 |
193 |
|
|
194 |
//! helper for debugging purposes, feed this data into gnuplot for visual inspection
|
|
195 |
void iCubDataReceiver::dump_incoming_data() {
|
|
196 |
// use gnuplot for viz:
|
|
197 |
// ./icub_humotion_server --robot icub | grep "INCOMING" |tee log
|
|
198 |
// @gnuplot: plot "log" using 0:1 w l t "p neck tilt", "log" using 0:2 w l t "v neck tilt", \
|
|
199 |
// "log" using 0:5 w l t "p neck pan", "log" using 0:6 w l t "v neck pan", \
|
|
200 |
// "log" using 0:7 w l t "p eyes ud", "log" using 0:8 w l t "v eyes ud", \
|
|
201 |
// "log" using 0:9 w l t "p eyes vergence", "log" using 0:10 w l t "v eyes vergence"
|
|
202 |
cout << "\n";
|
|
203 |
// publish data to humotion
|
|
204 |
for(int i=0; i<positions_.size(); i++){
|
|
205 |
cout << positions_[i] << " ";
|
|
206 |
cout << velocities_[i] << " ";
|
|
207 |
}
|
|
208 |
cout << " #INCOMING_DATA_DUMP\n";
|
|
209 |
}
|
|
210 |
|
| 170 |
211 |
|