.. rubric:: H1 Robot Control
    
.. rubric::  Modes Selection

.. important::

    Most functionalities are disable for Unitree H1. Startup requires remote.

.. rubric:: Posture & State Commands

.. list-table::
   :header-rows: 1
   :widths: 20 50

   * - Command
     - Description
   * - ``damp``
     - Set all motors to damping mode.
   * - ``start``
     - Start locomotion control.
   * - ``stand_up``
     - Stand up from sitting/squatting.
   * - ``zero_torque``
     - Disable torque on all motors.
   * - ``stop_move``
     - Stop all motion immediately.
   * - ``high_stand``
     - Stand at high position.
   * - ``low_stand``
     - Stand at low position.
   * - ``balance_stand``
     - Stand with balance mode active.


.. rubric:: Setter Commands (Change Robot State)

.. list-table::
   :header-rows: 1
   :widths: 30 50

   * - Command
     - Parameters
   * - ``set_fsm_id=<id>``
     - Integer FSM ID to switch to.
   * - ``set_balance_mode=<0/1>``
     - Enable (1) or disable (0) balance mode.
   * - ``set_swing_height=<value>``
     - Set swing height in meters.
   * - ``set_stand_height=<value>``
     - Set stand height in meters.
   * - ``set_velocity="vx vy ω [duration]"``
     - Set velocity (m/s, m/s, rad/s, [duration in s]).
   * - ``move="vx vy ω"``
     - Command motion without duration.
   * - ``set_task_id=<id>``
     - Set active task ID.
   * - ``set_speed_mode=<mode>``
     - Change locomotion speed mode.



.. rubric:: Getter Commands (Query Current Robot State)

.. list-table::
   :header-rows: 1
   :widths: 25 50

   * - Command
     - Returns
   * - ``get_fsm_id``
     - Current finite state machine ID.
   * - ``get_fsm_mode``
     - Current FSM mode.
   * - ``get_balance_mode``
     - Current balance mode (0/1).
   * - ``get_swing_height``
     - Current swing foot height.
   * - ``get_stand_height``
     - Current standing height.
   * - ``get_phase``
     - Current gait phase vector.


.. rubric:: Toggle Commands

.. list-table::
   :header-rows: 1
   :widths: 30 15 50

   * - Command
     - Parameters
     - Description
   * - ``continous_gait=<true/false>``
     - bool
     - Enable or disable continuous gait mode.
   * - ``switch_move_mode=<true/false>``
     - bool
     - Switch between movement mode and standing mode.


.. rubric:: Examples

Damp State:

.. code-block:: bash

    ROS_DOMAIN_ID=10 ros2 service call /h1_unit_238/hardware_modes h1_interface/srv/H1Modes \
    '{"request_data": "damp"}'


.. rubric:: Dual Arm Record H1-2

.. warning::
    
    When launch the arm driver the arm violently snaps to the H1-2 initial position. Ensure the arms are near to the start position to 
    prevent damage to the robot and keep surrounding area clear.

.. code-block:: bash
    
    ros2 launch h1_platform h1_2_arm_record.launch.py


.. rubric:: Configuration

Update to requirements the `/opt/mybotshop/src/mybotshop/h1_platform/config/h1_2_dual_arm_controller.yaml` file

.. rubric:: Start recording

.. important::
    
    Keep in mind the arms will start moving from this area so try to keep away from the body to avoid scratches. In the end of the recording be 
    sure to bring the arms in a location close to the initial position for the next recording session.

.. code-block:: bash

    ROS_DOMAIN_ID=10 ros2 service call /h1_unit_238/dual_arm/record std_srvs/srv/SetBool "data: true"


.. rubric:: Stop recording and save

.. code-block:: bash
    
    ROS_DOMAIN_ID=10 ros2 service call /h1_unit_238/dual_arm/record std_srvs/srv/SetBool "data: false"


.. rubric:: Play last recorded trajectory  

.. code-block:: bash

    ROS_DOMAIN_ID=10 ros2 service call /h1_unit_238/dual_arm/playback h1_interface/srv/H1Modes "request_data: ''"


.. rubric:: Play specific file

.. code-block:: bash

    ROS_DOMAIN_ID=10 ros2 service call /h1_unit_238/dual_arm/playback h1_interface/srv/H1Modes "request_data: '/opt/mybotshop/h1_2_trajectories/trajectory_20250925_160052.csv'"


.. rubric:: Dual Arm Control H1-2

.. code-block:: bash
    
    ros2 launch h1_platform h1_2_arm_server.launch.py

.. rubric:: Configuration

Update to requirements the `/opt/mybotshop/src/mybotshop/h1_platform/config/h1_2_dual_arm_controller.yaml` file

.. rubric:: Poses

T pose

.. code-block:: bash

    ROS_DOMAIN_ID=10 ros2 action send_goal /h1_unit_1789/dual_arm/follow_joint_trajectory \
        control_msgs/action/FollowJointTrajectory \
        "{trajectory:
            {joint_names: [
                left_shoulder_pitch, left_shoulder_roll, left_shoulder_yaw, left_elbow,
                left_wrist_roll, left_wrist_pitch, left_wrist_yaw,
                right_shoulder_pitch, right_shoulder_roll, right_shoulder_yaw, right_elbow,
                right_wrist_roll, right_wrist_pitch, right_wrist_yaw],
            points: [
                {positions: [0.0, 0.3, 0.0, 1.57, 0.0, 0.0, 0.0,   0.0,-0.3, 0.0, 1.57, 0.0, 0.0, 0.0],
                time_from_start: {sec: 2, nanosec: 0}},
                {positions: [0.0, 1.57, 0.0, 1.57, 0.0, 0.0, 0.0,  0.0,-1.57,0.0, 1.57, 0.0, 0.0, 0.0],
                time_from_start: {sec: 5, nanosec: 0}}
            ]}}"


- Ready pose

.. code-block:: bash

    ROS_DOMAIN_ID=10 ros2 action send_goal /h1_unit_1789/dual_arm/follow_joint_trajectory \
    control_msgs/action/FollowJointTrajectory \
    "{trajectory:
        {joint_names: [
            left_shoulder_pitch, left_shoulder_roll, left_shoulder_yaw, left_elbow,
            left_wrist_roll, left_wrist_pitch, left_wrist_yaw,
            right_shoulder_pitch, right_shoulder_roll, right_shoulder_yaw, right_elbow,
            right_wrist_roll, right_wrist_pitch, right_wrist_yaw],
        points: [
            {positions: [0.0, 0.3, 0.0, 0.7, 0.0, 0.0, 0.0,   
                        0.0,-0.3, 0.0, 0.7, 0.0, 0.0, 0.0],
            time_from_start: {sec: 2, nanosec: 0}},
        ]}}"