16 comment out debugging out for intermediate calculation result

.pre-commit-config.yaml

@@ -7,6 +7,20 @@ repos:
         args:
           - --maxkb=2048
       - id: check-merge-conflict
+  - repo: https://github.com/astral-sh/ruff-pre-commit
+    # Ruff version.
+    rev: v0.12.7
+    hooks:
+      # Run the linter.
+      - id: ruff-check
+        args: [ --fix, --exit-non-zero-on-fix ]
+      # Run the formatter.
+      - id: ruff-format
+  - repo: https://github.com/python-poetry/poetry
+    rev: 2.1.3
+    hooks:
+      - id: poetry-check
+      - id: poetry-lock
   - repo: https://github.com/gitleaks/gitleaks
     rev: v8.28.0
     hooks:
@@ -49,3 +63,31 @@ repos:
             exit 1
           - --
 
+  - repo: local
+    hooks:
+      - id: ament_cppcheck
+        name: ament_cppcheck
+        description: Static code analysis of C/C++ files.
+        entry: env AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS=1 ament_cppcheck
+        language: system
+        files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$
+
+  - repo: local
+    hooks:
+      - id: ament_cpplint
+        name: ament_cpplint
+        description: Static code analysis of C/C++ files.
+        entry: ament_cpplint
+        language: system
+        files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$
+        args: ["--linelength=100", "--filter=-whitespace/newline,-legal/copyright"]
+
+  # Cmake hooks
+  - repo: local
+    hooks:
+      - id: ament_lint_cmake
+        name: ament_lint_cmake
+        description: Check format of CMakeLists.txt files.
+        entry: ament_lint_cmake
+        language: system
+        files: CMakeLists\.txt$

controller_tuning.xml

@@ -0,0 +1,208 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root>
+ <tabbed_widget parent="main_window" name="Main Window">
+  <Tab tab_name="tab1" containers="1">
+   <Container>
+    <DockSplitter count="1" sizes="1" orientation="-">
+     <DockSplitter count="2" sizes="0.5;0.5" orientation="|">
+      <DockSplitter count="3" sizes="0.333333;0.333333;0.333333" orientation="-">
+       <DockArea name="...">
+        <plot style="LinesAndDots" flip_x="false" line_width="1.0" mode="TimeSeries" flip_y="false">
+         <range left="0.000010" right="499.998746" bottom="0.302482" top="0.517024"/>
+         <limitY/>
+         <curve color="#1f77b4" name="/target_pose/pose/position/x"/>
+         <curve color="#d62728" name="/franka_robot_state_broadcaster/current_pose/pose/position/x"/>
+        </plot>
+       </DockArea>
+       <DockArea name="...">
+        <plot style="LinesAndDots" flip_x="false" line_width="1.0" mode="TimeSeries" flip_y="false">
+         <range left="0.000010" right="499.998746" bottom="-0.256232" top="0.005503"/>
+         <limitY/>
+         <curve color="#1ac938" name="/target_pose/pose/position/y"/>
+         <curve color="#ff7f0e" name="/franka_robot_state_broadcaster/current_pose/pose/position/y"/>
+        </plot>
+       </DockArea>
+       <DockArea name="...">
+        <plot style="LinesAndDots" flip_x="false" line_width="1.0" mode="TimeSeries" flip_y="false">
+         <range left="0.000010" right="499.998746" bottom="0.133326" top="0.597370"/>
+         <limitY/>
+         <curve color="#f14cc1" name="/target_pose/pose/position/z"/>
+         <curve color="#9467bd" name="/franka_robot_state_broadcaster/current_pose/pose/position/z"/>
+        </plot>
+       </DockArea>
+      </DockSplitter>
+      <DockSplitter count="3" sizes="0.333333;0.333333;0.333333" orientation="-">
+       <DockArea name="...">
+        <plot style="LinesAndDots" flip_x="false" line_width="1.0" mode="TimeSeries" flip_y="false">
+         <range left="0.000010" right="499.998746" bottom="-3.298670" top="3.298672"/>
+         <limitY/>
+         <curve color="#bcbd22" name="/franka_robot_state_broadcaster/current_pose/pose/orientation/roll"/>
+         <curve color="#1f77b4" name="/target_pose/pose/orientation/roll"/>
+        </plot>
+       </DockArea>
+       <DockArea name="...">
+        <plot style="LinesAndDots" flip_x="false" line_width="1.0" mode="TimeSeries" flip_y="false">
+         <range left="0.000010" right="499.998746" bottom="-0.002580" top="0.105792"/>
+         <limitY/>
+         <curve color="#d62728" name="/franka_robot_state_broadcaster/current_pose/pose/orientation/pitch"/>
+         <curve color="#27d62e" name="/target_pose/pose/orientation/pitch"/>
+        </plot>
+       </DockArea>
+       <DockArea name="...">
+        <plot style="LinesAndDots" flip_x="false" line_width="1.0" mode="TimeSeries" flip_y="false">
+         <range left="0.000010" right="499.998746" bottom="-0.808317" top="0.019715"/>
+         <limitY/>
+         <curve color="#ff7f0e" name="/franka_robot_state_broadcaster/current_pose/pose/orientation/yaw"/>
+         <curve color="#1ac938" name="/target_pose/pose/orientation/yaw"/>
+        </plot>
+       </DockArea>
+      </DockSplitter>
+     </DockSplitter>
+    </DockSplitter>
+   </Container>
+  </Tab>
+  <Tab tab_name="tab2" containers="1">
+   <Container>
+    <DockSplitter count="3" sizes="0.333333;0.333333;0.333333" orientation="-">
+     <DockArea name="...">
+      <plot style="LinesAndDots" flip_x="false" line_width="1.0" mode="TimeSeries" flip_y="false">
+       <range left="-61.841533" right="499.996788" bottom="-9.753422" top="0.311473"/>
+       <limitY/>
+       <curve color="#1f77b4" name="/target_pose/pose/position/x">
+        <transform name="Derivative" alias="/target_pose/pose/position/x[Derivative]">
+         <options radioChecked="radioCustom" lineEdit="0.02"/>
+        </transform>
+       </curve>
+       <curve color="#ff7f0e" name="/current_twist/twist/linear/x"/>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot style="LinesAndDots" flip_x="false" line_width="1.0" mode="TimeSeries" flip_y="false">
+       <range left="-61.841533" right="499.996788" bottom="-0.406827" top="12.759870"/>
+       <limitY/>
+       <curve color="#d62728" name="/target_pose/pose/position/y">
+        <transform name="Derivative" alias="/target_pose/pose/position/y[Derivative]">
+         <options radioChecked="radioCustom" lineEdit="0.02"/>
+        </transform>
+       </curve>
+       <curve color="#f14cc1" name="/current_twist/twist/linear/y"/>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot style="LinesAndDots" flip_x="false" line_width="1.0" mode="TimeSeries" flip_y="false">
+       <range left="-61.841533" right="499.996788" bottom="-0.727604" top="22.974609"/>
+       <limitY/>
+       <curve color="#1ac938" name="/target_pose/pose/position/z">
+        <transform name="Derivative" alias="/target_pose/pose/position/z[Derivative]">
+         <options radioChecked="radioCustom" lineEdit="0.02"/>
+        </transform>
+       </curve>
+       <curve color="#9467bd" name="/current_twist/twist/linear/z"/>
+      </plot>
+     </DockArea>
+    </DockSplitter>
+   </Container>
+  </Tab>
+  <Tab tab_name="tab2" containers="1">
+   <Container>
+    <DockSplitter count="7" sizes="0.14243;0.143426;0.14243;0.143426;0.14243;0.143426;0.14243" orientation="-">
+     <DockArea name="...">
+      <plot flip_x="false" line_width="1.0" mode="TimeSeries" flip_y="false">
+       <range left="455.759423" right="500.000314" bottom="-0.003474" top="0.003665"/>
+       <limitY/>
+       <curve color="#ff7f0e" name="/joint_states/fr3_joint1/velocity"/>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot flip_x="false" line_width="1.0" mode="TimeSeries" flip_y="false">
+       <range left="455.759423" right="500.000314" bottom="-0.003863" top="0.004216"/>
+       <limitY/>
+       <curve color="#f14cc1" name="/joint_states/fr3_joint2/velocity"/>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot flip_x="false" line_width="1.0" mode="TimeSeries" flip_y="false">
+       <range left="455.759423" right="500.000314" bottom="-0.003722" top="0.003731"/>
+       <limitY/>
+       <curve color="#9467bd" name="/joint_states/fr3_joint3/velocity"/>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot flip_x="false" line_width="1.0" mode="TimeSeries" flip_y="false">
+       <range left="455.759423" right="500.000314" bottom="-0.003950" top="0.004110"/>
+       <limitY/>
+       <curve color="#17becf" name="/joint_states/fr3_joint4/velocity"/>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot flip_x="false" line_width="1.0" mode="TimeSeries" flip_y="false">
+       <range left="455.759423" right="500.000314" bottom="-0.003752" top="0.003946"/>
+       <limitY/>
+       <curve color="#bcbd22" name="/joint_states/fr3_joint5/velocity"/>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot flip_x="false" line_width="1.0" mode="TimeSeries" flip_y="false">
+       <range left="455.759423" right="500.000314" bottom="-0.003633" top="0.003760"/>
+       <limitY/>
+       <curve color="#1f77b4" name="/joint_states/fr3_joint6/velocity"/>
+      </plot>
+     </DockArea>
+     <DockArea name="...">
+      <plot flip_x="false" line_width="1.0" mode="TimeSeries" flip_y="false">
+       <range left="455.759423" right="500.000314" bottom="-0.004825" top="0.004774"/>
+       <limitY/>
+       <curve color="#d62728" name="/joint_states/fr3_joint7/velocity"/>
+      </plot>
+     </DockArea>
+    </DockSplitter>
+   </Container>
+  </Tab>
+  <currentTabIndex index="2"/>
+ </tabbed_widget>
+ <use_relative_time_offset enabled="1"/>
+ <!-- - - - - - - - - - - - - - - -->
+ <!-- - - - - - - - - - - - - - - -->
+ <Plugins>
+  <plugin ID="DataLoad CSV">
+   <parameters time_axis="" delimiter="0"/>
+  </plugin>
+  <plugin ID="DataLoad MCAP"/>
+  <plugin ID="DataLoad ROS2 bags">
+   <use_header_stamp value="false"/>
+   <discard_large_arrays value="true"/>
+   <max_array_size value="100"/>
+   <boolean_strings_to_number value="true"/>
+   <remove_suffix_from_strings value="true"/>
+   <selected_topics value=""/>
+  </plugin>
+  <plugin ID="DataLoad ULog"/>
+  <plugin ID="ROS2 Topic Subscriber">
+   <use_header_stamp value="false"/>
+   <discard_large_arrays value="true"/>
+   <max_array_size value="100"/>
+   <boolean_strings_to_number value="true"/>
+   <remove_suffix_from_strings value="true"/>
+   <selected_topics value="/current_pose;/current_twist;/franka_robot_state_broadcaster/current_pose;/target_pose;/joint_states"/>
+  </plugin>
+  <plugin ID="UDP Server"/>
+  <plugin ID="WebSocket Server"/>
+  <plugin ID="ZMQ Subscriber"/>
+  <plugin ID="Fast Fourier Transform"/>
+  <plugin ID="Quaternion to RPY"/>
+  <plugin ID="Reactive Script Editor">
+   <library code="--[[ Helper function to create a series from arrays&#xa;&#xa; new_series: a series previously created with ScatterXY.new(name)&#xa; prefix:     prefix of the timeseries, before the index of the array&#xa; suffix_X:   suffix to complete the name of the series containing the X value. If [nil], use the index of the array.&#xa; suffix_Y:   suffix to complete the name of the series containing the Y value&#xa; timestamp:   usually the tracker_time variable&#xa;              &#xa; Example:&#xa; &#xa; Assuming we have multiple series in the form:&#xa; &#xa;   /trajectory/node.{X}/position/x&#xa;   /trajectory/node.{X}/position/y&#xa;   &#xa; where {N} is the index of the array (integer). We can create a reactive series from the array with:&#xa; &#xa;   new_series = ScatterXY.new(&quot;my_trajectory&quot;) &#xa;   CreateSeriesFromArray( new_series, &quot;/trajectory/node&quot;, &quot;position/x&quot;, &quot;position/y&quot;, tracker_time );&#xa;--]]&#xa;&#xa;function CreateSeriesFromArray( new_series, prefix, suffix_X, suffix_Y, timestamp )&#xa;  &#xa;  --- clear previous values&#xa;  new_series:clear()&#xa;  &#xa;  --- Append points to new_series&#xa;  index = 0&#xa;  while(true) do&#xa;&#xa;    x = index;&#xa;    -- if not nil, get the X coordinate from a series&#xa;    if suffix_X ~= nil then &#xa;      series_x = TimeseriesView.find( string.format( &quot;%s.%d/%s&quot;, prefix, index, suffix_X) )&#xa;      if series_x == nil then break end&#xa;      x = series_x:atTime(timestamp)&#x9; &#xa;    end&#xa;    &#xa;    series_y = TimeseriesView.find( string.format( &quot;%s.%d/%s&quot;, prefix, index, suffix_Y) )&#xa;    if series_y == nil then break end &#xa;    y = series_y:atTime(timestamp)&#xa;    &#xa;    new_series:push_back(x,y)&#xa;    index = index+1&#xa;  end&#xa;end&#xa;&#xa;--[[ Similar to the built-in function GetSeriesNames(), but select only the names with a give prefix. --]]&#xa;&#xa;function GetSeriesNamesByPrefix(prefix)&#xa;  -- GetSeriesNames(9 is a built-in function&#xa;  all_names = GetSeriesNames()&#xa;  filtered_names = {}&#xa;  for i, name in ipairs(all_names)  do&#xa;    -- check the prefix&#xa;    if name:find(prefix, 1, #prefix) then&#xa;      table.insert(filtered_names, name);&#xa;    end&#xa;  end&#xa;  return filtered_names&#xa;end&#xa;&#xa;--[[ Modify an existing series, applying offsets to all their X and Y values&#xa;&#xa; series: an existing timeseries, obtained with TimeseriesView.find(name)&#xa; delta_x: offset to apply to each x value&#xa; delta_y: offset to apply to each y value &#xa;  &#xa;--]]&#xa;&#xa;function ApplyOffsetInPlace(series, delta_x, delta_y)&#xa;  -- use C++ indices, not Lua indices&#xa;  for index=0, series:size()-1 do&#xa;    x,y = series:at(index)&#xa;    series:set(index, x + delta_x, y + delta_y)&#xa;  end&#xa;end&#xa;"/>
+   <scripts/>
+  </plugin>
+  <plugin ID="CSV Exporter"/>
+  <plugin ID="ROS2 Topic Re-Publisher"/>
+ </Plugins>
+ <!-- - - - - - - - - - - - - - - -->
+ <previouslyLoaded_Datafiles/>
+ <previouslyLoaded_Streamer name="ROS2 Topic Subscriber"/>
+ <!-- - - - - - - - - - - - - - - -->
+ <customMathEquations/>
+ <snippets/>
+ <!-- - - - - - - - - - - - - - - -->
+</root>
+

include/crisp_controllers/cartesian_controller.hpp

@@ -14,10 +14,10 @@
 #include <controller_interface/controller_interface.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <geometry_msgs/msg/wrench_stamped.hpp>
-#include <std_msgs/msg/float64_multi_array.hpp>
 #include <pinocchio/algorithm/kinematics.hpp>
 #include <pinocchio/multibody/fwd.hpp>
 #include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/float64_multi_array.hpp>
 
 #include <crisp_controllers/utils/ros2_version.hpp>
 

script/config/roslog_to_csv.json

@@ -0,0 +1,22 @@
+{
+  "keys": {
+    "q": ["joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6", "joint_7"],
+    "q target": ["joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6", "joint_7"],
+    "q ref": ["joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6", "joint_7"],
+    "dq": ["joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6", "joint_7"],
+    "current eef": ["x", "y", "z", "yaw", "pitch", "roll", "qx", "qy", "qz", "qw"],
+    "interpolated eef": ["x", "y", "z", "yaw", "pitch", "roll","qx", "qy", "qz", "qw"],
+    "desired eef": ["x", "y", "z", "yaw", "pitch", "roll", "qx", "qy", "qz", "qw"],
+    "target eef": ["x", "y", "z", "yaw", "pitch", "roll", "qx", "qy", "qz", "qw"],
+    "error": ["x", "y", "z", "yaw", "pitch", "roll"],
+    "unnormalized new_angular": ["omega_x", "omega_y", "omega_z"],
+    "unnormalized filtered_angular": ["omega_x", "omega_y", "omega_z"],
+    "normalized new_angular": ["omega_x", "omega_y", "omega_z"],
+    "normalized filtered_angular": ["omega_x", "omega_y", "omega_z"],
+    "angle_diff": ["angle_diff"],
+    "inter angular": ["omega_x", "omega_y", "omega_z"],
+    "control input": ["x", "y", "z", "yaw", "pitch", "roll"],
+    "task wrench": ["x", "y", "z", "yaw", "pitch", "roll"],
+    "task tau": ["joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6", "joint_7"]
+  }
+}