feat: finish simulation loop, sending command to sim

urc_arm/config/controller_config.yaml

@@ -1,3 +1,27 @@
 arm_mj_sim:
   ros__parameters:
     model_dir: "/home/keseterg/Documents/RoboJackets/urc_ws/src/urc-software/urc_arm_models/urdf/walli_arm_v2_block_mjcf/walli_arm_v2_block.xml"
+
+arm_qp_controller:
+  ros__parameters:
+    control_config:
+      control_topic: arm_position_controller/commands
+
+controller_manager:
+  ros__parameters:
+    update_rate: 300 # Hz
+
+    arm_position_controller:
+      type: position_controllers/JointGroupPositionController
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+arm_position_controller:
+  ros__parameters:
+    joints:
+      - base_yaw_joint
+      - upper_arm_joint
+      - fore_arm_joint
+      - ee_roll_jont
+      - ee_pitch_joint

urc_arm/launch/mjsim.launch.py

@@ -25,7 +25,6 @@ def generate_launch_description():
         executable="arm_sim_mj",
         parameters=[controller_config_file_dir],
     )
-
     control_node = Node(
         package="controller_manager",
         executable="ros2_control_node",
@@ -46,18 +45,19 @@ def generate_launch_description():
         ],
         remappings=[("/joint_states", "/arm_joint_states")],
     )
-
-    # spawn_controller = IncludeLaunchDescription(
-    #     # PythonLaunchDescriptionSource(
-    #     #     str(moveit_config.package_path / "launch/spawn_controllers.launch.py")
-    #     # ),
-    # )
+    spawn_controller = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["arm_position_controller"],
+        output="screen",
+    )
+    qp_controller = Node(
+        package="urc_arm_py",
+        executable="arm_qp_control",
+        parameters=[controller_config_file_dir],
+        output="screen",
+    )
 
     return LaunchDescription(
-        [
-            rsp_node,
-            control_node,
-            mjsim,
-            # spawn_controller,
-        ]
+        [rsp_node, control_node, mjsim, spawn_controller, qp_controller]
     )

urc_arm_py/urc_arm_py/arm_qp_control.py

@@ -16,6 +16,7 @@ def __init__(self):
         self.initialized = False
         self.control_activated = False
 
+        self.param_listener = arm_qp_control_parameters.ParamListener(self)
         latching_qos = rclpy.qos.QoSProfile(
             depth=1,
             durability=rclpy.qos.QoSDurabilityPolicy.TRANSIENT_LOCAL,
@@ -26,7 +27,12 @@ def __init__(self):
         )
         self.previous_feedback_time = self.get_clock().now()
 
-        self.pos_ctrl_pub = self.create_publisher(Float64MultiArray, "~/pos_ctrl", 10)
+        print(self.param_listener.get_params().control_config.control_topic)
+        self.pos_ctrl_pub = self.create_publisher(
+            Float64MultiArray,
+            self.param_listener.get_params().control_config.control_topic,
+            10,
+        )
         self.pos_ctrl_command = Float64MultiArray()
 
         self.robot_description_sub = self.create_subscription(
@@ -56,7 +62,6 @@ def on_init(self, msg: String):
 
         # invalidate the subscription
         self.robot_description_sub = None
-        self.param_listener = arm_qp_control_parameters.ParamListener(self)
 
         # setup tasks
         self.setup_tasks()
@@ -67,10 +72,12 @@ def on_init(self, msg: String):
             1.0 / self.param_listener.get_params().control_config.rate_hz,
             self.control_loop,
         )
+        self.robot_description_sub = None
 
     def setup_tasks(self):
         self.solver = placo.KinematicsSolver(self.robot)
         self.solver.mask_fbase(True)
+        self.solver.dt = 1.0 / self.param_listener.get_params().control_config.rate_hz
 
         param = self.param_listener.get_params()
         self.ee_name = param.robot_config.end_effector_frame_name
@@ -92,6 +99,9 @@ def activate_control(self, request: Trigger.Request, response: Trigger.Response)
         self.control_activated = True
         response.success = True
         self.se3_ee_target = self.robot.get_T_world_frame(self.ee_name)
+        self.se3_ee_target[0][3] = 0.5
+        self.se3_ee_target[1][3] = 0.5
+        self.se3_ee_target[2][3] = 0.5
         self.get_logger().info("Activated control!")
         return response
 

urc_arm_py/urc_arm_py/arm_qp_control_parameters.py

@@ -10,6 +10,7 @@
 from rclpy.time import Time
 import copy
 import rclpy
+import rclpy.parameter
 from generate_parameter_library_py.python_validators import ParameterValidators
 
 
@@ -24,6 +25,7 @@ class __RobotConfig:
             end_effector_frame_name = "ee"
         robot_config = __RobotConfig()
         class __ControlConfig:
+            control_topic = "~/pos_ctrl"
             rate_hz = 120
             enable_manipulability_constraint = False
         control_config = __ControlConfig()
@@ -53,6 +55,31 @@ def get_params(self):
         def is_old(self, other_param):
             return self.params_.stamp_ != other_param.stamp_
 
+        def unpack_parameter_dict(self, namespace: str, parameter_dict: dict):
+            """
+            Flatten a parameter dictionary recursively.
+
+            :param namespace: The namespace to prepend to the parameter names.
+            :param parameter_dict: A dictionary of parameters keyed by the parameter names
+            :return: A list of rclpy Parameter objects
+            """
+            parameters = []
+            for param_name, param_value in parameter_dict.items():
+                full_param_name = namespace + param_name
+                # Unroll nested parameters
+                if isinstance(param_value, dict):
+                    nested_params = self.unpack_parameter_dict(
+                            namespace=full_param_name + rclpy.parameter.PARAMETER_SEPARATOR_STRING,
+                            parameter_dict=param_value)
+                    parameters.extend(nested_params)
+                else:
+                    parameters.append(rclpy.parameter.Parameter(full_param_name, value=param_value))
+            return parameters
+
+        def set_params_from_dict(self, param_dict):
+            params_to_set = self.unpack_parameter_dict('', param_dict)
+            self.update(params_to_set)
+
         def refresh_dynamic_parameters(self):
             updated_params = self.get_params()
             # TODO remove any destroyed dynamic parameters
@@ -68,6 +95,10 @@ def update(self, parameters):
                     updated_params.robot_config.end_effector_frame_name = param.value
                     self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
 
+                if param.name == self.prefix_ + "control_config.control_topic":
+                    updated_params.control_config.control_topic = param.value
+                    self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+
                 if param.name == self.prefix_ + "control_config.rate_hz":
                     updated_params.control_config.rate_hz = param.value
                     self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
@@ -93,6 +124,11 @@ def declare_params(self):
                 parameter = updated_params.robot_config.end_effector_frame_name
                 self.node_.declare_parameter(self.prefix_ + "robot_config.end_effector_frame_name", parameter, descriptor)
 
+            if not self.node_.has_parameter(self.prefix_ + "control_config.control_topic"):
+                descriptor = ParameterDescriptor(description="Destination of control commands from the controller.", read_only = False)
+                parameter = updated_params.control_config.control_topic
+                self.node_.declare_parameter(self.prefix_ + "control_config.control_topic", parameter, descriptor)
+
             if not self.node_.has_parameter(self.prefix_ + "control_config.rate_hz"):
                 descriptor = ParameterDescriptor(description="Control frequency. Default to 120Hz.", read_only = False)
                 parameter = updated_params.control_config.rate_hz
@@ -108,6 +144,9 @@ def declare_params(self):
             param = self.node_.get_parameter(self.prefix_ + "robot_config.end_effector_frame_name")
             self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
             updated_params.robot_config.end_effector_frame_name = param.value
+            param = self.node_.get_parameter(self.prefix_ + "control_config.control_topic")
+            self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
+            updated_params.control_config.control_topic = param.value
             param = self.node_.get_parameter(self.prefix_ + "control_config.rate_hz")
             self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
             updated_params.control_config.rate_hz = param.value

urc_arm_py/urc_arm_py/arm_qp_control_parameters.yaml

@@ -6,6 +6,10 @@ arm_qp_control_parameters:
       description: "end effector name."
 
   control_config:
+    control_topic:
+      type: string
+      default_value: "~/pos_ctrl"
+      description: "Destination of control commands from the controller."
     rate_hz:
       type: int
       default_value: 120

urc_arm_py/urc_arm_py/arm_sim_mj.py

@@ -11,11 +11,16 @@ def __init__(self):
         super().__init__("arm_mj_sim")
 
         self.declare_parameter("model_dir", "")
+        self.declare_parameter("kp", 10.0)
+        self.declare_parameter("kd", 0)
         model_dir = self.get_parameter("model_dir").get_parameter_value().string_value
+        self.kp = self.get_parameter("kp").get_parameter_value().double_value
+        self.kd = self.get_parameter("kd").get_parameter_value().double_value
         self.get_logger().warn(f"Loading model from: {model_dir}")
         # Load the MuJoCo model
         self.model = mujoco.MjModel.from_xml_path(model_dir)
         self.data = mujoco.MjData(self.model)
+        self.joint_target_positions = np.zeros(self.model.nu)
 
         # Initialize GLFW
         if not glfw.init():
@@ -61,11 +66,30 @@ def __init__(self):
             for i in range(self.model.njnt)
         ]
 
+        # Joint command subscriber
+        self.joint_command_sub = self.create_subscription(
+            JointState, "/arm_joint_commands", self.accept_position_command, 10
+        )
+
         # Interaction state
         self.is_paused = False
         self.mouse_last_x, self.mouse_last_y = None, None
         self.mouse_left_button = False
 
+    def accept_position_command(self, msg: JointState):
+        try:
+            for idx, name in enumerate(msg.name):
+                joint_id = self.joint_names.index(name)
+                self.joint_target_positions[joint_id] = msg.position[idx]
+        except ValueError as e:
+            # self.get_logger().error(f"Invalid joint name in torque command: {e}")
+            pass
+
+    def apply_position_command(self):
+        self.data.ctrl[:] = (
+            self.joint_target_positions - self.data.qpos
+        ) * self.kp + self.data.qvel * self.kd
+
     def key_callback(self, window, key, scancode, action, mods):
         """Handle keyboard input for simulation control."""
         if action == glfw.PRESS:
@@ -102,6 +126,7 @@ def scroll_callback(self, window, xoffset, yoffset):
     def mj_update(self):
         if not glfw.window_should_close(self.window) and rclpy.ok():
             if not self.is_paused:
+                self.apply_position_command()
                 mujoco.mj_step(self.model, self.data)
 
             # Get the viewport dimensions