KNR-PW · BartlomiejK2 · Sep 30, 2025 · Sep 30, 2025 · Oct 4, 2025 · Oct 5, 2025
diff --git a/.gitignore b/.gitignore
@@ -53,5 +53,8 @@ qtcreator-*
 CATKIN_IGNORE
 
 
-#vscode
-.vscode/
+# vscode
+.vscode/
+
+# Package path generated header files 
+package_path.h
diff --git a/meldog_description/cl_commands/single_joint_test.txt b/meldog_description/cl_commands/single_joint_test.txt
@@ -1,6 +1,6 @@
 // Wysyłanie pozycji 1.0 dla jointa "body_1_joint" 
 
-ros2 topic pub /joint_controller/joint_commands joint_controller_msgs/msg/JointCommand "{name: {'LFT_joint', 'LFH_joint', 'LFK_joint', 'RFT_joint', 'RFH_joint', 'RFK_joint', 'LRT_joint', 'LRH_joint', 'LRK_joint', 'RRT_joint', 'RRH_joint', 'RRK_joint'}, desired_position: [0.0, 1.0, 1.0, 0.0, 1.0, 1.0, 0.0, 1.0, 1.0, 0.0, 1.0, 1.0], desired_velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], kp_scale: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], kd_scale: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], feedforward_effort: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]}"
+ros2 topic pub /joint_controller/joint_commands joint_controller_msgs/msg/JointCommand "{name: {'LFT_joint', 'LFH_joint', 'LFK_joint', 'RFT_joint', 'RFH_joint', 'RFK_joint', 'LRT_joint', 'LRH_joint', 'LRK_joint', 'RRT_joint', 'RRH_joint', 'RRK_joint'}, desired_position: [0, -0.785398163, 1.57079632679, 0, -0.785398163, 1.557079632679, 0, -0.785398163, 1.57079632679, 0, -0.785398163, 1.57079632679], desired_velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], kp_scale: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], kd_scale: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], feedforward_effort: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]}"
 
 
-ros2 topic pub /joint_controller/joint_commands joint_controller_msgs/msg/JointCommand "{name: {'LFT_joint', 'LFH_joint', 'LFK_joint', 'RFT_joint', 'RFH_joint', 'RFK_joint', 'LRT_joint', 'LRH_joint', 'LRK_joint', 'RRT_joint', 'RRH_joint', 'RRK_joint'}, desired_position: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], desired_velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], kp_scale: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], kd_scale: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], feedforward_effort: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]}"
+ros2 topic pub /joint_controller/joint_commands joint_controller_msgs/msg/JointCommand "{name: {'LFT_joint', 'LFH_joint', 'LFK_joint', 'RFT_joint', 'RFH_joint', 'RFK_joint', 'LRT_joint', 'LRH_joint', 'LRK_joint', 'RRT_joint', 'RRH_joint', 'RRK_joint'}, desired_position: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], desired_velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], kp_scale: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], kd_scale: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], feedforward_effort: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]}"
diff --git a/meldog_description/controllers/meldog_chained_joint_controllers.yaml b/meldog_description/controllers/meldog_chained_joint_controllers.yaml
@@ -0,0 +1,215 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 200
+
+    joint_controller:
+      type: joint_controller/JointController
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+    imu_sensor_broadcaster:
+      type: imu_sensor_broadcaster/IMUSensorBroadcaster
+
+    joint_forward_trajectory_controller:
+      type: joint_forward_trajectory_controller/JointForwardTrajectoryController
+
+imu_sensor_broadcaster:
+  ros__parameters:
+    sensor_name: "imu_sensor"
+    frame_id: "imu"
+
+joint_forward_trajectory_controller:
+  ros__parameters:
+    joints:
+      - joint_controller/LFT_joint
+      - joint_controller/LFH_joint
+      - joint_controller/LFK_joint
+      - joint_controller/RFT_joint
+      - joint_controller/RFH_joint
+      - joint_controller/RFK_joint
+      - joint_controller/LRT_joint
+      - joint_controller/LRH_joint
+      - joint_controller/LRK_joint
+      - joint_controller/RRT_joint
+      - joint_controller/RRH_joint
+      - joint_controller/RRK_joint
+
+    command_interfaces:
+      - position
+      - velocity
+      - effort
+
+    interpolate_from_desired_state: true
+
+    allow_partial_joints_goal: true
+
+joint_controller:
+  ros__parameters:
+    joint_names:
+      - LFT_joint
+      - LFH_joint
+      - LFK_joint
+      - RFT_joint
+      - RFH_joint
+      - RFK_joint
+      - LRT_joint
+      - LRH_joint
+      - LRK_joint
+      - RRT_joint
+      - RRH_joint
+      - RRK_joint
+
+    reference_interfaces:
+      - "position"
+      - "velocity"
+      - "effort"
+
+    joint_params:
+      LFT_joint: 
+        position_max: 3.14
+        position_min: -3.14
+        position_offset: 0.0
+        velocity_max: 15.0
+        effort_max: 34.895
+
+      LFH_joint: 
+        position_max: 10000.0
+        position_min: -10000.0
+        position_offset: 0.0
+        velocity_max: 15.0
+        effort_max: 34.895
+
+      LFK_joint: 
+        position_max: 2.793
+        position_min: -2.793
+        position_offset: 0.0
+        velocity_max: 15.0
+        effort_max: 34.895
+
+      RFT_joint: 
+        position_max: 3.14
+        position_min: -3.14
+        position_offset: 0.0
+        velocity_max: 15.0
+        effort_max: 34.895
+
+      RFH_joint: 
+        position_max: 10000.0
+        position_min: -10000.0
+        position_offset: 0.0
+        velocity_max: 15.0
+        effort_max: 34.895
+
+      RFK_joint: 
+        position_max: 2.793
+        position_min: -2.793
+        position_offset: 0.0
+        velocity_max: 15.0
+        effort_max: 34.895
+
+      LRT_joint: 
+        position_max: 3.14
+        position_min: -3.14
+        position_offset: 0.0
+        velocity_max: 15.0
+        effort_max: 34.895
+
+      LRH_joint: 
+        position_max: 10000.0
+        position_min: -10000.0
+        position_offset: 0.0
+        velocity_max: 15.0
+        effort_max: 34.895
+
+      LRK_joint: 
+        position_max: 2.793
+        position_min: -2.793
+        position_offset: 0.0
+        velocity_max: 15.0
+        effort_max: 34.895
+
+      RRT_joint: 
+        position_max: 3.14
+        position_min: -3.14
+        position_offset: 0.0
+        velocity_max: 15.0
+        effort_max: 34.895
+
+      RRH_joint: 
+        position_max: 10000.0
+        position_min: -10000.0
+        position_offset: 0.0
+        velocity_max: 15.0
+        effort_max: 34.895
+
+      RRK_joint: 
+        position_max: 2.793
+        position_min: -2.793
+        position_offset: 0.0
+        velocity_max: 15.0
+        effort_max: 34.895
+
+    frequency: 1000.0
+
+    pid_gains:
+      LFT_joint:
+        p: 235.0
+        d: 5.0
+        i: 0.0
+        ilimit: 0.0
+      LFH_joint:
+        p: 235.0
+        d: 5.0
+        i: 0.0
+        ilimit: 0.0
+      LFK_joint:
+        p: 235.0
+        d: 5.0
+        i: 0.0
+        ilimit: 0.0
+      RFT_joint:
+        p: 235.0
+        d: 5.0
+        i: 0.0
+        ilimit: 0.0
+      RFH_joint:
+        p: 235.0
+        d: 5.0
+        i: 0.0
+        ilimit: 0.0
+      RFK_joint:
+        p: 235.0
+        d: 5.0
+        i: 0.0
+        ilimit: 0.0
+      LRT_joint:
+        p: 235.0
+        d: 5.0
+        i: 0.0
+        ilimit: 0.0
+      LRH_joint:
+        p: 235.0
+        d: 5.0
+        i: 0.0
+        ilimit: 0.0
+      LRK_joint:
+        p: 235.0
+        d: 5.0
+        i: 0.0
+        ilimit: 0.0
+      RRT_joint:
+        p: 235.0
+        d: 5.0
+        i: 0.0
+        ilimit: 0.0
+      RRH_joint:
+        p: 235.0
+        d: 5.0
+        i: 0.0
+        ilimit: 0.0
+      RRK_joint:
+        p: 235.0
+        d: 5.0
+        i: 0.0
+        ilimit: 0.0
diff --git a/meldog_description/controllers/meldog_position_controllers.yaml b/meldog_description/controllers/meldog_position_controllers.yaml
@@ -0,0 +1,53 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 100
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+    imu_sensor_broadcaster:
+      type: imu_sensor_broadcaster/IMUSensorBroadcaster
+
+    forward_position_controller:
+      type: forward_command_controller/ForwardCommandController
+
+    diagnostic_broadcaster:
+      type: diagnostic_broadcaster/DiagnosticBroadcaster
+
+imu_sensor_broadcaster:
+  ros__parameters:
+    sensor_name: "imu_sensor"
+    frame_id: "imu"
+
+forward_position_controller:
+  ros__parameters:
+    joints:
+      - LFT_joint
+      - LFH_joint
+      - LFK_joint
+      - RFT_joint
+      - RFH_joint
+      - RFK_joint
+      - LRT_joint
+      - LRH_joint
+      - LRK_joint
+      - RRT_joint
+      - RRH_joint
+      - RRK_joint
+    interface_name: position
+
+diagnostic_broadcaster:
+  ros__parameters:
+    joint_names:
+      - LFT_joint
+      - LFH_joint
+      - LFK_joint
+      - RFT_joint
+      - RFH_joint
+      - RFK_joint
+      - LRT_joint
+      - LRH_joint
+      - LRK_joint
+      - RRT_joint
+      - RRH_joint
+      - RRK_joint
diff --git a/meldog_description/description/gazebo/gazebo_properties.urdf.xacro b/meldog_description/description/gazebo/gazebo_properties.urdf.xacro
@@ -11,7 +11,7 @@
     <xacro:property name="implicitSpringDamper_value" value="1" />
     <xacro:property name="springStiffness_value" value="0" />
     <xacro:property name="springReference_value" value="0" />
-    <xacro:property name="implicitSpringDamper_value" value="0" />
+    <!-- <xacro:property name="implicitSpringDamper_value" value="0" /> -->
     <xacro:property name="stopErp_value" value="0" />
     <xacro:property name="stopCfm_value" value="0" />
 
@@ -24,12 +24,26 @@
     <xacro:property name="dampingFactor_value" value="0" />
 
     <!-- Gazebo link parameters for feet -->
-    <xacro:property name="mu1_value_foot" value="0" />
-    <xacro:property name="mu2_value_foot" value="0" />
-    <xacro:property name="kp_value_foot" value="0" />
-    <xacro:property name="kd_value_foot" value="0" />
-    <xacro:property name="maxVel_value_foot" value="0" />
-    <xacro:property name="dampingFactor_value_foot" value="0" />
+    <xacro:property name="mu1_value_foot" value="1.0" />
+    <xacro:property name="mu2_value_foot" value="1.0" />
+
+    <xacro:property name="kp_value_foot" value="100000.0" />
+    <xacro:property name="kd_value_foot" value="5000.0" />
+
+    <xacro:property name="maxVel_value_foot" value="0.01" />
+    <xacro:property name="minDepth_value_foot" value="0.001" />
+
+    <xacro:property name="dampingFactor_value_foot" value="0.25" />
+    <!-- <xacro:property name="mu1_value_foot" value="2.0" />
+    <xacro:property name="mu2_value_foot" value="2.0" />
+    <xacro:property name="kp_value_foot" value="1000000.0" />
+    <xacro:property name="kd_value_foot" value="1000.0" />
+    <xacro:property name="maxVel_value_foot" value="0.0" />
+    <xacro:property name="dampingFactor_value_foot" value="0.1" /> -->
+    <!-- <xacro:property name="kp_value_foot" value="10000.0"/>
+    <xacro:property name="kd_value_foot" value="10000.0"/>
+    <xacro:property name="maxVel_value_foot" value="0.0"/>
+    <xacro:property name="dampingFactor_value_foot" value="10.1"/> -->
 
 
     <!--      GAZEBO IMU NOICE PARAMETERS      -->

diff --git a/meldog_description/description/gazebo/meldog_gazebo.urdf.xacro b/meldog_description/description/gazebo/meldog_gazebo.urdf.xacro
@@ -15,6 +15,7 @@
             <kd value="${kd}" />
             <maxVel value="${maxVel}"/>
             <dampingFactor value="${dampingFactor}"/>
+            <minDepth value="0.0002"/>
         </gazebo>
 
     </xacro:macro>

diff --git a/meldog_description/description/joints/properties/H_joint_properties.urdf.xacro b/meldog_description/description/joints/properties/H_joint_properties.urdf.xacro
@@ -15,7 +15,7 @@
     <xacro:property name="H_velocity" value="${motor_max_vel*pi/30.0/actuator_reduction}" />
 
     <!-- Hip joint friction and damping -->
-    <xacro:property name="H_damping" value="0"/>
-    <xacro:property name="H_friction" value="0.0312"/>
+    <xacro:property name="H_damping" value="0.2"/>
+    <xacro:property name="H_friction" value="0.1312"/>
 
 </robot>
diff --git a/meldog_description/description/joints/properties/K_joint_properties.urdf.xacro b/meldog_description/description/joints/properties/K_joint_properties.urdf.xacro
@@ -10,7 +10,7 @@
     <xacro:property name="K_velocity" value="${motor_max_vel*pi/30.0/actuator_reduction}" />
 
     <!-- Knee joint friction and damping -->
-    <xacro:property name="K_damping" value="0"/>
-    <xacro:property name="K_friction" value="0.0312"/>
+    <xacro:property name="K_damping" value="0.2"/>
+    <xacro:property name="K_friction" value="0.1312"/>
 
 </robot>
diff --git a/meldog_description/description/joints/properties/T_joint_properties.urdf.xacro b/meldog_description/description/joints/properties/T_joint_properties.urdf.xacro
@@ -10,7 +10,7 @@
     <xacro:property name="T_velocity" value="${motor_max_vel*pi/30.0/actuator_reduction}" />
 
     <!-- Trunk joint friction and damping -->
-    <xacro:property name="T_damping" value="0"/>
-    <xacro:property name="T_friction" value="0.0312"/>
+    <xacro:property name="T_damping" value="0.2"/>
+    <xacro:property name="T_friction" value="0.1312"/>
 
 </robot>