Control Examples

This package contains basic code examples for controlling the robot using ROS.
It provides demonstrations for position, velocity, and torque control modes.

I. Overview

Supported Control Modes

The robot supports three control modes:

Mode	Description
Position Control	Directly sets joint positions.
Velocity Control	Controls joint velocities.
Torque Control	Controls joint torques.

⚙️ Orbita Limitation

⚠️ The Orbita joints (neck and hands) are controllable only in position mode.
Velocity and torque control modes are not available for these joints.
Attempting to use other modes on these joints will have no effect.

Joint Limits

Joint	Range (radians)	Description
Fingers	-1.4 (closed) → 1.5 (open)
Thumb	-0.6 (closed) → 0.75 (open)
Arm joints	Vary depending on joint type

Check Joint Limits Yourself

After completing the startup process , you can estimate limits without sending any target command:

Choose the measurement topic for the joint you want to inspect (examples: /measurements/arms/left/fingers, /measurements/arms/left/thumb, /measurements/left_arm).
In a terminal, monitor the topic:

ros2 topic echo /measurements/arms/left/fingers

Manually move the joint slowly to each mechanical end (minimum then maximum).
Read the reported values from ros2 topic echo; those values correspond to the observed joint limits.

⚠️ Move joints slowly and carefully to avoid collisions or excessive force.

II. Robot Platform

A. Setup

Before running any control example, make sure to configure Zenoh and enable User Control Mode:

a. configure Zenoh:

Edit the following file on the robot:

install/share/travel/configs/enchanted_bringup/zenoh.json5

To allow all topics to pass through Zenoh, remove the allow block from the configuration file.

b. enable User Control Mode and joints:

Open the Wizard interface.
Navigate to the Debug window.
Switch the robot to User Control Mode.
Enable joints.
Reboot the robot so that switch mode will take effect.

⚠️ Important:
The robot will ignore ROS control commands if: - User Control Mode is not active, or
- Joints are not enabled.

Always verify both before proceeding.

B. Launch

a. Launching Zenoh communication

Once setup is complete, launch the visualization with the Zenoh option to enable communication with the robot:

python3 visualization/native_visualization/launch.py --character <character_name> --with-zenoh --target-hostname <hostname> --target-ip <robot_ip>

Replace: - <hostname> with your robot’s hostname (e.g., base-a2f15) - <robot_ip> with your robot’s IP address

b. Launch joint control example

This package depends on the enchanted_msgs package.
Ensure both packages are built, under sdk folder run:

colcon build
source install/setup.bash

Start the control node with:

ros2 run control_examples joint_position_control_example

III. Simulation Platform

A. Setup & Launch

Run simulation in User Control Mode:

mirokai_simulation_client run --travel -ucm

then build and run the control example as in the previous section

IV. Node Architecture

The node uses a publisher-subscriber architecture.

Subscribers (Measurement Topics)

/measurements/neck
/measurements/ears
/measurements/left_arm
/measurements/right_arm
/measurements/arms/left/fingers
/measurements/arms/left/thumb
/measurements/arms/right/fingers
/measurements/arms/right/thumb

Publishers (Target Topics)

/targets/neck
/targets/ears
/targets/left_arm
/targets/right_arm
/targets/arms/left/fingers
/targets/arms/left/thumb
/targets/arms/right/fingers
/targets/arms/right/thumb

V. Control Strategy

The node performs the following steps:

Subscribes to all measurement topics.
Waits for initial joint state measurements.
Once all measurements are received:
Unsubscribes from measurement topics.
Starts publishing target positions.
Publishes updates every 10 ms for smooth movement.
Uses linear interpolation between current and target positions.

Error Handling

The node includes several safety mechanisms: - Validates message integrity (non-null values).
- Checks for empty joint state data.
- Automatically unsubscribes once initial states are received.
- Logs all errors for debugging.

Default Target Positions

Joint Group	Target Position
Ears	(0.0, 0.0)
Fingers	1.0
Thumb	0.0
Neck	(0.0, 0.2, 0.0)
Arms	(-1.0, 0.2, 0.0, -1.2, 0.0, 0.0, 0.0)

VI. Configuration

⚠️ Safety Warning

Ensure that target positions do not cause self-collisions.
There are no built-in safety checks — you are directly controlling the joints.

Self-Collision Detection

The robot publishes a /measurements/self_collision topic: - Set to true when a self-collision is detected. - It is strongly recommended to monitor this topic and halt movement when a collision occurs.

You can switch to FREE mode to manually reposition the robot if needed.

a. Modifying Target Positions

Edit the targetPositions_ field in joint_position_control_example.cc:

targetPositions_ = {
    {"ears", {0.0, -0.0}},
    {"right_fingers", {1.0}},
    {"right_thumb", {0.0}},
    {"left_fingers", {1.0}},
    {"left_thumb", {0.0}},
    {"neck", {0.0, 0.2, 0.0}},
    {"left_arm", {-1.0, 0.2, 0.0, -1.2, 0.0, 0.0, 0.0}},
    {"right_arm", {-1.0, 0.2, 0.0, -1.2, 0.0, 0.0, 0.0}}
};

b. Mandatory Joint Names

Each target (e.g., neck, left_arm) must have associated joint names:

{"ears", {"HED_EAR_LEFT_JOINT", "HED_EAR_RIGHT_JOINT"}},
{"right_fingers", {"RIGHT_FINGERS"}},
{"right_thumb", {"RIGHT_THUMB"}},
{"left_fingers", {"LEFT_FINGERS"}},
{"left_thumb", {"LEFT_THUMB"}},
{"neck", {"HED_NECK_FRONTAL_JOINT", "HED_NECK_SAGITTAL_JOINT", "HED_NECK_TRANSVERSAL_JOINT"}},
{"left_arm", {"ARM_LEFT_SHOULDER_SAGITTAL_JOINT", "ARM_LEFT_SHOULDER_FRONTAL_JOINT",
              "ARM_LEFT_SHOULDER_TRANSVERSAL_JOINT", "ARM_LEFT_ELBOW_SAGITTAL_JOINT",
              "ARM_LEFT_WRIST_FRONTAL_JOINT", "ARM_LEFT_WRIST_TRANSVERSAL_JOINT",
              "ARM_LEFT_WRIST_SAGITTAL_JOINT"}},
{"right_arm", {"ARM_RIGHT_SHOULDER_SAGITTAL_JOINT", "ARM_RIGHT_SHOULDER_FRONTAL_JOINT",
               "ARM_RIGHT_SHOULDER_TRANSVERSAL_JOINT", "ARM_RIGHT_ELBOW_SAGITTAL_JOINT",
               "ARM_RIGHT_WRIST_FRONTAL_JOINT", "ARM_RIGHT_WRIST_TRANSVERSAL_JOINT",
               "ARM_RIGHT_WRIST_SAGITTAL_JOINT"}}