/* 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;
}
