#include <balance.h>

Inheritance diagram for rm_chassis_controllers::BalanceController:

[legend]

Public Member Functions
	BalanceController ()=default

bool	init (hardware_interface::RobotHW *robot_hw, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh) override

Public Member Functions inherited from rm_chassis_controllers::ChassisBase< rm_control::RobotStateInterface, hardware_interface::ImuSensorInterface, hardware_interface::EffortJointInterface >
	ChassisBase ()=default

bool	init (hardware_interface::RobotHW *robot_hw, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh) override
	Get and check params for covariances. Setup odometry realtime publisher + odom message constant fields. init odom tf. More...

void	update (const ros::Time &time, const ros::Duration &period) override
	Receive real_time command from manual. Execute different action according to current mode. Set necessary params of chassis. Execute power limit. More...

Public Member Functions inherited from controller_interface::MultiInterfaceController< T... >
virtual bool	init (hardware_interface::RobotHW *, ros::NodeHandle &)

	MultiInterfaceController (bool allow_optional_interfaces=false)

Public Member Functions inherited from controller_interface::ControllerBase
virtual void	aborting (const ros::Time &)

virtual void	aborting (const ros::Time &)

bool	abortRequest (const ros::Time &time)

bool	abortRequest (const ros::Time &time)

	ControllerBase ()=default

	ControllerBase (const ControllerBase &)=delete

	ControllerBase (ControllerBase &&)=delete

bool	isAborted () const

bool	isAborted () const

bool	isInitialized () const

bool	isInitialized () const

bool	isRunning () const

bool	isRunning () const

bool	isStopped () const

bool	isStopped () const

bool	isWaiting () const

bool	isWaiting () const

ControllerBase &	operator= (const ControllerBase &)=delete

ControllerBase &	operator= (ControllerBase &&)=delete

virtual void	starting (const ros::Time &)

virtual void	starting (const ros::Time &)

bool	startRequest (const ros::Time &time)

bool	startRequest (const ros::Time &time)

virtual void	stopping (const ros::Time &)

virtual void	stopping (const ros::Time &)

bool	stopRequest (const ros::Time &time)

bool	stopRequest (const ros::Time &time)

void	updateRequest (const ros::Time &time, const ros::Duration &period)

void	updateRequest (const ros::Time &time, const ros::Duration &period)

virtual void	waiting (const ros::Time &)

virtual void	waiting (const ros::Time &)

bool	waitRequest (const ros::Time &time)

bool	waitRequest (const ros::Time &time)

virtual	~ControllerBase ()=default

Private Types
enum	BalanceMode { NORMAL, BLOCK }

typedef std::shared_ptr< realtime_tools::RealtimePublisher< rm_msgs::BalanceState > >	RtpublisherPtr

Private Member Functions
void	block (const ros::Time &time, const ros::Duration &period)

void	moveJoint (const ros::Time &time, const ros::Duration &period) override

void	normal (const ros::Time &time, const ros::Duration &period)

geometry_msgs::Twist	odometry () override

Private Attributes
Eigen::Matrix< double, STATE_DIM, STATE_DIM >	a_ {}

geometry_msgs::Vector3	angular_vel_base_

double	anti_block_effort_

Eigen::Matrix< double, STATE_DIM, CONTROL_DIM >	b_ {}

int	balance_mode_

bool	balance_state_changed_ = false

double	block_angle_

double	block_duration_

double	block_effort_

double	block_overtime_

ros::Time	block_time_

double	block_velocity_

hardware_interface::ImuSensorHandle	imu_handle_

Eigen::Matrix< double, CONTROL_DIM, STATE_DIM >	k_ {}

ros::Time	last_block_time_

hardware_interface::JointHandle	left_momentum_block_joint_handle_

hardware_interface::JointHandle	left_wheel_joint_handle_

bool	maybe_block_ = false

double	pitch_

double	position_clear_threshold_ = 0.

double	position_des_ = 0

double	position_offset_ = 0.

Eigen::Matrix< double, STATE_DIM, STATE_DIM >	q_ {}

Eigen::Matrix< double, CONTROL_DIM, CONTROL_DIM >	r_ {}

hardware_interface::JointHandle	right_momentum_block_joint_handle_

hardware_interface::JointHandle	right_wheel_joint_handle_

double	roll_

RtpublisherPtr	state_pub_

double	wheel_base_

double	wheel_radius_

Eigen::Matrix< double, STATE_DIM, 1 >	x_

double	yaw_

double	yaw_des_ = 0

Static Private Attributes
static const int	CONTROL_DIM = 4

static const int	STATE_DIM = 10

Additional Inherited Members
Public Types inherited from controller_interface::ControllerBase
typedef std::vector< hardware_interface::InterfaceResources >	ClaimedResources

enum	ControllerState { ControllerState::CONSTRUCTED, ControllerState::INITIALIZED, ControllerState::RUNNING, ControllerState::STOPPED, ControllerState::WAITING, ControllerState::ABORTED }

Public Attributes inherited from controller_interface::ControllerBase
ControllerState	state_

Protected Types inherited from rm_chassis_controllers::ChassisBase< rm_control::RobotStateInterface, hardware_interface::ImuSensorInterface, hardware_interface::EffortJointInterface >
enum

Protected Member Functions inherited from rm_chassis_controllers::ChassisBase< rm_control::RobotStateInterface, hardware_interface::ImuSensorInterface, hardware_interface::EffortJointInterface >
void	cmdChassisCallback (const rm_msgs::ChassisCmdConstPtr &msg)
	Write current command from rm_msgs::ChassisCmd. More...

void	cmdVelCallback (const geometry_msgs::Twist::ConstPtr &msg)
	Write current command from geometry_msgs::Twist. More...

void	follow (const ros::Time &time, const ros::Duration &period)
	The mode FOLLOW: chassis will follow gimbal. More...

void	outsideOdomCallback (const nav_msgs::Odometry::ConstPtr &msg)

void	powerLimit ()
	To limit the chassis power according to current power limit. More...

void	raw ()

void	recovery ()
	Set chassis velocity to zero. More...

void	tfVelToBase (const std::string &from)
	Transform tf velocity to base link frame. More...

void	twist (const ros::Time &time, const ros::Duration &period)
	The mode TWIST: Just moving chassis. More...

void	updateOdom (const ros::Time &time, const ros::Duration &period)
	Init frame on base_link. Integral vel to pos and angle. More...

Protected Member Functions inherited from controller_interface::MultiInterfaceController< T... >
bool	initRequest (hardware_interface::RobotHW *robot_hw, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh, ClaimedResources &claimed_resources) override

Static Protected Member Functions inherited from controller_interface::MultiInterfaceController< T... >
static void	clearClaims (hardware_interface::RobotHW *robot_hw)

static void	extractInterfaceResources (hardware_interface::RobotHW robot_hw_in, hardware_interface::RobotHW robot_hw_out)

static bool	hasRequiredInterfaces (hardware_interface::RobotHW *robot_hw)

static void	populateClaimedResources (hardware_interface::RobotHW *robot_hw, ClaimedResources &claimed_resources)

Protected Attributes inherited from rm_chassis_controllers::ChassisBase< rm_control::RobotStateInterface, hardware_interface::ImuSensorInterface, hardware_interface::EffortJointInterface >
ros::Subscriber	cmd_chassis_sub_

realtime_tools::RealtimeBuffer< Command >	cmd_rt_buffer_

Command	cmd_struct_

ros::Subscriber	cmd_vel_sub_

std::string	command_source_frame_

double	effort_coeff_

hardware_interface::EffortJointInterface *	effort_joint_interface_

bool	enable_odom_tf_

std::string	follow_source_frame_

std::vector< hardware_interface::JointHandle >	joint_handles_

ros::Time	last_publish_time_

double	max_odom_vel_

geometry_msgs::TransformStamped	odom2base_

realtime_tools::RealtimeBuffer< nav_msgs::Odometry >	odom_buffer_

std::shared_ptr< realtime_tools::RealtimePublisher< nav_msgs::Odometry > >	odom_pub_

ros::Subscriber	outside_odom_sub_

control_toolbox::Pid	pid_follow_

double	power_offset_

bool	publish_odom_tf_

double	publish_rate_

RampFilter< double > *	ramp_w_

RampFilter< double > *	ramp_x_

RampFilter< double > *	ramp_y_

rm_control::RobotStateHandle	robot_state_handle_

int	state_

bool	state_changed_

rm_common::TfRtBroadcaster	tf_broadcaster_

double	timeout_

bool	topic_update_

double	twist_angular_

geometry_msgs::Vector3	vel_cmd_

double	velocity_coeff_

double	wheel_radius_

tf2::Transform	world2odom_

Protected Attributes inherited from controller_interface::MultiInterfaceController< T... >
bool	allow_optional_interfaces_

hardware_interface::RobotHW	robot_hw_ctrl_

Detailed Description

Definition at line 18 of file balance.h.

Member Typedef Documentation

◆ RtpublisherPtr

typedef std::shared_ptr<realtime_tools::RealtimePublisher<rm_msgs::BalanceState> > rm_chassis_controllers::BalanceController::RtpublisherPtr

private

Definition at line 58 of file balance.h.

Member Enumeration Documentation

◆ BalanceMode

enum rm_chassis_controllers::BalanceController::BalanceMode

private

Enumerator
NORMAL
BLOCK

Definition at line 21 of file balance.h.

Constructor & Destructor Documentation

◆ BalanceController()

rm_chassis_controllers::BalanceController::BalanceController ( )

default

Member Function Documentation

◆ block()

void rm_chassis_controllers::BalanceController::block	(	const ros::Time &	time,
		const ros::Duration &	period
	)

private

Definition at line 468 of file balance.cpp.

◆ init()

bool rm_chassis_controllers::BalanceController::init	(	hardware_interface::RobotHW *	robot_hw,
		ros::NodeHandle &	root_nh,
		ros::NodeHandle &	controller_nh
	)

overridevirtual

Reimplemented from controller_interface::MultiInterfaceController< T... >.

Definition at line 17 of file balance.cpp.

◆ moveJoint()

void rm_chassis_controllers::BalanceController::moveJoint	(	const ros::Time &	time,
		const ros::Duration &	period
	)

overrideprivatevirtual

Implements rm_chassis_controllers::ChassisBase< rm_control::RobotStateInterface, hardware_interface::ImuSensorInterface, hardware_interface::EffortJointInterface >.

Definition at line 322 of file balance.cpp.

◆ normal()

void rm_chassis_controllers::BalanceController::normal	(	const ros::Time &	time,
		const ros::Duration &	period
	)

private

Definition at line 407 of file balance.cpp.

◆ odometry()

geometry_msgs::Twist rm_chassis_controllers::BalanceController::odometry ( )

overrideprivatevirtual

Implements rm_chassis_controllers::ChassisBase< rm_control::RobotStateInterface, hardware_interface::ImuSensorInterface, hardware_interface::EffortJointInterface >.

Definition at line 492 of file balance.cpp.

Member Data Documentation

◆ a_

Eigen::Matrix<double, STATE_DIM, STATE_DIM> rm_chassis_controllers::BalanceController::a_ {}

private

Definition at line 39 of file balance.h.

◆ angular_vel_base_

geometry_msgs::Vector3 rm_chassis_controllers::BalanceController::angular_vel_base_

private

Definition at line 60 of file balance.h.

◆ anti_block_effort_

double rm_chassis_controllers::BalanceController::anti_block_effort_

private

Definition at line 51 of file balance.h.

◆ b_

Eigen::Matrix<double, STATE_DIM, CONTROL_DIM> rm_chassis_controllers::BalanceController::b_ {}

private

Definition at line 40 of file balance.h.

◆ balance_mode_

int rm_chassis_controllers::BalanceController::balance_mode_

private

Definition at line 49 of file balance.h.

◆ balance_state_changed_

bool rm_chassis_controllers::BalanceController::balance_state_changed_ = false

private

Definition at line 52 of file balance.h.

◆ block_angle_

double rm_chassis_controllers::BalanceController::block_angle_

private

Definition at line 51 of file balance.h.

◆ block_duration_

double rm_chassis_controllers::BalanceController::block_duration_

private

Definition at line 51 of file balance.h.

◆ block_effort_

double rm_chassis_controllers::BalanceController::block_effort_

private

Definition at line 51 of file balance.h.

◆ block_overtime_

double rm_chassis_controllers::BalanceController::block_overtime_

private

Definition at line 51 of file balance.h.

◆ block_time_

ros::Time rm_chassis_controllers::BalanceController::block_time_

private

Definition at line 50 of file balance.h.

◆ block_velocity_

double rm_chassis_controllers::BalanceController::block_velocity_

private

Definition at line 51 of file balance.h.

◆ CONTROL_DIM

const int rm_chassis_controllers::BalanceController::CONTROL_DIM = 4

staticprivate

Definition at line 37 of file balance.h.

◆ imu_handle_

hardware_interface::ImuSensorHandle rm_chassis_controllers::BalanceController::imu_handle_

private

Definition at line 54 of file balance.h.

◆ k_

Eigen::Matrix<double, CONTROL_DIM, STATE_DIM> rm_chassis_controllers::BalanceController::k_ {}

private

Definition at line 38 of file balance.h.

◆ last_block_time_

ros::Time rm_chassis_controllers::BalanceController::last_block_time_

private

Definition at line 50 of file balance.h.

◆ left_momentum_block_joint_handle_

hardware_interface::JointHandle rm_chassis_controllers::BalanceController::left_momentum_block_joint_handle_

private

Definition at line 56 of file balance.h.

◆ left_wheel_joint_handle_

hardware_interface::JointHandle rm_chassis_controllers::BalanceController::left_wheel_joint_handle_

private

Definition at line 55 of file balance.h.

◆ maybe_block_

bool rm_chassis_controllers::BalanceController::maybe_block_ = false

private

Definition at line 52 of file balance.h.

◆ pitch_

double rm_chassis_controllers::BalanceController::pitch_

private

Definition at line 61 of file balance.h.

◆ position_clear_threshold_

double rm_chassis_controllers::BalanceController::position_clear_threshold_ = 0.

private

Definition at line 46 of file balance.h.

◆ position_des_

double rm_chassis_controllers::BalanceController::position_des_ = 0

private

Definition at line 44 of file balance.h.

◆ position_offset_

double rm_chassis_controllers::BalanceController::position_offset_ = 0.

private

Definition at line 45 of file balance.h.

◆ q_

Eigen::Matrix<double, STATE_DIM, STATE_DIM> rm_chassis_controllers::BalanceController::q_ {}

private

Definition at line 39 of file balance.h.

◆ r_

Eigen::Matrix<double, CONTROL_DIM, CONTROL_DIM> rm_chassis_controllers::BalanceController::r_ {}

private

Definition at line 41 of file balance.h.

◆ right_momentum_block_joint_handle_

hardware_interface::JointHandle rm_chassis_controllers::BalanceController::right_momentum_block_joint_handle_

private

Definition at line 56 of file balance.h.

◆ right_wheel_joint_handle_

hardware_interface::JointHandle rm_chassis_controllers::BalanceController::right_wheel_joint_handle_

private

Definition at line 55 of file balance.h.

◆ roll_

double rm_chassis_controllers::BalanceController::roll_

private

Definition at line 61 of file balance.h.

◆ STATE_DIM

const int rm_chassis_controllers::BalanceController::STATE_DIM = 10

staticprivate

Definition at line 36 of file balance.h.

◆ state_pub_

RtpublisherPtr rm_chassis_controllers::BalanceController::state_pub_

private

Definition at line 59 of file balance.h.

◆ wheel_base_

double rm_chassis_controllers::BalanceController::wheel_base_

private

Definition at line 43 of file balance.h.

◆ wheel_radius_

double rm_chassis_controllers::BalanceController::wheel_radius_

private

Definition at line 43 of file balance.h.

◆ x_

Eigen::Matrix<double, STATE_DIM, 1> rm_chassis_controllers::BalanceController::x_

private

Definition at line 42 of file balance.h.

◆ yaw_

double rm_chassis_controllers::BalanceController::yaw_

private

Definition at line 61 of file balance.h.

◆ yaw_des_

double rm_chassis_controllers::BalanceController::yaw_des_ = 0

private

Definition at line 47 of file balance.h.

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

Public Member Functions

Private Types

Private Member Functions

Private Attributes

Static Private Attributes

Additional Inherited Members

Detailed Description

Member Typedef Documentation

◆ RtpublisherPtr

Member Enumeration Documentation

◆ BalanceMode

Constructor & Destructor Documentation

◆ BalanceController()

Member Function Documentation

◆ block()

◆ init()

◆ moveJoint()

◆ normal()

◆ odometry()

Member Data Documentation

◆ a_

◆ angular_vel_base_

◆ anti_block_effort_

◆ b_

◆ balance_mode_

◆ balance_state_changed_

◆ block_angle_

◆ block_duration_

◆ block_effort_

◆ block_overtime_

◆ block_time_

◆ block_velocity_

◆ CONTROL_DIM

◆ imu_handle_

◆ k_

◆ last_block_time_

◆ left_momentum_block_joint_handle_

◆ left_wheel_joint_handle_

◆ maybe_block_

◆ pitch_

◆ position_clear_threshold_

◆ position_des_

◆ position_offset_

◆ q_

◆ r_

◆ right_momentum_block_joint_handle_

◆ right_wheel_joint_handle_

◆ roll_

◆ STATE_DIM

◆ state_pub_

◆ wheel_base_

◆ wheel_radius_

◆ x_

◆ yaw_

◆ yaw_des_