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