static void test_threadex(void) { struct ymsglooper *ml; struct ymsghandler *mh; srand(time(NULL)); ml = ymsglooper_start_looper_thread(FALSE); mh = ymsghandler_create(ml, NULL); /* use default handle */ tc1(mh); tc2(mh); tc3(mh); tc4(mh); /* clean up all others */ ymsghandler_destroy(mh); ymsglooper_stop(ml); while (YMSGLOOPER_TERMINATED != ymsglooper_get_state(ml)) usleep(10*1000); ymsglooper_destroy(ml); }
void DoorTraj::evaluate_abs(OWD::Trajectory::TrajControl &tc, double t) { if (OWD::Kinematics::max_condition > 25) { ROS_ERROR_NAMED("OpenDoor","Jacobian Psuedo-Inverse condition number reached %2f; aborting", OWD::Kinematics::max_condition); runstate=ABORT; return; } // a place to record log values for debugging std::vector<double> data; // First, call evaluate on the base class to see where we would want // to be if we weren't also dealing with the door TrajControl tc2(tc); OWD::MacJointTraj::evaluate_abs(tc2,t); SE3 traj_endpoint_pose = interpolate_ee_pose(tc2.q); data.push_back(time); // time, column 1 data.insert(data.end(),tc.q.begin(), tc.q.end()); // current positions, col 2-8 data.insert(data.end(),tc2.q.begin(), tc2.q.end()); // traj positions, col 9-15 std::vector<double> pose; pose.resize(16); memcpy(&pose[0],(double *)traj_endpoint_pose,16*sizeof(double)); data.insert(data.end(),pose.begin(), pose.end()); // traj pose, col 16-31 // calculate how much the endpoint position changed R3 traj_endpoint_movement = (R3)traj_endpoint_pose - last_traj_endpoint; last_traj_endpoint = (R3) traj_endpoint_pose; // update the endpoint position goal endpoint_position_goal += traj_endpoint_movement; // endpoint_position_goal = (R3) traj_endpoint_pose; ROS_DEBUG_STREAM_NAMED("OpenDoor","Current traj endpoint: " << (R3)traj_endpoint_pose); ROS_DEBUG_STREAM_NAMED("OpenDoor","Current actual endpoint: " << (R3)hybridplug->endpoint); ROS_DEBUG_STREAM_NAMED("OpenDoor","New endpoint goal: " << endpoint_position_goal); // calculate the endpoint position error R3 endpoint_error = endpoint_position_goal - (R3)hybridplug->endpoint; memcpy(&pose[0],(const double *)hybridplug->endpoint,16*sizeof(double)); data.insert(data.end(),pose.begin(), pose.end()); // actual pose, col 32-47 data.push_back(endpoint_error[0]); // endpoint error, col 48-50 data.push_back(endpoint_error[1]); data.push_back(endpoint_error[2]); // project the position error onto the pull direction R3 world_PullDirection = (SO3)hybridplug->endpoint * PullDirection; R3 endpoint_longitudinal_error = (endpoint_error * world_PullDirection) * world_PullDirection; R3 endpoint_lateral_error = endpoint_error - endpoint_longitudinal_error; if (OWD::Plugin::simulation) { ROS_DEBUG_NAMED("OpenDoor","Net ee movement of %2.3f",endpoint_longitudinal_error.norm()); } data.push_back(endpoint_lateral_error[0]); // lateral error cols 51-53 data.push_back(endpoint_lateral_error[1]); data.push_back(endpoint_lateral_error[2]); data.push_back(endpoint_longitudinal_error[0]); // longitudinal error cols 54-56 data.push_back(endpoint_longitudinal_error[1]); data.push_back(endpoint_longitudinal_error[2]); // subtract the lateral error from the goal endpoint_position_goal -= endpoint_lateral_error; // calculate the endpoint rotation error so3 endpoint_rotation_error_so3 =(so3)((SO3)traj_endpoint_pose * (! (SO3)hybridplug->endpoint)); // rotation error in world frame R3 endpoint_rotation_error = endpoint_rotation_error_so3.t() * endpoint_rotation_error_so3.w(); data.push_back(endpoint_rotation_error[0]); // rotational error cols 57-59 data.push_back(endpoint_rotation_error[1]); data.push_back(endpoint_rotation_error[2]); // combine the longitudinal translation error with the rotation error R6 endpoint_correction(endpoint_longitudinal_error,endpoint_rotation_error); OWD::JointPos joint_correction; try { if (OWD::Plugin::simulation) { ROS_DEBUG_STREAM_NAMED("OpenDoor","Endpoint pos/rot correction of" << std::endl << endpoint_correction); } joint_correction = hybridplug->JacobianPseudoInverse_times_vector(endpoint_correction); // data.insert(data.end(),joint_correction.begin(), // joint_correction.end()); // joint change to correct ep, col 60-66 if (OWD::Plugin::simulation) { ROS_DEBUG_NAMED("OpenDoor","JacobianPsueoInverse condition number %3.3f",OWD::Kinematics::max_condition); ROS_DEBUG_NAMED("OpenDoor","JacobianPsuedoInverse Matrix:"); for (int i=0; i<7; ++i) { ROS_DEBUG_NAMED("OpenDoor"," [%3.3f %3.3f %3.3f %3.3f %3.3f %3.3f]", OWD::Kinematics::Jacobian0PseudoInverse[0][i], OWD::Kinematics::Jacobian0PseudoInverse[1][i], OWD::Kinematics::Jacobian0PseudoInverse[2][i], OWD::Kinematics::Jacobian0PseudoInverse[3][i], OWD::Kinematics::Jacobian0PseudoInverse[4][i], OWD::Kinematics::Jacobian0PseudoInverse[5][i]); } ROS_DEBUG_NAMED("OpenDoor","Joint correction [%1.3f, %1.3f, %1.3f, %1.3f, %1.3f, %1.3f, %1.3f]", joint_correction[0], joint_correction[1], joint_correction[2], joint_correction[3], joint_correction[4], joint_correction[5], joint_correction[6]); ROS_DEBUG_NAMED("OpenDoor"," (length of of %2.3f)", joint_correction.length()); } } catch (const char *err) { // no valid Jacobian, for whatever reason, so we won't get // any endpoint correction this timestep data.insert(data.end(),tc.q.size(),-1); // invalid joint change (col 60-66) if (OWD::Plugin::simulation) { ROS_WARN_NAMED("OpenDoor","No JacobianPseudoInverse available"); } } // Finally, project our joint error into the nullspace so that // we can correct as much as we can without changing the endpoint OWD::JointPos joint_error(tc2.q - tc.q); try { OWD::JointPos configuration_correction = hybridplug->Nullspace_projection(joint_error); data.insert(data.end(),configuration_correction.begin(), configuration_correction.end()); // joint change to correct config, col 67-73 (was 58-64) if (OWD::Plugin::simulation) { ROS_DEBUG_NAMED("OpenDoor","Configuration correction of %2.3f", configuration_correction.length()); } if (joint_correction.size() > 0) { joint_correction += configuration_correction; } else { joint_correction = configuration_correction; } } catch (const char *err) { // don't worry about it data.insert(data.end(),tc.q.size(),-1); // invalid joint change (col 67-73) if (OWD::Plugin::simulation) { ROS_WARN_NAMED("OpenDoor","Nullspace projection failed"); } } if (OWD::Plugin::simulation) { ROS_DEBUG_NAMED("OpenDoor","Total joint correction of %2.3f", joint_correction.length()); } if (joint_correction.size() == tc.q.size()) { tc.q += joint_correction; } data.insert(data.end(),tc.q.begin(), tc.q.end()); // target positions, col 74-80 (was 65-71) recorder->add(data); if (runstate == DONE) { end_position = tc.q; } }