Eigen::Isometry3d DecodePose(const bot_core::position_3d_t& msg) {
Isometry3d ret;
ret.translation() = DecodeVector3d(msg.translation);
auto quaternion = DecodeQuaternion(msg.rotation);
ret.linear() = drake::math::quat2rotmat(quaternion);
ret.makeAffine();
return ret;
}
Isometry3d PrismaticJoint::jointTransform(double* const q) const
{
Isometry3d ret;
ret.linear().setIdentity();
ret.translation() = q[0] * translation_axis;
ret.makeAffine();
return ret;
}
Isometry3d RollPitchYawFloatingJoint::jointTransform(const Eigen::Ref<const VectorXd>& q) const
{
Isometry3d ret;
auto pos = q.middleRows<SPACE_DIMENSION>(0);
auto rpy = q.middleRows<RPY_SIZE>(SPACE_DIMENSION);
ret.linear() = rpy2rotmat(rpy);
ret.translation() = pos;
ret.makeAffine();
return ret;
}
Isometry3d RollPitchYawFloatingJoint::jointTransform(double* const q) const
{
Isometry3d ret;
Map<Vector3d> pos(&q[0]);
Map<Vector3d> rpy(&q[3]);
ret.linear() = rpy2rotmat(rpy);
ret.translation() = pos;
ret.makeAffine();
return ret;
}
void CollisionInterface::updateBodyNodes() {
int numNodes = mNodeMap.size();
for (std::map<BodyNode*, RigidBody*>::iterator it = mNodeMap.begin(); it != mNodeMap.end(); ++it) {
BodyNode *bn = it->first;
RigidBody *rb = it->second;
if (bn->getSkeleton()->getName() == "pinata")
continue;
Isometry3d W;
W.setIdentity();
W.linear() = rb->mOrientation;
W.translation() = rb->mPosition;
W.makeAffine();
bn->getSkeleton()->getJoint("freeJoint")->setTransformFromParentBodyNode(W);
//bn->updateTransform();
bn->getSkeleton()->computeForwardKinematics(true, false, false);
}
}
