void poseKDLToTF(const KDL::Frame& k, tf::Pose& t) { t.setOrigin(tf::Vector3(k.p[0], k.p[1], k.p[2])); t.setBasis(tf::Matrix3x3(k.M.data[0], k.M.data[1], k.M.data[2], k.M.data[3], k.M.data[4], k.M.data[5], k.M.data[6], k.M.data[7], k.M.data[8])); }
void StepCostKey::mirrorPoseOnXPlane(tf::Pose &mirror, const tf::Pose &orig) const { mirror.setBasis(orig.getBasis()); tf::Matrix3x3& basis = mirror.getBasis(); basis[0][1] = -basis[0][1]; basis[1][0] = -basis[1][0]; basis[2][1] = -basis[2][1]; mirror.setOrigin(orig.getOrigin()); tf::Vector3& origin = mirror.getOrigin(); origin[1] = -origin[1]; // double r, p, y; // tf::Matrix3x3(orig.getRotation()).getRPY(r, p, y); // mirror.setRotation(tf::createQuaternionFromRPY(-r, p, -y)); // tf::Vector3 v = orig.getOrigin(); // v.setY(-v.getY()); // mirror.setOrigin(v); // tfScalar m[16]; // ROS_INFO("------------------"); // mirror.getOpenGLMatrix(m); // ROS_INFO("%f %f %f %f", m[0], m[4], m[8], m[12]); // ROS_INFO("%f %f %f %f", m[1], m[5], m[9], m[13]); // ROS_INFO("%f %f %f %f", m[2], m[6], m[10], m[14]); // ROS_INFO("%f %f %f %f", m[3], m[7], m[11], m[15]); // ROS_INFO("-"); // orig.getOpenGLMatrix(m); // ROS_INFO("%f %f %f %f", m[0], m[4], m[8], m[12]); // ROS_INFO("%f %f %f %f", m[1], m[5], m[9], m[13]); // ROS_INFO("%f %f %f %f", m[2], m[6], m[10], m[14]); // ROS_INFO("%f %f %f %f", m[3], m[7], m[11], m[15]); // ROS_INFO("-"); // ROS_INFO("%f %f %f %f", m[0], -m[4], m[8], m[12]); // ROS_INFO("%f %f %f %f", -m[1], m[5], m[9], -m[13]); // ROS_INFO("%f %f %f %f", m[2], -m[6], m[10], m[14]); // ROS_INFO("%f %f %f %f", m[3], m[7], m[11], m[15]); }