ROS 2 Interface for the Whole-Body Controller specific to bipedal walking. More...

#include <biped_controller.hpp>

Inheritance diagram for wbc_ros::BipedController:

Public Member Functions
	BipedController (const rclcpp::NodeOptions &options)

	~BipedController ()

virtual CallbackReturn	on_configure (const rclcpp_lifecycle::State &previous_state) override

virtual CallbackReturn	on_activate (const rclcpp_lifecycle::State &previous_state) override

virtual CallbackReturn	on_deactivate (const rclcpp_lifecycle::State &previous_state) override

virtual CallbackReturn	on_cleanup (const rclcpp_lifecycle::State &previous_state) override

virtual CallbackReturn	on_error (const rclcpp_lifecycle::State &previous_state) override

virtual CallbackReturn	on_shutdown (const rclcpp_lifecycle::State &previous_state) override

void	updateController ()

void	handleContacts (const ContactsMsgPtr msg)

void	joint_weight_callback (const DoubleArrayMsgPtr msg)

void	robot_state_callback (const RobotStateMsgPtr msg)

void	joint_state_callback (const JointStateMsgPtr msg)

void	contacts_callback (const ContactsMsgPtr msg)

Protected Attributes
RbsTaskInterfacePtr	body_task_iface

RbsTaskInterfacePtr	foot_l_iface

RbsTaskInterfacePtr	foot_r_iface

JointPositionTaskInterfacePtr	joint_pos_iface

std::vector< TaskInterfacePtr >	task_interfaces

wbc::ContactForceTaskPtr	force_l_task

wbc::ContactForceTaskPtr	force_r_task

wbc::ScenePtr	scene

wbc::RobotModelPtr	robot_model

wbc::QPSolverPtr	solver

wbc::types::JointState	joint_state

wbc::types::RigidBodyState	floating_base_state

std::vector< wbc::types::Contact >	contacts

wbc::types::Wrench	contact_force_l

wbc::types::Wrench	contact_force_r

bool	has_robot_state

bool	has_joint_state

int	update_rate

wbc::types::JointCommand	solver_output

wbc::HierarchicalQP	qp

wbc::JointIntegrator	joint_integrator

bool	integrate_from_current_state

std::vector< double >	p_gain_stance

std::vector< double >	d_gain_stance

std::vector< double >	p_gain_swing

std::vector< double >	d_gain_swing

std::vector< double >	p_gain

std::vector< double >	d_gain

JointCommandMsg	solver_output_msg

JointCommandPublisher::SharedPtr	solver_output_publisher

std::unique_ptr< RTJointCommandPublisher >	rt_solver_output_publisher

TimingStatsMsg	timing_stats

TimingStatsPublisher::SharedPtr	timing_stats_publisher

std::unique_ptr< RTTimingStatsPublisher >	rt_timing_stats_publisher

DoubleArraySubscription	joint_weight_subscriber

RTDoubleArrayBuffer	rt_joint_weight_buffer

DoubleArrayMsgPtr	joint_weight_msg

RobotStateSubscription	robot_state_subscriber

RTRobotStateBuffer	rt_robot_state_buffer

RobotStateMsgPtr	robot_state_msg

ContactsSubscription	contacts_subscriber

RTContactsBuffer	rt_contacts_buffer

ContactsMsgPtr	contacts_msg

JointStateSubscription	joint_state_subscriber

RTJointStateBuffer	rt_joint_state_buffer

JointStateMsgPtr	joint_state_msg

rclcpp::Time	stamp

rclcpp::TimerBase::SharedPtr	timer

std::shared_ptr< biped_controller::ParamListener >	param_listener

biped_controller::Params	params

std::vector< int >	joint_idx_map

std::vector< int >	joint_idx_map_left_leg

std::vector< int >	joint_idx_map_right_leg

Detailed Description

ROS 2 Interface for the Whole-Body Controller specific to bipedal walking.

Published Topics:

~/solver_output (robot_control_msgs/msg/JointCommand) - The QP solution (position, velocity, torque)
~/timing_stats (wbc_msgs/msg/TimingStats) - Debug message showing the computation time

Subscribed Topics:

~/body_pose/setpoint (robot_control_msgs/msg/RigidBodyState) - Setpoint for the base of the robot (pose, twist, spatial acceleration)
~/foot_l_pose/setpoint (robot_control_msgs/msg/RigidBodyState) - Setpoint for the left foot (position, linear velocity, linear acceleration)
~/foot_r_pose/setpoint (robot_control_msgs/msg/RigidBodyState) - Setpoint for the right foot (position, linear velocity, linear acceleration)
~/joint_position/setpoint (robot_control_msgs/msg/JointCommand) - Setpoint for all joints (position, velocity, acceleration)
~/robot_state (robot_control_msgs/msg/RobotState) - Joint and floating base state of the robot
~/joint_state (robot_control_msgs/msg/JointState) - Joint state of the robot. Can be used alternatively, if there is no floating base
~/contacts (robot_control_msgs/msg/Contacts) - Planned contacts (0/1) and contact expected contact wrenches
~/joint_weights (std_msgs/msg/Float64MultiArray) - Joint weights to control contribution of each individual joint

Parameters:

See src/biped_controller_parameters.yaml

This Controller implements full or reduced TSID (https://github.com/stack-of-tasks/tsid), depending on the scene.type parameter. It gets the feet position, velocities and acceleration, the CoM pose and twist, as well as the feet contact forces as input, and produces a joint position, velocity, acceleration and torque as output. The reduced TSID sets up and solves the following QP:

\[ \begin{array}{ccc} \underset{\mathbf{\ddot{q}},\mathbf{f}}{\text{minimize}} & \| \underbrace{\left(\mathbf{J}_{com}\ddot{\mathbf{q}} - \dot{\mathbf{v}}_{d,com} + \dot{\mathbf{J}}_{com}\dot{\mathbf{q}}\right)}_{\text{Center of mass}} + \underbrace{\sum_i \left(\mathbf{J}_i\ddot{\mathbf{q}} - \dot{\mathbf{v}}_{d,i} + \dot{\mathbf{J}}_i\dot{\mathbf{q}}\right)}_{\text{Feet positions}} + \underbrace{\sum_i\left(\mathbf{f}_{d,i} - \mathbf{f}\right)}_{\text{Feet contact forces}}\|_2 \\ & & \\ s.t. & \mathbf{H}\mathbf{\ddot{q}} + \mathbf{h} = \mathbf{S}\mathbf{\tau} + \mathbf{J}_{c,i}^T\mathbf{f} \quad \forall i & \text{(Equations of motion)}\\ & \mathbf{J}_{c,i}\mathbf{\ddot{q}} = -\dot{\mathbf{J}}_{c,i}\dot{q} \quad \forall i & \text{(Rigid Contacts)}\\ & \mathbf{e}^T_x \mathbf{f}_i ≤ \mu\mathbf{e}_z \mathbf{f}_i, \quad \forall i & \text{(Friction cone)} \\ & \mathbf{\tau}_m \leq \mathbf{\tau} \leq \mathbf{\tau}_M & \text{(Torque limits)}\\ \end{array} \]

and computes

\[ \tau = \mathbf{H}\ddot{\mathbf{q}} + \mathbf{h} - \sum_i \mathbf{J}_{c,i}^T\mathbf{f} \]

where

\[ \dot{\mathbf{v}}_{d,i} = \dot{\mathbf{v}}_{r,i} + \mathbf{K}_d(\mathbf{v}_{r,i}-\mathbf{v}_i) + \mathbf{K}_p(\mathbf{x}_{r,i}-\mathbf{x}_i) \]

with

\[ \begin{array}{ccc} i & - & \text{Foot index} \\ n & - & \text{No. of robot joints} \\ \mathbf{J}_i \in \mathbb{R}^{3 \times (n+6)} & - & \text{i-th Foot Jacobian} \\ \mathbf{q},\dot{\mathbf{q}},\ddot{\mathbf{q}} \in \mathbb{R}^{n+6}& - & \text{Joint position, velocity, acceleration} \\ \mathbf{\tau} \in \mathbb{R}^{n} & - & \text{Joint torques} \\ \mathbf{f}_i \in \mathbb{R}^{3} & - & \text{Contact force of the i-th foot} \\ \mathbf{f}_{d,i} \in \mathbb{R}^{3} & - & \text{Desired contact force of the i-th foot} \\ \mathbf{H} \in \mathbb{R}^{(n+6) \times (n+6)} & - & \text{Joint space mass-inertia matrix} \\ \mathbf{J}_c \in \mathbb{R}^{3 \times (n+6)} & - & \text{Contact Jacobian} \\ \mathbf{S} \in \mathbb{R}^{(n+6) \times (n)}& - & \text{Actuator selection matrix} \\ \mathbf{h} \in \mathbb{R}^{n+6}& - & \text{Bias forces/torques} \\ \mathbf{e} \in \mathbb{R}^3 &- &\text{Unit vector}\\ \mu \in \mathbb{R} &- &\text{Contact friction coefficient}\\ \mathbf{\tau}_m,\mathbf{\tau}_M \in \mathbb{R}^{n}& - & \text{Min./Max. joint torques} \\ \mathbf{x}_i,\mathbf{v}_i & - & \text{Position, velocity of the feet} \\ \mathbf{x}_{r,i},\mathbf{v}_{r,i},\dot{\mathbf{v}}_{r,i} & - & \text{Desired position, velocity, acceleration of the feet} \\ \end{array} \]

Note:

The QP considers the floating base of the robot, so most of the quantities include the virtual joints of the floating base (dimension n+6)
The constraints above are simplified. Additionally, there are terms considering the joint position/velocity/acceleration limits

Constructor & Destructor Documentation

◆ BipedController()

wbc_ros::BipedController::BipedController ( const rclcpp::NodeOptions & options )

◆ ~BipedController()

wbc_ros::BipedController::~BipedController ( )

inline

Member Function Documentation

◆ contacts_callback()

void wbc_ros::BipedController::contacts_callback ( const ContactsMsgPtr msg )

◆ handleContacts()

void wbc_ros::BipedController::handleContacts ( const ContactsMsgPtr msg )

◆ joint_state_callback()

void wbc_ros::BipedController::joint_state_callback ( const JointStateMsgPtr msg )

◆ joint_weight_callback()

void wbc_ros::BipedController::joint_weight_callback ( const DoubleArrayMsgPtr msg )

◆ on_activate()

rclcpp_lifecycle::LifecycleNode::CallbackReturn wbc_ros::BipedController::on_activate ( const rclcpp_lifecycle::State & previous_state )

overridevirtual

◆ on_cleanup()

rclcpp_lifecycle::LifecycleNode::CallbackReturn wbc_ros::BipedController::on_cleanup ( const rclcpp_lifecycle::State & previous_state )

overridevirtual

◆ on_configure()

rclcpp_lifecycle::LifecycleNode::CallbackReturn wbc_ros::BipedController::on_configure ( const rclcpp_lifecycle::State & previous_state )

overridevirtual

◆ on_deactivate()

rclcpp_lifecycle::LifecycleNode::CallbackReturn wbc_ros::BipedController::on_deactivate ( const rclcpp_lifecycle::State & previous_state )

overridevirtual

◆ on_error()

rclcpp_lifecycle::LifecycleNode::CallbackReturn wbc_ros::BipedController::on_error ( const rclcpp_lifecycle::State & previous_state )

overridevirtual

◆ on_shutdown()

rclcpp_lifecycle::LifecycleNode::CallbackReturn wbc_ros::BipedController::on_shutdown ( const rclcpp_lifecycle::State & previous_state )

overridevirtual

◆ robot_state_callback()

void wbc_ros::BipedController::robot_state_callback ( const RobotStateMsgPtr msg )

◆ updateController()

void wbc_ros::BipedController::updateController ( )

Member Data Documentation

◆ body_task_iface

RbsTaskInterfacePtr wbc_ros::BipedController::body_task_iface

protected

◆ contact_force_l

wbc::types::Wrench wbc_ros::BipedController::contact_force_l

protected

◆ contact_force_r

wbc::types::Wrench wbc_ros::BipedController::contact_force_r

protected

◆ contacts

std::vector<wbc::types::Contact> wbc_ros::BipedController::contacts

protected

◆ contacts_msg

ContactsMsgPtr wbc_ros::BipedController::contacts_msg

protected

◆ contacts_subscriber

ContactsSubscription wbc_ros::BipedController::contacts_subscriber

protected

◆ d_gain

std::vector<double> wbc_ros::BipedController::d_gain

protected

◆ d_gain_stance

std::vector<double> wbc_ros::BipedController::d_gain_stance

protected

◆ d_gain_swing

std::vector<double> wbc_ros::BipedController::d_gain_swing

protected

◆ floating_base_state

wbc::types::RigidBodyState wbc_ros::BipedController::floating_base_state

protected

◆ foot_l_iface

RbsTaskInterfacePtr wbc_ros::BipedController::foot_l_iface

protected

◆ foot_r_iface

RbsTaskInterfacePtr wbc_ros::BipedController::foot_r_iface

protected

◆ force_l_task

wbc::ContactForceTaskPtr wbc_ros::BipedController::force_l_task

protected

◆ force_r_task

wbc::ContactForceTaskPtr wbc_ros::BipedController::force_r_task

protected

◆ has_joint_state

bool wbc_ros::BipedController::has_joint_state

protected

◆ has_robot_state

bool wbc_ros::BipedController::has_robot_state

protected

◆ integrate_from_current_state

bool wbc_ros::BipedController::integrate_from_current_state

protected

◆ joint_idx_map

std::vector<int> wbc_ros::BipedController::joint_idx_map

protected

◆ joint_idx_map_left_leg

std::vector<int> wbc_ros::BipedController::joint_idx_map_left_leg

protected

◆ joint_idx_map_right_leg

std::vector<int> wbc_ros::BipedController::joint_idx_map_right_leg

protected

◆ joint_integrator

wbc::JointIntegrator wbc_ros::BipedController::joint_integrator

protected

◆ joint_pos_iface

JointPositionTaskInterfacePtr wbc_ros::BipedController::joint_pos_iface

protected

◆ joint_state

wbc::types::JointState wbc_ros::BipedController::joint_state

protected

◆ joint_state_msg

JointStateMsgPtr wbc_ros::BipedController::joint_state_msg

protected

◆ joint_state_subscriber

JointStateSubscription wbc_ros::BipedController::joint_state_subscriber

protected

◆ joint_weight_msg

DoubleArrayMsgPtr wbc_ros::BipedController::joint_weight_msg

protected

◆ joint_weight_subscriber

DoubleArraySubscription wbc_ros::BipedController::joint_weight_subscriber

protected

◆ p_gain

std::vector<double> wbc_ros::BipedController::p_gain

protected

◆ p_gain_stance

std::vector<double> wbc_ros::BipedController::p_gain_stance

protected

◆ p_gain_swing

std::vector<double> wbc_ros::BipedController::p_gain_swing

protected

◆ param_listener

std::shared_ptr<biped_controller::ParamListener> wbc_ros::BipedController::param_listener

protected

◆ params

biped_controller::Params wbc_ros::BipedController::params

protected

◆ qp

wbc::HierarchicalQP wbc_ros::BipedController::qp

protected

◆ robot_model

wbc::RobotModelPtr wbc_ros::BipedController::robot_model

protected

◆ robot_state_msg

RobotStateMsgPtr wbc_ros::BipedController::robot_state_msg

protected

◆ robot_state_subscriber

RobotStateSubscription wbc_ros::BipedController::robot_state_subscriber

protected

◆ rt_contacts_buffer

RTContactsBuffer wbc_ros::BipedController::rt_contacts_buffer

protected

◆ rt_joint_state_buffer

RTJointStateBuffer wbc_ros::BipedController::rt_joint_state_buffer

protected

◆ rt_joint_weight_buffer

RTDoubleArrayBuffer wbc_ros::BipedController::rt_joint_weight_buffer

protected

◆ rt_robot_state_buffer

RTRobotStateBuffer wbc_ros::BipedController::rt_robot_state_buffer

protected

◆ rt_solver_output_publisher

std::unique_ptr<RTJointCommandPublisher> wbc_ros::BipedController::rt_solver_output_publisher

protected

◆ rt_timing_stats_publisher

std::unique_ptr<RTTimingStatsPublisher> wbc_ros::BipedController::rt_timing_stats_publisher

protected

◆ scene

wbc::ScenePtr wbc_ros::BipedController::scene

protected

◆ solver

wbc::QPSolverPtr wbc_ros::BipedController::solver

protected

◆ solver_output

wbc::types::JointCommand wbc_ros::BipedController::solver_output

protected

◆ solver_output_msg

JointCommandMsg wbc_ros::BipedController::solver_output_msg

protected

◆ solver_output_publisher

JointCommandPublisher::SharedPtr wbc_ros::BipedController::solver_output_publisher

protected

◆ stamp

rclcpp::Time wbc_ros::BipedController::stamp

protected

◆ task_interfaces

std::vector<TaskInterfacePtr> wbc_ros::BipedController::task_interfaces

protected

◆ timer

rclcpp::TimerBase::SharedPtr wbc_ros::BipedController::timer

protected

◆ timing_stats

TimingStatsMsg wbc_ros::BipedController::timing_stats

protected

◆ timing_stats_publisher

TimingStatsPublisher::SharedPtr wbc_ros::BipedController::timing_stats_publisher

protected

◆ update_rate

int wbc_ros::BipedController::update_rate

protected

The documentation for this class was generated from the following files:

include/wbc_ros/biped_controller.hpp
src/biped_controller.cpp

Public Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

◆ BipedController()

◆ ~BipedController()

Member Function Documentation

◆ contacts_callback()

◆ handleContacts()

◆ joint_state_callback()

◆ joint_weight_callback()

◆ on_activate()

◆ on_cleanup()

◆ on_configure()

◆ on_deactivate()

◆ on_error()

◆ on_shutdown()

◆ robot_state_callback()

◆ updateController()

Member Data Documentation

◆ body_task_iface

◆ contact_force_l

◆ contact_force_r

◆ contacts

◆ contacts_msg

◆ contacts_subscriber

◆ d_gain

◆ d_gain_stance

◆ d_gain_swing

◆ floating_base_state

◆ foot_l_iface

◆ foot_r_iface

◆ force_l_task

◆ force_r_task

◆ has_joint_state

◆ has_robot_state

◆ integrate_from_current_state

◆ joint_idx_map

◆ joint_idx_map_left_leg

◆ joint_idx_map_right_leg

◆ joint_integrator

◆ joint_pos_iface

◆ joint_state

◆ joint_state_msg

◆ joint_state_subscriber

◆ joint_weight_msg

◆ joint_weight_subscriber

◆ p_gain

◆ p_gain_stance

◆ p_gain_swing

◆ param_listener

◆ params

◆ qp

◆ robot_model

◆ robot_state_msg

◆ robot_state_subscriber

◆ rt_contacts_buffer

◆ rt_joint_state_buffer

◆ rt_joint_weight_buffer

◆ rt_robot_state_buffer

◆ rt_solver_output_publisher

◆ rt_timing_stats_publisher

◆ scene

◆ solver

◆ solver_output

◆ solver_output_msg

◆ solver_output_publisher

◆ stamp

◆ task_interfaces

◆ timer

◆ timing_stats

◆ timing_stats_publisher

◆ update_rate