Revision 708960ff
| examples/yarp_icub/src/icub_data_receiver.cpp | ||
|---|---|---|
| 115 | 115 |
//! \param position |
| 116 | 116 |
//! \param timestamp |
| 117 | 117 |
void iCubDataReceiver::store_incoming_position(int icub_id, double position, double timestamp) {
|
| 118 |
cout << "iCubDataReceiver::store_incoming_position(icub=" << icub_id << ", " << position << ")\n"; |
|
| 118 |
//cout << "iCubDataReceiver::store_incoming_position(icub=" << icub_id << ", " << position << ")\n";
|
|
| 119 | 119 |
|
| 120 | 120 |
// store joint position in humotion backend |
| 121 | 121 |
if ((icub_id == iCubJointInterface::ICUB_ID_EYES_PAN) || |
| ... | ... | |
| 157 | 157 |
//! \param velocity |
| 158 | 158 |
//! \param timestamp |
| 159 | 159 |
void iCubDataReceiver::store_incoming_velocity(int icub_id, double velocity, double timestamp) {
|
| 160 |
cout << "iCubDataReceiver::store_incoming_velocity(icub=" << icub_id << ", " << velocity << ")\n"; |
|
| 160 |
//cout << "iCubDataReceiver::store_incoming_velocity(icub=" << icub_id << ", " << velocity << ")\n";
|
|
| 161 | 161 |
|
| 162 | 162 |
// store joint position in humotion backend |
| 163 | 163 |
if ((icub_id == iCubJointInterface::ICUB_ID_EYES_PAN) || |
| examples/yarp_icub/src/icub_jointinterface.cpp | ||
|---|---|---|
| 200 | 200 |
//execute: |
| 201 | 201 |
//ivel->velocityMove(id, speed); |
| 202 | 202 |
if ((icub_id != ICUB_ID_NECK_PAN) |
| 203 |
//&& (icub_id != ICUB_ID_EYES_BOTH_UD)
|
|
| 204 |
//&& (icub_id != ICUB_ID_NECK_TILT)
|
|
| 203 |
&& (icub_id != ICUB_ID_EYES_BOTH_UD) |
|
| 204 |
&& (icub_id != ICUB_ID_NECK_TILT) |
|
| 205 | 205 |
&& (icub_id != ICUB_ID_EYES_PAN) |
| 206 | 206 |
&& (icub_id != ICUB_ID_EYES_VERGENCE) |
| 207 | 207 |
//&& (icub_id != ICUB_ID_NECK_TILT) |
| src/server/joint_interface.cpp | ||
|---|---|---|
| 80 | 80 |
//following atomic instructions: |
| 81 | 81 |
mutex::scoped_lock scoped_lock(joint_ts_position_map_access_mutex); |
| 82 | 82 |
|
| 83 |
printf("> humotion: incoming joint position for joint id 0x%02X = %4.2f (ts=%.2f)\n",joint_id,position,timestamp.to_seconds());
|
|
| 83 |
//printf("> humotion: incoming joint position for joint id 0x%02X = %4.2f (ts=%.2f)\n",joint_id,position,timestamp.to_seconds());
|
|
| 84 | 84 |
joint_ts_position_map[joint_id].insert(timestamp, position); |
| 85 | 85 |
|
| 86 | 86 |
incoming_position_count++; |
| src/server/middleware_ros.cpp | ||
|---|---|---|
| 38 | 38 |
: Middleware(scope, c){
|
| 39 | 39 |
|
| 40 | 40 |
printf("> using ROS middleware\n");
|
| 41 |
|
|
| 41 | 42 |
//start ros core? |
| 42 | 43 |
if (ros::isInitialized()){
|
| 43 | 44 |
//oh another process is doing ros comm, fine, we do not need to call it |
| 44 | 45 |
tick_necessary = false; |
| 45 | 46 |
}else{
|
| 46 | 47 |
//we have to take care of ros |
| 47 |
printf("> no active ros middleware, calling ros::init() and we will call tick() periodically!");
|
|
| 48 |
printf("> no active ros middleware, calling ros::init() and we will call tick() periodically!\n");
|
|
| 48 | 49 |
string node_name = "humotion_server__"+ scope; |
| 49 |
node_name.erase(std::remove(node_name.begin(), node_name.end(), '/'), node_name.end());
|
|
| 50 |
node_name.erase(std::remove(node_name.begin(), node_name.end(), '/'), node_name.end());
|
|
| 50 | 51 |
printf("> registering on ROS as node %s\n",node_name.c_str());
|
| 51 | 52 |
ros::M_string no_remapping; |
| 52 | 53 |
ros::init(no_remapping, node_name); |
| src/server/neck_motion_generator.cpp | ||
|---|---|---|
| 174 | 174 |
reflexxes_position_output->NewPositionVector->VecData[2], |
| 175 | 175 |
reflexxes_position_output->NewVelocityVector->VecData[2]); |
| 176 | 176 |
|
| 177 |
printf("\n%f %f %f %f %f DBG\n",
|
|
| 177 |
/*printf("\n%f %f %f %f %f DBG\n",
|
|
| 178 | 178 |
neck_pan_now, neck_pan_target, |
| 179 | 179 |
reflexxes_position_output->NewPositionVector->VecData[0], |
| 180 | 180 |
joint_interface->get_ts_speed(JointInterface::ID_NECK_PAN).get_newest_value(), |
| 181 | 181 |
reflexxes_position_output->NewVelocityVector->VecData[0] |
| 182 |
); |
|
| 182 |
);*/
|
|
| 183 | 183 |
} |
| 184 | 184 |
|
| 185 | 185 |
//! publish targets to motor boards: |
| src/server/reflexxes_motion_generator.cpp | ||
|---|---|---|
| 67 | 67 |
reflexxes_position_input->MaxAccelerationVector->VecData[dof] = max_accel; |
| 68 | 68 |
reflexxes_position_input->TargetVelocityVector->VecData[dof] = 0.0; //target speed is zero (really?) |
| 69 | 69 |
|
| 70 |
/* |
|
| 70 | 71 |
// feed back current pos & velocity |
| 71 | 72 |
// as we have to deal with some latency we will forecast the current |
| 72 | 73 |
// position using the old speed, position and the latency: |
| ... | ... | |
| 80 | 81 |
// under the assumption that the speed is "constant" during this short period |
| 81 | 82 |
float position_now = current_position + time_diff * current_speed; |
| 82 | 83 |
|
| 83 |
printf("HTS: diff = %f ms, pos=%f --> %f\n", time_diff*1000.0,current_position, position_now);
|
|
| 84 | 84 |
|
| 85 |
|
|
| 86 |
//reflexxes_position_input->CurrentPositionVector->VecData[dof] = position_now;
|
|
| 87 |
//reflexxes_position_input->CurrentVelocityVector->VecData[dof] = current_speed;
|
|
| 85 |
reflexxes_position_input->CurrentPositionVector->VecData[dof] = position_now; |
|
| 86 |
reflexxes_position_input->CurrentVelocityVector->VecData[dof] = current_speed;
|
|
| 87 |
*/
|
|
| 88 | 88 |
|
| 89 | 89 |
// safety: libreflexxes does not like zero accellerations... |
| 90 | 90 |
if (reflexxes_position_input->MaxAccelerationVector->VecData[dof] == 0.0){
|
Also available in: Unified diff