void OpenBagMotionPlanner::WaitForExecution()
{
ROS_INFO_STREAM("START WAITING");
robot_state::RobotState prev_state(*groupPtr->getCurrentState());
robot_state::RobotState curr_state(*groupPtr->getCurrentState());
bool moving = true;
ros::Duration(2.0).sleep();
while (moving)
{
ros::Duration(1.0).sleep();
curr_state = *groupPtr->getCurrentState();
std::vector<double> diffs;
diffs.push_back(*(curr_state.getJointPositions(JOINT1NAME)) - *(prev_state.getJointPositions(JOINT1NAME)));
diffs.push_back(*(curr_state.getJointPositions(JOINT2NAME)) - *(prev_state.getJointPositions(JOINT2NAME)));
diffs.push_back(*(curr_state.getJointPositions(JOINT3NAME)) - *(prev_state.getJointPositions(JOINT3NAME)));
diffs.push_back(*(curr_state.getJointPositions(JOINT4NAME)) - *(prev_state.getJointPositions(JOINT4NAME)));
diffs.push_back(*(curr_state.getJointPositions(JOINT5NAME)) - *(prev_state.getJointPositions(JOINT5NAME)));
diffs.push_back(*(curr_state.getJointPositions(JOINT6NAME)) - *(prev_state.getJointPositions(JOINT6NAME)));
for (std::vector<double>::iterator diff_it = diffs.begin(); diff_it != diffs.end(); ++diff_it)
{
if (fabs(*diff_it) > 0.0005)
{
moving = true;
break;
}
else
{
moving = false;
}
}
prev_state = curr_state;
}
ROS_INFO_STREAM("STOP WAITING");
}
void crank_nicholson(double current_time, double time_step,
int state_size, double *state,
double *new_state, struct sim_parms *parms,
tensor (*rhs)(double time, const tensor &state,
struct sim_parms *parms))
{
tensor prev_state(1,state_size);
tensor last_guess(1,state_size);
tensor current_guess(1,state_size);
tensor temp(1,state_size);
double delta;
double time_step_2 = time_step/2.0;
int i, counter;
for( i = 0; i < state_size; i++ ) prev_state.Set(state[i],i);
last_guess <= prev_state;
current_guess <= prev_state;
delta = 1e200;
counter = 0;
while( delta > CN_TOL && counter <= CN_MAX_ITERATES)
{
temp <= time_step_2*( rhs(current_time, prev_state, parms) +
rhs(current_time + time_step, last_guess, parms));
current_guess <= prev_state + temp;
temp <= current_guess - last_guess;
delta = temp.max();
last_guess <= current_guess;
counter++;
}
for( i = 0; i < state_size; i++ )
new_state[i] = current_guess.Val(i);
}
