diff --git a/.gitmodules b/.gitmodules index e013bbf1b..547ba513a 100644 --- a/.gitmodules +++ b/.gitmodules @@ -8,3 +8,7 @@ url = https://dawn.googlesource.com/dawn shallow = true branch = chromium/6108 +[submodule "deps/manif"] + path = deps/manif + url = https://github.com/artivis/manif.git + shallow = true diff --git a/CMakeLists.txt b/CMakeLists.txt index 65406ed51..5df7f4ffa 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -91,12 +91,12 @@ if (MROVER_BUILD_SIM) find_package(glfw3 REQUIRED) include(cmake/webgpu.cmake) - - add_subdirectory(deps/glfw3webgpu) + add_subdirectory(deps/glfw3webgpu SYSTEM) endif () find_package(OpenCV REQUIRED) find_package(ZED QUIET) find_package(Eigen3 REQUIRED) +add_subdirectory(deps/manif SYSTEM) find_package(PkgConfig REQUIRED) pkg_search_module(NetLink libnl-3.0) pkg_search_module(NetLinkRoute libnl-route-3.0) @@ -146,6 +146,7 @@ catkin_package() ## Libraries mrover_add_library(lie src/util/lie/*.cpp src/util/lie) +target_link_libraries(lie PUBLIC manif) ## ESW @@ -187,7 +188,7 @@ endif () ## Perception mrover_add_nodelet(tag_detector src/perception/tag_detector/*.cpp src/perception/tag_detector src/perception/tag_detector/pch.hpp) -mrover_nodelet_link_libraries(tag_detector opencv_core opencv_objdetect opencv_aruco opencv_imgproc opencv_highgui lie) +mrover_nodelet_link_libraries(tag_detector opencv_core opencv_objdetect opencv_aruco opencv_imgproc opencv_highgui lie manif) if (CUDA_FOUND) mrover_add_library(streaming src/esw/streaming/*.c* src/esw/streaming) @@ -200,7 +201,7 @@ endif () if (ZED_FOUND) mrover_add_nodelet(zed src/perception/zed_wrapper/*.c* src/perception/zed_wrapper src/perception/zed_wrapper/pch.hpp) mrover_nodelet_include_directories(zed ${ZED_INCLUDE_DIRS} ${CUDA_INCLUDE_DIRS}) - mrover_nodelet_link_libraries(zed ${ZED_LIBRARIES} ${SPECIAL_OS_LIBS} lie) + mrover_nodelet_link_libraries(zed ${ZED_LIBRARIES} ${SPECIAL_OS_LIBS} lie manif) mrover_nodelet_defines(zed ALLOW_BUILD_DEBUG # Ignore ZED warnings about Debug mode __CUDA_INCLUDE_COMPILER_INTERNAL_HEADERS__ # Eigen includes some files it should not, ignore @@ -216,7 +217,7 @@ mrover_add_node(arm_controller src/teleoperation/arm_controller/*.cpp) if (MROVER_BUILD_SIM) mrover_add_nodelet(simulator src/simulator/*.cpp src/simulator src/simulator/pch.hpp) mrover_nodelet_include_directories(simulator ${ASSIMP_INCLUDE_DIRS} ${BULLET_INCLUDE_DIRS} ${OPENCV_INCLUDE_DIRS} src/simulator/imgui) - mrover_nodelet_link_libraries(simulator ${ASSIMP_LIBRARIES} ${BULLET_LIBRARIES} glfw3webgpu webgpu glfw opencv_core opencv_imgcodecs opencv_imgproc lie) + mrover_nodelet_link_libraries(simulator ${ASSIMP_LIBRARIES} ${BULLET_LIBRARIES} glfw3webgpu webgpu glfw opencv_core opencv_imgcodecs opencv_imgproc lie manif) mrover_nodelet_defines(simulator BOOST_THREAD_PROVIDES_FUTURE) endif () diff --git a/deps/manif b/deps/manif new file mode 160000 index 000000000..c8a9fcbfe --- /dev/null +++ b/deps/manif @@ -0,0 +1 @@ +Subproject commit c8a9fcbfe157b55a7a860577febdc5350043a722 diff --git a/src/perception/tag_detector/pch.hpp b/src/perception/tag_detector/pch.hpp index 8b8ed1c34..dfa943021 100644 --- a/src/perception/tag_detector/pch.hpp +++ b/src/perception/tag_detector/pch.hpp @@ -30,5 +30,6 @@ #include #include +#include #include -#include +#include diff --git a/src/perception/tag_detector/tag_detector.cpp b/src/perception/tag_detector/tag_detector.cpp index ee9af68ca..7ece83885 100644 --- a/src/perception/tag_detector/tag_detector.cpp +++ b/src/perception/tag_detector/tag_detector.cpp @@ -140,5 +140,5 @@ namespace mrover { res.success = true; return true; } - + } // namespace mrover diff --git a/src/perception/tag_detector/tag_detector.hpp b/src/perception/tag_detector/tag_detector.hpp index b14541478..b6fb3a45b 100644 --- a/src/perception/tag_detector/tag_detector.hpp +++ b/src/perception/tag_detector/tag_detector.hpp @@ -6,7 +6,7 @@ namespace mrover { int id = -1; int hitCount = 0; cv::Point2f imageCenter{}; - std::optional tagInCam; + std::optional tagInCam; }; class TagDetectorNodelet : public nodelet::Nodelet { @@ -53,7 +53,7 @@ namespace mrover { void publishThresholdedImage(); - std::optional getTagInCamFromPixel(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t u, size_t v); + std::optional getTagInCamFromPixel(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t u, size_t v); public: TagDetectorNodelet() = default; diff --git a/src/perception/tag_detector/tag_detector.processing.cpp b/src/perception/tag_detector/tag_detector.processing.cpp index b3e47fbc0..b922bb075 100644 --- a/src/perception/tag_detector/tag_detector.processing.cpp +++ b/src/perception/tag_detector/tag_detector.processing.cpp @@ -8,7 +8,7 @@ namespace mrover { * @param u X Pixel Position * @param v Y Pixel Position */ - std::optional TagDetectorNodelet::getTagInCamFromPixel(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t u, size_t v) { + std::optional TagDetectorNodelet::getTagInCamFromPixel(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t u, size_t v) { assert(cloudPtr); if (u >= cloudPtr->width || v >= cloudPtr->height) { @@ -23,7 +23,7 @@ namespace mrover { return std::nullopt; } - return std::make_optional(R3{point.x, point.y, point.z}, SO3{}); + return std::make_optional(R3{point.x, point.y, point.z}, SO3d::Identity()); } /** @@ -81,7 +81,7 @@ namespace mrover { if (tag.tagInCam) { // Publish tag to immediate std::string immediateFrameId = "immediateFiducial" + std::to_string(tag.id); - SE3::pushToTfTree(mTfBroadcaster, immediateFrameId, mCameraFrameId, tag.tagInCam.value()); + SE3Conversions::pushToTfTree(mTfBroadcaster, immediateFrameId, mCameraFrameId, tag.tagInCam.value()); } } @@ -108,8 +108,8 @@ namespace mrover { std::string immediateFrameId = "immediateFiducial" + std::to_string(tag.id); // Publish tag to odom std::string const& parentFrameId = mUseOdom ? mOdomFrameId : mMapFrameId; - SE3 tagInParent = SE3::fromTfTree(mTfBuffer, parentFrameId, immediateFrameId); - SE3::pushToTfTree(mTfBroadcaster, "fiducial" + std::to_string(id), parentFrameId, tagInParent); + SE3d tagInParent = SE3Conversions::fromTfTree(mTfBuffer, parentFrameId, immediateFrameId); + SE3Conversions::pushToTfTree(mTfBroadcaster, "fiducial" + std::to_string(id), parentFrameId, tagInParent); } catch (tf2::ExtrapolationException const&) { NODELET_WARN("Old data for immediate tag"); } catch (tf2::LookupException const&) { diff --git a/src/perception/zed_wrapper/pch.hpp b/src/perception/zed_wrapper/pch.hpp index 8088686af..4df657b64 100644 --- a/src/perception/zed_wrapper/pch.hpp +++ b/src/perception/zed_wrapper/pch.hpp @@ -24,6 +24,5 @@ #include #include -#include -#include #include +#include diff --git a/src/perception/zed_wrapper/zed_wrapper.bridge.cu b/src/perception/zed_wrapper/zed_wrapper.bridge.cu index 5be3946bd..cacb63f77 100644 --- a/src/perception/zed_wrapper/zed_wrapper.bridge.cu +++ b/src/perception/zed_wrapper/zed_wrapper.bridge.cu @@ -1,5 +1,4 @@ // Be careful what you include in this file, it is compiled with nvcc (NVIDIA CUDA compiler) -// For example OpenCV and lie includes cause problems #include "zed_wrapper.hpp" diff --git a/src/perception/zed_wrapper/zed_wrapper.cpp b/src/perception/zed_wrapper/zed_wrapper.cpp index dd943c431..e05d61b61 100644 --- a/src/perception/zed_wrapper/zed_wrapper.cpp +++ b/src/perception/zed_wrapper/zed_wrapper.cpp @@ -1,5 +1,8 @@ #include "zed_wrapper.hpp" +#include +#include + namespace mrover { /** @@ -252,11 +255,11 @@ namespace mrover { sl::Translation const& translation = pose.getTranslation(); sl::Orientation const& orientation = pose.getOrientation(); try { - SE3 leftCameraInOdom{{translation.x, translation.y, translation.z}, - Eigen::Quaterniond{orientation.w, orientation.x, orientation.y, orientation.z}.normalized()}; - SE3 leftCameraInBaseLink = SE3::fromTfTree(mTfBuffer, "base_link", "zed2i_left_camera_frame"); - SE3 baseLinkInOdom = leftCameraInBaseLink * leftCameraInOdom; - SE3::pushToTfTree(mTfBroadcaster, "base_link", "odom", baseLinkInOdom); + SE3d leftCameraInOdom{{translation.x, translation.y, translation.z}, + Eigen::Quaterniond{orientation.w, orientation.x, orientation.y, orientation.z}.normalized()}; + SE3d baseLinkToLeftCamera = SE3Conversions::fromTfTree(mTfBuffer, "base_link", "zed2i_left_camera_frame"); + SE3d baseLinkInOdom = leftCameraInOdom * baseLinkToLeftCamera; + SE3Conversions::pushToTfTree(mTfBroadcaster, "base_link", "odom", baseLinkInOdom); } catch (tf2::TransformException& e) { NODELET_WARN_STREAM("Failed to get transform: " << e.what()); } diff --git a/src/simulator/pch.hpp b/src/simulator/pch.hpp index 233333e9b..e6ca84c02 100644 --- a/src/simulator/pch.hpp +++ b/src/simulator/pch.hpp @@ -68,9 +68,10 @@ #include #include +#include #include #include -#include +#include #include #include diff --git a/src/simulator/simulator.controls.cpp b/src/simulator/simulator.controls.cpp index 6d50b1f91..24ac9e169 100644 --- a/src/simulator/simulator.controls.cpp +++ b/src/simulator/simulator.controls.cpp @@ -82,40 +82,37 @@ namespace mrover { } auto SimulatorNodelet::freeLook(Clock::duration dt) -> void { - float flySpeed = mFlySpeed * std::chrono::duration_cast>(dt).count(); - if (glfwGetKey(mWindow.get(), mCamRightKey) == GLFW_PRESS) { - mCameraInWorld = SE3{R3{0, -flySpeed, 0}, SO3{}} * mCameraInWorld; - } - if (glfwGetKey(mWindow.get(), mCamLeftKey) == GLFW_PRESS) { - mCameraInWorld = SE3{R3{0, flySpeed, 0}, SO3{}} * mCameraInWorld; - } - if (glfwGetKey(mWindow.get(), mCamForwardKey) == GLFW_PRESS) { - mCameraInWorld = SE3{R3{flySpeed, 0, 0}, SO3{}} * mCameraInWorld; - } - if (glfwGetKey(mWindow.get(), mCamBackwardKey) == GLFW_PRESS) { - mCameraInWorld = SE3{R3{-flySpeed, 0, 0}, SO3{}} * mCameraInWorld; - } - if (glfwGetKey(mWindow.get(), mCamUpKey) == GLFW_PRESS) { - mCameraInWorld = mCameraInWorld * SE3{R3{0, 0, flySpeed}, SO3{}}; - } - if (glfwGetKey(mWindow.get(), mCamDownKey) == GLFW_PRESS) { - mCameraInWorld = mCameraInWorld * SE3{R3{0, 0, -flySpeed}, SO3{}}; - } + auto axis = [this](int positive, int negative) -> double { + return (glfwGetKey(mWindow.get(), positive) == GLFW_PRESS) - (glfwGetKey(mWindow.get(), negative) == GLFW_PRESS); + }; - Eigen::Vector2i size; - glfwGetWindowSize(mWindow.get(), &size.x(), &size.y()); - - Eigen::Vector2d center = (size / 2).cast(); + float flySpeed = mFlySpeed * std::chrono::duration_cast>(dt).count(); + R3 keyDelta = R3{axis(mCamForwardKey, mCamBackwardKey), axis(mCamLeftKey, mCamRightKey), axis(mCamUpKey, mCamDownKey)} * flySpeed; + Eigen::Vector2i windowSize; + glfwGetWindowSize(mWindow.get(), &windowSize.x(), &windowSize.y()); + Eigen::Vector2d windowCenter = (windowSize / 2).cast(); Eigen::Vector2d mouse; glfwGetCursorPos(mWindow.get(), &mouse.x(), &mouse.y()); + Eigen::Vector2d mouseDelta = (mouse - windowCenter) * mLookSense; + + SE3d::Tangent cameraRelativeTwist; + cameraRelativeTwist << keyDelta.x(), keyDelta.y(), 0.0, 0.0, mouseDelta.y(), 0.0; + + SE3d::Tangent worldRelativeTwist; + worldRelativeTwist << 0.0, 0.0, keyDelta.z(), 0.0, 0.0, 0.0; + + // The plus operator is overloaded and corresponds to lplus and rplus in the micro Lie paper + // It applies the exponential map to the tangent space element + // The result can be acted upon a group element (in this case SE3) + // The side of the plus operator determines word vs. camera space application + mCameraInWorld = worldRelativeTwist + mCameraInWorld + cameraRelativeTwist; - Eigen::Vector2d delta = (mouse - center) * mLookSense; + SO3d::Tangent worldRelativeAngularVelocity; + worldRelativeAngularVelocity << 0.0, 0.0, -mouseDelta.x(); - // TODO(quintin): use lie algebra more here? we have a perturbation in the tangent space - R3 p = mCameraInWorld.position(); - SO3 q = SO3{delta.y(), R3::UnitY()} * mCameraInWorld.rotation() * SO3{-delta.x(), R3::UnitZ()}; - mCameraInWorld = SE3{p, q}; + // TODO: Is there a way to combine this with the above? + mCameraInWorld.asSO3() = worldRelativeAngularVelocity + mCameraInWorld.asSO3(); centerCursor(); } @@ -160,4 +157,4 @@ namespace mrover { } } -} // namespace mrover +} // namespace mrover \ No newline at end of file diff --git a/src/simulator/simulator.gui.cpp b/src/simulator/simulator.gui.cpp index be565cdd5..5888cff53 100644 --- a/src/simulator/simulator.gui.cpp +++ b/src/simulator/simulator.gui.cpp @@ -94,15 +94,15 @@ namespace mrover { URDF const& rover = it->second; { - SE3 baseLinkInMap = rover.linkInWorld("base_link"); - R3 p = baseLinkInMap.position(); - S3 q = baseLinkInMap.rotation().quaternion(); + SE3d baseLinkInMap = rover.linkInWorld("base_link"); + R3 p = baseLinkInMap.translation(); + S3 q = baseLinkInMap.quat(); ImGui::Text("Rover Position: (%.2f, %.2f, %.2f)", p.x(), p.y(), p.z()); ImGui::Text("Rover Orientation: (%.2f, %.2f, %.2f, %.2f)", q.w(), q.x(), q.y(), q.z()); } { - R3 p = mCameraInWorld.position(); - S3 q = mCameraInWorld.rotation().quaternion(); + R3 p = mCameraInWorld.translation(); + S3 q = mCameraInWorld.quat(); ImGui::Text("Camera Position: (%.2f, %.2f, %.2f)", p.x(), p.y(), p.z()); ImGui::Text("Camera Orientation: (%.2f, %.2f, %.2f, %.2f)", q.w(), q.x(), q.y(), q.z()); } diff --git a/src/simulator/simulator.hpp b/src/simulator/simulator.hpp index 1076d3aa1..8a1782f52 100644 --- a/src/simulator/simulator.hpp +++ b/src/simulator/simulator.hpp @@ -97,7 +97,7 @@ namespace mrover { auto makeCameraForLink(SimulatorNodelet& simulator, btMultibodyLink const* link) -> Camera; - [[nodiscard]] auto linkInWorld(std::string const& linkName) const -> SE3; + [[nodiscard]] auto linkInWorld(std::string const& linkName) const -> SE3d; }; struct PeriodicTask { @@ -305,7 +305,7 @@ namespace mrover { auto getUrdf(std::string const& name) -> std::optional>; - SE3 mCameraInWorld{R3{-3.0, 0.0, 1.5}, SO3{}}; + SE3d mCameraInWorld{R3{-3.0, 0.0, 1.5}, SO3d::Identity()}; std::vector mStereoCameras; std::vector mCameras; @@ -432,7 +432,7 @@ namespace mrover { auto urdfPoseToBtTransform(urdf::Pose const& pose) -> btTransform; - auto btTransformToSe3(btTransform const& transform) -> SE3; + auto btTransformToSe3(btTransform const& transform) -> SE3d; auto computeCameraToClip(float fovY, float aspect, float zNear, float zFar) -> Eigen::Matrix4f; diff --git a/src/simulator/simulator.physics.cpp b/src/simulator/simulator.physics.cpp index 5f6d82d2e..b8f0c9c3e 100644 --- a/src/simulator/simulator.physics.cpp +++ b/src/simulator/simulator.physics.cpp @@ -2,10 +2,12 @@ namespace mrover { - auto btTransformToSe3(btTransform const& transform) -> SE3 { + auto btTransformToSe3(btTransform const& transform) -> SE3d { btVector3 const& p = transform.getOrigin(); btQuaternion const& q = transform.getRotation(); - return SE3{R3{p.x(), p.y(), p.z()}, SO3{q.w(), q.x(), q.y(), q.z()}}; + // Note: Must convert the Bullet quaternion (floats) to a normalized Eigen quaternion (doubles). + // Otherwise the normality check will fail in the SO3 constructor. + return SE3d{R3{p.x(), p.y(), p.z()}, Eigen::Quaterniond{q.w(), q.x(), q.y(), q.z()}.normalized()}; } auto SimulatorNodelet::initPhysics() -> void { @@ -76,9 +78,9 @@ namespace mrover { int index = urdf.linkNameToMeta.at(link->name).index; // TODO(quintin): figure out why we need to negate rvector btTransform parentToChild{urdf.physics->getParentToLocalRot(index), -urdf.physics->getRVector(index)}; - SE3 childInParent = btTransformToSe3(parentToChild.inverse()); + SE3d childInParent = btTransformToSe3(parentToChild.inverse()); - SE3::pushToTfTree(mTfBroadcaster, link->name, link->getParent()->name, childInParent); + SE3Conversions::pushToTfTree(mTfBroadcaster, link->name, link->getParent()->name, childInParent); } for (urdf::JointSharedPtr const& child_joint: link->child_joints) { @@ -95,17 +97,17 @@ namespace mrover { if (auto modelOpt = getUrdf(modelName)) { URDF const& model = *modelOpt; - SE3 modelInMap = btTransformToSe3(model.physics->getBaseWorldTransform()); - SE3 roverInMap = btTransformToSe3(rover.physics->getBaseWorldTransform()); + SE3d modelInMap = btTransformToSe3(model.physics->getBaseWorldTransform()); + SE3d roverInMap = btTransformToSe3(rover.physics->getBaseWorldTransform()); - R3 roverToModel = modelInMap.position() - roverInMap.position(); + R3 roverToModel = modelInMap.translation() - roverInMap.translation(); double roverDistanceToModel = roverToModel.norm(); roverToModel /= roverDistanceToModel; R3 roverForward = roverInMap.rotation().matrix().col(0); double roverDotModel = roverToModel.dot(roverForward); if (roverDotModel > 0 && roverDistanceToModel < threshold) { - SE3::pushToTfTree(mTfBroadcaster, modelName, "map", modelInMap); + SE3Conversions::pushToTfTree(mTfBroadcaster, modelName, "map", modelInMap); } } }; @@ -115,7 +117,7 @@ namespace mrover { } } - auto URDF::linkInWorld(std::string const& linkName) const -> SE3 { + auto URDF::linkInWorld(std::string const& linkName) const -> SE3d { int index = linkNameToMeta.at(linkName).index; return btTransformToSe3(index == -1 ? physics->getBaseWorldTransform() : physics->getLink(index).m_cachedWorldTransform); } diff --git a/src/simulator/simulator.render.cpp b/src/simulator/simulator.render.cpp index 6c2a3c116..14ed062e4 100644 --- a/src/simulator/simulator.render.cpp +++ b/src/simulator/simulator.render.cpp @@ -409,10 +409,10 @@ namespace mrover { if (auto urdfMesh = std::dynamic_pointer_cast(visual->geometry)) { Model& model = mUriToModel.at(urdfMesh->filename); URDF::LinkMeta& meta = urdf.linkNameToMeta.at(link->name); - SE3 linkInWorld = urdf.linkInWorld(link->name); - SE3 modelInLink = btTransformToSe3(urdfPoseToBtTransform(link->visual->origin)); - SE3 modelInWorld = linkInWorld * modelInLink; - renderModel(pass, model, meta.visualUniforms.at(visualIndex), SIM3{modelInWorld.position(), modelInWorld.rotation(), R3::Ones()}); + SE3d linkInWorld = urdf.linkInWorld(link->name); + SE3d modelInLink = btTransformToSe3(urdfPoseToBtTransform(link->visual->origin)); + SE3d modelInWorld = linkInWorld * modelInLink; + renderModel(pass, model, meta.visualUniforms.at(visualIndex), SIM3{modelInWorld, R3::Ones()}); visualIndex++; } } @@ -439,27 +439,27 @@ namespace mrover { assert(collider); btCollisionShape* shape = collider->getCollisionShape(); assert(shape); - SE3 linkInWorld = urdf.linkInWorld(link->name); + SE3d linkInWorld = urdf.linkInWorld(link->name); if (auto* compound = dynamic_cast(shape)) { for (int i = 0; i < compound->getNumChildShapes(); ++i) { - SE3 modelInLink = btTransformToSe3(urdfPoseToBtTransform(link->collision_array.at(i)->origin)); - SE3 shapeToWorld = modelInLink * linkInWorld; + SE3d modelInLink = btTransformToSe3(urdfPoseToBtTransform(link->collision_array.at(i)->origin)); + SE3d shapeToWorld = modelInLink * linkInWorld; auto* shape = compound->getChildShape(i); if (auto* box = dynamic_cast(shape)) { btVector3 extents = box->getHalfExtentsWithoutMargin() * 2; - SIM3 modelToWorld{shapeToWorld.position(), shapeToWorld.rotation(), R3{extents.x(), extents.y(), extents.z()}}; + SIM3 modelToWorld{shapeToWorld, R3{extents.x(), extents.y(), extents.z()}}; renderModel(pass, mUriToModel.at(CUBE_PRIMITIVE_URI), meta.collisionUniforms.at(i), modelToWorld); } else if (auto* sphere = dynamic_cast(shape)) { btScalar diameter = sphere->getRadius() * 2; - SIM3 modelToWorld{shapeToWorld.position(), shapeToWorld.rotation(), R3{diameter, diameter, diameter}}; + SIM3 modelToWorld{shapeToWorld, R3{diameter, diameter, diameter}}; renderModel(pass, mUriToModel.at(SPHERE_PRIMITIVE_URI), meta.collisionUniforms.at(i), modelToWorld); } else if (auto* cylinder = dynamic_cast(shape)) { btVector3 extents = cylinder->getHalfExtentsWithoutMargin() * 2; - SIM3 modelToWorld{shapeToWorld.position(), shapeToWorld.rotation(), R3{extents.x(), extents.y(), extents.z()}}; + SIM3 modelToWorld{shapeToWorld, R3{extents.x(), extents.y(), extents.z()}}; renderModel(pass, mUriToModel.at(CYLINDER_PRIMITIVE_URI), meta.collisionUniforms.at(i), modelToWorld); } else if (auto* mesh = dynamic_cast(shape)) { - SIM3 modelToWorld{shapeToWorld.position(), shapeToWorld.rotation(), R3::Ones()}; + SIM3 modelToWorld{shapeToWorld, R3::Ones()}; renderModel(pass, mUriToModel.at(mMeshToUri.at(const_cast(mesh))), meta.collisionUniforms.at(i), modelToWorld); } else { NODELET_WARN_STREAM_ONCE(std::format("Tried to render unsupported collision shape: {}", shape->getName())); @@ -694,8 +694,8 @@ namespace mrover { glfwGetFramebufferSize(mWindow.get(), &width, &height); float aspect = static_cast(width) / static_cast(height); mSceneUniforms.value.cameraToClip = computeCameraToClip(mFovDegrees * DEG_TO_RAD, aspect, NEAR, FAR).cast(); - mSceneUniforms.value.worldToCamera = mCameraInWorld.matrix().inverse().cast(); - mSceneUniforms.value.cameraInWorld = mCameraInWorld.position().cast().homogeneous(); + mSceneUniforms.value.worldToCamera = mCameraInWorld.inverse().transform().cast(); + mSceneUniforms.value.cameraInWorld = mCameraInWorld.translation().cast().homogeneous(); mSceneUniforms.enqueueWrite(); wgpu::BindGroupEntry entry; diff --git a/src/simulator/simulator.sensors.cpp b/src/simulator/simulator.sensors.cpp index af4bc60db..798fc9f12 100644 --- a/src/simulator/simulator.sensors.cpp +++ b/src/simulator/simulator.sensors.cpp @@ -11,9 +11,9 @@ namespace mrover { camera.sceneUniforms.value.lightInWorld = {0, 0, 5, 1}; float aspect = static_cast(camera.resolution.x()) / static_cast(camera.resolution.y()); camera.sceneUniforms.value.cameraToClip = computeCameraToClip(camera.fov * DEG_TO_RAD, aspect, NEAR, FAR).cast(); - SE3 cameraInWorld = btTransformToSe3(camera.link->m_cachedWorldTransform); - camera.sceneUniforms.value.worldToCamera = cameraInWorld.matrix().inverse().cast(); - camera.sceneUniforms.value.cameraInWorld = cameraInWorld.position().cast().homogeneous(); + SE3d cameraInWorld = btTransformToSe3(camera.link->m_cachedWorldTransform); + camera.sceneUniforms.value.worldToCamera = cameraInWorld.inverse().transform().cast(); + camera.sceneUniforms.value.cameraInWorld = cameraInWorld.translation().cast().homogeneous(); camera.sceneUniforms.enqueueWrite(); wgpu::BindGroupEntry entry; @@ -111,22 +111,22 @@ namespace mrover { return {lat, lon, alt}; } - auto computeNavSatFix(SE3 const& gpuInMap, Eigen::Vector3d const& referenceGeodetic, double referenceHeadingDegrees) -> sensor_msgs::NavSatFix { + auto computeNavSatFix(SE3d const& gpsInMap, Eigen::Vector3d const& referenceGeodetic, double referenceHeadingDegrees) -> sensor_msgs::NavSatFix { sensor_msgs::NavSatFix gpsMessage; gpsMessage.header.stamp = ros::Time::now(); gpsMessage.header.frame_id = "map"; - auto geodetic = cartesianToGeodetic(gpuInMap.position(), referenceGeodetic, referenceHeadingDegrees); + auto geodetic = cartesianToGeodetic(gpsInMap.translation(), referenceGeodetic, referenceHeadingDegrees); gpsMessage.latitude = geodetic(0); gpsMessage.longitude = geodetic(1); gpsMessage.altitude = geodetic(2); return gpsMessage; } - auto computeImu(SE3 const& imuInMap, R3 const& imuAngularVelocity, R3 const& linearAcceleration, R3 const& magneticField) -> ImuAndMag { + auto computeImu(SE3d const& imuInMap, R3 const& imuAngularVelocity, R3 const& linearAcceleration, R3 const& magneticField) -> ImuAndMag { ImuAndMag imuMessage; imuMessage.header.stamp = ros::Time::now(); imuMessage.header.frame_id = "map"; - S3 q = imuInMap.rotation().quaternion(); + S3 q = imuInMap.quat(); imuMessage.imu.orientation.w = q.w(); imuMessage.imu.orientation.x = q.x(); imuMessage.imu.orientation.y = q.y(); @@ -152,15 +152,15 @@ namespace mrover { URDF const& rover = *lookup; { - SE3 baseLinkInMap = rover.linkInWorld("base_link"); + SE3d baseLinkInMap = rover.linkInWorld("base_link"); nav_msgs::Odometry odometry; odometry.header.stamp = ros::Time::now(); odometry.header.frame_id = "map"; - R3 p = baseLinkInMap.position(); + R3 p = baseLinkInMap.translation(); odometry.pose.pose.position.x = p.x(); odometry.pose.pose.position.y = p.y(); odometry.pose.pose.position.z = p.z(); - S3 q = baseLinkInMap.rotation().quaternion(); + S3 q = baseLinkInMap.quat(); odometry.pose.pose.orientation.w = q.w(); odometry.pose.pose.orientation.x = q.x(); odometry.pose.pose.orientation.y = q.y(); @@ -169,7 +169,7 @@ namespace mrover { mGroundTruthPub.publish(odometry); } if (mGpsTask.shouldUpdate()) { - SE3 gpsInMap = rover.linkInWorld("chassis_link"); + SE3d gpsInMap = rover.linkInWorld("chassis_link"); mGpsPub.publish(computeNavSatFix(gpsInMap, mGpsLinerizationReferencePoint, mGpsLinerizationReferenceHeading)); } if (mImuTask.shouldUpdate()) { @@ -177,8 +177,8 @@ namespace mrover { R3 roverLinearVelocity = btVector3ToR3(rover.physics->getBaseVel()); R3 imuLinearAcceleration = (roverLinearVelocity - mRoverLinearVelocity) / std::chrono::duration_cast>(dt).count(); mRoverLinearVelocity = roverLinearVelocity; - SE3 imuInMap = rover.linkInWorld("imu"); - mImuPub.publish(computeImu(imuInMap, imuAngularVelocity, imuLinearAcceleration, imuInMap.rotation().matrix().transpose().col(1))); + SE3d imuInMap = rover.linkInWorld("imu"); + mImuPub.publish(computeImu(imuInMap, imuAngularVelocity, imuLinearAcceleration, imuInMap.asSO3().inverse().rotation().col(1))); } } } diff --git a/src/util/lie/lie.cpp b/src/util/lie/lie.cpp new file mode 100644 index 000000000..b58beee26 --- /dev/null +++ b/src/util/lie/lie.cpp @@ -0,0 +1,75 @@ +#include "lie.hpp" + +SIM3::SIM3(SE3d const& se3, R3 const& scale) { + mTransform.fromPositionOrientationScale(se3.translation(), se3.rotation(), scale); +} + +auto SIM3::matrix() const -> Eigen::Matrix4d { + return mTransform.matrix(); +} + +auto SIM3::position() const -> R3 { + return mTransform.translation(); +} + +auto SE3Conversions::fromTfTree(tf2_ros::Buffer const& buffer, std::string const& fromFrameId, std::string const& toFrameId) -> SE3d { + geometry_msgs::TransformStamped transform = buffer.lookupTransform(fromFrameId, toFrameId, ros::Time{}); + return fromTf(transform.transform); +} + +auto SE3Conversions::pushToTfTree(tf2_ros::TransformBroadcaster& broadcaster, std::string const& childFrameId, std::string const& parentFrameId, SE3d const& tf) -> void { + broadcaster.sendTransform(toTransformStamped(tf, parentFrameId, childFrameId)); +} + +auto SE3Conversions::fromTf(geometry_msgs::Transform const& transform) -> SE3d { + return {{transform.translation.x, transform.translation.y, transform.translation.z}, + Eigen::Quaterniond{transform.rotation.w, transform.rotation.x, transform.rotation.y, transform.rotation.z}}; +} + +auto SE3Conversions::fromPose(geometry_msgs::Pose const& pose) -> SE3d { + return {{pose.position.x, pose.position.y, pose.position.z}, + Eigen::Quaterniond{pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z}}; +} + +auto SE3Conversions::toPose(SE3d const& tf) -> geometry_msgs::Pose { + geometry_msgs::Pose pose; + pose.position.x = tf.x(); + pose.position.y = tf.y(); + pose.position.z = tf.z(); + SE3d::Quaternion const& q = tf.quat(); + pose.orientation.x = q.x(); + pose.orientation.y = q.y(); + pose.orientation.z = q.z(); + pose.orientation.w = q.w(); + return pose; +} + +auto SE3Conversions::toTransform(SE3d const& tf) -> geometry_msgs::Transform { + geometry_msgs::Transform transform; + transform.translation.x = tf.x(); + transform.translation.y = tf.y(); + transform.translation.z = tf.z(); + SE3d::Quaternion const& q = tf.quat(); + transform.rotation.x = q.x(); + transform.rotation.y = q.y(); + transform.rotation.z = q.z(); + transform.rotation.w = q.w(); + return transform; +} + +auto SE3Conversions::toPoseStamped(SE3d const& tf, std::string const& frameId) -> geometry_msgs::PoseStamped { + geometry_msgs::PoseStamped pose; + pose.pose = toPose(tf); + pose.header.frame_id = frameId; + pose.header.stamp = ros::Time::now(); + return pose; +} + +auto SE3Conversions::toTransformStamped(SE3d const& tf, std::string const& parentFrameId, std::string const& childFrameId) -> geometry_msgs::TransformStamped { + geometry_msgs::TransformStamped transform; + transform.transform = toTransform(tf); + transform.header.frame_id = parentFrameId; + transform.header.stamp = ros::Time::now(); + transform.child_frame_id = childFrameId; + return transform; +} diff --git a/src/util/lie/lie.hpp b/src/util/lie/lie.hpp new file mode 100644 index 000000000..1e51e4dab --- /dev/null +++ b/src/util/lie/lie.hpp @@ -0,0 +1,50 @@ +#pragma once + +#include +#include + +#include +#include +#include + +#include +#include + +using manif::SE3d, manif::SO3d; + +using R3 = Eigen::Vector3d; +using S3 = Eigen::Quaterniond; + +class SE3Conversions { +public: + static auto fromTf(geometry_msgs::Transform const& transform) -> SE3d; + + static auto fromPose(geometry_msgs::Pose const& pose) -> SE3d; + + [[nodiscard]] static auto toPose(SE3d const& tf) -> geometry_msgs::Pose; + + [[nodiscard]] static auto toTransform(SE3d const& tf) -> geometry_msgs::Transform; + + [[nodiscard]] static auto toPoseStamped(SE3d const& tf, std::string const& frameId) -> geometry_msgs::PoseStamped; + + [[nodiscard]] static auto toTransformStamped(SE3d const& tf, std::string const& parentFrameId, std::string const& childFrameId) -> geometry_msgs::TransformStamped; + + [[nodiscard]] static auto fromTfTree(tf2_ros::Buffer const& buffer, std::string const& fromFrameId, std::string const& toFrameId) -> SE3d; + + static auto pushToTfTree(tf2_ros::TransformBroadcaster& broadcaster, std::string const& childFrameId, std::string const& parentFrameId, SE3d const& tf) -> void; +}; + +class SIM3 { + using Transform = Eigen::Transform; + + Transform mTransform = Transform::Identity(); + +public: + SIM3() = default; + + SIM3(SE3d const& se3, R3 const& scale); + + [[nodiscard]] auto matrix() const -> Eigen::Matrix4d; + + [[nodiscard]] auto position() const -> R3; +}; diff --git a/src/util/lie/se3.cpp b/src/util/lie/se3.cpp deleted file mode 100644 index ce4e4593d..000000000 --- a/src/util/lie/se3.cpp +++ /dev/null @@ -1,40 +0,0 @@ -#include "se3.hpp" - -SE3 SE3::fromTfTree(tf2_ros::Buffer const& buffer, std::string const& fromFrameId, std::string const& toFrameId) { - geometry_msgs::TransformStamped transform = buffer.lookupTransform(fromFrameId, toFrameId, ros::Time{}); - return fromTf(transform.transform); -} - -void SE3::pushToTfTree(tf2_ros::TransformBroadcaster& broadcaster, std::string const& childFrameId, std::string const& parentFrameId, SE3 const& tf) { - broadcaster.sendTransform(tf.toTransformStamped(parentFrameId, childFrameId)); -} - -SE3::SE3(R3 const& position, SO3 const& rotation) { - assert(!position.hasNaN()); - mTransform.translate(position); - mTransform.rotate(rotation.mAngleAxis); -} - -Eigen::Matrix4d SE3::matrix() const { - return mTransform.matrix(); -} - -R3 SE3::position() const { - return mTransform.translation(); -} - -SO3 SE3::rotation() const { - return mTransform.rotation(); -} - -double SE3::distanceTo(SE3 const& other) const { - return (position() - other.position()).norm(); -} - -auto SE3::inverse() const -> SE3 { - return SE3{mTransform.inverse()}; -} - -SE3 SE3::operator*(SE3 const& other) const { - return other.mTransform * mTransform; -} diff --git a/src/util/lie/se3.hpp b/src/util/lie/se3.hpp deleted file mode 100644 index f821d32eb..000000000 --- a/src/util/lie/se3.hpp +++ /dev/null @@ -1,112 +0,0 @@ -#pragma once - -#include -#include -#include - -#include -#include -#include - -#include - -using R3 = Eigen::Vector3d; -using S3 = Eigen::Quaterniond; - -/** - * @brief A 3D rotation. - */ -class SO3 { - using AngleAxis = Eigen::AngleAxis; - - AngleAxis mAngleAxis = AngleAxis::Identity(); - -public: - friend class SE3; - friend class SIM3; - - SO3() = default; - - SO3(double w, double x, double y, double z) : mAngleAxis{Eigen::Quaterniond{w, x, y, z}} {} - - template>, - typename = std::enable_if_t<(sizeof...(Args) > 0)>> - SO3(Args&&... args) : mAngleAxis{std::forward(args)...} {} - - [[nodiscard]] SO3 operator*(SO3 const& other) const; - - [[nodiscard]] R3 operator*(R3 const& other) const; - - [[nodiscard]] Eigen::Matrix3d matrix() const; - - [[nodiscard]] Eigen::Quaterniond quaternion() const; - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -/** - * @brief A 3D rigid transformation (direct isometry). - * - * In simpler terms: a 3D rotation followed by a 3D translation. - */ -class SE3 { - using Transform = Eigen::Transform; - - Transform mTransform = Transform::Identity(); - - static SE3 fromTf(geometry_msgs::Transform const& transform); - - static SE3 fromPose(geometry_msgs::Pose const& pose); - - [[nodiscard]] geometry_msgs::Pose toPose() const; - - [[nodiscard]] geometry_msgs::Transform toTransform() const; - - [[nodiscard]] geometry_msgs::PoseStamped toPoseStamped(std::string const& frameId) const; - - [[nodiscard]] geometry_msgs::TransformStamped toTransformStamped(std::string const& parentFrameId, std::string const& childFrameId) const; - -public: - [[nodiscard]] static SE3 fromTfTree(tf2_ros::Buffer const& buffer, std::string const& fromFrameId, std::string const& toFrameId); - - static void pushToTfTree(tf2_ros::TransformBroadcaster& broadcaster, std::string const& childFrameId, std::string const& parentFrameId, SE3 const& tf); - - SE3() = default; - - SE3(R3 const& position, SO3 const& rotation); - - template>, - typename = std::enable_if_t<(sizeof...(Args) > 0)>> - SE3(Args&&... args) : mTransform{std::forward(args)...} {} - - [[nodiscard]] SE3 operator*(SE3 const& other) const; - - [[nodiscard]] Eigen::Matrix4d matrix() const; - - [[nodiscard]] R3 position() const; - - [[nodiscard]] SO3 rotation() const; - - [[nodiscard]] double distanceTo(SE3 const& other) const; - - [[nodiscard]] auto inverse() const -> SE3; - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -class SIM3 { - using Transform = Eigen::Transform; - - Transform mTransform = Transform::Identity(); - -public: - SIM3() = default; - - SIM3(R3 const& position, SO3 const& rotation, R3 const& scale); - - [[nodiscard]] auto matrix() const -> Eigen::Matrix4d; - - [[nodiscard]] auto position() const -> R3; -}; diff --git a/src/util/lie/se3_conversion.cpp b/src/util/lie/se3_conversion.cpp deleted file mode 100644 index 44a28774f..000000000 --- a/src/util/lie/se3_conversion.cpp +++ /dev/null @@ -1,52 +0,0 @@ -#include "se3.hpp" - -SE3 SE3::fromTf(geometry_msgs::Transform const& transform) { - return {{transform.translation.x, transform.translation.y, transform.translation.z}, - {Eigen::Quaterniond{transform.rotation.w, transform.rotation.x, transform.rotation.y, transform.rotation.z}}}; -} - -SE3 SE3::fromPose(geometry_msgs::Pose const& pose) { - return {{pose.position.x, pose.position.y, pose.position.z}, - {Eigen::Quaterniond{pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z}}}; -} - -geometry_msgs::Pose SE3::toPose() const { - geometry_msgs::Pose pose; - pose.position.x = position().x(); - pose.position.y = position().y(); - pose.position.z = position().z(); - pose.orientation.x = rotation().quaternion().x(); - pose.orientation.y = rotation().quaternion().y(); - pose.orientation.z = rotation().quaternion().z(); - pose.orientation.w = rotation().quaternion().w(); - return pose; -} - -geometry_msgs::Transform SE3::toTransform() const { - geometry_msgs::Transform transform; - transform.translation.x = position().x(); - transform.translation.y = position().y(); - transform.translation.z = position().z(); - transform.rotation.x = rotation().quaternion().x(); - transform.rotation.y = rotation().quaternion().y(); - transform.rotation.z = rotation().quaternion().z(); - transform.rotation.w = rotation().quaternion().w(); - return transform; -} - -geometry_msgs::PoseStamped SE3::toPoseStamped(std::string const& frameId) const { - geometry_msgs::PoseStamped pose; - pose.pose = toPose(); - pose.header.frame_id = frameId; - pose.header.stamp = ros::Time::now(); - return pose; -} - -geometry_msgs::TransformStamped SE3::toTransformStamped(const std::string& parentFrameId, const std::string& childFrameId) const { - geometry_msgs::TransformStamped transform; - transform.transform = toTransform(); - transform.header.frame_id = parentFrameId; - transform.header.stamp = ros::Time::now(); - transform.child_frame_id = childFrameId; - return transform; -} diff --git a/src/util/lie/sim3.cpp b/src/util/lie/sim3.cpp deleted file mode 100644 index 65f9110f9..000000000 --- a/src/util/lie/sim3.cpp +++ /dev/null @@ -1,13 +0,0 @@ -#include "se3.hpp" - -SIM3::SIM3(R3 const& position, SO3 const& rotation, R3 const& scale) { - mTransform.fromPositionOrientationScale(position, rotation.mAngleAxis, scale); -} - -auto SIM3::matrix() const -> Eigen::Matrix4d { - return mTransform.matrix(); -} - -auto SIM3::position() const -> R3 { - return mTransform.translation(); -} diff --git a/src/util/lie/so3.cpp b/src/util/lie/so3.cpp deleted file mode 100644 index 422cd5c42..000000000 --- a/src/util/lie/so3.cpp +++ /dev/null @@ -1,17 +0,0 @@ -#include "se3.hpp" - -Eigen::Quaterniond SO3::quaternion() const { - return Eigen::Quaterniond{mAngleAxis}; -} - -Eigen::Matrix3d SO3::matrix() const { - return mAngleAxis.toRotationMatrix(); -} - -SO3 SO3::operator*(SO3 const& other) const { - return other.quaternion() * quaternion(); -} - -R3 SO3::operator*(R3 const& other) const { - return quaternion() * other; -}