Statistics
| Branch: | Tag: | Revision:

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