humotion / examples / meka / src / mekajointinterface.cpp @ 39209d06
History | View | Annotate | Download (8.016 KB)
1 |
#include "mekajointinterface.h" |
---|---|
2 |
using namespace std; |
3 |
|
4 |
//WARNING: DO NOT CHANGE THIS; VELOCITYMODE IS NOT YET IMPLEMENTED
|
5 |
#define POSITION_CONTROL 1 |
6 |
|
7 |
void MekaJointInterface::incoming_jointstates(const sensor_msgs::JointState & msg){ |
8 |
//fetch current timestamp
|
9 |
double timestamp = get_timestamp_ms();
|
10 |
|
11 |
//iterate through incoming joints and filter out joints we need:
|
12 |
for(int i=0; i<msg.name.size(); i++){ |
13 |
string name = msg.name[i];
|
14 |
//printf("incoming data for joint '%s'\n", name.c_str());
|
15 |
|
16 |
int id = -1; |
17 |
if (name == "head_j1"){ |
18 |
id = ID_NECK_PAN; |
19 |
}else if(name == "head_j0"){ |
20 |
id = ID_NECK_TILT; |
21 |
} |
22 |
|
23 |
//store data:
|
24 |
if (id != -1){ |
25 |
//printf("> storing joint data for joint id %d\n", id);
|
26 |
if (i >= msg.position.size()){
|
27 |
printf("> msg is missing position data for joint %s. exiting! make sure you did start the publisher...\n", name.c_str());
|
28 |
exit(EXIT_FAILURE); |
29 |
} |
30 |
if (i >= msg.velocity.size()){
|
31 |
printf("> msg is missing velocity data for joint %s. exiting! make sure you did start the publisher...\n", name.c_str());
|
32 |
exit(EXIT_FAILURE); |
33 |
} |
34 |
//ok, safe to access data
|
35 |
JointInterface::store_incoming_position(id, msg.position[i], timestamp); |
36 |
JointInterface::store_incoming_speed( id, msg.velocity[i], timestamp); |
37 |
} |
38 |
} |
39 |
|
40 |
//store dummy positions for joints we do not know about:
|
41 |
store_dummy_data(ID_LIP_LEFT_UPPER, timestamp); |
42 |
store_dummy_data(ID_LIP_LEFT_LOWER, timestamp); |
43 |
store_dummy_data(ID_LIP_CENTER_UPPER, timestamp); |
44 |
store_dummy_data(ID_LIP_CENTER_LOWER, timestamp); |
45 |
store_dummy_data(ID_LIP_RIGHT_UPPER, timestamp); |
46 |
store_dummy_data(ID_LIP_RIGHT_LOWER, timestamp); |
47 |
|
48 |
store_dummy_data(ID_NECK_ROLL, timestamp); |
49 |
store_dummy_data(ID_EYES_BOTH_UD, timestamp); |
50 |
store_dummy_data(ID_EYES_LEFT_LR, timestamp); |
51 |
store_dummy_data(ID_EYES_RIGHT_LR, timestamp); |
52 |
store_dummy_data(ID_EYES_LEFT_LID_LOWER, timestamp); |
53 |
store_dummy_data(ID_EYES_LEFT_LID_UPPER, timestamp); |
54 |
store_dummy_data(ID_EYES_LEFT_BROW, timestamp); |
55 |
store_dummy_data(ID_EYES_RIGHT_LID_LOWER, timestamp); |
56 |
store_dummy_data(ID_EYES_RIGHT_LID_UPPER, timestamp); |
57 |
store_dummy_data(ID_EYES_RIGHT_BROW, timestamp); |
58 |
|
59 |
} |
60 |
|
61 |
void MekaJointInterface::store_dummy_data(int id, double timestamp){ |
62 |
JointInterface::store_incoming_position(id, 0.0, timestamp); |
63 |
JointInterface::store_incoming_speed(id, 0.0, timestamp); |
64 |
} |
65 |
|
66 |
//! constructor
|
67 |
MekaJointInterface::MekaJointInterface(string _input_scope, string _output_scope) : humotion::server::JointInterface(){ |
68 |
input_scope = _input_scope; |
69 |
output_scope = _output_scope; |
70 |
|
71 |
//subscribe to meka joint states
|
72 |
int argc = 0; |
73 |
ros::init(argc, (char**)NULL, "meka_humotion"); |
74 |
ros::NodeHandle n; |
75 |
|
76 |
printf("> listening on jointstates on '%s'\n",input_scope.c_str());
|
77 |
joint_state_subscriber = n.subscribe(input_scope, 150, &MekaJointInterface::incoming_jointstates , this); |
78 |
|
79 |
printf("> sending targets on '%s'\n", output_scope.c_str());
|
80 |
target_publisher = n.advertise<trajectory_msgs::JointTrajectory>(output_scope, 100);
|
81 |
|
82 |
//tell humotion about min/max joint values:
|
83 |
init_joints(); |
84 |
} |
85 |
|
86 |
//! destructor
|
87 |
MekaJointInterface::~MekaJointInterface(){ |
88 |
} |
89 |
|
90 |
|
91 |
|
92 |
void MekaJointInterface::run(){
|
93 |
//iCubDataReceiver *data_receiver = new iCubDataReceiver(10/*1000.0 / MAIN_LOOP_FREQUENCY*/, iencs, this);
|
94 |
//data_receiver->start();
|
95 |
ros::spin(); |
96 |
} |
97 |
|
98 |
|
99 |
double MekaJointInterface::get_timestamp_ms(){
|
100 |
struct timespec spec;
|
101 |
clock_gettime(CLOCK_REALTIME, &spec); |
102 |
return spec.tv_sec*1000 + spec.tv_nsec / 1.0e6; |
103 |
} |
104 |
|
105 |
//! set the target position of a joint
|
106 |
//! \param enum id of joint
|
107 |
//! \param float value
|
108 |
void MekaJointInterface::publish_target_position(int e){ |
109 |
#if 0
|
110 |
//first: convert humotion enum to our enum:
|
111 |
int id = convert_enum_to_motorid(e);
|
112 |
if (id == -1){
|
113 |
return; //we are not interested in that data, so we just return here
|
114 |
}
|
115 |
|
116 |
if (id == ICUB_ID_NECK_PAN){
|
117 |
//PAN seems to be swapped
|
118 |
store_joint(ICUB_ID_NECK_PAN, -joint_target[e]);
|
119 |
}else if ((id == ICUB_ID_EYES_LEFT_LR) || ( id == ICUB_ID_EYES_RIGHT_LR)){
|
120 |
//icub handles eyes differently, we have to set pan angle + vergence
|
121 |
float pan = (joint_target[ID_EYES_LEFT_LR] + joint_target[ID_EYES_RIGHT_LR]) / 2;
|
122 |
float vergence = (joint_target[ID_EYES_LEFT_LR] - joint_target[ID_EYES_RIGHT_LR]);
|
123 |
//printf("EYEDBG %3.2f %3.2f --_> pan %3.2f verg=%3.2f\n",joint_target[ID_EYES_LEFT_LR], joint_target[ID_EYES_RIGHT_LR],pan,vergence);
|
124 |
|
125 |
store_joint(ICUB_ID_EYES_PAN, pan);
|
126 |
store_joint(ICUB_ID_EYES_VERGENCE, vergence);
|
127 |
}else{
|
128 |
store_joint(id, joint_target[e]);
|
129 |
}
|
130 |
#endif
|
131 |
}
|
132 |
|
133 |
|
134 |
//! actually execute the scheduled motion commands
|
135 |
void MekaJointInterface::execute_motion(){
|
136 |
//build msg
|
137 |
trajectory_msgs::JointTrajectory msg;
|
138 |
msg.joint_names.push_back("head_j0");
|
139 |
msg.joint_names.push_back("head_j1");
|
140 |
|
141 |
trajectory_msgs::JointTrajectoryPoint p;
|
142 |
p.positions.push_back(joint_target[ID_NECK_TILT] * M_PI / 180.0);
|
143 |
p.positions.push_back(joint_target[ID_NECK_PAN] * M_PI / 180.0);
|
144 |
|
145 |
p.time_from_start = ros::Duration(1.2 * 1.0 / humotion::server::Server::MOTION_UPDATERATE);
|
146 |
p.time_from_start = ros::Duration(1.2 * 1.0 / humotion::server::Server::MOTION_UPDATERATE);
|
147 |
|
148 |
msg.points.push_back(p);
|
149 |
|
150 |
target_publisher.publish(msg);
|
151 |
|
152 |
/*
|
153 |
void MekaJointInterface::store_min_max(int id, float min, float max){
|
154 |
header:
|
155 |
seq: 636
|
156 |
stamp:
|
157 |
secs: 0
|
158 |
nsecs: 0
|
159 |
frame_id: ''
|
160 |
joint_names: ['head_j0', 'head_j1']
|
161 |
points:
|
162 |
-
|
163 |
positions: [-0.31, 0.01954768762234005]
|
164 |
velocities: []
|
165 |
accelerations: []
|
166 |
effort: []
|
167 |
time_from_start:
|
168 |
secs: 1
|
169 |
nsecs: 0
|
170 |
|
171 |
*/
|
172 |
|
173 |
#if 0
|
174 |
// set up neck and eye motion commands:
|
175 |
if (POSITION_CONTROL){
|
176 |
//position control
|
177 |
for(int i=ICUB_ID_NECK_TILT; i<=ICUB_ID_EYES_VERGENCE; i++){
|
178 |
set_target_in_positionmode(i, target_angle[i]);
|
179 |
}
|
180 |
}else{
|
181 |
//velocity control
|
182 |
for(int i=ICUB_ID_NECK_TILT; i<=ICUB_ID_EYES_VERGENCE; i++){
|
183 |
set_target_in_velocitymode(i, target_angle[i]);
|
184 |
}
|
185 |
}
|
186 |
//printf("> TARGET PAN = %3.2f\n",target_angle[ICUB_ID_NECK_PAN]);
|
187 |
|
188 |
|
189 |
//eyelids: unfortuantely the icub has only 1dof for eyelids, so we use only one dof here:
|
190 |
set_eyelid_angle(target_angle[ICUB_ID_EYES_RIGHT_LID_UPPER]);
|
191 |
|
192 |
//eyebrows are set using a special command as well:
|
193 |
set_eyebrow_angle(ICUB_ID_EYES_LEFT_BROW);
|
194 |
set_eyebrow_angle(ICUB_ID_EYES_RIGHT_BROW);
|
195 |
|
196 |
//mouth
|
197 |
set_mouth();
|
198 |
|
199 |
#endif
|
200 |
}
|
201 |
|
202 |
|
203 |
//! prepare and enable a joint
|
204 |
//! NOTE: this should also prefill the min/max positions for this joint
|
205 |
//! \param the enum id of a joint
|
206 |
void MekaJointInterface::enable_joint(int e){
|
207 |
//meka does not support this, joints are always enabled
|
208 |
}
|
209 |
|
210 |
//! shutdown and disable a joint
|
211 |
//! \param the enum id of a joint
|
212 |
void MekaJointInterface::disable_joint(int e){
|
213 |
//meka does not support this, joints are always enabled
|
214 |
}
|
215 |
|
216 |
void MekaJointInterface::store_min_max(int id, float min, float max){
|
217 |
joint_min[id] = min;
|
218 |
joint_max[id] = max;
|
219 |
}
|
220 |
|
221 |
void MekaJointInterface::init_joints(){
|
222 |
store_min_max(ID_NECK_TILT, -10, 10);
|
223 |
store_min_max(ID_NECK_PAN, -10, 10);
|
224 |
|
225 |
store_min_max(ID_NECK_ROLL, -1, 1);
|
226 |
store_min_max(ID_EYES_BOTH_UD, -1, 1);
|
227 |
store_min_max(ID_EYES_LEFT_LR, -1, 1);
|
228 |
store_min_max(ID_EYES_RIGHT_LR, -1, 1);
|
229 |
store_min_max(ID_EYES_LEFT_LID_UPPER, -1, 1);
|
230 |
store_min_max(ID_EYES_LEFT_LID_LOWER, -1, 1);
|
231 |
store_min_max(ID_EYES_RIGHT_LID_UPPER, -1, 1);
|
232 |
store_min_max(ID_EYES_RIGHT_LID_LOWER, -1, 1);
|
233 |
store_min_max(ID_EYES_LEFT_BROW, -1, 1);
|
234 |
store_min_max(ID_EYES_RIGHT_BROW, -1, 1);
|
235 |
store_min_max(ID_LIP_CENTER_UPPER, -1, 1);
|
236 |
store_min_max(ID_LIP_CENTER_LOWER, -1, 1);
|
237 |
store_min_max(ID_LIP_LEFT_UPPER, -1, 1);
|
238 |
store_min_max(ID_LIP_LEFT_LOWER, -1, 1);
|
239 |
store_min_max(ID_LIP_RIGHT_UPPER, -1, 1);
|
240 |
store_min_max(ID_LIP_RIGHT_LOWER, -1, 1);
|
241 |
}
|
242 |
|
243 |
|