/* CENTRY */ void APIENTRY glutMainLoop(void) { #if !defined(_WIN32) if (!__glutDisplay) __glutFatalUsage("main loop entered with out proper initialization."); #endif if (!__glutWindowListSize) __glutFatalUsage( "main loop entered with no windows created."); for (;;) { if (__glutWindowWorkList) { GLUTwindow *remainder, *work; work = __glutWindowWorkList; __glutWindowWorkList = NULL; if (work) { remainder = processWindowWorkList(work); if (remainder) { *beforeEnd = __glutWindowWorkList; __glutWindowWorkList = remainder; } } } if (__glutIdleFunc || __glutWindowWorkList) { idleWait(); } else { if (__glutTimerList) { waitForSomething(); } else { processEventsAndTimeouts(); } } } }
/*********************************************************** * FUNCTION: glutMainLoop (3.1) * * DESCRIPTION: enter the event processing loop ***********************************************************/ void glutMainLoop() { if (!gState.windowListSize) __glutFatalUsage("main loop entered with no windows created."); if(gState.currentWindow) gState.currentWindow->UnlockGL(); for (;;) { if (gState.idle) { idleWait(); } else { if (__glutTimerList) { waitForSomething(); } else { processEventsAndTimeouts(); } } } }
/* CENTRY */ void APIENTRY glutMainLoop(void) { #if !defined(_WIN32) if (!__glutDisplay) __glutFatalUsage("main loop entered with out proper initialization."); #endif if (!__glutWindowListSize) __glutFatalUsage( "main loop entered with no windows created."); for (;;) { if (__glutWindowWorkList) { GLUTwindow *remainder, *work; work = __glutWindowWorkList; __glutWindowWorkList = NULL; if (work) { remainder = processWindowWorkList(work); if (remainder) { *beforeEnd = __glutWindowWorkList; __glutWindowWorkList = remainder; } } } if (__glutIdleFunc || __glutWindowWorkList) { idleWait(); } else { if (__glutTimerList) { waitForSomething(); } else { processEventsAndTimeouts(); } #if defined(_WIN32) // If there is no idle function, go to sleep for a millisecond (we // still need to possibly service timers) or until there is some // event in our queue. MsgWaitForMultipleObjects(0, NULL, FALSE, 1, QS_ALLEVENTS); #endif } } }
/* CENTRY */ void GLUTAPIENTRY glutMainLoop(void) { #if !defined(_WIN32) if (!__glutDisplay) __glutFatalUsage("main loop entered with out proper initialization."); #endif if (!__glutWindowListSize) __glutFatalUsage( "main loop entered with no windows created."); for (;;) { __glutProcessWindowWorkLists(); if (__glutIdleFunc || __glutWindowWorkList) { idleWait(); } else { if (__glutTimerList) { waitForSomething(); } else { processEventsAndTimeouts(); } } } }
void Katana::testSpeed() { ros::Rate idleWait(5); std::vector<double> pos1_angles(NUM_MOTORS); std::vector<double> pos2_angles(NUM_MOTORS); // these are safe values, i.e., no self-collision is possible pos1_angles[0] = 2.88; pos2_angles[0] = -3.02; pos1_angles[1] = 0.15; pos2_angles[1] = 2.16; pos1_angles[2] = 1.40; pos2_angles[2] = -2.21; pos1_angles[3] = 0.50; pos2_angles[3] = -2.02; pos1_angles[4] = 2.86; pos2_angles[4] = -2.98; pos1_angles[5] = -0.44; pos2_angles[5] = 0.30; for (size_t i = 0; i < NUM_MOTORS; i++) { int pos1_encoders = (int)converter->angle_rad2enc(i, pos1_angles[i]); int pos2_encoders = (int)converter->angle_rad2enc(i, pos2_angles[i]); int accel = kni->getMotorAccelerationLimit(i); int max_vel = kni->getMotorVelocityLimit(i); ROS_INFO("Motor %zu - acceleration: %d (= %f), max speed: %d (=%f)", i, accel, 2.0 * converter->acc_enc2rad(i, accel), max_vel, converter->vel_enc2rad(i, max_vel)); ROS_INFO("KNI encoders: %d, %d", kni->GetBase()->GetMOT()->arr[i].GetEncoderMinPos(), kni->GetBase()->GetMOT()->arr[i].GetEncoderMaxPos()); ROS_INFO("moving to encoders: %d, %d", pos1_encoders, pos2_encoders); ROS_INFO("current encoders: %d", kni->getMotorEncoders(i, true)); ROS_INFO("Moving to min"); { boost::recursive_mutex::scoped_lock lock(kni_mutex); kni->moveMotorToEnc(i, pos1_encoders); } do { idleWait.sleep(); refreshMotorStatus(); } while (!allMotorsReady()); ROS_INFO("Moving to max"); { boost::recursive_mutex::scoped_lock lock(kni_mutex); kni->moveMotorToEnc(i, pos2_encoders); } do { idleWait.sleep(); refreshMotorStatus(); } while (!allMotorsReady()); } // Result: // Motor 0 - acceleration: 2 (= -4.908739), max speed: 180 (=-2.208932) // Motor 1 - acceleration: 2 (= -2.646220), max speed: 180 (=-1.190799) // Motor 2 - acceleration: 2 (= 5.292440), max speed: 180 (=2.381598) // --> wrong! the measured values are more like 2.6, 1.2 // // Motor 3 - acceleration: 2 (= -4.908739), max speed: 180 (=-2.208932) // Motor 4 - acceleration: 2 (= -4.908739), max speed: 180 (=-2.208932) // Motor 5 - acceleration: 2 (= 1.597410), max speed: 180 (=0.718834) // (TODO: the gripper duration can be calculated from this) }
/** * Sends the spline parameters to the Katana. * * @param traj */ bool Katana::executeTrajectory(boost::shared_ptr<SpecifiedTrajectory> traj, boost::function<bool ()> isPreemptRequested) { assert(traj->size() > 0); try { // ------- wait until all motors idle ros::Rate idleWait(10); while (!allMotorsReady()) { refreshMotorStatus(); ROS_DEBUG("Motor status: %d, %d, %d, %d, %d, %d", motor_status_[0], motor_status_[1], motor_status_[2], motor_status_[3], motor_status_[4], motor_status_[5]); // ------- check if motors are blocked // it is important to do this inside the allMotorsReady() loop, otherwise we // could get stuck in a deadlock if the motors crash while we wait for them to // become ready if (someMotorCrashed()) { ROS_WARN("Motors are crashed before executing trajectory! Unblocking..."); boost::recursive_mutex::scoped_lock lock(kni_mutex); kni->unBlock(); } idleWait.sleep(); } //// ------- move to start position //{ // assert(traj->at(0).splines.size() == NUM_JOINTS); // // boost::recursive_mutex::scoped_lock lock(kni_mutex); // std::vector<int> encoders; // // for (size_t i = 0; i < NUM_JOINTS; i++) { // encoders.push_back(converter->angle_rad2enc(i, traj->at(0).splines[i].coef[0])); // } // // std::vector<int> current_encoders = kni->getRobotEncoders(true); // ROS_INFO("current encoders: %d %d %d %d %d", current_encoders[0], current_encoders[1], current_encoders[2], current_encoders[3], current_encoders[4]); // ROS_INFO("target encoders: %d %d %d %d %d", encoders[0], encoders[1], encoders[2], encoders[3], encoders[4]); // // kni->moveRobotToEnc(encoders, false); // ros::Duration(2.0).sleep(); //} // ------- wait until start time ros::Time start_time = ros::Time(traj->at(0).start_time); double time_until_start = (start_time - ros::Time::now()).toSec(); if (fabs(traj->at(0).start_time) > 0.01 && time_until_start < -0.01) { // only print warning if traj->at(0).start_time != 0 (MoveIt usually doesn't set start time) ROS_WARN("Trajectory started %f s too late! Scheduled: %f, started: %f", -time_until_start, start_time.toSec(), ros::Time::now().toSec()); } else if (time_until_start > 0.0) { ROS_DEBUG("Sleeping %f seconds until scheduled start of trajectory", time_until_start); ros::Time::sleepUntil(start_time); } // ------- start trajectory boost::recursive_mutex::scoped_lock lock(kni_mutex); // fix start times: set the trajectory start time to now(); since traj is a shared pointer, // this fixes the current_trajectory_ in joint_trajectory_action_controller, which synchronizes // the "state" publishing to the actual start time (more or less) double delay = ros::Time::now().toSec() - traj->at(0).start_time; for (size_t i = 0; i < traj->size(); i++) { traj->at(i).start_time += delay; } for (size_t i = 0; i < traj->size(); i++) { Segment seg = traj->at(i); if (seg.splines.size() != joint_names_.size()) { ROS_ERROR("Wrong number of joints in specified trajectory (was: %zu, expected: %zu)!", seg.splines.size(), joint_names_.size()); } // set and start movement int activityflag = 0; if (i == (traj->size() - 1)) { // last spline, start movement activityflag = 1; // no_next } else if (seg.start_time - traj->at(0).start_time < 0.4) { // more splines following, do not start movement yet activityflag = 2; // no_start } else { // more splines following, start movement activityflag = 0; } std::vector<short> polynomial; short s_time = round(seg.duration * KNI_TO_ROS_TIME); if (s_time <= 0) s_time = 1; for (size_t j = 0; j < seg.splines.size(); j++) { // some parts taken from CLMBase::movLM2P polynomial.push_back(s_time); // duration polynomial.push_back(converter->angle_rad2enc(j, seg.splines[j].target_position)); // target position // the four spline coefficients // the actual position, round polynomial.push_back(round(converter->angle_rad2enc(j, seg.splines[j].coef[0]))); // p0 // shift to be firmware compatible and round polynomial.push_back(round(64 * converter->vel_rad2enc(j, seg.splines[j].coef[1]))); // p1 polynomial.push_back(round(1024 * converter->acc_rad2enc(j, seg.splines[j].coef[2]))); // p2 polynomial.push_back(round(32768 * converter->jerk_rad2enc(j, seg.splines[j].coef[3]))); // p3 } // gripper: hold current position polynomial.push_back(s_time); // duration polynomial.push_back(converter->angle_rad2enc(5, motor_angles_[5])); // target position (angle) polynomial.push_back(converter->angle_rad2enc(5, motor_angles_[5])); // p0 polynomial.push_back(0); // p1 polynomial.push_back(0); // p2 polynomial.push_back(0); // p3 ROS_DEBUG("setAndStartPolyMovement(%d): ", activityflag); for (size_t k = 5; k < polynomial.size(); k += 6) { ROS_DEBUG(" time: %d target: %d p0: %d p1: %d p2: %d p3: %d", polynomial[k-5], polynomial[k-4], polynomial[k-3], polynomial[k-2], polynomial[k-1], polynomial[k]); } kni->setAndStartPolyMovement(polynomial, false, activityflag); } return true; } catch (const WrongCRCException &e) { ROS_ERROR("WrongCRCException: Two threads tried to access the KNI at once. This means that the locking in the Katana node is broken. (exception in executeTrajectory(): %s)", e.message().c_str()); } catch (const ReadNotCompleteException &e) { ROS_ERROR("ReadNotCompleteException: Another program accessed the KNI. Please stop it and restart the Katana node. (exception in executeTrajectory(): %s)", e.message().c_str()); } catch (const FirmwareException &e) { // TODO: find out what the real cause of this is when it happens again // the message returned by the Katana is: // FirmwareException : 'StopperThread: collision on axis: 1 (axis N)' ROS_ERROR("FirmwareException: Motor collision? Perhaps we tried to send a trajectory that the arm couldn't follow. (exception in executeTrajectory(): %s)", e.message().c_str()); } catch (const Exception &e) { ROS_ERROR("Unhandled exception in executeTrajectory(): %s", e.message().c_str()); } catch (...) { ROS_ERROR("Unhandled exception in executeTrajectory()"); } return false; }