Custom mpc behavior and residual #425

dyackzan · 2025-09-30T20:53:52Z

No description provided.

bkanator · 2025-10-02T16:59:17Z

src/custom_mpc_behavior/include/custom_mpc_behavior/custom_mpc_behavior.hpp

+/**
+ * @brief TODO(...)
+ */
+class CustomMpcBehavior : public moveit_studio::behaviors::MPCBehaviorBase<CustomMpcBehavior>


so if this is custom, I assume we already have a bunch of core ones? and this is an example of a user creating one?

I believe we have MPCPoseTracking and MPCPointCloudClearance so far.

bkanator · 2025-10-02T17:00:22Z

src/custom_mpc_behavior/include/custom_mpc_behavior/custom_mpc_behavior.hpp

+   * @brief Define the residuals used by this MPC behavior.
+   * @details This method allows the use of a MPCProblem instance to add residual-specific ports.
+   * @return A BT::PortsList containing the required ports for the behavior.
+   */


so, as with the README, I am getting a bit confused by residuals, so they are required to be defined for every behavior and contribute to the cost optimization? Are there some always populated by default, or does it depend on what the model is?

I think they are essentially costs in the cost function, so they're always required, and there's no default. It's tightly coupled to the model, since the mujoco entities used in the residual must exist in the model.

bkanator · 2025-10-02T18:12:52Z

src/custom_mpc_behavior/src/custom_mpc_behavior.cpp

+  : moveit_studio::behaviors::MPCBehaviorBase<CustomMpcBehavior>(name, config, shared_resources)
+{
+}
+


since this doesn't have much in it, is this just to facilitate the creation of the new 'site_tracking' residual?

Not just site_tracking. All the residuals are defined in getResiduals() in the .hpp. It does seem a little bit strange. We should probably add some documentation to the docs page about how to make an MPC behavior? (e.g., copy this template, then fill out the getResiduals() function tuple?)

bkanator · 2025-10-02T18:13:42Z

src/custom_mpc_behavior/src/site_tracking.cpp

+
+int SiteTrackingResidualFunction::getNumDims() const
+{
+  return 6;


is this always true? is this a specific mujoco model that's always used?

This is for a 3D spatial pose target, which is always 6 dof.

bkanator · 2025-10-02T18:37:38Z

src/custom_mpc_behavior/test/test_behavior_plugins.cpp

+  // Test that ClassLoader is able to find and instantiate each behavior using the package's plugin description info.
+  EXPECT_NO_THROW((void)factory.instantiateTreeNode("test_behavior_name", "CustomMpcBehavior", BT::NodeConfiguration()));
+}
+


should there be a more specific residuals based unit test? Making sure your simulated timestep forward produces a valid position/orientation, checking for bad inputs, etc

bkanator · 2025-10-02T18:41:32Z

src/custom_mpc_behavior/src/site_tracking.cpp

+
+  // ---------- Residual (0-2) ----------
+  const mjtNum delta_time = data.time - initial_data->time;
+


is it ok if this is negative?

bkanator · 2025-10-02T18:42:24Z

src/custom_mpc_behavior/src/site_tracking.cpp

+  current_target_pos[2] = target_position[2] + target_position_velocity[2] * delta_time;
+
+  const mjtNum* tip_xpos_current = &data.site_xpos[3 * site_tip_ind_];
+


index is always guaranteed to be 1?

bkanator · 2025-10-02T18:46:49Z

src/custom_mpc_behavior/src/site_tracking.cpp

+  residual[2] = tip_xpos_current[2] - current_target_pos[2];
+
+  // ---------- Residual (3-5) ----------
+


so you mean the residual state of the rotation after simulating forward one timestep?

bkanator · 2025-10-02T18:48:25Z

src/custom_mpc_behavior/src/site_tracking.cpp

+  residual[3] = relative_quat_target[0];
+  residual[4] = relative_quat_target[1];
+  residual[5] = relative_quat_target[2];
+}


I'm still confused how this is actually used. So you move the position and orientation forward one timestep then subtract it from data from mujoco? to get the difference between the actual and the desired?

then how is this being used later?

This should be finding the difference between the target_pose and the tip_pose at the same (current) time-step.

bkanator

just adding comments to get familiar with the purpose of the code

rlpratt12

FYI - I guess we'll need to retarget to re-cored example_ws, so just move all these changes into your Core branch?

There are a ton of ports for CustomMpcBehavior, that I don't see documented anywhere. We should probably make that a section in the docs? Default ports? And explain them and some heuristics on reasonable values and how to tune them?
differentiable, follow_joint_trajectory_topic, gradient_num_trajectory, gradient_spline_points, sampling_control_width, sampling_exploration, sampling_sample_width, sampling_spline_points, set_ctrl_command_to_current_state, total_return, warmup_iterations...

residual specific:
the "max" param for each residual: e.g. max_cartesian_acceleration_goal

rlpratt12 · 2025-10-05T22:00:03Z

src/custom_mpc_behavior/src/site_tracking.cpp

+
+int SiteTrackingResidualFunction::getNumDims() const
+{
+  return 6;


This is for a 3D spatial pose target, which is always 6 dof.

rlpratt12 · 2025-10-06T11:54:49Z

src/custom_mpc_behavior/include/custom_mpc_behavior/custom_mpc_behavior.hpp

+   * @brief Define the residuals used by this MPC behavior.
+   * @details This method allows the use of a MPCProblem instance to add residual-specific ports.
+   * @return A BT::PortsList containing the required ports for the behavior.
+   */


I think they are essentially costs in the cost function, so they're always required, and there's no default. It's tightly coupled to the model, since the mujoco entities used in the residual must exist in the model.

rlpratt12 · 2025-10-06T13:13:02Z

src/custom_mpc_behavior/include/custom_mpc_behavior/custom_mpc_behavior.hpp

+/**
+ * @brief TODO(...)
+ */
+class CustomMpcBehavior : public moveit_studio::behaviors::MPCBehaviorBase<CustomMpcBehavior>


I believe we have MPCPoseTracking and MPCPointCloudClearance so far.

rlpratt12 · 2025-10-06T13:14:18Z

src/custom_mpc_behavior/src/custom_mpc_behavior.cpp

+
+BT::KeyValueVector CustomMpcBehavior::metadata()
+{
+  return { { "description", "Custom MPC behavior" }, { "subcategory", "Motion - Execute" } };


Update category. Add the inline constexpr auto kDescription... above.

rlpratt12 · 2025-10-06T13:18:51Z

src/custom_mpc_behavior/include/custom_mpc_behavior/site_tracking.hpp

+/**
+ * \brief Residual functions for tracking a moving pose.
+ */
+class SiteTrackingResidualFunction final : public moveit_pro_mpc::ResidualFunctionInterface


We shoudn't be duplicating SiteTrackingResidualFunction here. See my comment in your Core PR, we need a better way to expose examples like this without copying code.

rlpratt12 · 2025-10-06T13:27:02Z

src/custom_mpc_behavior/src/custom_mpc_behavior.cpp

+  : moveit_studio::behaviors::MPCBehaviorBase<CustomMpcBehavior>(name, config, shared_resources)
+{
+}
+


Not just site_tracking. All the residuals are defined in getResiduals() in the .hpp. It does seem a little bit strange. We should probably add some documentation to the docs page about how to make an MPC behavior? (e.g., copy this template, then fill out the getResiduals() function tuple?)

rlpratt12 · 2025-10-06T13:35:18Z

src/custom_mpc_behavior/src/site_tracking.cpp

+  residual[3] = relative_quat_target[0];
+  residual[4] = relative_quat_target[1];
+  residual[5] = relative_quat_target[2];
+}


This should be finding the difference between the target_pose and the tip_pose at the same (current) time-step.

dyackzan added 2 commits September 30, 2025 14:26

Add custom MPC behavior using pre-defined residuals

be98382

Define a custom site tracking residual

f54a406

dyackzan requested review from bkanator, marioprats and rlpratt12 October 2, 2025 14:40

bkanator reviewed Oct 2, 2025

View reviewed changes

rlpratt12 reviewed Oct 6, 2025

View reviewed changes

davetcoleman closed this Oct 7, 2025


		// ---------- Residual (0-2) ----------
		const mjtNum delta_time = data.time - initial_data->time;

		current_target_pos[2] = target_position[2] + target_position_velocity[2] * delta_time;

		const mjtNum* tip_xpos_current = &data.site_xpos[3 * site_tip_ind_];

		residual[2] = tip_xpos_current[2] - current_target_pos[2];

		// ---------- Residual (3-5) ----------

Custom mpc behavior and residual #425

Custom mpc behavior and residual #425

Uh oh!

Conversation

dyackzan commented Sep 30, 2025

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

bkanator Oct 2, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

bkanator Oct 2, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

bkanator Oct 2, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

bkanator left a comment

Choose a reason for hiding this comment

Uh oh!

rlpratt12 left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

5 participants

bkanator Oct 2, 2025 •

edited

Loading

bkanator Oct 2, 2025 •

edited

Loading

bkanator Oct 2, 2025 •

edited

Loading