#include <single_leg_controller.h>

Collaboration diagram for SingleLegController:

Public Types
enum	GaitPhase { stance = 1 , swing = 2 , uninitialized = 3 }
	Gait phases. More...

Public Member Functions
	SingleLegController (double _publish_frequency)
	Constructor. More...

virtual	~SingleLegController ()
	Destructor. More...

void	initROS ()
	This function initilizes ROS. More...

void	generalizedCoordinatesCallback (const std_msgs::Float64MultiArrayConstPtr &_msg)
	Listens to joint angle messages from the simulated leg. More...

void	generalizedVelocitiesCallback (const std_msgs::Float64MultiArrayConstPtr &_msg)
	Listens to joint velocity messages from the simulated leg. More...

void	generalizedForcesCallback (const std_msgs::Float64MultiArrayConstPtr &_msg)
	Listens to joint force messages from the simulated leg. More...

void	readyToProceedCallback (const std_msgs::Bool &_msg)
	The ready_to_proceed parameter is changed based on the incomming message. More...

void	updateSwingFootPositionTrajectory ()
	Update the swing foot trajectory for the leg. More...

void	updateStanceFootPositionTrajectory ()
	Update the stance foot tractory for the leg. More...

void	updateJointReferences ()
	Update The joint trajectory reference based on the spatial foot trajectory. More...

void	updateJointTorqueCommands (const Eigen::Matrix< double, 3, 1 > &_joint_pos_ref, const Eigen::Matrix< double, 3, 1 > &_joint_vel_ref, const Eigen::Matrix< double, 3, 1 > &_joint_acc_ref, const Eigen::Matrix< double, 3, 1 > &_joint_pos, const Eigen::Matrix< double, 3, 1 > &_joint_vel)
	Update the joint torqes references based on the joint trajectories and joint states. More...

void	updateJointTorqueCommands ()
	Overloaded function that uses the current joint references and joint states. More...

void	updateClosedLoopTorqueCommands ()
	A non model based torque controller. More...

void	updateJointVelocityCommands ()
	Updates the joint velocity control commands based on the joint velocity reference and the joint position error. More...

void	sendJointPositionCommands ()
	Sends a joint position commands to the motors. More...

void	sendJointVelocityCommands ()
	Updates the joint velocity commands and send them to the motors. More...

void	sendJointTorqueCommands ()
	Updates the joint torque control commands based on the desired foot position. More...

void	increaseIterator ()
	Increment the gait cycle iterator. It stops iterating when the final iteration is reached. More...

bool	updateGaitPhase ()
	Increments the gait cycle iterator and updates the leg state. More...

void	updateFootTrajectoryReference ()
	Update the foot trajectory based on the leg state. More...

bool	updatePoseControlJointTrajectoryReference ()
	Update the pose control trajectory reference for the single leg. More...

void	setPoseGoal (Eigen::Matrix< double, 3, 1 > _foot_pos)
	Set the final and initial states for pose control trajectories. More...

void	setMotorGains ()
	Tries to set the control gains in the motors. The function loops until it succeeds. More...

bool	moveFootToPosition (Eigen::Matrix< double, 3, 1 > _foot_goal_pos)
	Creates a trajectory from the current foot position to the goal foot position and moves the foot to the target position along the trajectory. Function returns when the foot is standing still at the goal position. More...

bool	moveJointsToSetpoints ()
	The function tries to move the foot to the foot position given by the input parameters. More...

bool	moveFootToNominalPosition ()
	The function tries to move the foot to the nominal trajectory position. More...

void	checkForNewMessages ()
	Function used to check if any new ROS messages has been received. More...

bool	readyToProceed ()
	Check whether or not a ready to proceed message has been received. More...

void	resetReadyToProceed ()
	Set the ready_to_proceed flag to false. More...

bool	isTargetPositionReached ()
	Check if the squared error between the current joint angles and target joint angles is smaller than some threshold. More...

bool	isJointVelocitySmall ()
	Checks if the sum of squared joint velocities are smaller than a certain threshold. More...

bool	isInitialStateReceived ()
	Check if the uninitalized joint state of the robot has been overridden by measurements. More...

void	printTorqueReferences ()
	Print the joint torque references. More...

void	printSpatialTrajectories ()
	Print the foot trajectory reference. More...

void	printJointTrajectories ()
	Print the joint trajectory references. More...

void	printAllStates ()
	Print various information about all joints. More...

void	writeToLog ()
	Write the joint references and estimated states to the log. More...

GaitPhase	getGaitPhase ()
	Return the leg's phase state. More...

Private Member Functions
void	jointStateCallback (const sensor_msgs::JointStatePtr &_msg)
	Listens to joint state messages from the motors The messages contains joint angles, velocities and torques for all three motors. More...

void	jointSetpointCallback (const std_msgs::Float64MultiArrayConstPtr &_msg)
	Listens to joint setpoints messages. More...

void	motorConfirmationCallback (const std_msgs::Bool &_msg)
	Listens to a confirmation message from the motors which is used to indicate if the motor gains have been set successfully. More...

Eigen::Matrix< double, 3, 1 >	calculateSwingLegHeightTrajectory (double _percentage, double _period, double _max_swing_height, double _hip_height)
	This function calculates the swing leg height trajectory based on various gait cycle parameters. More...

void	calculateSingleAxisTrajectory (const double &_percentage, const double &_period, const double &_max_travel, double &_axis_pos, double &_axis_vel, double &_axis_acc)
	This functions calculates a smooth trajectory along a single linear axis. Please see the report for details. More...

Private Attributes
Eigen::Matrix< double, 3, 1 >	foot_pos_ref = Eigen::Matrix<double, 3, 1>::Zero()
	The foot position reference relative to the hip. More...

Eigen::Matrix< double, 3, 1 >	foot_vel_ref = Eigen::Matrix<double, 3, 1>::Zero()
	The foot velocity reference relative to the hip. More...

Eigen::Matrix< double, 3, 1 >	foot_acc_ref = Eigen::Matrix<double, 3, 1>::Zero()
	The foot acceleration reference relative to the hip. More...

Eigen::Matrix< double, 3, 1 >	foot_pos_goal = Eigen::Matrix<double, 3, 1>::Zero()
	When using setpointTrajectory control these are the joint angles we want to reach eventually. More...

Eigen::Matrix< double, 3, 1 >	joint_pos_goal = Eigen::Matrix<double, 3, 1>::Zero()
	The joint angles we want to eventually reach when using pose trajectory control. More...

Eigen::Matrix< double, 3, 1 >	joint_pos_initial = Eigen::Matrix<double, 3, 1>::Zero()
	The joint angles at the beginning of a trajectory. More...

Eigen::Matrix< double, 3, 1 >	joint_pos = Eigen::Matrix<double, 3, 1>::Zero()
	The estimated joint angles of the motors. More...

Eigen::Matrix< double, 3, 1 >	joint_vel = Eigen::Matrix<double, 3, 1>::Zero()
	The estimated joint velocities of the motors. More...

Eigen::Matrix< double, 3, 1 >	joint_torque = Eigen::Matrix<double, 3, 1>::Zero()
	The estimated joint torques of the motors. More...

Eigen::Matrix< double, 3, 1 >	joint_pos_ref = Eigen::Matrix<double, 3, 1>::Zero()
	The reference joint angles for the motors. More...

Eigen::Matrix< double, 3, 1 >	joint_vel_ref = Eigen::Matrix<double, 3, 1>::Zero()
	The reference joint velocities for the motors. More...

Eigen::Matrix< double, 3, 1 >	joint_acc_ref = Eigen::Matrix<double, 3, 1>::Zero()
	The reference joint accelerations for the motors. More...

Eigen::Matrix< double, 3, 1 >	joint_torque_ref = Eigen::Matrix<double, 3, 1>::Zero()
	The reference joint torques for the motors. More...

Eigen::Matrix< double, 3, 1 >	joint_vel_commands = Eigen::Matrix<double, 3, 1>::Zero()
	The velocity commands sent to the motors. More...

Eigen::Matrix< double, 3, 1 >	joint_torque_commands = Eigen::Matrix<double, 3, 1>::Zero()
	The joint torque command being sent to the motors. More...

GaitPhase	gait_phase = GaitPhase::stance
	The current gait phase of the leg. More...

bool	ready_to_proceed = false
	A variable used to control the flow of the program. It can be set throuh a boolean message received by the ready_to_proceed_subscriber. More...

double	current_iteration = 0
	The current iteration in a gait phase. This should be incremented for every control loop. More...

double	final_iteration = 400.0
	The final iteration of a gait phase. More...

double	current_pose_iteration = 0
	The current iteration used for pose control. More...

double	final_pose_iteration = 100
	The final iteration used for pose control. More...

bool	gains_set = false
	Keeps track of whether or not the motor gains have been set. More...

const int	NUMBER_OF_MOTORS = 3
	The number of motors used in single leg control tests. More...

const double	UNINITIALIZED_STATE = 100.0
	The value of an uninitialized state. More...

const double	POSITION_CONVERGENCE_CRITERIA = 0.01
	Convergence crieria for position control test. More...

const double	POSITION_COMMAND = 1.0
	The identifier to be used for position command messages. More...

const double	VELOCITY_COMMAND = 2.0
	The identifier to be used for velocity command messages. More...

const double	TORQUE_COMMAND = 3.0
	The identifier to be used for torque command messages. More...

Eigen::Matrix< double, 3, 3 >	K_p = Eigen::Matrix<double, 3, 3>::Zero()
	A diagonal matrix of proportional gains for the closed loop torque controller. More...

Eigen::Matrix< double, 3, 3 >	K_d = Eigen::Matrix<double, 3, 3>::Zero()
	A diagonal matrix of derivative gains for the closed loop torque controller. More...

Eigen::Matrix< double, 3, 3 >	K_pos_error_vel_control = Eigen::Matrix<double, 3, 3>::Zero()
	A digonal matrix for of gains used for the position error in the closed loop velocity controller. More...

double	publish_frequency = 200.0
	The expected publish frequency of the robot. This is used to calculate the maximum number of iterations per gait phase. More...

double	phase_period = 0.5
	The duration of a phase period in seconds. More...

double	hip_height = 0.35
	The standard height of the hips above the ground. More...

double	x_nominal = 0.1
	The nominal x position of the foot relative to the hip. More...

double	y_nominal = 0.3
	The nominal y position of the foot relative to the hip. More...

double	x_step_distance = 0.2
	The step distance in the x direction. More...

double	y_step_distance = 0.0
	The step distance in the y direction. More...

double	max_swing_height = 0.12
	The maximum height above the ground to lift the foot. More...

double	pose_period
	The time used to change from one pose to the next. More...

double	max_pose_vel = math_utils::degToRad(50)
	Max pose control joint angle velocity [rad/s]. More...

double	k_p_pos_hy
	Hip yaw joint motor position control proportional gain. More...

double	k_i_pos_hy
	Hip yaw joint motor position control integral gain. More...

double	k_p_pos_hp
	Hip pitch joint motor position control proportional gain. More...

double	k_i_pos_hp
	Hip pitch joint motor position control integral gain. More...

double	k_p_pos_kp
	Knee pitch joint motor position control proportional gain. More...

double	k_i_pos_kp
	Knee pitch joint motor position control integral gain. More...

double	k_p_vel_hy
	Hip yaw joint motor velocity control proportional gain. More...

double	k_i_vel_hy
	Hip yaw joint motor velocity control integral gain. More...

double	k_p_vel_hp
	Hip pitch joint motor velocity control proportional gain. More...

double	k_i_vel_hp
	Hip pitch joint motor velocity control integral gain. More...

double	k_p_vel_kp
	Knee pitch joint motor velocity control proportional gain. More...

double	k_i_vel_kp
	Knee pitch joint motor velocity control integral gain. More...

double	k_p_torque_hy
	Hip yaw motor torque control proportional gain. More...

double	k_i_torque_hy
	Hip yaw motor torque control integral gain. More...

double	k_p_torque_hp
	Hip pitch motor torque control proportional gain. More...

double	k_i_torque_hp
	Hip pitch motor torque control integral gain. More...

double	k_p_torque_kp
	Knee pitch motor torque control proportional gain. More...

double	k_i_torque_kp
	Knee pitch motor torque control integral gain. More...

std::unique_ptr< ros::NodeHandle >	node_handle
	ROS node handle. More...

ros::Subscriber	generalized_coordinates_subscriber
	Subscribes to generalized coordinates messages. More...

ros::Subscriber	generalized_velocities_subscriber
	Subscribes to generalized velocities messages. More...

ros::Subscriber	generalized_forces_subscriber
	Subscribes to generalized forces messages. More...

ros::Subscriber	joint_state_subscriber
	Subscribes to joint state messages from the motors. More...

ros::Subscriber	ready_to_proceed_subscriber
	Subscribes to messages deciding whether or not we should proceed in the script. More...

ros::Subscriber	joint_setpoint_subscriber
	Subscribes to joint setpoint messages. More...

ros::Subscriber	motor_confirmation_subscriber
	Subscribes to confirmation messages from the motor. More...

ros::Publisher	joint_state_publisher
	Publishes velocity commands to the teensy to control the motors. More...

ros::Publisher	motor_gain_publisher
	Publishes desired motor gains to the motors. More...

sensor_msgs::JointState	motor_command_msg
	The joint state message that is sent to the Teensy. More...

ros::Publisher	log_joint_states_publisher
	Publishes joint positions for logging. More...

sensor_msgs::JointState	joint_state_log_msg
	The joint state message that is used to log states. More...

ros::Publisher	log_joint_references_publisher
	Publishes joint position references for logging. More...

sensor_msgs::JointState	joint_reference_log_msg
	The joint state message that is used to log references. More...

Kinematics	kinematics
	Kinematics object. More...

Detailed Description

Definition at line 27 of file single_leg_controller.h.

Member Enumeration Documentation

◆ GaitPhase

enum SingleLegController::GaitPhase

Gait phases.

Enumerator
stance
swing
uninitialized

Definition at line 30 of file single_leg_controller.h.

Constructor & Destructor Documentation

◆ SingleLegController()

SingleLegController::SingleLegController ( double _publish_frequency )

Constructor.

Definition at line 3 of file single_leg_controller.cpp.

◆ ~SingleLegController()

virtual SingleLegController::~SingleLegController ( )

inlinevirtual

Destructor.

Definition at line 38 of file single_leg_controller.h.

Member Function Documentation

◆ initROS()

void SingleLegController::initROS ( )

This function initilizes ROS.

Definition at line 100 of file single_leg_controller.cpp.

◆ generalizedCoordinatesCallback()

void SingleLegController::generalizedCoordinatesCallback ( const std_msgs::Float64MultiArrayConstPtr & _msg )

Listens to joint angle messages from the simulated leg.

Parameters

[in] _msg A float array containing the simulated single leg joint angles

Definition at line 194 of file single_leg_controller.cpp.

◆ generalizedVelocitiesCallback()

void SingleLegController::generalizedVelocitiesCallback ( const std_msgs::Float64MultiArrayConstPtr & _msg )

Listens to joint velocity messages from the simulated leg.

Parameters

[in] _msg A float array containing the simulated single leg joint velocities

Definition at line 204 of file single_leg_controller.cpp.

◆ generalizedForcesCallback()

void SingleLegController::generalizedForcesCallback ( const std_msgs::Float64MultiArrayConstPtr & _msg )

Listens to joint force messages from the simulated leg.

Parameters

[in] _msg A float array containing the simulated single leg joint forces

Definition at line 214 of file single_leg_controller.cpp.

◆ jointStateCallback()

void SingleLegController::jointStateCallback ( const sensor_msgs::JointStatePtr & _msg )

private

Listens to joint state messages from the motors The messages contains joint angles, velocities and torques for all three motors.

Parameters

[in] _msg A float array containing the motor joint states

Definition at line 224 of file single_leg_controller.cpp.

◆ readyToProceedCallback()

void SingleLegController::readyToProceedCallback ( const std_msgs::Bool & _msg )

The ready_to_proceed parameter is changed based on the incomming message.

Parameters

[in] _msg A bool message indicating whether or not a script can proceed

Definition at line 239 of file single_leg_controller.cpp.

◆ jointSetpointCallback()

void SingleLegController::jointSetpointCallback ( const std_msgs::Float64MultiArrayConstPtr & _msg )

private

Listens to joint setpoints messages.

Parameters

[in] _msg A float array containing the desired hy, hp, and kp angles

Definition at line 245 of file single_leg_controller.cpp.

◆ motorConfirmationCallback()

void SingleLegController::motorConfirmationCallback ( const std_msgs::Bool & _msg )

private

Listens to a confirmation message from the motors which is used to indicate if the motor gains have been set successfully.

Parameters

[in] _msg A bool indicating whether or not the gains have been set successfully

Definition at line 254 of file single_leg_controller.cpp.

◆ calculateSwingLegHeightTrajectory()

Eigen::Matrix< double, 3, 1 > SingleLegController::calculateSwingLegHeightTrajectory	(	double	_percentage,
		double	_period,
		double	_max_swing_height,
		double	_hip_height
	)

private

This function calculates the swing leg height trajectory based on various gait cycle parameters.

Parameters

[in]	_percentage	Our location in the gait cycle in the interval [0, 1]
[in]	_period	The gait cycle period [s]
[in]	_max_swing_height	The maximum foot height above the ground
[in]	_hip_height	The height of the hip relative to the ground

Returns: A vector containing the desired foot height [m], foot vertical velocity [m/s] and foot vertical acceleration [m/s^2] in the hip frame

Definition at line 263 of file single_leg_controller.cpp.

◆ calculateSingleAxisTrajectory()

void SingleLegController::calculateSingleAxisTrajectory	(	const double &	_percentage,
		const double &	_period,
		const double &	_max_travel,
		double &	_axis_pos,
		double &	_axis_vel,
		double &	_axis_acc
	)

private

This functions calculates a smooth trajectory along a single linear axis. Please see the report for details.

Parameters

[in]	_percentage	The location in the gait cycle in the interval [0, 1]
[in]	_period	The gait cycle period
[in]	_max_travel	The maximum distance to move in a direction along the linear axis from its origin
[out]	_x	The desired position along the axis
[out]	_x_d	The desired velocity along the axis
[out]	_x_dd	The desired acceleration along the axis

Definition at line 289 of file single_leg_controller.cpp.

◆ updateSwingFootPositionTrajectory()

void SingleLegController::updateSwingFootPositionTrajectory ( )

Update the swing foot trajectory for the leg.

Definition at line 315 of file single_leg_controller.cpp.

◆ updateStanceFootPositionTrajectory()

void SingleLegController::updateStanceFootPositionTrajectory ( )

Update the stance foot tractory for the leg.

Definition at line 367 of file single_leg_controller.cpp.

◆ updateJointReferences()

void SingleLegController::updateJointReferences ( )

Update The joint trajectory reference based on the spatial foot trajectory.

Definition at line 525 of file single_leg_controller.cpp.

◆ updateJointTorqueCommands() [1/2]

void SingleLegController::updateJointTorqueCommands	(	const Eigen::Matrix< double, 3, 1 > &	_joint_pos_ref,
		const Eigen::Matrix< double, 3, 1 > &	_joint_vel_ref,
		const Eigen::Matrix< double, 3, 1 > &	_joint_acc_ref,
		const Eigen::Matrix< double, 3, 1 > &	_joint_pos,
		const Eigen::Matrix< double, 3, 1 > &	_joint_vel
	)

Update the joint torqes references based on the joint trajectories and joint states.

Parameters

[in]	_joint_pos_ref	The joint angle references
[in]	_joint_vel_ref	the joint velocity references
[in]	_joint_acc_ref	The joint acceleration references
[in]	_joint_pos	The estimated joint angles
[in]	_joint_vel	The estimated joint velocities

Definition at line 559 of file single_leg_controller.cpp.

◆ updateJointTorqueCommands() [2/2]

void SingleLegController::updateJointTorqueCommands ( )

Overloaded function that uses the current joint references and joint states.

Definition at line 553 of file single_leg_controller.cpp.

◆ updateClosedLoopTorqueCommands()

void SingleLegController::updateClosedLoopTorqueCommands ( )

A non model based torque controller.

Definition at line 596 of file single_leg_controller.cpp.

◆ updateJointVelocityCommands()

void SingleLegController::updateJointVelocityCommands ( )

Updates the joint velocity control commands based on the joint velocity reference and the joint position error.

Definition at line 548 of file single_leg_controller.cpp.

◆ sendJointPositionCommands()

void SingleLegController::sendJointPositionCommands ( )

Sends a joint position commands to the motors.

Definition at line 601 of file single_leg_controller.cpp.

◆ sendJointVelocityCommands()

void SingleLegController::sendJointVelocityCommands ( )

Updates the joint velocity commands and send them to the motors.

Definition at line 634 of file single_leg_controller.cpp.

◆ sendJointTorqueCommands()

void SingleLegController::sendJointTorqueCommands ( )

Updates the joint torque control commands based on the desired foot position.

Definition at line 658 of file single_leg_controller.cpp.

◆ increaseIterator()

void SingleLegController::increaseIterator ( )

Increment the gait cycle iterator. It stops iterating when the final iteration is reached.

Definition at line 763 of file single_leg_controller.cpp.

◆ updateGaitPhase()

bool SingleLegController::updateGaitPhase ( )

Increments the gait cycle iterator and updates the leg state.

Definition at line 775 of file single_leg_controller.cpp.

◆ updateFootTrajectoryReference()

void SingleLegController::updateFootTrajectoryReference ( )

Update the foot trajectory based on the leg state.

Definition at line 804 of file single_leg_controller.cpp.

◆ updatePoseControlJointTrajectoryReference()

bool SingleLegController::updatePoseControlJointTrajectoryReference ( )

Update the pose control trajectory reference for the single leg.

Definition at line 466 of file single_leg_controller.cpp.

◆ setPoseGoal()

void SingleLegController::setPoseGoal ( Eigen::Matrix< double, 3, 1 > _foot_pos )

Set the final and initial states for pose control trajectories.

Definition at line 428 of file single_leg_controller.cpp.

◆ setMotorGains()

void SingleLegController::setMotorGains ( )

Tries to set the control gains in the motors. The function loops until it succeeds.

Definition at line 924 of file single_leg_controller.cpp.

◆ moveFootToPosition()

bool SingleLegController::moveFootToPosition ( Eigen::Matrix< double, 3, 1 > _foot_goal_pos )

Creates a trajectory from the current foot position to the goal foot position and moves the foot to the target position along the trajectory. Function returns when the foot is standing still at the goal position.

Definition at line 682 of file single_leg_controller.cpp.

◆ moveJointsToSetpoints()

bool SingleLegController::moveJointsToSetpoints ( )

The function tries to move the foot to the foot position given by the input parameters.

Definition at line 751 of file single_leg_controller.cpp.

◆ moveFootToNominalPosition()

bool SingleLegController::moveFootToNominalPosition ( )

The function tries to move the foot to the nominal trajectory position.

Definition at line 756 of file single_leg_controller.cpp.

◆ checkForNewMessages()

void SingleLegController::checkForNewMessages ( )

Function used to check if any new ROS messages has been received.

Definition at line 826 of file single_leg_controller.cpp.

◆ readyToProceed()

bool SingleLegController::readyToProceed ( )

inline

Check whether or not a ready to proceed message has been received.

Returns: The status of the ready_to_proceed boolean value

Definition at line 183 of file single_leg_controller.h.

◆ resetReadyToProceed()

void SingleLegController::resetReadyToProceed ( )

inline

Set the ready_to_proceed flag to false.

Definition at line 186 of file single_leg_controller.h.

◆ isTargetPositionReached()

bool SingleLegController::isTargetPositionReached ( )

Check if the squared error between the current joint angles and target joint angles is smaller than some threshold.

Definition at line 831 of file single_leg_controller.cpp.

◆ isJointVelocitySmall()

bool SingleLegController::isJointVelocitySmall ( )

Checks if the sum of squared joint velocities are smaller than a certain threshold.

Definition at line 848 of file single_leg_controller.cpp.

◆ isInitialStateReceived()

bool SingleLegController::isInitialStateReceived ( )

Check if the uninitalized joint state of the robot has been overridden by measurements.

Definition at line 862 of file single_leg_controller.cpp.

◆ printTorqueReferences()

void SingleLegController::printTorqueReferences ( )

Print the joint torque references.

Definition at line 875 of file single_leg_controller.cpp.

◆ printSpatialTrajectories()

void SingleLegController::printSpatialTrajectories ( )

Print the foot trajectory reference.

Definition at line 880 of file single_leg_controller.cpp.

◆ printJointTrajectories()

void SingleLegController::printJointTrajectories ( )

Print the joint trajectory references.

Definition at line 885 of file single_leg_controller.cpp.

◆ printAllStates()

void SingleLegController::printAllStates ( )

Print various information about all joints.

Definition at line 890 of file single_leg_controller.cpp.

◆ writeToLog()

void SingleLegController::writeToLog ( )

Write the joint references and estimated states to the log.

Definition at line 901 of file single_leg_controller.cpp.

◆ getGaitPhase()

GaitPhase SingleLegController::getGaitPhase ( )

inline

Return the leg's phase state.

Returns: The phase state of the single leg

Definition at line 215 of file single_leg_controller.h.

Member Data Documentation

◆ foot_pos_ref

Eigen::Matrix<double, 3, 1> SingleLegController::foot_pos_ref = Eigen::Matrix<double, 3, 1>::Zero()

private

The foot position reference relative to the hip.

Definition at line 221 of file single_leg_controller.h.

◆ foot_vel_ref

Eigen::Matrix<double, 3, 1> SingleLegController::foot_vel_ref = Eigen::Matrix<double, 3, 1>::Zero()

private

The foot velocity reference relative to the hip.

Definition at line 224 of file single_leg_controller.h.

◆ foot_acc_ref

Eigen::Matrix<double, 3, 1> SingleLegController::foot_acc_ref = Eigen::Matrix<double, 3, 1>::Zero()

private

The foot acceleration reference relative to the hip.

Definition at line 227 of file single_leg_controller.h.

◆ foot_pos_goal

Eigen::Matrix<double, 3, 1> SingleLegController::foot_pos_goal = Eigen::Matrix<double, 3, 1>::Zero()

private

When using setpointTrajectory control these are the joint angles we want to reach eventually.

Definition at line 230 of file single_leg_controller.h.

◆ joint_pos_goal

Eigen::Matrix<double, 3, 1> SingleLegController::joint_pos_goal = Eigen::Matrix<double, 3, 1>::Zero()

private

The joint angles we want to eventually reach when using pose trajectory control.

Definition at line 233 of file single_leg_controller.h.

◆ joint_pos_initial

Eigen::Matrix<double, 3, 1> SingleLegController::joint_pos_initial = Eigen::Matrix<double, 3, 1>::Zero()

private

The joint angles at the beginning of a trajectory.

Definition at line 236 of file single_leg_controller.h.

◆ joint_pos

Eigen::Matrix<double, 3, 1> SingleLegController::joint_pos = Eigen::Matrix<double, 3, 1>::Zero()

private

The estimated joint angles of the motors.

Definition at line 239 of file single_leg_controller.h.

◆ joint_vel

Eigen::Matrix<double, 3, 1> SingleLegController::joint_vel = Eigen::Matrix<double, 3, 1>::Zero()

private

The estimated joint velocities of the motors.

Definition at line 242 of file single_leg_controller.h.

◆ joint_torque

Eigen::Matrix<double, 3, 1> SingleLegController::joint_torque = Eigen::Matrix<double, 3, 1>::Zero()

private

The estimated joint torques of the motors.

Definition at line 245 of file single_leg_controller.h.

◆ joint_pos_ref

Eigen::Matrix<double, 3, 1> SingleLegController::joint_pos_ref = Eigen::Matrix<double, 3, 1>::Zero()

private

The reference joint angles for the motors.

Definition at line 248 of file single_leg_controller.h.

◆ joint_vel_ref

Eigen::Matrix<double, 3, 1> SingleLegController::joint_vel_ref = Eigen::Matrix<double, 3, 1>::Zero()

private

The reference joint velocities for the motors.

Definition at line 251 of file single_leg_controller.h.

◆ joint_acc_ref

Eigen::Matrix<double, 3, 1> SingleLegController::joint_acc_ref = Eigen::Matrix<double, 3, 1>::Zero()

private

The reference joint accelerations for the motors.

Definition at line 254 of file single_leg_controller.h.

◆ joint_torque_ref

Eigen::Matrix<double, 3, 1> SingleLegController::joint_torque_ref = Eigen::Matrix<double, 3, 1>::Zero()

private

The reference joint torques for the motors.

Definition at line 257 of file single_leg_controller.h.

◆ joint_vel_commands

Eigen::Matrix<double, 3, 1> SingleLegController::joint_vel_commands = Eigen::Matrix<double, 3, 1>::Zero()

private

The velocity commands sent to the motors.

Definition at line 260 of file single_leg_controller.h.

◆ joint_torque_commands

Eigen::Matrix<double, 3, 1> SingleLegController::joint_torque_commands = Eigen::Matrix<double, 3, 1>::Zero()

private

The joint torque command being sent to the motors.

Definition at line 263 of file single_leg_controller.h.

◆ gait_phase

GaitPhase SingleLegController::gait_phase = GaitPhase::stance

private

The current gait phase of the leg.

Definition at line 266 of file single_leg_controller.h.

◆ ready_to_proceed

bool SingleLegController::ready_to_proceed = false

private

A variable used to control the flow of the program. It can be set throuh a boolean message received by the ready_to_proceed_subscriber.

Definition at line 270 of file single_leg_controller.h.

◆ current_iteration

double SingleLegController::current_iteration = 0

private

The current iteration in a gait phase. This should be incremented for every control loop.

Definition at line 274 of file single_leg_controller.h.

◆ final_iteration

double SingleLegController::final_iteration = 400.0

private

The final iteration of a gait phase.

Definition at line 277 of file single_leg_controller.h.

◆ current_pose_iteration

double SingleLegController::current_pose_iteration = 0

private

The current iteration used for pose control.

Definition at line 280 of file single_leg_controller.h.

◆ final_pose_iteration

double SingleLegController::final_pose_iteration = 100

private

The final iteration used for pose control.

Definition at line 283 of file single_leg_controller.h.

◆ gains_set

bool SingleLegController::gains_set = false

private

Keeps track of whether or not the motor gains have been set.

Definition at line 286 of file single_leg_controller.h.

◆ NUMBER_OF_MOTORS

const int SingleLegController::NUMBER_OF_MOTORS = 3

private

The number of motors used in single leg control tests.

Definition at line 293 of file single_leg_controller.h.

◆ UNINITIALIZED_STATE

const double SingleLegController::UNINITIALIZED_STATE = 100.0

private

The value of an uninitialized state.

Definition at line 296 of file single_leg_controller.h.

◆ POSITION_CONVERGENCE_CRITERIA

const double SingleLegController::POSITION_CONVERGENCE_CRITERIA = 0.01

private

Convergence crieria for position control test.

Definition at line 299 of file single_leg_controller.h.

◆ POSITION_COMMAND

const double SingleLegController::POSITION_COMMAND = 1.0

private

The identifier to be used for position command messages.

Definition at line 302 of file single_leg_controller.h.

◆ VELOCITY_COMMAND

const double SingleLegController::VELOCITY_COMMAND = 2.0

private

The identifier to be used for velocity command messages.

Definition at line 305 of file single_leg_controller.h.

◆ TORQUE_COMMAND

const double SingleLegController::TORQUE_COMMAND = 3.0

private

The identifier to be used for torque command messages.

Definition at line 308 of file single_leg_controller.h.

◆ K_p

Eigen::Matrix<double, 3, 3> SingleLegController::K_p = Eigen::Matrix<double, 3, 3>::Zero()

private

A diagonal matrix of proportional gains for the closed loop torque controller.

Definition at line 311 of file single_leg_controller.h.

◆ K_d

Eigen::Matrix<double, 3, 3> SingleLegController::K_d = Eigen::Matrix<double, 3, 3>::Zero()

private

A diagonal matrix of derivative gains for the closed loop torque controller.

Definition at line 314 of file single_leg_controller.h.

◆ K_pos_error_vel_control

Eigen::Matrix<double, 3, 3> SingleLegController::K_pos_error_vel_control = Eigen::Matrix<double, 3, 3>::Zero()

private

A digonal matrix for of gains used for the position error in the closed loop velocity controller.

Definition at line 317 of file single_leg_controller.h.

◆ publish_frequency

double SingleLegController::publish_frequency = 200.0

private

The expected publish frequency of the robot. This is used to calculate the maximum number of iterations per gait phase.

Definition at line 321 of file single_leg_controller.h.

◆ phase_period

double SingleLegController::phase_period = 0.5

private

The duration of a phase period in seconds.

Definition at line 324 of file single_leg_controller.h.

◆ hip_height

double SingleLegController::hip_height = 0.35

private

The standard height of the hips above the ground.

Definition at line 327 of file single_leg_controller.h.

◆ x_nominal

double SingleLegController::x_nominal = 0.1

private

The nominal x position of the foot relative to the hip.

Definition at line 330 of file single_leg_controller.h.

◆ y_nominal

double SingleLegController::y_nominal = 0.3

private

The nominal y position of the foot relative to the hip.

Definition at line 333 of file single_leg_controller.h.

◆ x_step_distance

double SingleLegController::x_step_distance = 0.2

private

The step distance in the x direction.

Definition at line 336 of file single_leg_controller.h.

◆ y_step_distance

double SingleLegController::y_step_distance = 0.0

private

The step distance in the y direction.

Definition at line 339 of file single_leg_controller.h.

◆ max_swing_height

double SingleLegController::max_swing_height = 0.12

private

The maximum height above the ground to lift the foot.

Definition at line 342 of file single_leg_controller.h.

◆ pose_period

double SingleLegController::pose_period

private

The time used to change from one pose to the next.

Definition at line 345 of file single_leg_controller.h.

◆ max_pose_vel

double SingleLegController::max_pose_vel = math_utils::degToRad(50)

private

Max pose control joint angle velocity [rad/s].

Definition at line 348 of file single_leg_controller.h.

◆ k_p_pos_hy

double SingleLegController::k_p_pos_hy

private

Hip yaw joint motor position control proportional gain.

Definition at line 351 of file single_leg_controller.h.

◆ k_i_pos_hy

double SingleLegController::k_i_pos_hy

private

Hip yaw joint motor position control integral gain.

Definition at line 354 of file single_leg_controller.h.

◆ k_p_pos_hp

double SingleLegController::k_p_pos_hp

private

Hip pitch joint motor position control proportional gain.

Definition at line 357 of file single_leg_controller.h.

◆ k_i_pos_hp

double SingleLegController::k_i_pos_hp

private

Hip pitch joint motor position control integral gain.

Definition at line 360 of file single_leg_controller.h.

◆ k_p_pos_kp

double SingleLegController::k_p_pos_kp

private

Knee pitch joint motor position control proportional gain.

Definition at line 363 of file single_leg_controller.h.

◆ k_i_pos_kp

double SingleLegController::k_i_pos_kp

private

Knee pitch joint motor position control integral gain.

Definition at line 366 of file single_leg_controller.h.

◆ k_p_vel_hy

double SingleLegController::k_p_vel_hy

private

Hip yaw joint motor velocity control proportional gain.

Definition at line 369 of file single_leg_controller.h.

◆ k_i_vel_hy

double SingleLegController::k_i_vel_hy

private

Hip yaw joint motor velocity control integral gain.

Definition at line 372 of file single_leg_controller.h.

◆ k_p_vel_hp

double SingleLegController::k_p_vel_hp

private

Hip pitch joint motor velocity control proportional gain.

Definition at line 375 of file single_leg_controller.h.

◆ k_i_vel_hp

double SingleLegController::k_i_vel_hp

private

Hip pitch joint motor velocity control integral gain.

Definition at line 378 of file single_leg_controller.h.

◆ k_p_vel_kp

double SingleLegController::k_p_vel_kp

private

Knee pitch joint motor velocity control proportional gain.

Definition at line 381 of file single_leg_controller.h.

◆ k_i_vel_kp

double SingleLegController::k_i_vel_kp

private

Knee pitch joint motor velocity control integral gain.

Definition at line 384 of file single_leg_controller.h.

◆ k_p_torque_hy

double SingleLegController::k_p_torque_hy

private

Hip yaw motor torque control proportional gain.

Definition at line 387 of file single_leg_controller.h.

◆ k_i_torque_hy

double SingleLegController::k_i_torque_hy

private

Hip yaw motor torque control integral gain.

Definition at line 390 of file single_leg_controller.h.

◆ k_p_torque_hp

double SingleLegController::k_p_torque_hp

private

Hip pitch motor torque control proportional gain.

Definition at line 393 of file single_leg_controller.h.

◆ k_i_torque_hp

double SingleLegController::k_i_torque_hp

private

Hip pitch motor torque control integral gain.

Definition at line 396 of file single_leg_controller.h.

◆ k_p_torque_kp

double SingleLegController::k_p_torque_kp

private

Knee pitch motor torque control proportional gain.

Definition at line 399 of file single_leg_controller.h.

◆ k_i_torque_kp

double SingleLegController::k_i_torque_kp

private

Knee pitch motor torque control integral gain.

Definition at line 402 of file single_leg_controller.h.

◆ node_handle

std::unique_ptr<ros::NodeHandle> SingleLegController::node_handle

private

ROS node handle.

Definition at line 409 of file single_leg_controller.h.

◆ generalized_coordinates_subscriber

ros::Subscriber SingleLegController::generalized_coordinates_subscriber

private

Subscribes to generalized coordinates messages.

Definition at line 412 of file single_leg_controller.h.

◆ generalized_velocities_subscriber

ros::Subscriber SingleLegController::generalized_velocities_subscriber

private

Subscribes to generalized velocities messages.

Definition at line 415 of file single_leg_controller.h.

◆ generalized_forces_subscriber

ros::Subscriber SingleLegController::generalized_forces_subscriber

private

Subscribes to generalized forces messages.

Definition at line 418 of file single_leg_controller.h.

◆ joint_state_subscriber

ros::Subscriber SingleLegController::joint_state_subscriber

private

Subscribes to joint state messages from the motors.

Definition at line 421 of file single_leg_controller.h.

◆ ready_to_proceed_subscriber

ros::Subscriber SingleLegController::ready_to_proceed_subscriber

private

Subscribes to messages deciding whether or not we should proceed in the script.

Definition at line 424 of file single_leg_controller.h.

◆ joint_setpoint_subscriber

ros::Subscriber SingleLegController::joint_setpoint_subscriber

private

Subscribes to joint setpoint messages.

Definition at line 427 of file single_leg_controller.h.

◆ motor_confirmation_subscriber

ros::Subscriber SingleLegController::motor_confirmation_subscriber

private

Subscribes to confirmation messages from the motor.

Definition at line 430 of file single_leg_controller.h.

◆ joint_state_publisher

ros::Publisher SingleLegController::joint_state_publisher

private

Publishes velocity commands to the teensy to control the motors.

Definition at line 433 of file single_leg_controller.h.

◆ motor_gain_publisher

ros::Publisher SingleLegController::motor_gain_publisher

private

Publishes desired motor gains to the motors.

Definition at line 436 of file single_leg_controller.h.

◆ motor_command_msg

sensor_msgs::JointState SingleLegController::motor_command_msg

private

The joint state message that is sent to the Teensy.

Definition at line 439 of file single_leg_controller.h.

◆ log_joint_states_publisher

ros::Publisher SingleLegController::log_joint_states_publisher

private

Publishes joint positions for logging.

Definition at line 442 of file single_leg_controller.h.

◆ joint_state_log_msg

sensor_msgs::JointState SingleLegController::joint_state_log_msg

private

The joint state message that is used to log states.

Definition at line 445 of file single_leg_controller.h.

◆ log_joint_references_publisher

ros::Publisher SingleLegController::log_joint_references_publisher

private

Publishes joint position references for logging.

Definition at line 448 of file single_leg_controller.h.

◆ joint_reference_log_msg

sensor_msgs::JointState SingleLegController::joint_reference_log_msg

private

The joint state message that is used to log references.

Definition at line 451 of file single_leg_controller.h.

◆ kinematics

Kinematics SingleLegController::kinematics

private

Kinematics object.

Definition at line 456 of file single_leg_controller.h.

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

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

Public Types

Public Member Functions

Private Member Functions

Private Attributes

Detailed Description

Member Enumeration Documentation

◆ GaitPhase

Constructor & Destructor Documentation

◆ SingleLegController()

◆ ~SingleLegController()

Member Function Documentation

◆ initROS()

◆ generalizedCoordinatesCallback()

◆ generalizedVelocitiesCallback()

◆ generalizedForcesCallback()

◆ jointStateCallback()

◆ readyToProceedCallback()

◆ jointSetpointCallback()

◆ motorConfirmationCallback()

◆ calculateSwingLegHeightTrajectory()

◆ calculateSingleAxisTrajectory()

◆ updateSwingFootPositionTrajectory()

◆ updateStanceFootPositionTrajectory()

◆ updateJointReferences()

◆ updateJointTorqueCommands() [1/2]

◆ updateJointTorqueCommands() [2/2]

◆ updateClosedLoopTorqueCommands()

◆ updateJointVelocityCommands()

◆ sendJointPositionCommands()

◆ sendJointVelocityCommands()

◆ sendJointTorqueCommands()

◆ increaseIterator()

◆ updateGaitPhase()

◆ updateFootTrajectoryReference()

◆ updatePoseControlJointTrajectoryReference()

◆ setPoseGoal()

◆ setMotorGains()

◆ moveFootToPosition()

◆ moveJointsToSetpoints()

◆ moveFootToNominalPosition()

◆ checkForNewMessages()

◆ readyToProceed()

◆ resetReadyToProceed()

◆ isTargetPositionReached()

◆ isJointVelocitySmall()

◆ isInitialStateReceived()

◆ printTorqueReferences()

◆ printSpatialTrajectories()

◆ printJointTrajectories()

◆ printAllStates()

◆ writeToLog()

◆ getGaitPhase()

Member Data Documentation

◆ foot_pos_ref

◆ foot_vel_ref

◆ foot_acc_ref

◆ foot_pos_goal

◆ joint_pos_goal

◆ joint_pos_initial

◆ joint_pos

◆ joint_vel

◆ joint_torque

◆ joint_pos_ref

◆ joint_vel_ref

◆ joint_acc_ref

◆ joint_torque_ref

◆ joint_vel_commands

◆ joint_torque_commands

◆ gait_phase

◆ ready_to_proceed

◆ current_iteration

◆ final_iteration

◆ current_pose_iteration

◆ final_pose_iteration

◆ gains_set

◆ NUMBER_OF_MOTORS

◆ UNINITIALIZED_STATE

◆ POSITION_CONVERGENCE_CRITERIA

◆ POSITION_COMMAND

◆ VELOCITY_COMMAND