Fix the resource_retriever usage.

agimus_controller/agimus_controller/factory/robot_model.py

@@ -208,6 +208,9 @@ def _load_full_pinocchio_models(self) -> None:
                 robot_attachment_frame_id = env_model.getFrameId(
                     self._params.robot_attachment_frame
                 )
+            print(
+                f"Attaching robot model to environment frame '{self._params.robot_attachment_frame}' (id={robot_attachment_frame_id})"
+            )
             _, self._visual_model = pin.appendModel(
                 env_model,
                 self._full_robot_model,

agimus_controller/agimus_controller/ocp/ocp_croco_generic.py

@@ -201,7 +201,9 @@ class ResidualModelFramePlacement(ResidualModel):
     pref: T.Optional[npt.NDArray[np.float64]] = None
 
     def update(self, data, obj, pt: WeightedTrajectoryPoint):
-        assert len(pt.point.end_effector_poses) == 1
+        assert len(pt.point.end_effector_poses) == 1, (
+            f"ResidualModelFramePlacement requires exactly one end-effector pose, current is {pt.point.end_effector_poses}."
+        )
         ee_name, ee_pose = next(iter(pt.point.end_effector_poses.items()))
         obj.id = get_frame_id(data.state, ee_name)
         obj.reference = ee_pose
@@ -227,8 +229,11 @@ class ResidualModelFramePlacementStatic(ResidualModel):
     pref: T.Optional[npt.NDArray[np.float64]] = None
 
     def update(self, data, obj, pt: WeightedTrajectoryPoint):
+        assert len(pt.point.end_effector_poses) == 1, (
+            f"ResidualModelFramePlacementStatic requires exactly one end-effector pose, current is {pt.point.end_effector_poses}."
+        )
         assert self.frame_id in pt.point.end_effector_poses, (
-            f"end_effector_poses should contains key {self.frame_id}"
+            f"ResidualModelFramePlacementStatic: end_effector_poses should contain the key {self.frame_id}"
         )
         obj.reference = pt.point.end_effector_poses[self.frame_id]
         return pt.weights.w_end_effector_poses[self.frame_id]
@@ -250,7 +255,9 @@ class ResidualModelFrameTranslation(ResidualModel):
     pref: T.Optional[npt.NDArray[np.float64]] = None
 
     def update(self, data, obj, pt: WeightedTrajectoryPoint):
-        assert len(pt.point.end_effector_poses) == 1
+        assert len(pt.point.end_effector_poses) == 1, (
+            f"ResidualModelFrameTranslation requires exactly one end-effector pose, current is {pt.point.end_effector_poses}."
+        )
         ee_name, ee_pose = next(iter(pt.point.end_effector_poses.items()))
         obj.id = get_frame_id(data.state, ee_name)
         obj.reference = ee_pose.translation
@@ -276,6 +283,9 @@ class ResidualModelFrameTranslationStatic(ResidualModel):
     pref: T.Optional[npt.NDArray[np.float64]] = None
 
     def update(self, data, obj, pt: WeightedTrajectoryPoint):
+        assert len(pt.point.end_effector_poses) == 1, (
+            f"ResidualModelFrameTranslation requires exactly one end-effector pose, current is {pt.point.end_effector_poses}."
+        )
         assert self.frame_id in pt.point.end_effector_poses, (
             f"end_effector_poses should contains key {self.frame_id}"
         )
@@ -299,7 +309,9 @@ class ResidualModelFrameRotation(ResidualModel):
     pref: T.Optional[npt.NDArray[np.float64]] = None
 
     def update(self, data, obj, pt: WeightedTrajectoryPoint):
-        assert len(pt.point.end_effector_poses) == 1
+        assert len(pt.point.end_effector_poses) == 1, (
+            f"ResidualModelFrameRotation requires exactly one end-effector pose, current is {pt.point.end_effector_poses}."
+        )
         ee_name, ee_pose = next(iter(pt.point.end_effector_poses.items()))
         obj.id = get_frame_id(data.state, ee_name)
         obj.reference = ee_pose.rotation
@@ -325,8 +337,11 @@ class ResidualModelFrameRotationStatic(ResidualModel):
     pref: T.Optional[npt.NDArray[np.float64]] = None
 
     def update(self, data, obj, pt: WeightedTrajectoryPoint):
+        assert len(pt.point.end_effector_poses) == 1, (
+            f"ResidualModelFrameRotationStatic requires exactly one end-effector pose, current is {pt.point.end_effector_poses}."
+        )
         assert self.frame_id in pt.point.end_effector_poses, (
-            f"end_effector_poses should contains key {self.frame_id}"
+            f"ResidualModelFrameRotationStatic: end_effector_poses should contain the key {self.frame_id}"
         )
         obj.reference = pt.point.end_effector_poses[self.frame_id].rotation
         return pt.weights.w_end_effector_poses[self.frame_id][3:]
@@ -358,7 +373,9 @@ def __post_init__(self):
         )
 
     def update(self, data, obj, pt: WeightedTrajectoryPoint):
-        assert len(pt.point.end_effector_velocities) == 1
+        assert len(pt.point.end_effector_velocities) == 1, (
+            f"ResidualModelFrameVelocity requires exactly one end-effector velocity, current is {pt.point.end_effector_velocities}."
+        )
         ee_name, ee_vel = next(iter(pt.point.end_effector_velocities.items()))
         obj.id = get_frame_id(data.state, ee_name)
         obj.reference = ee_vel
@@ -395,11 +412,14 @@ def __post_init__(self):
         )
 
     def update(self, data, obj, pt: WeightedTrajectoryPoint):
+        assert len(pt.point.end_effector_velocities) == 1, (
+            f"ResidualModelFrameVelocityStatic requires exactly one end-effector velocity, current is {pt.point.end_effector_velocities}."
+        )
         assert self.frame_id in pt.point.end_effector_velocities, (
-            f"end_effector_velocities should contains key {self.frame_id}"
+            f"ResidualModelFrameVelocityStatic: end_effector_velocities should contain the key {self.frame_id}"
         )
         obj.reference = pt.point.end_effector_velocities[self.frame_id]
-        return pt.weights.w_end_effector_poses[self.frame_id]
+        return pt.weights.w_end_effector_velocities[self.frame_id]
 
     def build(self, data: BuildData):
         id = get_frame_id(data.state, self.frame_id)
@@ -432,6 +452,9 @@ class ResidualModelVisualServoing(ResidualModel):
     robot_frame: str
 
     def update(self, data: BuildData, obj, pt: WeightedTrajectoryPoint):
+        assert len(pt.point.end_effector_poses) == 1, (
+            f"ResidualModelVisualServoing requires exactly one end-effector, current is {pt.point.end_effector_poses}."
+        )
         assert self.input_key in pt.point.end_effector_poses, (
             f"end_effector_poses should contains key {self.input_key}"
         )

agimus_controller/agimus_controller/plots/PLOT_DATA_FORMAT.md

@@ -0,0 +1,35 @@
+# Plot Data Dump Format
+
+The plotting utilities now automatically dump plot data and metadata to a JSON file for each plot. This allows you to reproduce and beautify plots later.
+
+## File Naming
+- Each plot creates a file named `<title>_plotdata.json` (spaces replaced by underscores).
+
+## JSON Structure
+```
+{
+  "title": "Plot Title",
+  "time": [ ... ],           // List of time values (x-axis)
+  "values": [ [...], ... ],  // 2D list of y-values (one list per series)
+  "labels": [ ... ],         // Series labels (if any)
+  "ylabels": [ ... ],        // Y-axis labels (if any)
+  "semilogs": [ ... ],       // List of booleans for semilog axes (if any)
+  "ylimits": [ [...], ... ], // List of [min, max] for y-limits (if any)
+  "colors": [ ... ]          // List of color codes for each series
+}
+```
+
+## Usage
+- You can load this JSON in Python and use the data to recreate or beautify the plot with any plotting library.
+- Example:
+```python
+import json
+with open('my_plot_plotdata.json') as f:
+    data = json.load(f)
+# data['time'], data['values'], data['labels'], ...
+```
+
+## Notes
+- All arrays are saved as lists for compatibility.
+- Colors are Matplotlib color codes.
+- Not all fields are always present; empty lists are used if not specified.

agimus_controller/agimus_controller/plots/dump_mpc_plot_utils.py

@@ -0,0 +1,29 @@
+import json
+import numpy as np
+
+
+def dump_mpc_plot_data(filename, plot_data_dict):
+    """
+    Dump all plot data and metadata for MPC plots to a JSON file.
+    plot_data_dict: dict of dicts, one per figure, with keys:
+      - title
+      - x (time or index)
+      - y (data series, possibly 2D)
+      - labels
+      - ylabels
+      - colors
+      - etc.
+    """
+
+    # Convert all numpy arrays to lists for JSON serialization
+    def convert(obj):
+        if isinstance(obj, np.ndarray):
+            return obj.tolist()
+        if isinstance(obj, dict):
+            return {k: convert(v) for k, v in obj.items()}
+        if isinstance(obj, list):
+            return [convert(v) for v in obj]
+        return obj
+
+    with open(filename, "w") as f:
+        json.dump(convert(plot_data_dict), f, indent=2)

agimus_controller/agimus_controller/plots/dump_utils.py

@@ -0,0 +1,28 @@
+import json
+import numpy as np
+
+
+def dump_plot_data(
+    filename: str,
+    title: str,
+    time: np.ndarray,
+    values: np.ndarray,
+    labels=None,
+    ylabels=None,
+    semilogs=None,
+    ylimits=None,
+    colors=None,
+):
+    """Dump plot data and metadata to a JSON file."""
+    data = {
+        "title": title,
+        "time": time.tolist(),
+        "values": values.tolist(),
+        "labels": labels if labels is not None else [],
+        "ylabels": ylabels if ylabels is not None else [],
+        "semilogs": semilogs if semilogs is not None else [],
+        "ylimits": ylimits if ylimits is not None else [],
+        "colors": colors if colors is not None else [],
+    }
+    with open(filename, "w") as f:
+        json.dump(data, f, indent=2)