#include <controller.h>

Inheritance diagram for Controller:

Collaboration diagram for Controller:

Public Member Functions
	Controller (int controller_freq)

virtual	~Controller ()

bool	RunStandUpSequence ()

void	waitForReadyToProceed ()

virtual void	UpdateFeetReferences ()=0

bool	UpdateJointCommands ()

bool	UpdateVelocityCommands ()

bool	sendJointPositionCommands ()

void	StandStill (const double &duration)

virtual void	setInitialConfiguration ()

void	SetTwistCommand (double lin_vel_cmd_x, double lin_vel_cmd_y, double ang_vel_cmd_z)

void	initROS ()

void	WaitForInitialState ()

bool	MoveFeetToPositions (Eigen::Matrix< double, 3, 1 > fl_goal_foot_pos, Eigen::Matrix< double, 3, 1 > fr_goal_foot_pos, Eigen::Matrix< double, 3, 1 > rl_goal_foot_pos, Eigen::Matrix< double, 3, 1 > rr_goal_foot_pos)

bool	MoveJointsToSetpoints (Eigen::Matrix< double, 12, 1 > goal_joint_angles)

bool	MoveFootToPosition (const Kinematics::LegType &leg_type, const Eigen::Matrix< double, 3, 1 > &goal_foot_position)

void	UpdateFeetPositions ()

bool	CheckReadyToProceed ()

void	ResetReadyToProceedFlag ()

void	SetReadyToProceedFlag ()

bool	ReadyToProceed (std_srvs::Empty::Request &_req, std_srvs::Empty::Response &_res)
	The ReadyToProceed function handles an incoming readyToProceed service request. More...

bool	Shutdown (std_srvs::Empty::Request &_req, std_srvs::Empty::Response &_res)
	The Shutdown function handles an incoming shutdown service request. More...

void	WriteToLog ()

Protected Types
using	JointState = Eigen::Matrix< double, 12, 1 >

Protected Member Functions
void	jointStateCallback (const sensor_msgs::JointStatePtr &msg)

void	readyToProceedCallback (const std_msgs::Bool &msg)

void	TwistCommandCallback (const geometry_msgs::TwistConstPtr &_msg)
	Converts twist command messages into desired linear and angular body velocities. More...

void	TwistStateCallback (const geometry_msgs::TwistConstPtr &_msg)

void	BasePoseStateCallback (const std_msgs::Float64MultiArrayConstPtr &msg)

Protected Attributes
std::string	node_name = "controller_node"

std::string	robot_name = ROBOT_NAME

int	controller_freq

Kinematics	kinematics

ros::NodeHandlePtr	nodeHandle

ros::Publisher	joint_command_publisher

ros::Subscriber	joint_state_subscriber

ros::Subscriber	base_pose_state_subscriber

ros::Subscriber	ready_to_proceed_subscriber

ros::Subscriber	twist_command_subscriber
	Subscribes to twist command messages from an external controller. More...

ros::Subscriber	twist_state_subscriber

Eigen::Vector3d	fl_offset = Eigen::Vector3d(LEG_OFFSET_LENGTH, LEG_OFFSET_LENGTH, 0)

Eigen::Vector3d	fr_offset = Eigen::Vector3d(LEG_OFFSET_LENGTH, -LEG_OFFSET_LENGTH, 0)

Eigen::Vector3d	rl_offset = Eigen::Vector3d(-LEG_OFFSET_LENGTH, LEG_OFFSET_LENGTH, 0)

Eigen::Vector3d	rr_offset = Eigen::Vector3d(-LEG_OFFSET_LENGTH, -LEG_OFFSET_LENGTH, 0)

Eigen::Vector3d	fl_position_body

Eigen::Vector3d	fr_position_body

Eigen::Vector3d	rl_position_body

Eigen::Vector3d	rr_position_body

Eigen::Vector3d	fl_velocity_body

Eigen::Vector3d	fr_velocity_body

Eigen::Vector3d	rl_velocity_body

Eigen::Vector3d	rr_velocity_body

Eigen::Vector3d	fl_acceleration_body

Eigen::Vector3d	fr_acceleration_body

Eigen::Vector3d	rl_acceleration_body

Eigen::Vector3d	rr_acceleration_body

JointState	joint_angle_commands = JointState::Zero()

JointState	joint_velocity_commands = JointState::Zero()

JointState	joint_acceleration_commands = JointState::Zero()

JointState	joint_torque_commands = JointState::Zero()

JointState	joint_angles = JointState::Constant(UNINITIALIZED_JOINT_STATE)

JointState	joint_velocities = JointState::Zero()

JointState	joint_torques = JointState::Zero()

Eigen::Matrix< double, 6, 1 >	base_pose_state = Eigen::Matrix<double, 6, 1>::Zero()

Eigen::Matrix< double, 6, 1 >	base_pose_commands = Eigen::Matrix<double, 6, 1>::Zero()

double	lin_vel_x = 0.0
	The desired linear robot velocity in the body frame's x direction. More...

double	lin_vel_y = 0.0
	The desired linear robot velocity in the body frame's y direction. More...

double	ang_vel_z = 0.0
	The desired angular robot velocity around the robot's z axis. More...

double	_lin_vel_x_estimated = 0.0

double	_lin_vel_y_estimated = 0.0

double	_ang_vel_z_estimated = 0.0

double	_lin_vel_z_measured = 0.0

double	_ang_vel_x_measured = 0.0

double	_ang_vel_y_measured = 0.0

double	nominal_base_height = 0.35

Private Attributes
bool	ready_to_proceed = false

ros::ServiceServer	shutdownService
	ROS Shutdown Service. More...

ros::ServiceServer	readyToProceedService
	ROS Ready to Proceed Service. More...

ros::Publisher	joint_state_logger

ros::Publisher	joint_command_logger

ros::Publisher	base_twist_state_logger

ros::Publisher	base_pose_state_logger

ros::Publisher	base_twist_command_logger

ros::Publisher	base_pose_command_logger

Detailed Description

Definition at line 29 of file controller.h.

Member Typedef Documentation

◆ JointState

using Controller::JointState = Eigen::Matrix<double, 12, 1>

protected

Definition at line 30 of file controller.h.

Constructor & Destructor Documentation

◆ Controller()

Controller::Controller ( int controller_freq )

Definition at line 3 of file controller.cpp.

◆ ~Controller()

Controller::~Controller ( )

virtual

Definition at line 9 of file controller.cpp.

Member Function Documentation

◆ RunStandUpSequence()

bool Controller::RunStandUpSequence ( )

Definition at line 386 of file controller.cpp.

◆ waitForReadyToProceed()

void Controller::waitForReadyToProceed ( )

Definition at line 14 of file controller.cpp.

◆ UpdateFeetReferences()

virtual void Controller::UpdateFeetReferences ( )

pure virtual

Implemented in RobotController, and GaitController.

◆ UpdateJointCommands()

bool Controller::UpdateJointCommands ( )

Definition at line 28 of file controller.cpp.

◆ UpdateVelocityCommands()

bool Controller::UpdateVelocityCommands ( )

Definition at line 65 of file controller.cpp.

◆ sendJointPositionCommands()

bool Controller::sendJointPositionCommands ( )

Definition at line 85 of file controller.cpp.

◆ StandStill()

void Controller::StandStill ( const double & duration )

Definition at line 824 of file controller.cpp.

◆ setInitialConfiguration()

void Controller::setInitialConfiguration ( )

virtual

Definition at line 120 of file controller.cpp.

◆ SetTwistCommand()

void Controller::SetTwistCommand	(	double	lin_vel_cmd_x,
		double	lin_vel_cmd_y,
		double	ang_vel_cmd_z
	)

Definition at line 280 of file controller.cpp.

◆ initROS()

void Controller::initROS ( )

Definition at line 139 of file controller.cpp.

◆ WaitForInitialState()

void Controller::WaitForInitialState ( )

Definition at line 376 of file controller.cpp.

◆ MoveFeetToPositions()

bool Controller::MoveFeetToPositions	(	Eigen::Matrix< double, 3, 1 >	fl_goal_foot_pos,
		Eigen::Matrix< double, 3, 1 >	fr_goal_foot_pos,
		Eigen::Matrix< double, 3, 1 >	rl_goal_foot_pos,
		Eigen::Matrix< double, 3, 1 >	rr_goal_foot_pos
	)

Definition at line 513 of file controller.cpp.

◆ MoveJointsToSetpoints()

bool Controller::MoveJointsToSetpoints ( Eigen::Matrix< double, 12, 1 > goal_joint_angles )

Definition at line 668 of file controller.cpp.

◆ MoveFootToPosition()

bool Controller::MoveFootToPosition	(	const Kinematics::LegType &	leg_type,
		const Eigen::Matrix< double, 3, 1 > &	goal_foot_position
	)

Definition at line 720 of file controller.cpp.

◆ UpdateFeetPositions()

void Controller::UpdateFeetPositions ( )

Definition at line 712 of file controller.cpp.

◆ jointStateCallback()

void Controller::jointStateCallback ( const sensor_msgs::JointStatePtr & msg )

protected

Definition at line 237 of file controller.cpp.

◆ readyToProceedCallback()

void Controller::readyToProceedCallback ( const std_msgs::Bool & msg )

protected

Definition at line 232 of file controller.cpp.

◆ TwistCommandCallback()

void Controller::TwistCommandCallback ( const geometry_msgs::TwistConstPtr & _msg )

protected

Converts twist command messages into desired linear and angular body velocities.

Definition at line 254 of file controller.cpp.

◆ TwistStateCallback()

void Controller::TwistStateCallback ( const geometry_msgs::TwistConstPtr & _msg )

protected

Definition at line 261 of file controller.cpp.

◆ BasePoseStateCallback()

void Controller::BasePoseStateCallback ( const std_msgs::Float64MultiArrayConstPtr & msg )

protected

Definition at line 272 of file controller.cpp.

◆ CheckReadyToProceed()

bool Controller::CheckReadyToProceed ( )

inline

Definition at line 81 of file controller.h.

◆ ResetReadyToProceedFlag()

void Controller::ResetReadyToProceedFlag ( )

inline

Definition at line 83 of file controller.h.

◆ SetReadyToProceedFlag()

void Controller::SetReadyToProceedFlag ( )

inline

Definition at line 85 of file controller.h.

◆ ReadyToProceed()

bool Controller::ReadyToProceed	(	std_srvs::Empty::Request &	_req,
		std_srvs::Empty::Response &	_res
	)

The ReadyToProceed function handles an incoming readyToProceed service request.

Parameters

[in]	_req	Service request.
[out]	_res	Service response.

Returns: Returns true if success, and false if not.

Definition at line 842 of file controller.cpp.

◆ Shutdown()

bool Controller::Shutdown	(	std_srvs::Empty::Request &	_req,
		std_srvs::Empty::Response &	_res
	)

The Shutdown function handles an incoming shutdown service request.

Parameters

[in]	_req	Service request.
[out]	_res	Service response.

Returns: Returns true if success, and false if not.

Definition at line 850 of file controller.cpp.

◆ WriteToLog()

void Controller::WriteToLog ( )

Definition at line 287 of file controller.cpp.

Member Data Documentation

◆ ready_to_proceed

bool Controller::ready_to_proceed = false

private

Definition at line 104 of file controller.h.

◆ node_name

std::string Controller::node_name = "controller_node"

protected

Definition at line 106 of file controller.h.

◆ robot_name

std::string Controller::robot_name = ROBOT_NAME

protected

Definition at line 108 of file controller.h.

◆ controller_freq

int Controller::controller_freq

protected

Definition at line 110 of file controller.h.

◆ kinematics

Kinematics Controller::kinematics

protected

Definition at line 112 of file controller.h.

◆ nodeHandle

ros::NodeHandlePtr Controller::nodeHandle

protected

Definition at line 114 of file controller.h.

◆ shutdownService

ros::ServiceServer Controller::shutdownService

private

ROS Shutdown Service.

Definition at line 117 of file controller.h.

◆ readyToProceedService

ros::ServiceServer Controller::readyToProceedService

private

ROS Ready to Proceed Service.

Definition at line 120 of file controller.h.

◆ joint_command_publisher

ros::Publisher Controller::joint_command_publisher

protected

Definition at line 122 of file controller.h.

◆ joint_state_subscriber

ros::Subscriber Controller::joint_state_subscriber

protected

Definition at line 124 of file controller.h.

◆ base_pose_state_subscriber

ros::Subscriber Controller::base_pose_state_subscriber

protected

Definition at line 126 of file controller.h.

◆ ready_to_proceed_subscriber

ros::Subscriber Controller::ready_to_proceed_subscriber

protected

Definition at line 128 of file controller.h.

◆ twist_command_subscriber

ros::Subscriber Controller::twist_command_subscriber

protected

Subscribes to twist command messages from an external controller.

Definition at line 131 of file controller.h.

◆ twist_state_subscriber

ros::Subscriber Controller::twist_state_subscriber

protected

Definition at line 133 of file controller.h.

◆ fl_offset

Eigen::Vector3d Controller::fl_offset = Eigen::Vector3d(LEG_OFFSET_LENGTH, LEG_OFFSET_LENGTH, 0)

protected

Definition at line 135 of file controller.h.

◆ fr_offset

Eigen::Vector3d Controller::fr_offset = Eigen::Vector3d(LEG_OFFSET_LENGTH, -LEG_OFFSET_LENGTH, 0)

protected

Definition at line 137 of file controller.h.

◆ rl_offset

Eigen::Vector3d Controller::rl_offset = Eigen::Vector3d(-LEG_OFFSET_LENGTH, LEG_OFFSET_LENGTH, 0)

protected

Definition at line 139 of file controller.h.

◆ rr_offset

Eigen::Vector3d Controller::rr_offset = Eigen::Vector3d(-LEG_OFFSET_LENGTH, -LEG_OFFSET_LENGTH, 0)

protected

Definition at line 141 of file controller.h.

◆ fl_position_body

Eigen::Vector3d Controller::fl_position_body

protected

Definition at line 143 of file controller.h.

◆ fr_position_body

Eigen::Vector3d Controller::fr_position_body

protected

Definition at line 145 of file controller.h.

◆ rl_position_body

Eigen::Vector3d Controller::rl_position_body

protected

Definition at line 147 of file controller.h.

◆ rr_position_body

Eigen::Vector3d Controller::rr_position_body

protected

Definition at line 149 of file controller.h.

◆ fl_velocity_body

Eigen::Vector3d Controller::fl_velocity_body

protected

Definition at line 151 of file controller.h.

◆ fr_velocity_body

Eigen::Vector3d Controller::fr_velocity_body

protected

Definition at line 153 of file controller.h.

◆ rl_velocity_body

Eigen::Vector3d Controller::rl_velocity_body

protected

Definition at line 155 of file controller.h.

◆ rr_velocity_body

Eigen::Vector3d Controller::rr_velocity_body

protected

Definition at line 157 of file controller.h.

◆ fl_acceleration_body

Eigen::Vector3d Controller::fl_acceleration_body

protected

Definition at line 159 of file controller.h.

◆ fr_acceleration_body

Eigen::Vector3d Controller::fr_acceleration_body

protected

Definition at line 161 of file controller.h.

◆ rl_acceleration_body

Eigen::Vector3d Controller::rl_acceleration_body

protected

Definition at line 163 of file controller.h.

◆ rr_acceleration_body

Eigen::Vector3d Controller::rr_acceleration_body

protected

Definition at line 165 of file controller.h.

◆ joint_angle_commands

JointState Controller::joint_angle_commands = JointState::Zero()

protected

Definition at line 167 of file controller.h.

◆ joint_velocity_commands

JointState Controller::joint_velocity_commands = JointState::Zero()

protected

Definition at line 169 of file controller.h.

◆ joint_acceleration_commands

JointState Controller::joint_acceleration_commands = JointState::Zero()

protected

Definition at line 171 of file controller.h.

◆ joint_torque_commands

JointState Controller::joint_torque_commands = JointState::Zero()

protected

Definition at line 173 of file controller.h.

◆ joint_angles

JointState Controller::joint_angles = JointState::Constant(UNINITIALIZED_JOINT_STATE)

protected

Definition at line 175 of file controller.h.

◆ joint_velocities

JointState Controller::joint_velocities = JointState::Zero()

protected

Definition at line 177 of file controller.h.

◆ joint_torques

JointState Controller::joint_torques = JointState::Zero()

protected

Definition at line 179 of file controller.h.

◆ base_pose_state

Eigen::Matrix<double, 6, 1> Controller::base_pose_state = Eigen::Matrix<double, 6, 1>::Zero()

protected

Definition at line 181 of file controller.h.

◆ base_pose_commands

Eigen::Matrix<double, 6, 1> Controller::base_pose_commands = Eigen::Matrix<double, 6, 1>::Zero()

protected

Definition at line 183 of file controller.h.

◆ lin_vel_x

double Controller::lin_vel_x = 0.0

protected

The desired linear robot velocity in the body frame's x direction.

Definition at line 186 of file controller.h.

◆ lin_vel_y

double Controller::lin_vel_y = 0.0

protected

The desired linear robot velocity in the body frame's y direction.

Definition at line 189 of file controller.h.

◆ ang_vel_z

double Controller::ang_vel_z = 0.0

protected

The desired angular robot velocity around the robot's z axis.

Definition at line 192 of file controller.h.

◆ _lin_vel_x_estimated

double Controller::_lin_vel_x_estimated = 0.0

protected

Definition at line 194 of file controller.h.

◆ _lin_vel_y_estimated

double Controller::_lin_vel_y_estimated = 0.0

protected

Definition at line 196 of file controller.h.

◆ _ang_vel_z_estimated

double Controller::_ang_vel_z_estimated = 0.0

protected

Definition at line 198 of file controller.h.

◆ _lin_vel_z_measured

double Controller::_lin_vel_z_measured = 0.0

protected

Definition at line 200 of file controller.h.

◆ _ang_vel_x_measured

double Controller::_ang_vel_x_measured = 0.0

protected

Definition at line 202 of file controller.h.

◆ _ang_vel_y_measured

double Controller::_ang_vel_y_measured = 0.0

protected

Definition at line 204 of file controller.h.

◆ nominal_base_height

double Controller::nominal_base_height = 0.35

protected

Definition at line 206 of file controller.h.

◆ joint_state_logger

ros::Publisher Controller::joint_state_logger

private

Definition at line 212 of file controller.h.

◆ joint_command_logger

ros::Publisher Controller::joint_command_logger

private

Definition at line 214 of file controller.h.

◆ base_twist_state_logger

ros::Publisher Controller::base_twist_state_logger

private

Definition at line 216 of file controller.h.

◆ base_pose_state_logger

ros::Publisher Controller::base_pose_state_logger

private

Definition at line 218 of file controller.h.

◆ base_twist_command_logger

ros::Publisher Controller::base_twist_command_logger

private

Definition at line 220 of file controller.h.

◆ base_pose_command_logger

ros::Publisher Controller::base_pose_command_logger

private

Definition at line 222 of file controller.h.

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

catkin_ws/src/control/controller/include/controller/controller.h
catkin_ws/src/control/controller/src/controller.cpp

Public Member Functions

Protected Types

Protected Member Functions

Protected Attributes

Private Attributes

Detailed Description

Member Typedef Documentation

◆ JointState

Constructor & Destructor Documentation

◆ Controller()

◆ ~Controller()

Member Function Documentation

◆ RunStandUpSequence()

◆ waitForReadyToProceed()

◆ UpdateFeetReferences()

◆ UpdateJointCommands()

◆ UpdateVelocityCommands()

◆ sendJointPositionCommands()

◆ StandStill()

◆ setInitialConfiguration()

◆ SetTwistCommand()

◆ initROS()

◆ WaitForInitialState()

◆ MoveFeetToPositions()

◆ MoveJointsToSetpoints()

◆ MoveFootToPosition()

◆ UpdateFeetPositions()

◆ jointStateCallback()

◆ readyToProceedCallback()

◆ TwistCommandCallback()

◆ TwistStateCallback()

◆ BasePoseStateCallback()

◆ CheckReadyToProceed()

◆ ResetReadyToProceedFlag()

◆ SetReadyToProceedFlag()

◆ ReadyToProceed()

◆ Shutdown()

◆ WriteToLog()

Member Data Documentation

◆ ready_to_proceed

◆ node_name

◆ robot_name

◆ controller_freq

◆ kinematics

◆ nodeHandle

◆ shutdownService

◆ readyToProceedService

◆ joint_command_publisher

◆ joint_state_subscriber

◆ base_pose_state_subscriber

◆ ready_to_proceed_subscriber

◆ twist_command_subscriber

◆ twist_state_subscriber

◆ fl_offset

◆ fr_offset

◆ rl_offset

◆ rr_offset

◆ fl_position_body

◆ fr_position_body

◆ rl_position_body

◆ rr_position_body

◆ fl_velocity_body

◆ fr_velocity_body

◆ rl_velocity_body

◆ rr_velocity_body

◆ fl_acceleration_body

◆ fr_acceleration_body

◆ rl_acceleration_body

◆ rr_acceleration_body

◆ joint_angle_commands

◆ joint_velocity_commands

◆ joint_acceleration_commands

◆ joint_torque_commands

◆ joint_angles

◆ joint_velocities

◆ joint_torques

◆ base_pose_state

◆ base_pose_commands

◆ lin_vel_x

◆ lin_vel_y