Merged
Conversation
Contributor
📝 WalkthroughWalkthroughReplaces RobotJointAction with RobotJointFeedbackAction across model, HAL, drivers, simulator, and controller APIs; expands controller computeJointControlAction to accept current and desired RobotState; renames joint-state fields (position→q, velocity→v, measured_effort→effort); introduces templated JointPDController and tests. Changes
Sequence Diagram(s)sequenceDiagram
participant Controller as Controller\n(JointPDController)
participant RobotHardware as RobotHardware
participant Driver as DriverBase
participant Simulator as MujocoSimInterface
rect rgba(200,230,255,0.5)
Controller->>Controller: computeJointControlAction(current_state, desired_state)
Controller-->>RobotHardware: RobotJointFeedbackAction (joint_action)
end
rect rgba(200,255,230,0.5)
RobotHardware->>Driver: setJointFeedbackAction(joint_action)
Driver->>Simulator: setJointFeedbackAction(joint_action)
Simulator->>Simulator: apply feedback (actuator loop, compute torques)
end
Estimated code review effort🎯 4 (Complex) | ⏱️ ~50 minutes Possibly related PRs
Poem
🚥 Pre-merge checks | ✅ 1 | ❌ 2❌ Failed checks (1 warning, 1 inconclusive)
✅ Passed checks (1 passed)
✏️ Tip: You can configure your own custom pre-merge checks in the settings. ✨ Finishing touches
🧪 Generate unit tests (beta)
Thanks for using CodeRabbit! It's free for OSS, and your support helps us grow. If you like it, consider giving us a shout-out. Comment |
![coderabbitai[bot]](https://avatars.githubusercontent.com/in/347564?s=60&v=4){kind=small}
Contributor
There was a problem hiding this comment.
Actionable comments posted: 1
🤖 Fix all issues with AI agents
In `@motorium/motorium_mujoco/src/MujocoSimInterface.cpp`:
- Around line 367-368: The local variable name JointFeedbackAction shadows the
type motorium::model::JointFeedbackAction and violates naming conventions;
rename the local to a conventional variable name (e.g., jointFeedbackAction or
joint_feedback_action) where it is declared from action_internal_.at(idx) and
update its single usage in the mj_data_->ctrl assignment that calls
getTotalFeedbackTorque(...), keeping the same arguments (mj_data_->qpos[i + 7],
mj_data_->qvel[i + 6]) and leaving the rest of the expression unchanged.
Contributor
There was a problem hiding this comment.
Actionable comments posted: 3
Caution
Some comments are outside the diff and can’t be posted inline due to platform limitations.
⚠️ Outside diff range comments (1)
motorium/motorium_control/test/testImplicitPDController.cpp (1)
103-119: 🧹 Nitpick | 🔵 TrivialExercise the new desired_state path in tests.
Right now the tests pass the same state for current/desired and only verify gains; adding desired-state values and assertions for q_des/v_des/feed_forward_effort would lock in the new behavior.✅ Suggested test enhancement
@@ TEST_F(ImplicitPDControllerTest, ComputeAction) { - RobotState state(*robot_description_); - RobotJointFeedbackAction action(*robot_description_); - - controller.computeJointControlAction(0.0, state, state, action); + RobotState current_state(*robot_description_); + RobotState desired_state(*robot_description_); + joint_index_t idx1 = robot_description_->getJointIndex("joint1"); + joint_index_t idx2 = robot_description_->getJointIndex("joint2"); + desired_state.setJointPosition(idx1, 0.25); + desired_state.setJointVelocity(idx1, 0.5); + desired_state.setJointEffort(idx1, 3.0); + desired_state.setJointPosition(idx2, -0.1); + desired_state.setJointVelocity(idx2, 0.2); + desired_state.setJointEffort(idx2, 4.0); + RobotJointFeedbackAction action(*robot_description_); + + controller.computeJointControlAction(0.0, current_state, desired_state, action); @@ - joint_index_t idx1 = robot_description_->getJointIndex("joint1"); EXPECT_DOUBLE_EQ(action[idx1].kp, 10.0); EXPECT_DOUBLE_EQ(action[idx1].kd, 1.0); + EXPECT_DOUBLE_EQ(action[idx1].q_des, 0.25); + EXPECT_DOUBLE_EQ(action[idx1].v_des, 0.5); + EXPECT_DOUBLE_EQ(action[idx1].feed_forward_effort, 3.0); @@ - joint_index_t idx2 = robot_description_->getJointIndex("joint2"); EXPECT_DOUBLE_EQ(action[idx2].kp, 20.0); EXPECT_DOUBLE_EQ(action[idx2].kd, 2.0); + EXPECT_DOUBLE_EQ(action[idx2].q_des, -0.1); + EXPECT_DOUBLE_EQ(action[idx2].v_des, 0.2); + EXPECT_DOUBLE_EQ(action[idx2].feed_forward_effort, 4.0); } @@ TEST_F(ImplicitPDControllerTest, PartialControl) { - RobotState state(*robot_description_); - RobotJointFeedbackAction action(*robot_description_); + RobotState current_state(*robot_description_); + RobotState desired_state(*robot_description_); + joint_index_t idx1 = robot_description_->getJointIndex("joint1"); + joint_index_t idx2 = robot_description_->getJointIndex("joint2"); + desired_state.setJointPosition(idx2, 0.4); + desired_state.setJointVelocity(idx2, 0.6); + desired_state.setJointEffort(idx2, 2.5); + RobotJointFeedbackAction action(*robot_description_); @@ - joint_index_t idx1 = robot_description_->getJointIndex("joint1"); - joint_index_t idx2 = robot_description_->getJointIndex("joint2"); action[idx1].kp = 0.0; action[idx2].kp = 0.0; @@ - controller.computeJointControlAction(0.0, state, state, action); + controller.computeJointControlAction(0.0, current_state, desired_state, action); @@ EXPECT_DOUBLE_EQ(action[idx1].kp, 0.0); // Should be untouched EXPECT_DOUBLE_EQ(action[idx2].kp, 5.0); EXPECT_DOUBLE_EQ(action[idx2].kd, 0.5); + EXPECT_DOUBLE_EQ(action[idx2].q_des, 0.4); + EXPECT_DOUBLE_EQ(action[idx2].v_des, 0.6); + EXPECT_DOUBLE_EQ(action[idx2].feed_forward_effort, 2.5); }Also applies to: 134-147
🤖 Fix all issues with AI agents
In `@motorium/motorium_control/src/ImplicitPDController.cpp`:
- Around line 51-62: Rename the source file from ImplicitPDController.cpp to
ImplicitJointPDController.cpp to match the actual implementation name, and
update any references (build target, includes) accordingly; also mark the unused
parameter current_state in ImplicitJointPDController::computeJointControlAction
with [[maybe_unused]] (matching the existing annotation on time) so the
signature reads with both time and current_state annotated for clarity and
consistency.
In `@motorium/motorium_model/include/motorium_model/RobotState.h`:
- Around line 97-104: Change the parameter type for both getJointPositions and
getJointVelocities from std::vector<joint_index_t> (by-value) to const
std::vector<joint_index_t>& to avoid copying the vector; update the function
declarations and any callers if necessary, while keeping the body using
joint_state_map_.toVector(joint_ids, ...) unchanged (functions:
getJointPositions and getJointVelocities).
- Around line 83-93: Parameters in the RobotState setters use mixed naming
styles; make them consistent by renaming the parameters in setJointVelocity and
setJointEffort to snake_case (e.g., joint_velocity, joint_effort) to match
setJointPosition's joint_position, and update their uses inside the functions
(setJointVelocity -> joint_state_map_.at(joint_id).v = joint_velocity;
setJointEffort -> joint_state_map_.at(joint_id).effort = joint_effort) so
identifiers match; keep the getter signatures unchanged.
Contributor
There was a problem hiding this comment.
Actionable comments posted: 2
🤖 Fix all issues with AI agents
In `@motorium/motorium_control/src/ImplicitJointPDController.cpp`:
- Around line 52-55: Add a brief clarifying comment inside
ImplicitJointPDController::computeJointControlAction explaining why the
current_state parameter is intentionally unused: state that this controller only
sets gains and desired setpoints and that actual feedback computation is
performed at the driver level (via getTotalFeedbackTorque), so current_state is
not needed here. Place the comment near the function signature or at the top of
the function body and reference current_state and getTotalFeedbackTorque to make
the design intent explicit for future maintainers.
In `@motorium/motorium_control/test/testImplicitJointPDController.cpp`:
- Line 108: The test calls controller.computeJointControlAction(0.0, state,
state, action) using the same State for current and desired, so it doesn't
verify that desired q/v/ff are copied from desired_state; add new assertions by
creating a second State (e.g., desired_state) with different joint
positions/velocities/feed_forward_effort and call computeJointControlAction(0.0,
state, desired_state, action) (and similarly for PartialControl test), then
assert that action.q_des, action.v_des, and action.feed_forward_effort match the
values set in desired_state while control outputs reflect the difference between
current_state and desired_state.
Contributor
There was a problem hiding this comment.
Actionable comments posted: 2
🤖 Fix all issues with AI agents
In `@motorium/motorium_control/CMakeLists.txt`:
- Around line 64-73: CMakeLists.txt declares a test target
test_explicit_pd_controller that references
test/testExplicitJointPDController.cpp which doesn't exist; either add a new
test source file named testExplicitJointPDController.cpp under the
motorium_control/test/ directory implementing the explicit PD controller unit
test (matching the style of testImplicitJointPDController.cpp), or remove the
ament_add_gtest and associated target_link_libraries block for
test_explicit_pd_controller from CMakeLists.txt to eliminate the broken target;
search for the symbols test_explicit_pd_controller and
testExplicitJointPDController.cpp in CMakeLists.txt to locate the lines to
update.
- Around line 39-42: The CMake add_library lists
src/ExplicitJointPDController.cpp which (and its header
ExplicitJointPDController.h and potential test
testExplicitJointPDController.cpp) are missing and will break the build; add a
matching implementation and header for the ExplicitJointPDController class
(matching the interface/pattern used by ImplicitJointPDController: class name
ExplicitJointPDController, constructors/destructors, public control methods and
any virtual overrides) and create the test source
testExplicitJointPDController.cpp if CMake references a test target, or
alternatively remove the missing file entries from the add_library()/test target
if you intend not to provide them; ensure the new files compile and are
referenced consistently with the existing ImplicitJointPDController symbols and
the add_library call.
Contributor
There was a problem hiding this comment.
Actionable comments posted: 4
🤖 Fix all issues with AI agents
In `@motorium/motorium_control/include/motorium_control/JointPDController.h`:
- Around line 46-51: The comment block in JointPDController.h contains a typo:
"therms" should be "terms"; update the documentation in the block that describes
the implicit and explicit PD controllers (the comment above or within the
JointPDController class/definition) to replace both occurrences of "therms" with
"terms" so the description reads "...computes the feedback terms itself..." and
"...forwards the feedback torques to the driver."
- Line 59: The header declares an unused member function validateConfig() in
class JointPDController but there is no implementation and the code uses
config_.validate() instead; remove the validateConfig() declaration from
JointPDController.h (the unused symbol validateConfig) to clean up the interface
and avoid confusion, ensuring no remaining references to
JointPDController::validateConfig exist (update any calls or tests if found).
In `@motorium/motorium_control/test/testExplicitJointPDController.cpp`:
- Around line 56-78: Add test parity with the implicit controller by adding two
tests under JointPDControllerTest that mirror the implicit suite: (1)
InitializationMissingJoint — construct a JointPDControllerConfig with a joint
name that does not exist and assert construction fails (use the same
EXPECT_DEATH pattern used by the implicit tests) when creating
ExplicitJointPDController(*robot_description_, config); (2) PartialControl —
create a config where kp/kd vectors are larger than the provided joint_names,
instantiate ExplicitJointPDController, call the same control step / torque
computation helper used in the implicit PartialControl test, and assert the
controller only applies control for the specified joint_names and leaves other
joints unaffected (compare torques/outputs to expected values). Reference
ExplicitJointPDController, JointPDControllerConfig, and JointPDControllerTest to
locate where to add these tests.
- Around line 40-54: Rename the test fixture class JointPDControllerTest to
ExplicitJointPDControllerTest and update all TEST_F usages that reference
JointPDControllerTest to use ExplicitJointPDControllerTest instead; specifically
change the class declaration "class JointPDControllerTest" and every
TEST_F(JointPDControllerTest, ...) to TEST_F(ExplicitJointPDControllerTest, ...)
so the explicit-PD tests no longer collide with the implicit-PD fixture name.
motorium/motorium_control/include/motorium_control/JointPDController.h
Outdated
Show resolved
Hide resolved
motorium/motorium_control/test/testExplicitJointPDController.cpp
Outdated
Show resolved
Hide resolved
motorium/motorium_control/test/testExplicitJointPDController.cpp
Outdated
Show resolved
Hide resolved
Contributor
There was a problem hiding this comment.
Actionable comments posted: 3
Caution
Some comments are outside the diff and can’t be posted inline due to platform limitations.
⚠️ Outside diff range comments (1)
motorium/motorium_model/include/motorium_model/FixedIDArray.h (1)
32-36:⚠️ Potential issue | 🟡 MinorMissing
<optional>header include.The
is_optionaltrait specialization on line 51 usesstd::optional<T>, but<optional>is not included. This relies on transitive inclusion which is fragile.Suggested fix
`#include` <Eigen/Dense> `#include` <concepts> `#include` <memory> +#include <optional> `#include` <span>
🤖 Fix all issues with AI agents
In `@motorium/motorium_model/include/motorium_model/FixedIDArray.h`:
- Line 161: The debug-only assertion MT_DCHECK(std::ranges::size(range) ==
vector.size()) must be replaced with a runtime check to prevent out-of-bounds
access in release builds: change MT_DCHECK to MT_CHECK (i.e.,
MT_CHECK(std::ranges::size(range) == vector.size())) at the same location in
FixedIDArray.h, and include a clear error message describing the mismatched
sizes if your MT_CHECK API accepts a message (e.g., "range size must equal
vector.size()"); keep the surrounding code and logic unchanged.
- Around line 167-168: The insertion branch in FixedIDArray::(inserter usage)
calls val.value() for optional<T> which throws std::bad_optional_access and is
inconsistent with toEigenVector; add an explicit precondition check before
accessing the optional (e.g., assert/MT_CHECK(val.has_value()) with a clear
message referencing the ID/index and the FixedIDArray operation) or document the
requirement in the FixedIDArray API comment so callers know empty optionals are
not allowed; update the code path that uses inserter(val.value(), vector(index))
to first verify val.has_value() and emit a clear error if not.
In `@motorium/motorium_model/test/testFixedIDArray.cpp`:
- Around line 316-331: The test uses a non-deterministic matrix via
motorium::matrix3_t::Random() (variable transform) which makes failures
unreproducible; replace this with a fixed, deterministic 3x3 matrix (e.g.
explicit values) or seed the RNG before calling motorium::matrix3_t::Random() so
transform is repeatable, then leave the rest of the round-trip calls to
array.toEigenVector and array.fromEigenVector unchanged to verify values
deterministically.
Co-authored-by: coderabbitai[bot] <136622811+coderabbitai[bot]@users.noreply.github.com>
Contributor
There was a problem hiding this comment.
Actionable comments posted: 2
🤖 Fix all issues with AI agents
In `@motorium/motorium_control/test/testExplicitJointPDController.cpp`:
- Around line 117-123: Move the duplicated lambda into the test fixture by
adding a static helper method JointState makeJointState(scalar_t q, scalar_t v,
scalar_t effort) to the ExplicitJointPDControllerTest class (the fixture used by
the PartialControl and ComputeAction tests), implement it to construct and
return a JointState with q, v, effort, and then delete the local lambda
definitions inside the PartialControl and ComputeAction test cases so they call
ExplicitJointPDControllerTest::makeJointState instead.
- Around line 56-67: The test TEST_F(ExplicitJointPDControllerTest,
InitializationValidConfig) currently constructs ExplicitJointPDController
directly to assert no exception; make the intent explicit by wrapping the
construction call inside EXPECT_NO_THROW(...) so the test clearly checks that
ExplicitJointPDController(controller_args) does not throw; update the test body
to call EXPECT_NO_THROW(ExplicitJointPDController
controller(*robot_description_, config)); keeping the same config and
identifiers (ExplicitJointPDController, TEST_F, InitializationValidConfig).
Comment on lines
+56
to
+67
| TEST_F(ExplicitJointPDControllerTest, InitializationValidConfig) { | ||
| JointPDControllerConfig config; | ||
| config.joint_names = {"joint1"}; | ||
| vector_t kp(1); | ||
| kp << 10.0; | ||
| vector_t kd(1); | ||
| kd << 1.0; | ||
| config.kp = kp; | ||
| config.kd = kd; | ||
|
|
||
| ExplicitJointPDController controller(*robot_description_, config); | ||
| } |
Contributor
There was a problem hiding this comment.
🧹 Nitpick | 🔵 Trivial
Consider using EXPECT_NO_THROW to clarify test intent.
The test implicitly verifies that construction doesn't throw, but wrapping it in EXPECT_NO_THROW would make the intent explicit and improve test readability.
♻️ Suggested improvement
TEST_F(ExplicitJointPDControllerTest, InitializationValidConfig) {
JointPDControllerConfig config;
config.joint_names = {"joint1"};
vector_t kp(1);
kp << 10.0;
vector_t kd(1);
kd << 1.0;
config.kp = kp;
config.kd = kd;
- ExplicitJointPDController controller(*robot_description_, config);
+ EXPECT_NO_THROW(ExplicitJointPDController controller(*robot_description_, config));
}📝 Committable suggestion
‼️ IMPORTANT
Carefully review the code before committing. Ensure that it accurately replaces the highlighted code, contains no missing lines, and has no issues with indentation. Thoroughly test & benchmark the code to ensure it meets the requirements.
Suggested change
| TEST_F(ExplicitJointPDControllerTest, InitializationValidConfig) { | |
| JointPDControllerConfig config; | |
| config.joint_names = {"joint1"}; | |
| vector_t kp(1); | |
| kp << 10.0; | |
| vector_t kd(1); | |
| kd << 1.0; | |
| config.kp = kp; | |
| config.kd = kd; | |
| ExplicitJointPDController controller(*robot_description_, config); | |
| } | |
| TEST_F(ExplicitJointPDControllerTest, InitializationValidConfig) { | |
| JointPDControllerConfig config; | |
| config.joint_names = {"joint1"}; | |
| vector_t kp(1); | |
| kp << 10.0; | |
| vector_t kd(1); | |
| kd << 1.0; | |
| config.kp = kp; | |
| config.kd = kd; | |
| EXPECT_NO_THROW({ | |
| ExplicitJointPDController controller(*robot_description_, config); | |
| }); | |
| } |
🤖 Prompt for AI Agents
In `@motorium/motorium_control/test/testExplicitJointPDController.cpp` around
lines 56 - 67, The test TEST_F(ExplicitJointPDControllerTest,
InitializationValidConfig) currently constructs ExplicitJointPDController
directly to assert no exception; make the intent explicit by wrapping the
construction call inside EXPECT_NO_THROW(...) so the test clearly checks that
ExplicitJointPDController(controller_args) does not throw; update the test body
to call EXPECT_NO_THROW(ExplicitJointPDController
controller(*robot_description_, config)); keeping the same config and
identifiers (ExplicitJointPDController, TEST_F, InitializationValidConfig).
Comment on lines
+117
to
+123
| auto makeJointState = [](scalar_t q, scalar_t v, scalar_t effort) { | ||
| JointState js; | ||
| js.q = q; | ||
| js.v = v; | ||
| js.effort = effort; | ||
| return js; | ||
| }; |
Contributor
There was a problem hiding this comment.
🧹 Nitpick | 🔵 Trivial
Extract makeJointState helper to the test fixture to reduce duplication.
The makeJointState lambda is defined identically in both PartialControl and ComputeAction tests. Moving it to the fixture class would reduce duplication and improve maintainability.
♻️ Suggested refactor
Add to the fixture class:
class ExplicitJointPDControllerTest : public ::testing::Test {
protected:
void SetUp() override {
// ... existing setup ...
}
static JointState makeJointState(scalar_t q, scalar_t v, scalar_t effort) {
JointState js;
js.q = q;
js.v = v;
js.effort = effort;
return js;
}
std::unique_ptr<RobotDescription> robot_description_;
};Then remove the local lambdas from both test cases.
🤖 Prompt for AI Agents
In `@motorium/motorium_control/test/testExplicitJointPDController.cpp` around
lines 117 - 123, Move the duplicated lambda into the test fixture by adding a
static helper method JointState makeJointState(scalar_t q, scalar_t v, scalar_t
effort) to the ExplicitJointPDControllerTest class (the fixture used by the
PartialControl and ComputeAction tests), implement it to construct and return a
JointState with q, v, effort, and then delete the local lambda definitions
inside the PartialControl and ComputeAction test cases so they call
ExplicitJointPDControllerTest::makeJointState instead.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
1 participant
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Summary by CodeRabbit
New Features
Refactor
Documentation
Tests