acea_concert

Tools for optimizing and executing the CONCERT welding trajectory.

Pipeline

The workflow is split into two parts:

1. Offline: run weld_opt.py, optionally replay with replayer.py, and inspect the RViz visualization.
2. Online: launch simulation, start XBot GUI, publish the current gap pose, compensate gravity, home to the optimized start, drive the base to the optimized pose, and run the controller.

0. Docker Environment

Start the Docker container before running the offline or online pipeline:

xhost +local:docker
cd /home/user/concert_ws/src/acea_concert/docker
docker compose up -d --build
docker compose exec dev bash

Open another terminal in the same running container when needed:

cd /home/user/concert_ws/src/acea_concert/docker
docker compose exec dev bash

Stop the container when finished:

cd /home/user/concert_ws/src/acea_concert/docker
docker compose down

1. Offline Optimization

Command pipeline:

cd /home/user/concert_ws/src/acea_concert
python3 src/weld_opt.py
python3 src/plan_homing_from_mat.py
python3 src/replayer.py
rviz2 -d rviz/rviz_config.rviz

Run the optimizer from the package root:

cd /home/user/concert_ws/src/acea_concert
python3 src/weld_opt.py

src/weld_opt.py solves the Horizon optimization problem and saves:

mat_files/weld_concert.mat

The MAT file contains the optimized joint trajectory, the pipe/gap geometry, and the weld trajectory expressed in the gap frame. The online controller expects this file to exist before starting the simulation.

Optionally plan a collision-aware homing trajectory and save it back into the MAT file as q_homing:

cd /home/user/concert_ws/src/acea_concert
python3 src/plan_homing_from_mat.py

Optionally replay the optimized trajectory:

cd /home/user/concert_ws/src/acea_concert
python3 src/replayer.py

Use RViz to inspect the robot, pipe, and optimized weld trajectory:

rviz2 -d rviz/rviz_config.rviz

2. Online Controller

Start the standalone container with its default isolated Docker network:

docker compose -f docker/compose.yaml up -d

To communicate with ROS 2 outside the container, set the required host IP and use the ROS launcher:

ROS2_IP=<host-IP-on-the-robot-network> ROS_DOMAIN_ID=0 docker/up-ros

The external ROS 2 machine must use the same ROS_DOMAIN_ID and RMW_IMPLEMENTATION=rmw_cyclonedds_cpp.

Shut down either mode without setting ROS2_IP:

# Standalone
docker compose -f docker/compose.yaml down

# External ROS networking
docker compose \
  -f docker/compose.yaml \
  -f docker/compose.ros.yaml \
  down

Start each component in a separate terminal.

Run in this order:

1. weld_sim.launch.py
2. XBot GUI
3. gap pose GUI / publisher
4. gravity_comp_node.py
5. home_to_weld_start.py
6. drive_base_to_weld_pose.py
7. controller.py

Terminal Environment

Open a terminal and connect to the container:

docker exec -it [container-id] bash

If you have tmux, create one ready-to-launch terminal session:

cd /home/user/concert_ws/src/acea_concert
scripts/concert_tmux

The script opens one tiled tmux window with named panes, sources the ROS/XBot/workspace environment in each pane, and leaves the right command typed. Click panes and press Enter in the order listed above. If you prefer tmux tabs, run scripts/concert_tmux --windows.

For a normal terminal, source the same environment manually:

cd /home/user/concert_ws/src/acea_concert
source scripts/concert_env.bash

Simulation

cd /home/user/concert_ws
ros2 launch acea_concert weld_sim.launch.py

This launches the robot simulation and spawns the two pipe halves using the geometry stored in mat_files/weld_concert.mat.

To use a different optimization result, pass mat_file. To start with the gap at the optimized pose relative to the robot, add optimized_robot_pose:=true:

ros2 launch acea_concert weld_sim.launch.py mat_file:=mat_files/weld_concert.mat optimized_robot_pose:=true

XBot GUI

Start the XBot GUI after the simulation is running.

Gap Pose GUI / Ground Truth

cd /home/user/concert_ws/src/acea_concert
python3 src/gap_pose_publisher.py

Optional test faults can be added to the published /gap/pose_robot:

python3 src/gap_pose_publisher.py \
  --gap-pos-noise-std 0.01 \
  --gap-rot-noise-std 0.02 \
  --gap-disconnect-every 10 \
  --gap-disconnect-duration 2

This publishes the current gap pose with respect to base_link:

/gap/pose_robot

This node is ground truth for simulation. In the real setup, it should be replaced by perception.

Gravity Compensation

cd /home/user/concert_ws/src/acea_concert
python3 src/gravity_comp_node.py

Home Weld Joints To Optimized Start

cd /home/user/concert_ws/src/acea_concert
python3 src/home_to_weld_start.py

Drive Base To Optimized Weld Pose

cd /home/user/concert_ws/src/acea_concert
python3 src/drive_base_to_weld_pose.py

Welding Controller

cd /home/user/concert_ws/src/acea_concert
python3 src/controller.py

To smooth noisy camera gap poses, enable the controller-side low-pass filter:

python3 src/controller.py --gap-filter-tau 0.1

With scripts/concert_env.bash sourced, the same command is:

concert_controller --gap-filter-tau 0.1

0 disables the filter. A small value like 0.05-0.15 seconds is a good starting range.

To replay the optimized trajectory without /gap/pose_robot, run open-loop:

concert_controller --open-loop

For slow welding with noisy camera poses, use a history window and reject big camera jumps:

concert_controller \
  --gap-filter-history-size 50 \
  --gap-filter-tau 0.3 \
  --gap-filter-max-position-jump 0.02 \
  --gap-filter-max-angle-jump 10

The controller uses:

optimized posture trajectory from the MAT file
desired weld position in the gap frame
desired weld orientation in the gap frame
measured /gap/pose_robot from ground truth/perception

/gap/pose_robot is the only online gap input required by the controller. It is a PoseStamped in base_link: the position gives base_p_gap, and the quaternion gives base_R_gap.

At runtime it relocates the optimized weld trajectory onto the current measured gap:

position:    base_p_ee_des = base_p_gap + base_R_gap * gap_p_ee_des
orientation: base_R_ee_des = base_R_gap * gap_R_ee_des

Controller Pipeline

The controller does not blindly replay the optimized joint trajectory. The joint trajectory is used as a nominal posture, while the welding target is rebuilt online from the measured gap pose.

flowchart TD
    subgraph offline["offline optimization"]
        mat["weld_concert.mat"]
        qdes["q_des(t)<br/>optimal posture"]
        weldgap["gap_p_ee_des(t)<br/>gap_R_ee_des(t)<br/>weld target in gap frame"]
        mat --> qdes
        mat --> weldgap
    end

    subgraph online["online measurements"]
        perception["/gap/pose_robot<br/>ground truth now<br/>perception later"]
        gappose["base_p_gap<br/>base_R_gap<br/>derived from pose"]
        feedback["robot feedback<br/>q_meas<br/>base_p_ee_meas<br/>base_R_ee_meas"]
        perception --> gappose
    end

    subgraph controller["controller.py"]
        relocate["relocate weld target<br/>base_p_ee_des = base_p_gap + base_R_gap * gap_p_ee_des<br/>base_R_ee_des = base_R_gap * gap_R_ee_des"]
        project["project error in gap frame<br/>normal direction<br/>tangent direction"]
        pd["PD correction<br/>normal velocity<br/>tangent velocity"]
        ik["CartesIO IK<br/>EE pose task<br/>postural task"]
    end

    output["joint position command"]
    diag["diagnostics<br/>PlotJuggler"]

    qdes --> ik
    weldgap --> relocate
    gappose --> relocate
    gappose --> project
    feedback --> project
    relocate --> project
    project --> pd
    pd --> ik
    relocate --> ik
    ik --> output
    output --> feedback
    project --> diag
    pd --> diag

Loading

Controller Inputs

From the MAT file:

q_des(t)              optimized joint posture trajectory
gap_p_ee_des(t)       desired weld position expressed in the gap frame
gap_R_ee_des(t)       desired tool orientation expressed in the gap frame

From ground truth now, and from perception later:

/gap/pose_robot       PoseStamped in base_link
base_p_gap            gap origin from /gap/pose_robot.position
base_R_gap            gap orientation from /gap/pose_robot.orientation

From robot feedback:

q_meas                measured robot joint state
base_p_ee_meas        measured EE position from forward kinematics
base_R_ee_meas        measured EE orientation from forward kinematics

Control Loop

At each controller tick:

1. Read q_des(t) and send it to the CartesIO postural task.
2. Read the desired weld position and orientation in the gap frame.
3. Read the current measured /gap/pose_robot in base_link.
4. Derive base_p_gap and base_R_gap, then convert the desired weld target from gap frame to base_link.
5. Compare the measured EE position with the desired weld target along:
   • the gap normal direction, across the pipes
   • the gap tangent direction, along the weld
6. Compute PD correction velocities along those two directions.
7. Command the EE pose task and postural task through CartesIO.
8. Publish diagnostics for PlotJuggler.

The key idea is that if the robot or gap moves, the weld target moves with the measured gap:

base_p_ee_des = base_p_gap + base_R_gap * gap_p_ee_des
base_R_ee_des = base_R_gap * gap_R_ee_des

Main Files

• src/weld_opt.py: offline trajectory optimization and MAT file generation.
• src/plan_homing_from_mat.py: adds a collision-aware q_homing trajectory to a MAT file.
• src/replayer.py: replay and RViz visualization of the optimized trajectory.
• launch/weld_sim.launch.py: simulation launch file; accepts mat_file and optimized_start.
• src/gap_pose_publisher.py: simulation ground-truth gap pose publisher.
• src/gravity_comp_node.py: gravity compensation node.
• src/home_to_weld_start.py: moves weld joints to trajectory node 0.
• src/drive_base_to_weld_pose.py: drives the mobile base to the optimized relative gap pose.
• src/controller.py: online welding controller.
• src/controller_ros.py: ROS interface for the controller.
• src/utils/: controller geometry, trajectory, and diagnostic helpers.

Configuration

• config/weld.yaml: Horizon optimization task configuration.
• config/cartesio_stack.yaml: CartesIO stack used by the online controller.
• src/modular/: robot model generation.
• mat_files/: generated optimization results.

Dependencies

• ROS 2 Jazzy
• Horizon
• CasADi
• XBot2
• CartesIO
• NumPy, SciPy, Matplotlib