Tetrapod Project
RobotController Class Reference

#include <robot_controller.h>

Inheritance diagram for RobotController:
Collaboration diagram for RobotController:

Public Types

enum  SuperState { kIdle , kMoving }
 
enum  MotionState { kStancePreSwingFlRr , kSwingFlRr , kStancePreSwingFrRl , kSwingFrRl }
 
enum  SwingState {
  kStart , kRise , kHigh , kFall ,
  kEnd
}
 

Public Member Functions

 RobotController (int controller_freq, double gait_period)
 
void UpdateGuidanceCommands ()
 
void UpdateNonGuidanceCommands ()
 
void UpdateStepDistances ()
 
bool UpdateGaitState ()
 
void UpdateFeetReferences ()
 A function updating the foot positions of the robot in the hip frames. More...
 
void UpdateFeetTrajectories ()
 
void UpdateFeetReferencesPoseControl ()
 A pose controller updating robot's foot positions to control its pose. More...
 
void UpdateFeetReferencesGaitControl ()
 A gait controller updating the robot's foot positions to walk. More...
 
void SetFeetPositionsGoalPose ()
 A function that sets the goal positions as the targets to reach eventually. More...
 
Eigen::Matrix< double, 3, 1 > UpdateStanceFootPosition (Kinematics::LegType _leg_type, double _progress)
 
Eigen::Matrix< double, 3, 1 > UpdateSwingFootPosition (Kinematics::LegType _leg_type, double _progress)
 
void UpdateSwingFootTrajectory (Kinematics::LegType _leg_type, double progress)
 
void PrintParameters ()
 
void PrintFootPositions ()
 
void PrintVelCommands ()
 
- Public Member Functions inherited from Controller
 Controller (int controller_freq)
 
virtual ~Controller ()
 
bool RunStandUpSequence ()
 
void waitForReadyToProceed ()
 
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 ()
 

Private Attributes

SuperState super_state = SuperState::kIdle
 The main state of the robot deciding whether the robot is walking or not. More...
 
MotionState motion_state = MotionState::kStancePreSwingFlRr
 A variable deciding which of the four motion states we are currently in. More...
 
Eigen::Matrix< double, 3, 1 > fl_goal_position
 The front left foot target position in the hip frame when using pose control. More...
 
Eigen::Matrix< double, 3, 1 > fr_goal_position
 The front right foot target position in the hip frame when using pose control. More...
 
Eigen::Matrix< double, 3, 1 > rl_goal_position
 The rear left foot target position in the hip frame when using pose control. More...
 
Eigen::Matrix< double, 3, 1 > rr_goal_position
 The rear right foot target position in the hip frame when using pose control. More...
 
Eigen::Matrix< double, 12, 1 > initial_feet_positions_pose_control
 
double hip_height = 0.35
 
double step_distance_x_linear = 0.0
 
double step_distance_y_linear = 0.0
 
double step_distance_x_rotational = 0.0
 
double step_distance_y_rotational = 0.0
 
double gait_duration = 0.0
 
double vel_x = 0.0
 
double vel_y = 0.0
 
double yaw_rate = 0.0
 
int iteration = 0
 
double max_step_height = 0.1
 
double stance_phase_duration_percentage = 0.1
 
double swing_rise_percentage = 0.3
 
double swing_period
 
double stance_period
 
int swing_iterations
 
int stance_iterations
 
int final_iteration
 
double fl_step_distance_x
 
double fl_step_distance_y
 
double fr_step_distance_x
 
double fr_step_distance_y
 
double rl_step_distance_x
 
double rl_step_distance_y
 
double rr_step_distance_x
 
double rr_step_distance_y
 
double _lin_vel_x_command
 
double _lin_vel_y_command
 
double _ang_vel_z_command
 
double _guidance_lin_vel_gain = 1.0
 
double _guidance_ang_vel_gain = 1.0
 

Additional Inherited Members

- Protected Types inherited from Controller
using JointState = Eigen::Matrix< double, 12, 1 >
 
- Protected Member Functions inherited from Controller
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 inherited from Controller
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
 

Detailed Description

Definition at line 7 of file robot_controller.h.

Member Enumeration Documentation

◆ SuperState

Enumerator
kIdle 
kMoving 

Definition at line 9 of file robot_controller.h.

◆ MotionState

Enumerator
kStancePreSwingFlRr 
kSwingFlRr 
kStancePreSwingFrRl 
kSwingFrRl 

Definition at line 11 of file robot_controller.h.

◆ SwingState

Enumerator
kStart 
kRise 
kHigh 
kFall 
kEnd 

Definition at line 13 of file robot_controller.h.

Constructor & Destructor Documentation

◆ RobotController()

RobotController::RobotController ( int  controller_freq,
double  gait_period 
)

Definition at line 3 of file robot_controller.cpp.

Member Function Documentation

◆ UpdateGuidanceCommands()

void RobotController::UpdateGuidanceCommands ( )

Definition at line 498 of file robot_controller.cpp.

◆ UpdateNonGuidanceCommands()

void RobotController::UpdateNonGuidanceCommands ( )

Definition at line 514 of file robot_controller.cpp.

◆ UpdateStepDistances()

void RobotController::UpdateStepDistances ( )

Definition at line 42 of file robot_controller.cpp.

◆ UpdateGaitState()

bool RobotController::UpdateGaitState ( )

Definition at line 62 of file robot_controller.cpp.

◆ UpdateFeetReferences()

void RobotController::UpdateFeetReferences ( )
virtual

A function updating the foot positions of the robot in the hip frames.

Implements Controller.

Definition at line 109 of file robot_controller.cpp.

◆ UpdateFeetTrajectories()

void RobotController::UpdateFeetTrajectories ( )

Definition at line 157 of file robot_controller.cpp.

◆ UpdateFeetReferencesPoseControl()

void RobotController::UpdateFeetReferencesPoseControl ( )

A pose controller updating robot's foot positions to control its pose.

◆ UpdateFeetReferencesGaitControl()

void RobotController::UpdateFeetReferencesGaitControl ( )

A gait controller updating the robot's foot positions to walk.

◆ SetFeetPositionsGoalPose()

void RobotController::SetFeetPositionsGoalPose ( )

A function that sets the goal positions as the targets to reach eventually.

◆ UpdateStanceFootPosition()

Eigen::Matrix< double, 3, 1 > RobotController::UpdateStanceFootPosition ( Kinematics::LegType  _leg_type,
double  _progress 
)

Definition at line 219 of file robot_controller.cpp.

◆ UpdateSwingFootPosition()

Eigen::Matrix< double, 3, 1 > RobotController::UpdateSwingFootPosition ( Kinematics::LegType  _leg_type,
double  _progress 
)

Definition at line 307 of file robot_controller.cpp.

◆ UpdateSwingFootTrajectory()

void RobotController::UpdateSwingFootTrajectory ( Kinematics::LegType  _leg_type,
double  progress 
)

Definition at line 359 of file robot_controller.cpp.

◆ PrintParameters()

void RobotController::PrintParameters ( )

Definition at line 477 of file robot_controller.cpp.

◆ PrintFootPositions()

void RobotController::PrintFootPositions ( )

Definition at line 487 of file robot_controller.cpp.

◆ PrintVelCommands()

void RobotController::PrintVelCommands ( )

Definition at line 482 of file robot_controller.cpp.

Member Data Documentation

◆ super_state

SuperState RobotController::super_state = SuperState::kIdle
private

The main state of the robot deciding whether the robot is walking or not.

Definition at line 46 of file robot_controller.h.

◆ motion_state

MotionState RobotController::motion_state = MotionState::kStancePreSwingFlRr
private

A variable deciding which of the four motion states we are currently in.

Definition at line 49 of file robot_controller.h.

◆ fl_goal_position

Eigen::Matrix<double, 3, 1> RobotController::fl_goal_position
private

The front left foot target position in the hip frame when using pose control.

Definition at line 52 of file robot_controller.h.

◆ fr_goal_position

Eigen::Matrix<double, 3, 1> RobotController::fr_goal_position
private

The front right foot target position in the hip frame when using pose control.

Definition at line 55 of file robot_controller.h.

◆ rl_goal_position

Eigen::Matrix<double, 3, 1> RobotController::rl_goal_position
private

The rear left foot target position in the hip frame when using pose control.

Definition at line 58 of file robot_controller.h.

◆ rr_goal_position

Eigen::Matrix<double, 3, 1> RobotController::rr_goal_position
private

The rear right foot target position in the hip frame when using pose control.

Definition at line 61 of file robot_controller.h.

◆ initial_feet_positions_pose_control

Eigen::Matrix<double, 12, 1> RobotController::initial_feet_positions_pose_control
private

Definition at line 63 of file robot_controller.h.

◆ hip_height

double RobotController::hip_height = 0.35
private

Definition at line 65 of file robot_controller.h.

◆ step_distance_x_linear

double RobotController::step_distance_x_linear = 0.0
private

Definition at line 67 of file robot_controller.h.

◆ step_distance_y_linear

double RobotController::step_distance_y_linear = 0.0
private

Definition at line 69 of file robot_controller.h.

◆ step_distance_x_rotational

double RobotController::step_distance_x_rotational = 0.0
private

Definition at line 71 of file robot_controller.h.

◆ step_distance_y_rotational

double RobotController::step_distance_y_rotational = 0.0
private

Definition at line 73 of file robot_controller.h.

◆ gait_duration

double RobotController::gait_duration = 0.0
private

Definition at line 75 of file robot_controller.h.

◆ vel_x

double RobotController::vel_x = 0.0
private

Definition at line 77 of file robot_controller.h.

◆ vel_y

double RobotController::vel_y = 0.0
private

Definition at line 79 of file robot_controller.h.

◆ yaw_rate

double RobotController::yaw_rate = 0.0
private

Definition at line 81 of file robot_controller.h.

◆ iteration

int RobotController::iteration = 0
private

Definition at line 83 of file robot_controller.h.

◆ max_step_height

double RobotController::max_step_height = 0.1
private

Definition at line 85 of file robot_controller.h.

◆ stance_phase_duration_percentage

double RobotController::stance_phase_duration_percentage = 0.1
private

Definition at line 87 of file robot_controller.h.

◆ swing_rise_percentage

double RobotController::swing_rise_percentage = 0.3
private

Definition at line 89 of file robot_controller.h.

◆ swing_period

double RobotController::swing_period
private

Definition at line 91 of file robot_controller.h.

◆ stance_period

double RobotController::stance_period
private

Definition at line 93 of file robot_controller.h.

◆ swing_iterations

int RobotController::swing_iterations
private

Definition at line 95 of file robot_controller.h.

◆ stance_iterations

int RobotController::stance_iterations
private

Definition at line 97 of file robot_controller.h.

◆ final_iteration

int RobotController::final_iteration
private

Definition at line 99 of file robot_controller.h.

◆ fl_step_distance_x

double RobotController::fl_step_distance_x
private

Definition at line 101 of file robot_controller.h.

◆ fl_step_distance_y

double RobotController::fl_step_distance_y
private

Definition at line 103 of file robot_controller.h.

◆ fr_step_distance_x

double RobotController::fr_step_distance_x
private

Definition at line 105 of file robot_controller.h.

◆ fr_step_distance_y

double RobotController::fr_step_distance_y
private

Definition at line 107 of file robot_controller.h.

◆ rl_step_distance_x

double RobotController::rl_step_distance_x
private

Definition at line 109 of file robot_controller.h.

◆ rl_step_distance_y

double RobotController::rl_step_distance_y
private

Definition at line 111 of file robot_controller.h.

◆ rr_step_distance_x

double RobotController::rr_step_distance_x
private

Definition at line 113 of file robot_controller.h.

◆ rr_step_distance_y

double RobotController::rr_step_distance_y
private

Definition at line 115 of file robot_controller.h.

◆ _lin_vel_x_command

double RobotController::_lin_vel_x_command
private

Definition at line 117 of file robot_controller.h.

◆ _lin_vel_y_command

double RobotController::_lin_vel_y_command
private

Definition at line 119 of file robot_controller.h.

◆ _ang_vel_z_command

double RobotController::_ang_vel_z_command
private

Definition at line 121 of file robot_controller.h.

◆ _guidance_lin_vel_gain

double RobotController::_guidance_lin_vel_gain = 1.0
private

Definition at line 123 of file robot_controller.h.

◆ _guidance_ang_vel_gain

double RobotController::_guidance_ang_vel_gain = 1.0
private

Definition at line 125 of file robot_controller.h.


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