WholeBodyController

WBC Simulation

Steps

Start the simulation
Do not start any other controller

Start WBC

roslaunch talos_wbc talos_wbc.launch

Add Interactive Markers (RViz + MoveIt)

roslaunch talos_wbc interactive_markers.launch

Torque Control

Steps 1. Start the simulation
2. Start the default controller
3. Kill the default controller
4. Start the torque controller

Launch

roslaunch talos_pal_locomotion talos_dcm_walking_controller.launch

Parameter Notes - With PAL simulator + noise:

roslaunch talos_pal_locomotion talos_dcm_walking_controller.launch simulation:=true

- To control only the lower body:

roslaunch talos_pal_locomotion talos_dcm_walking_controller.launch robot:=full_v2_fixed_upper_body

WBC Config: `talos_wbc` vs `talos_wbc_subsampled`

The upstream talos_wbc.launch defaults to talos_wbc_subsampled as its stack config. The ias_grasping launch file uses talos_wbc instead. Key differences:

Parameter	`talos_wbc`	`talos_wbc_subsampled`
Controller type	`WholeBodyControlKinematicController`	`WholeBodyControlKinematicSubsampledController`
Control rate	single loop at 200 Hz (`dt: 0.005`)	inner loop 2 kHz (`dt: 0.0005`) / outer 200 Hz (`thread_dt: 0.005`)
Stabilizer	`use_stabilizer: true`	`use_stabilizer: false`
Home posture	not set	`use_home_posture: true`
Scheduler priority	not set	`sched_priority: 40`
Force/torque sensors	ankle + wrist FT links defined	not defined

Practical implications:

The subsampled controller is PAL's recommended default for the real robot — the higher inner-loop rate improves numerical stability.
talos_wbc enables the stabilizer, which uses ankle/wrist FT sensor readings for balance. This requires the FT sensors to be operational.
use_home_posture: true (subsampled only) adds a posture regularization task that keeps the robot close to a neutral pose. Without it the WBC has no secondary objective to stay upright, which can contribute to instability during startup.

Overview

What is PAL WBC?

PAL’s implementation of Stack of Tasks
Runs a hierarchical quadratic solver at 100 Hz
Can control all robot joints
Always active with highest priority:
Joint limit avoidance
Foot constraints (humanoids)
COM stabilizer (humanoids)
Self-collision avoidance

Difference to IK solvers:
WBC solves online while automatically avoiding collisions and respecting joint limits.

Main Packages

wbc_tasks → all implemented tasks
pal_wbc_tutorials → tutorials & examples
talos_wbc / tiago_wbc / reemc_wbc → launch files & default stacks
smach_c_wbc_states → WBC tasks as smach state machine blocks

WBC Tasks (documented & maintained)

Task	Description	Property Bag
GazePointKinematicMetaTask	Look at a 3D point	✅
GoToPositionMetaTask	Send link to a position	✅
GoToOrientationMetaTask	Send link to an orientation	✅
GoToPoseMetaTask	Send link to full pose	✅
GoToLocalVirtualAdmitance*	Pose with admittance control	✅
GoToPointRayAngleGazeKinematicMetatask	Gaze (avoids singularity)	✅
GoToRelative*	Constrain link relative to another	✅
ReferenceKinematicTask	Target configuration for joints	✅
ConstraintFIXCOMMetaTask	Fix COM position	❌
GoToCOMMetaTask / XY / HeightZ	Move or control COM	❌
SelfCollisionSafety / SelfCollision	Prevent self-collision	❌
JointPositionLimitTask	Enforce joint limits	❌
COMStabilizerKinematicTask	Stabilize COM (ZMP control)	❌

⚠️ Never remove safety tasks (self-collision, joint limits, foot constraints).

How to Use Tasks

Two ways to use tasks:

Custom Stack at Startup
Define tasks in a YAML config (e.g. talos_stack_both_hands_head.yaml)
Load via launch file
Dynamic Push
Implement configureConstraintsFromPropertyBag
Export as plugin (pluginlib)
Push tasks via:
- WBCServiceHelper (recommended)
- rosservice call (manual, error-prone)

WBC Service Helper

Utility class to: - Push / remove tasks
- Get task error
- Print stack description
- Wait for convergence

Example Interactive Tasks

Push Gaze Task → look at a point (GoToPointRayAngleGazeKinematicMetatask)
Push Pose Task → control link position + orientation
Push Relative Task → one link moves relative to another
Push Torso Height Task → fix torso at a given height
Push Reference Task → control joints via topic

Key Notes

Use Property Bags for dynamic task configuration
Only tasks exported as plugins can be pushed online
Prefer WBCServiceHelper over raw rosservice
Do not remove safety-critical tasks

Action Servers for Gaze and Pose Tasks in the WBC Stack

This repository provides ROS ActionServers for controlling the PAL Whole Body Control (WBC) stack of the TALOS robot.
They provide high-level control of arms and gaze direction using ROS Actions.
Each ActionServer pushes new tasks into the WBC stack, removes old ones, and monitors task errors.

Code Repository:
kinematicwbc_arms_control

How WBC Stack Integration Works

Both ActionServers follow the same pattern: 1. Create a PropertyBag with task parameters
2. Push the task into WBC via WBCServiceHelper
3. Remove previous tasks (popTask)
4. Monitor convergence with /get_task_error
5. Publish feedback (task error) until success

1. GoToPoseActionServer (WBC)

Purpose
Control left or right arm using a GoToPoseMetaTask.

Implementation
- Each goal generates a unique task_id (go_to_pose_action_task_left_0, …)
- PropertyBag fields: - taskType = pal_wbc/GoToPoseMetaTask
- tip_name = gripper_<arm>_base_link
- damping, p_pos_gain, p_orient_gain
- target_pose (from ActionGoal)
- Push task into WBC:

srv_helper_->pushTask(task, task_id, pal_wbc_msgs::Order::After, respect_task_id_);

- Remove old tasks with popTask(active_task_id_)
- Monitor /get_task_error → publish as feedback
- Success when error < tolerance (0.01 default)
- Provides services (/set_or_get_params_left/right) for runtime parameter tuning

Action Names
- /go_to_pose_left
- /go_to_pose_right

2. GazeTaskActionServer (WBC)

Purpose
Control robot head to look at a 3D point using GazePointKinematicMetaTask.

Implementation
- Each goal creates unique task_id (gaze_task_action_0, …)
- PropertyBag fields: - taskType = pal_wbc/GazePointKinematicMetaTask
- camera_frame = rgbd_rgb_optical_frame
- reference_type = pointer
- target_position (from ActionGoal)
- Push task into WBC:

srv_helper_->pushTask(gaze_task, task_id, pal_wbc_msgs::Order::After, respect_task_id_);

- Remove old gaze task with popTask(active_task_id_)
- Monitor /get_task_error → publish as feedback
- Success when error < 0.01

Action Name
- /gaze_at_point

Summary

GoToPoseActionServer → controls arm poses via WBC
GazeTaskActionServer → controls head gaze via WBC

Both follow the same WBC pattern:
- Build PropertyBag
- Push task
- Remove old task
- Monitor task error
- Publish feedback & result

This provides a clean, action-based API for safe, high-level control of TALOS arms and gaze with the PAL Whole Body Control framework.

Step	Action	Description
1️⃣ Configure	Build PropertyBag	The ActionServer prepares a `PropertyBag` with task parameters (taskType, tip_name, gains, target pose/point).
2️⃣ Push	pushTask()	The new task is pushed into the WBC stack via `WBCServiceHelper`. If successful, the task becomes active.
3️⃣ Remove	popTask()	If an old task of the same type is still active, it is removed to avoid conflicts.
4️⃣ Monitor	get_task_error() loop	At ~10 Hz, the ActionServer queries the task error, publishes feedback, and checks convergence.
5️⃣ Result	Action result	- If error < tolerance → `setSucceeded()` - If preempted → `setPreempted()` - If push failed → `setAborted()`

⚠️ Warning:
Always push the new task before removing the old one.
If you remove first, the robot may lose its reference and create discontinuities in the stack, leading to jerky and abrupt motions.

Code Examples

1. Configure and Push a New Pose Task

// Unique task id
std::stringstream ss;
ss << "go_to_pose_action_task_" << arm_ << "_" << task_counter_++;
std::string task_id = ss.str();

// Build PropertyBag
property_bag::PropertyBag task;
task.addProperty("taskType", std::string("pal_wbc/GoToPoseMetaTask"));
task.addProperty("task_id", task_id);
task.addProperty("tip_name", tip_name_);
task.addProperty("damping", damping_);
task.addProperty("p_pos_gain", p_gain_);
task.addProperty("p_orient_gain", p_orient_gain_);
task.addProperty("reference_frame", goal->target_pose.header.frame_id);
task.addProperty("source_data", std::string("pointer"));
task.addProperty("target_pose", goal->target_pose);

// Push into stack
srv_helper_->pushTask(task, task_id, pal_wbc_msgs::Order::After, respect_task_id_);

2. Remove the Previous Task

if (!active_task_id_.empty()) {
  popTask(active_task_id_);
}
active_task_id_ = task_id;

3. Monitor Task Error and Publish Feedback

double error = getTaskError(task_id);
ias_grasping::GoToPoseFeedback feedback;
feedback.task_error = error;
as_.publishFeedback(feedback);

if (error >= 0.0 && error < tolerance) {
  // task converged
  ias_grasping::GoToPoseResult result;
  result.success = true;
  result.message = "Target reached (task remains active)";
  as_.setSucceeded(result);
}