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