Tmp/reference filter quat#688
Conversation
Codecov Report❌ Patch coverage is Additional details and impacted files@@ Coverage Diff @@
## feat/reference-filter-quat #688 +/- ##
=============================================================
Coverage ? 37.96%
=============================================================
Files ? 82
Lines ? 5750
Branches ? 1763
=============================================================
Hits ? 2183
Misses ? 3107
Partials ? 460
Flags with carried forward coverage won't be shown. Click here to find out more.
🚀 New features to boost your workflow:
|
| extra_params = {} | ||
| if rpy_publish: | ||
| extra_params = { | ||
| "publish_rpy_debug": True, | ||
| "topics.guidance.dp_rpy": "guidance/dp", | ||
| "topics.guidance.dp": "guidance/dp_quat", | ||
| } | ||
|
|
There was a problem hiding this comment.
I like you kept both functionalities. I'm not sure if the reference filter itself will ever experience gimbal lock, but it is nice that we can publish straight quat and operate on quat errors instead of doing quat2euler.
| Eigen::Vector6d WaypointFollower::velocity() const { | ||
| std::lock_guard<std::mutex> lock(mutex_); | ||
| return state_.segment<6>(6); | ||
| } | ||
|
|
||
| Pose WaypointFollower::waypoint_goal() const { | ||
| std::lock_guard<std::mutex> lock(mutex_); | ||
| state_.head<6>() = reference_goal_; | ||
| return waypoint_goal_; | ||
| } |
There was a problem hiding this comment.
commented same on anbit, we usually lock but dont explicitly unlock mutexes, i guess they unlock themselves when exiting current scope but maybe for readability we should explicitly unlock them
| break; | ||
| } | ||
|
|
||
| return reference_out; |
There was a problem hiding this comment.
why change from reference to waypoint
There was a problem hiding this comment.
Makes it easier for non-control people to understand😅
So they don't mix the waypoint with current reference values being published
| publish_rpy_debug_ = this->declare_parameter<bool>("publish_rpy_debug"); | ||
| if (publish_rpy_debug_) { | ||
| std::string rpy_topic = this->declare_parameter<std::string>( | ||
| "topics.guidance.dp_rpy", guidance_topic + "_rpy"); | ||
| rpy_debug_pub_ = | ||
| this->create_publisher<vortex_msgs::msg::ReferenceFilter>( | ||
| rpy_topic, qos_sensor_data); | ||
| spdlog::info("RPY debug publisher enabled on topic: {}", rpy_topic); | ||
| } |
There was a problem hiding this comment.
maybe instead of using publish_rpy_debug as a flag only publish it when it is built in debug mode.
I.e colcon build --cmake-args -DCMAKE_BUILD_TYPE=Debug
and putting this near the top of the file
#ifdef NDEBUG
constexpr bool debug = false; // Release
#else
constexpr bool debug = true; // Debug
#endif
There was a problem hiding this comment.
I am planning on removing this in favor of just using the util node
| 1. **Compute reference error** — The 6D reference $r$ is the error between the waypoint goal and the nominal pose: | ||
| - $r_{0:3} = p_{goal} - p_{nominal}$ | ||
| - $r_{3:6} = \text{quaternion\_error}(q_{nominal},\ q_{goal})$ | ||
|
|
||
| The `quaternion_error` function returns $2 \cdot \text{vec}(q_{nominal}^{-1} \otimes q_{goal})$, which for small angles approximates the rotation vector from nominal to goal. | ||
|
|
||
| 2. **Integrate** — The standard filter step: | ||
| ```math | ||
| \dot{x} = A_d x + B_d r, \quad x \leftarrow x + \dot{x} \cdot dt | ||
| ``` |
There was a problem hiding this comment.
something here didnt work correctly when i try to view it as md on github
| pose: PoseData = PoseData() | ||
| pose.x = random.uniform(-10.0, 10.0) | ||
| pose.y = random.uniform(-10.0, 10.0) | ||
| pose.z = random.uniform(0.5, 3.0) |
There was a problem hiding this comment.
Maybe we should make default test scenario with no seabed, and then we are no longer constricted by depth
| super().__init__('reference_filter_quat_waypoint_client') | ||
|
|
||
| self._action_client = ActionClient( | ||
| self, ReferenceFilterQuatWaypoint, '/orca/reference_filter' |
| def __init__(self): | ||
| super().__init__('check_goal_node') | ||
| self.pose_sub_ = self.create_subscription( | ||
| PoseWithCovarianceStamped, '/orca/pose', self.pose_callback, best_effort_qos |
There was a problem hiding this comment.
orca? maybe resolve with param
jorgenfj
force-pushed
the
tmp/reference-filter-quat
branch
from
793789f to
fa5a167
Compare
* init package structure * Tmp/reference filter quat (#688) * update waypoint utils to quat * refernce filter quat implementation * RPY reference testing publisher * sim test for quat ref filter * update pkg maintainer :) * add reference quat sim test to workflow * helpers for step function * fix readme * ref_quat remove waypoint_utils and ref feedback * remove RPY debug publisher * pre-commit * update sim test * fix sim test * separate reference_filter_quat topic * ref quat to euler converter for sim test
2 participants
Reference filter that uses an error state formulation to get around the singularities of euler/RPY.
Added sim test to verify implementation is correct. Sim test can eventually be incorporated with quat controllers to test 90 degree pitch scenarios.
I created a temp branch for this PR for review purposes to easier highlight the differences between this implementation and the original reference filter implementation.