// KDL transform to fcl transform
void convert(const KDL::Frame &in, fcl::Transform3f &out) {
double x,y,z,w;
in.M.GetQuaternion(x, y, z, w);
fcl::Vec3f t(in.p[0], in.p[1], in.p[2]);
fcl::Quaternion3f q(w, x, y, z);
out.setQuatRotation(q);
out.setTranslation(t);
}
void convert(const fcl::Transform3f &in, tf::Transform &out) {
out.setOrigin(tf::Vector3(in.getTranslation()[0],
in.getTranslation()[1],
in.getTranslation()[2]) );
out.setRotation(tf::Quaternion(in.getQuatRotation().getX(),
in.getQuatRotation().getY(),
in.getQuatRotation().getZ(),
in.getQuatRotation().getW()) );
}
void drawMesh(const fcl::Transform3f &transform, const OpenGLWrapper::Color &color) const {
std::vector<double> glTransform = OpenGLWrapper::getOpenGLWrapper().getIdentity();
const fcl::Vec3f &translation = transform.getTranslation();
const fcl::Matrix3f &orientation = transform.getRotation();
glTransform[0] = orientation(0,0);
glTransform[1] = orientation(0,1);
glTransform[2] = orientation(0,2);
glTransform[4] = orientation(1,0);
glTransform[5] = orientation(1,1);
glTransform[6] = orientation(1,2);
glTransform[8] = orientation(2,0);
glTransform[9] = orientation(2,1);
glTransform[10] = orientation(2,2);
glTransform[12] = translation[0];
glTransform[13] = translation[1];
glTransform[14] = translation[2];
mesh.draw(color, glTransform);
}
py::object Transform3fToNdarray2(const fcl::Transform3f& transform) {
size_t dim0 = 4, dim1 = 4;
py::object out = np_mod.attr("empty")(py::make_tuple(dim0, dim1), type_traits<double>::npname);
double* pout = getPointer<double>(out);
memset(pout, 0, dim0*dim1*sizeof(double));
memcpy(pout, (double*)(transform.getRotation().data.rs[0].vs), 3*sizeof(double));
memcpy(pout+3, (double*)(transform.getTranslation().data.vs), 1*sizeof(double));
memcpy(pout+4, (double*)(transform.getRotation().data.rs[1].vs), 3*sizeof(double));
memcpy(pout+7, (double*)(transform.getTranslation().data.vs+1), 1*sizeof(double));
memcpy(pout+8, (double*)(transform.getRotation().data.rs[2].vs), 3*sizeof(double));
memcpy(pout+11, (double*)(transform.getTranslation().data.vs+2), 1*sizeof(double));
std::fill_n(pout+12, 0.0, 3);
std::fill_n(pout+15, 1.0, 1);
return out;
}
se3::SE3 toPinocchioSE3(const fcl::Transform3f & tf)
{
return se3::SE3(toMatrix3d(tf.getRotation()), toVector3d(tf.getTranslation()));
}
void fcl2Eigen(const fcl::Transform3f & fcl, Eigen::Vector3d & eigen)
{
eigen(0) = fcl.getTranslation().data.vs[0];
eigen(1) = fcl.getTranslation().data.vs[1];
eigen(2) = fcl.getTranslation().data.vs[2];
}
