single_leg_controller.h

Go to the documentation of this file.

#ifndef single_leg_controller_h
#define single_leg_controller_h
 
// ROS
#include "ros/ros.h"
#include "std_msgs/Empty.h"
#include "std_msgs/Bool.h"
#include "std_msgs/Float64.h"
#include "std_msgs/Float64MultiArray.h"
#include "sensor_msgs/JointState.h"
#include "eigen_conversions/eigen_msg.h"
 
// Eigen
#include <Eigen/Core>
 
// Standard Library
#include <thread>
#include <iostream>
#include <iomanip>
 
// Kinematics
#include <kinematics/kinematics.h>
 
// Utilities
#include <filter_utils/filter_utils.h>
 
class SingleLegController
{
    public: enum GaitPhase {stance = 1, swing = 2, uninitialized = 3};
 
    /*** Constructor/Destructor ***/
 
    public: SingleLegController(double _publish_frequency);
 
    public: virtual ~SingleLegController(){};
 
    /*** ROS FUNCTIONS ***/
 
    public: void initROS();
 
    public: void generalizedCoordinatesCallback(const std_msgs::Float64MultiArrayConstPtr &_msg);
 
    public: void generalizedVelocitiesCallback(const std_msgs::Float64MultiArrayConstPtr &_msg);
 
    public: void generalizedForcesCallback(const std_msgs::Float64MultiArrayConstPtr &_msg);
 
    private: void jointStateCallback(const sensor_msgs::JointStatePtr &_msg);
 
    public: void readyToProceedCallback(const std_msgs::Bool &_msg);
 
    private: void jointSetpointCallback(const std_msgs::Float64MultiArrayConstPtr &_msg);
 
    private: void motorConfirmationCallback(const std_msgs::Bool &_msg);
 
    /*** CONTROL FUNCTIONS ***/
 
    private: Eigen::Matrix<double, 3, 1> calculateSwingLegHeightTrajectory(double _percentage, double _period, double _max_swing_height, double _hip_height);
 
    private: void calculateSingleAxisTrajectory
    (
        const double &_percentage, 
        const double &_period, 
        const double &_max_travel,
        double &_axis_pos, 
        double &_axis_vel, 
        double &_axis_acc
    );
 
    public: void updateSwingFootPositionTrajectory();
 
    public: void updateStanceFootPositionTrajectory();
 
    public: void updateJointReferences();
 
    public: 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        
    );
 
    public: void updateJointTorqueCommands();
 
    public: void updateClosedLoopTorqueCommands();
 
    public: void updateJointVelocityCommands(); 
 
    public: void sendJointPositionCommands();
 
    public: void sendJointVelocityCommands();
 
    public: void sendJointTorqueCommands();
 
    public: void increaseIterator();
 
    public: bool updateGaitPhase();
 
    public: void updateFootTrajectoryReference();
 
    public: bool updatePoseControlJointTrajectoryReference();
 
    public: void setPoseGoal(Eigen::Matrix<double, 3, 1> _foot_pos);
 
    public: void setMotorGains();
 
    public: bool moveFootToPosition(Eigen::Matrix<double, 3, 1> _foot_goal_pos);
 
    public: bool moveJointsToSetpoints();
 
    public: bool moveFootToNominalPosition();
 
 
 
    /*** HELPER FUNCTIONS ***/
 
    public: void checkForNewMessages();
 
    public: bool readyToProceed(){return ready_to_proceed;}
 
    public: void resetReadyToProceed(){ready_to_proceed = false;}
 
    public: bool isTargetPositionReached();
 
    public: bool isJointVelocitySmall(); 
 
    public: bool isInitialStateReceived();
 
    public: void printTorqueReferences();
 
    public: void printSpatialTrajectories();
 
    public: void printJointTrajectories();
 
    public: void printAllStates();
 
    public: void writeToLog();
 
    public: GaitPhase getGaitPhase(){return gait_phase;}
 
 
    /*** STATE VARIABLES ***/
 
    private: Eigen::Matrix<double, 3, 1> foot_pos_ref = Eigen::Matrix<double, 3, 1>::Zero();
 
    private: Eigen::Matrix<double, 3, 1> foot_vel_ref = Eigen::Matrix<double, 3, 1>::Zero();
 
    private: Eigen::Matrix<double, 3, 1> foot_acc_ref = Eigen::Matrix<double, 3, 1>::Zero();
 
    private: Eigen::Matrix<double, 3, 1> foot_pos_goal = Eigen::Matrix<double, 3, 1>::Zero();
 
    private: Eigen::Matrix<double, 3, 1> joint_pos_goal = Eigen::Matrix<double, 3, 1>::Zero();
 
    private: Eigen::Matrix<double, 3, 1> joint_pos_initial = Eigen::Matrix<double, 3, 1>::Zero();
 
    private: Eigen::Matrix<double, 3, 1> joint_pos = Eigen::Matrix<double, 3, 1>::Zero();
 
    private: Eigen::Matrix<double, 3, 1> joint_vel = Eigen::Matrix<double, 3, 1>::Zero();
 
    private: Eigen::Matrix<double, 3, 1> joint_torque = Eigen::Matrix<double, 3, 1>::Zero();
 
    private: Eigen::Matrix<double, 3, 1> joint_pos_ref = Eigen::Matrix<double, 3, 1>::Zero();
 
    private: Eigen::Matrix<double, 3, 1> joint_vel_ref = Eigen::Matrix<double, 3, 1>::Zero();
 
    private: Eigen::Matrix<double, 3, 1> joint_acc_ref = Eigen::Matrix<double, 3, 1>::Zero();
 
    private: Eigen::Matrix<double, 3, 1> joint_torque_ref = Eigen::Matrix<double, 3, 1>::Zero();
 
    private: Eigen::Matrix<double, 3, 1> joint_vel_commands = Eigen::Matrix<double, 3, 1>::Zero();
 
    private: Eigen::Matrix<double, 3, 1> joint_torque_commands = Eigen::Matrix<double, 3, 1>::Zero();
 
    private: GaitPhase gait_phase = GaitPhase::stance;
 
    private: bool ready_to_proceed = false;
 
    private: double current_iteration = 0;
 
    private: double final_iteration = 400.0;
 
    private: double current_pose_iteration = 0;
 
    private: double final_pose_iteration = 100;
 
    private: bool gains_set = false;
 
 
 
    /*** PARAMETERS ***/
 
    private: const int NUMBER_OF_MOTORS = 3;
 
    private: const double UNINITIALIZED_STATE = 100.0;
 
    private: const double POSITION_CONVERGENCE_CRITERIA = 0.01; // Cirka 1 degree error for all joints
 
    private: const double POSITION_COMMAND = 1.0;
 
    private: const double VELOCITY_COMMAND = 2.0;
 
    private: const double TORQUE_COMMAND = 3.0;
 
    private: Eigen::Matrix<double, 3, 3> K_p = Eigen::Matrix<double, 3, 3>::Zero();
 
    private: Eigen::Matrix<double, 3, 3> K_d = Eigen::Matrix<double, 3, 3>::Zero();
 
    private: Eigen::Matrix<double, 3, 3> K_pos_error_vel_control = Eigen::Matrix<double, 3, 3>::Zero();
 
    private: double publish_frequency = 200.0;
 
    private: double phase_period = 0.5;
 
    private: double hip_height = 0.35;
 
    private: double x_nominal = 0.1;
 
    private: double y_nominal = 0.3;
 
    private: double x_step_distance = 0.2;
 
    private: double y_step_distance = 0.0;
 
    private: double max_swing_height = 0.12; 
 
    private: double pose_period;
 
    private: double max_pose_vel = math_utils::degToRad(50);
 
    double k_p_pos_hy;
 
    double k_i_pos_hy;
 
    double k_p_pos_hp;
 
    double k_i_pos_hp;
 
    double k_p_pos_kp;
 
    double k_i_pos_kp;
 
    double k_p_vel_hy;
 
    double k_i_vel_hy;
 
    double k_p_vel_hp;
 
    double k_i_vel_hp;
 
    double k_p_vel_kp;
 
    double k_i_vel_kp;
 
    double k_p_torque_hy;
 
    double k_i_torque_hy;
 
    double k_p_torque_hp;
 
    double k_i_torque_hp;
 
    double k_p_torque_kp;
 
    double k_i_torque_kp;
 
 
 
    /*** ROS VARIABLES ***/
 
    private: std::unique_ptr<ros::NodeHandle> node_handle;
 
    private: ros::Subscriber generalized_coordinates_subscriber;
 
    private: ros::Subscriber generalized_velocities_subscriber;
 
    private: ros::Subscriber generalized_forces_subscriber;
 
    private: ros::Subscriber joint_state_subscriber;
 
    private: ros::Subscriber ready_to_proceed_subscriber;
 
    private: ros::Subscriber joint_setpoint_subscriber;
 
    private: ros::Subscriber motor_confirmation_subscriber;
 
    private: ros::Publisher joint_state_publisher;
 
    private: ros::Publisher motor_gain_publisher;
 
    private: sensor_msgs::JointState motor_command_msg;
 
    private: ros::Publisher log_joint_states_publisher;
 
    private: sensor_msgs::JointState joint_state_log_msg;
 
    private: ros::Publisher log_joint_references_publisher;
 
    private: sensor_msgs::JointState joint_reference_log_msg;
 
    /*** UTILITY VARIABLES ***/
 
    private: Kinematics kinematics;
};
 
#endif