ahtf
--------
HEADERS
TF_TO_MSG
TF_TO_EIGEN
EIGEN_TO_MSG
EIGEN_STUFF
static_transform_publisher
TF_QUATERNION
TF_MATRIX3X3
TF_VECTOR3
TF_POSE and TF_TRANSFORM
/opt/ros/hydro/include/tf/LinearMath/Matrix3x3.h
/opt/ros/hydro/include/tf/LinearMath/MinMax.h
/opt/ros/hydro/include/tf/LinearMath/QuadWord.h
/opt/ros/hydro/include/tf/LinearMath/Quaternion.h
/opt/ros/hydro/include/tf/LinearMath/Scalar.h
/opt/ros/hydro/include/tf/LinearMath/Transform.h
/opt/ros/hydro/include/tf/LinearMath/Vector3.h
/opt/ros/hydro/include/tf/tf.h
/opt/ros/hydro/include/tf/transform_datatypes.h // Includes Vec3, Mat3x3, Quat, Pose, Trans
/opt/ros/hydro/include/tf/transform_broadcaster.h
/opt/ros/hydro/include/tf/transform_listener.h
/opt/ros/hydro/include/tf/OTHERS...
#include <tf/transform_datatypes.h>
vw /opt/ros/hydro/include/tf/transform_datatypes.h
package: ros-hydro-tf tf
void tf::quaternionTFToMsg( const tf::Quaternion& bt, geometry_msgs::Quaternion& msg)
void tf::quaternionStampedTFToMsg( const tf::Stamped<tf::Quaternion>& bt, geometry_msgs::QuaternionStamped & msg)
void tf::vector3TFToMsg( const tf::Vector3& bt_v, geometry_msgs::Vector3& msg_v)
void tf::vector3StampedTFToMsg( const tf::Stamped<tf::Vector3>& bt, geometry_msgs::Vector3Stamped & msg)
void tf::pointTFToMsg( const tf::Point& bt_v, geometry_msgs::Point& msg_v)
void tf::pointStampedTFToMsg( const tf::Stamped<tf::Point>& bt, geometry_msgs::PointStamped & msg)
void tf::transformTFToMsg( const tf::Transform& bt, geometry_msgs::Transform& msg)
void tf::transformStampedTFToMsg( const tf::StampedTransform& bt, geometry_msgs::TransformStamped & msg)
void tf::poseTFToMsg( const tf::Pose& bt, geometry_msgs::Pose& msg)
void tf::poseStampedTFToMsg( const tf::Stamped<tf::Pose>& bt, geometry_msgs::PoseStamped & msg)
MSG_TO_TF
void tf::quaternionMsgToTF( const geometry_msgs::Quaternion& msg, tf::Quaternion& bt)
void tf::quaternionStampedMsgToTF( const geometry_msgs::QuaternionStamped & msg, tf::Stamped<tf::Quaternion>& bt)
void tf::vector3MsgToTF( const geometry_msgs::Vector3& msg_v, tf::Vector3& bt_v)
void tf::vector3StampedMsgToTF( const geometry_msgs::Vector3Stamped & msg, tf::Stamped<tf::Vector3>& bt)
void tf::pointMsgToTF( const geometry_msgs::Point& msg_v, tf::Point& bt_v)
void tf::pointStampedMsgToTF( const geometry_msgs::PointStamped & msg, tf::Stamped<tf::Point>& bt)
void tf::transformMsgToTF( const geometry_msgs::Transform& msg, tf::Transform& bt)
void tf::transformStampedMsgToTF( const geometry_msgs::TransformStamped & msg, tf::StampedTransform& bt)
void tf::poseMsgToTF( const geometry_msgs::Pose& msg, tf::Pose& bt)
void tf::poseStampedMsgToTF( const geometry_msgs::PoseStamped & msg, tf::Stamped<tf::Pose>& bt)
#include <tf_conversions/tf_eigen.h>
vw /opt/ros/hydro/include/tf_conversions/tf_eigen.h
package: ros-hydro-tf-conversions tf_conversions
void tf::matrixTFToEigen( const tf::Matrix3x3 &t, Eigen::Matrix3d &e);
void tf::poseTFToEigen( const tf::Pose &t, Eigen::Affine3d &e);
void tf::quaternionTFToEigen(const tf::Quaternion &t, Eigen::Quaterniond &e);
void tf::transformTFToEigen( const tf::Transform &t, Eigen::Affine3d &e);
void tf::vectorTFToEigen( const tf::Vector3 &t, Eigen::Vector3d &e);
EIGEN_TO_TF
void tf::matrixEigenToTF( const Eigen::Matrix3d &e, tf::Matrix3x3 &t);
void tf::poseEigenToTF( const Eigen::Affine3d &e, tf::Pose &t);
void tf::quaternionEigenToTF(const Eigen::Quaterniond &e, tf::Quaternion &t);
void tf::transformEigenToTF( const Eigen::Affine3d &e, tf::Transform &t);
void tf::vectorEigenToTF( const Eigen::Vector3d &e, tf::Vector3 &t);
#include <eigen_conversions/eigen_msg.h>
vw /opt/ros/hydro/include/eigen_conversions/eigen_msg.h
package: ros-hydro-eigen-conversions eigen_conversions
void tf::pointEigenToMsg(const Eigen::Vector3d &e, geometry_msgs::Point &m);
void tf::poseEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Pose &m);
void tf::quaternionEigenToMsg(const Eigen::Quaterniond &e, geometry_msgs::Quaternion &m);
void tf::transformEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Transform &m);
void tf::twistEigenToMsg(const Eigen::Matrix<double,6,1> &e, geometry_msgs::Twist &m);
void tf::vectorEigenToMsg(const Eigen::Vector3d &e, geometry_msgs::Vector3 &m);
void tf::wrenchEigenToMsg(const Eigen::Matrix<double,6,1> &e, geometry_msgs::Wrench &m);
void tf::matrixEigenToMsg(const Eigen::MatrixBase<Derived> &e, std_msgs::Float64MultiArray &m)
MSG_TO_EIGEN
void tf::pointMsgToEigen(const geometry_msgs::Point &m, Eigen::Vector3d &e);
void tf::poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e);
void tf::quaternionMsgToEigen(const geometry_msgs::Quaternion &m, Eigen::Quaterniond &e);
void tf::transformMsgToEigen(const geometry_msgs::Transform &m, Eigen::Affine3d &e);
void tf::twistMsgToEigen(const geometry_msgs::Twist &m, Eigen::Matrix<double,6,1> &e);
void tf::vectorMsgToEigen(const geometry_msgs::Vector3 &m, Eigen::Vector3d &e);
void tf::wrenchMsgToEigen(const geometry_msgs::Wrench &m, Eigen::Matrix<double,6,1> &e);
Eigen::Quaterniond q(a.rotation()); // Affine3d a -> Quaternion q
a.translation() // Affine3d a -> translation vector
a.linear() // Affine3d a -> 3x3 matrix
a.rotation() // Affine3d a -> 3x3 matrix, rotation only
q.setFromTwoVectors(v1,v2); // set q to rotate v1 to v2 (v1 and v2 do not need to be normalized)
// NOTE: Translation cannot be assigned to Affine. Must multiply times unit Quaterniond. LAME!
a = Eigen::Translation3d(x,y,z) *
Eigen::AngleAxisd(PI/2, Eigen::Vector3d(1,0,0));
a = Eigen::Translation3d(x,y,z) *
Eigen::Quaterniond(1,0,0,0);
cout << v; // print vector
cout << m; // print matrix
cout << a.matrix(); // print Affine3d as matrix
cout << a.translation(); // print matrix translation
cout << q.vec() << q.w(); // print quaternion
q.normalize(); // normalize the quat
double angle = acos(q.w())*2.0f; // normalized quaternion -> angle
Eigen::Affine3d s = Eigen::Affine3d( Eigen::Scaling(x,y,z)); // scale matrix
Eigen::Affine3d s = Eigen::Affine3d( Eigen::Translation3d(x,y,z)); // translation matrix
Eigen::Affine3d s = Eigen::Affine3d( Eigen::AngleAxisd(PI/2, Eigen::Vector3d(0,0,1))); // rot
Eigen::Matrix4d m = affine.matrix(); // affine to matrix
TF and BULLET
-------------
Initialize stamped transform:
tf::Quaternion q(x,y,z,w); // create q
double roll, pitch, yaw;
tf::Matrix3x3(q).getRPY(roll, pitch, yaw); // puts q into rpy
tf::Matrix3x3(q).getRPY(rx, ry, rz); // puts q into rpy
static_transform_publisher RPY
------------------------------
<node
pkg="tf"
type="static_transform_publisher"
name="mimic_origin_broadcaste5"
args="x y z rz ry rx abdomen_link mimic_origi5 100" />
Resulting transform is
Eigen::Affine3d result =
Eigen::Translation3d(x,y,z) *
Eigen::AngleAxisd(rz, Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd(ry, Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(rx, Eigen::Vector3d::UnitX());
NOTE: When thinking about rotating the object:
FIRST rotate around z
SECOND rotate around y
THIRD rotate around x
NOTE: rpy in a URDF is REVERSE of the above. xyz="x y z" rpy="rx ry rz"
TF_QUATERNIONS
--------------
#include <tf/transform_datatypes.h>
vw /opt/ros/hydro/include/tf/transform_datatypes.h
TF_QUATERNIONS
tf::Quaternion new_q(x,y,z,w);
tf::Quaternion new_q(const tf::Vector3& axis, const double angle);
#include <tf/transform_datatypes.h>
rre Quaternion.h
Quaternion()
Quaternion(x,y,z,w)
Quaternion(axis, angle)
Quaternion(yaw, pitch, roll) DEPRECATED
// set q from params
q.setRotation(axis, angle)
q.setEuler(yaw, pitch, roll) // NOT same as RPY
q.setRPY(roll, pitch, yaw) // This is URDF rpy order (opposite from static_transform order)
q.setEulerZYX(yaw, pitch, roll) - same as setRPY(roll, pitch, yaw)
Not that useful: + - += -= *= dot length2() length()
q.normalize()
Quaternion q.normalized()
Quaternion q.inverse()
double q1.angle(q2) **HALF** angle from q1 to q2
double q1.angleShortestPath(q2) shortest angle from q1 to q2
Vector3 q.getAxis()
double q.getAngle()
double q.getAngleShortestPath()
Quaternion q1.slerp(q2, t)
Quaternion::getIdentity()
q = q1 * q2
q *= q2
q.x(), q.y(), q.z(), q.w() // access (read only)
q[0]=x; q[1]=y; q[2]=z; q[3]=w; // access (r/w)
rre Matrix3x3.h
Matrix3x3()
Matrix3x3(q)
Matrix3x3(xx,xy,xz, yx,yy,yz, zx,zy,zz)
Matrix3x3(m)
Vector3 m.getColumn(col)
Vector3 m.getRow(row)
Vector3& m[row]
double& m[row][col]
m = m1 * m2
m *= m2
m.setFromOpenGLSubMatrix(double*)
m.getOpenGLSubMatrix(double*)
m.setValue(xx,xy,xz, yx,yy,yz, zx,zy,zz)
m.setRotation(q) // sets m to match q
m.setEulerYPR(yaw, pitch, roll)
m.getEulerYPR(yaw, pitch, roll)
m.setRPY(roll, pitch, yaw) -- same as setEulerYPR(yaw, pitch, roll)
m.getRPY(roll, pitch, yaw) -- same as getEulerYPR(yaw, pitch, roll)
m.setIdentity()
Matrix3x3::getIdentity()
m.getRotation(q) -- sets q
Matrix3x3 m.scaled(s)
double m.determinant()
Matrix3x3 m.adjoint()
Matrix3x3 m.absolute()
Matrix3x3 m.transpose()
Matrix3x3 m.inverse()
Matrix3x3 m1.transposeTimes(m2)
Matrix3x3 m1.timesTranspose(m2)
double m.tdotx(v)
double m.tdoty(v)
double m.tdotz(v)
m.diagonalize(mrot, threshold, maxSteps)
rre Vector3.h
Vector3 v1; // uninitialized
Vector3 v1(x,y,z);
+ - * // standard scalar ops
v1[0] = x; v1[1] = y; v1[2] = z; [] operator (read/write)
double v1.length()
double v1.length2()
double v1.dot(v2)
Vector3 v1.cross(v2)
double v1.distance(v2)
double v1.distance2(v2)
Vector3 v.normalized()
v.normalize()
Vector3 v.rotate(axiz, angle)
double v1.angle(v2) -- always positive
Vector3 v.absolute()
int v.minAxis() - return axis (012=xyz) with smallest value
int v.maxAxis() - return axis (012=xyz) with largest value
Vector3 v1.lerp(v2, t);
v.x() v.y() v.z()
v.setMax(v2) v is per element min of itself and v2
v.setMin(v2) v is per element max of itself and v2
v.setValue(x,y,z)
#include <tf/transform_datatypes.h>
vw /opt/ros/hydro/include/tf/LinearMath/Transform.h
rre Vector3.h
rre Matrix3x3.h
rre Transform.h
rre Quaternion.h
rre tf.h
tf::Transform == tf::Pose (Pose is typedef of transform)
tf::Pose p(tf::Quaternion(axis, angle), vector3=(0,0,0))
tf::Pose p(tf::Matrix3x3, vector3=(0,0,0))
tf::Pose p(tf::Matrix3x3::getIdentity(), tf::Vector3(0,0,0))
tf::Pose::getIdentity(); // static func returns identity
Matrix3x3& p.getBasis() // can modify through this
Vector3& p.getOrigin() // can modify through this
tf::Pose p.inverse() // uses transpose (i.e. assume pure rot+trans)
tf::Pose tf::Pose::getIdentity()
PRINTING TF
-----------
std::string vec3ToStr(tf::Vector3& v)
{
std::stringstream ss;
ss << "(";
ss << v.x() << ", ";
ss << v.y() << ", ";
ss << v.z() << ")";
return ss.str();
}
void printPose(const tf::Pose& pose)
{
ROS_INFO(" %7.3f %7.3f %7.3f %7.3f",
pose.getBasis().getColumn(0).x(),
pose.getBasis().getColumn(1).x(),
pose.getBasis().getColumn(2).x(),
pose.getOrigin().x());
ROS_INFO(" %7.3f %7.3f %7.3f %7.3f",
pose.getBasis().getColumn(0).y(),
pose.getBasis().getColumn(1).y(),
pose.getBasis().getColumn(2).y(),
pose.getOrigin().y());
ROS_INFO(" %7.3f %7.3f %7.3f %7.3f",
pose.getBasis().getColumn(0).z(),
pose.getBasis().getColumn(1).z(),
pose.getBasis().getColumn(2).z(),
pose.getOrigin().z());
}