biped_controller.hpp

Go to the documentation of this file.

#ifndef WBC_ROS_BIPED_CONTROLLER_HPP
#define WBC_ROS_BIPED_CONTROLLER_HPP
 
#include <rclcpp_lifecycle/lifecycle_node.hpp>
#include <wbc_ros/conversions.hpp>
#include <realtime_tools/realtime_buffer.hpp>
#include <realtime_tools/realtime_publisher.hpp>
#include <wbc_msgs/msg/wbc_timing_stats.hpp>
#include <std_msgs/msg/float64_multi_array.hpp>
#include <std_msgs/msg/float64.hpp>
 
#include <wbc/core/RobotModel.hpp>
#include <wbc/core/Scene.hpp>
#include <wbc/core/QPSolver.hpp>
#include <wbc/tools/JointIntegrator.hpp>
#include <wbc/core/QuadraticProgram.hpp>
#include <wbc/controllers/CartesianPosPDController.hpp>
#include <wbc/controllers/JointPosPDController.hpp>
#include <wbc/tasks/SpatialAccelerationTask.hpp>
#include <wbc/tasks/JointAccelerationTask.hpp>
#include <wbc/tasks/ContactForceTask.hpp>
 
// auto-generated by generate_parameter_library
#include "biped_controller_parameters.hpp"
 
namespace wbc_ros{

    class BipedController : public rclcpp_lifecycle::LifecycleNode{
 
        using JointCommandMsg = robot_control_msgs::msg::JointCommand;
        using JointCommandMsgPtr = robot_control_msgs::msg::JointCommand::SharedPtr;
        using JointCommandPublisher = rclcpp::Publisher<JointCommandMsg>;
        using RTJointCommandPublisher = realtime_tools::RealtimePublisher<JointCommandMsg>;
        using JointCommandSubscription = rclcpp::Subscription<JointCommandMsg>::SharedPtr;
        using RTJointCommandBuffer = realtime_tools::RealtimeBuffer<JointCommandMsgPtr>;
 
        using JointStateMsg = robot_control_msgs::msg::JointState;
        using JointStateMsgPtr = std::shared_ptr<JointStateMsg>;
        using JointStateSubscription = rclcpp::Subscription<JointStateMsg>::SharedPtr;
        using RTJointStateBuffer = realtime_tools::RealtimeBuffer<JointStateMsgPtr>;
 
        using RobotStateMsg = robot_control_msgs::msg::RobotState;
        using RobotStateMsgPtr = std::shared_ptr<RobotStateMsg>;
        using RobotStateSubscription = rclcpp::Subscription<RobotStateMsg>::SharedPtr;
        using RTRobotStateBuffer = realtime_tools::RealtimeBuffer<RobotStateMsgPtr>;
 
        using ContactsMsg = robot_control_msgs::msg::Contacts;
        using ContactsMsgPtr = std::shared_ptr<ContactsMsg>;
        using ContactsSubscription = rclcpp::Subscription<ContactsMsg>::SharedPtr;
        using RTContactsBuffer = realtime_tools::RealtimeBuffer<ContactsMsgPtr>;
 
        using DoubleArrayMsg = std_msgs::msg::Float64MultiArray;
        using DoubleArrayMsgPtr = std::shared_ptr<DoubleArrayMsg>;
        using DoubleArraySubscription = rclcpp::Subscription<DoubleArrayMsg>::SharedPtr;
        using RTDoubleArrayBuffer = realtime_tools::RealtimeBuffer<DoubleArrayMsgPtr>;    
 
        using TimingStatsMsg = wbc_msgs::msg::WbcTimingStats;
        using TimingStatsPublisher = rclcpp::Publisher<TimingStatsMsg>;
        using RTTimingStatsPublisher = realtime_tools::RealtimePublisher<TimingStatsMsg>;
 
        using RigidBodyStateMsg = robot_control_msgs::msg::RigidBodyState;
        using RigidBodyStateMsgPtr = std::shared_ptr<RigidBodyStateMsg>;
        using RigidBodyStateSubscription = rclcpp::Subscription<RigidBodyStateMsg>::SharedPtr;
        using RTRigidBodyStateBuffer = realtime_tools::RealtimeBuffer<RigidBodyStateMsgPtr>;    
         
        using RigidBodyStatePublisher = rclcpp::Publisher<RigidBodyStateMsg>;
        using RTRigidBodyStatePublisher = realtime_tools::RealtimePublisher<RigidBodyStateMsg>;
 
        class TaskInterface{
            public:
                TaskInterface(){
                }
                virtual void update(wbc::RobotModelPtr robot_model) = 0;
                virtual void reset() = 0;
        };
        using TaskInterfacePtr = std::shared_ptr<TaskInterface>;   
 
        class RbsTaskInterface : public TaskInterface{
            RigidBodyStateSubscription setpoint_subscriber;
            RTRigidBodyStateBuffer rt_setpoint_buffer;
            RigidBodyStateMsgPtr setpoint_msg;
 
            RigidBodyStatePublisher::SharedPtr feedback_publisher;
            std::unique_ptr<RTRigidBodyStatePublisher> rt_feedback_publisher;
 
            public:
                RbsTaskInterface(std::string task_name, std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node) : 
                    TaskInterface(),
                    has_setpoint(false){
                    setpoint_subscriber = node->create_subscription<RigidBodyStateMsg>("~/" + task_name + "/setpoint", rclcpp::SystemDefaultsQoS(), 
                        bind(&RbsTaskInterface::setpoint_callback, this, std::placeholders::_1));   
                    feedback_publisher = node->create_publisher<RigidBodyStateMsg>("~/" + task_name + "/feedback", rclcpp::SystemDefaultsQoS());
                    rt_feedback_publisher = std::make_unique<RTRigidBodyStatePublisher>(feedback_publisher);
                }       
                void setpoint_callback(const RigidBodyStateMsgPtr msg){
                    rt_setpoint_buffer.writeFromNonRT(msg);
                    has_setpoint = true;
                }
                virtual void update(wbc::RobotModelPtr robot_model){
                    if(!has_setpoint)
                        return;
                    setpoint_msg = *rt_setpoint_buffer.readFromRT();
                    if(setpoint_msg.get()){
                        fromROS(*setpoint_msg, setpoint);
                        wbc::types::SpatialAcceleration ctrl_out = controller.update(setpoint.pose, 
                                                             setpoint.twist, 
                                                             setpoint.acceleration,
                                                             robot_model->pose(task->tipFrame()),
                                                             robot_model->twist(task->tipFrame()));
                        task->setReference(ctrl_out);
                    }
                    rt_feedback_publisher->lock();
                    toROS(robot_model->pose(task->tipFrame()), 
                          robot_model->twist(task->tipFrame()), 
                          robot_model->acceleration(task->tipFrame()),
                          rt_feedback_publisher->msg_);
                    rt_feedback_publisher->unlockAndPublish();
 
                }
                virtual void reset(){
                    task->reset();
                }
                bool has_setpoint;
                wbc::types::RigidBodyState setpoint;
                wbc::CartesianPosPDController controller;
                wbc::SpatialAccelerationTaskPtr task;
        };
        using RbsTaskInterfacePtr = std::shared_ptr<RbsTaskInterface>;   
 
        class JointPositionTaskInterface : public TaskInterface{
            JointCommandSubscription setpoint_subscriber;
            RTJointCommandBuffer rt_setpoint_buffer;
            JointCommandMsgPtr setpoint_msg;
            public:
                JointPositionTaskInterface(std::string task_name, std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node) : 
                    TaskInterface(),
                    has_setpoint(false){
                    setpoint_subscriber = node->create_subscription<JointCommandMsg>("~/" + task_name + "/setpoint", rclcpp::SystemDefaultsQoS(), 
                        bind(&JointPositionTaskInterface::setpoint_callback, this, std::placeholders::_1));   
 
                }       
                void setpoint_callback(const JointCommandMsgPtr msg){
                    rt_setpoint_buffer.writeFromNonRT(msg);
                    has_setpoint = true;
                }
                virtual void update(wbc::RobotModelPtr robot_model){
                    if(joint_idx_map.empty()){
                        for(auto n : task->jointNames())
                            joint_idx_map.push_back(robot_model->jointIndex(n));
                    }
                    if(!has_setpoint)
                        return;
                    setpoint_msg = *rt_setpoint_buffer.readFromRT();
                    if(setpoint_msg.get()){
                        fromROS(*setpoint_msg, joint_idx_map, setpoint);
                        Eigen::VectorXd ctrl_out = controller->update(setpoint.position, 
                                                                      setpoint.velocity, 
                                                                      setpoint.acceleration,
                                                                      robot_model->jointState().position,
                                                                      robot_model->jointState().velocity);
                        task->setReference(ctrl_out);
                    }
                }
                virtual void reset(){
                    task->reset();
                }
                bool has_setpoint;
                wbc::types::JointCommand setpoint;
                wbc::JointPosPDControllerPtr controller;
                wbc::JointAccelerationTaskPtr task;
                std::vector<int> joint_idx_map;
        };
        using JointPositionTaskInterfacePtr = std::shared_ptr<JointPositionTaskInterface>;  
 
        public:
            BipedController(const rclcpp::NodeOptions & options);
            ~BipedController(){};
 
            virtual CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override;
            virtual CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
            virtual CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
            virtual CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override;
            virtual CallbackReturn on_error(const rclcpp_lifecycle::State & previous_state) override;
            virtual CallbackReturn on_shutdown(const rclcpp_lifecycle::State & previous_state) override;
    
            void updateController();
            void handleContacts(const ContactsMsgPtr msg);
            void joint_weight_callback(const DoubleArrayMsgPtr msg);
            void robot_state_callback(const RobotStateMsgPtr msg);
            void joint_state_callback(const JointStateMsgPtr msg);
            void contacts_callback(const ContactsMsgPtr msg);
 
        protected:
            RbsTaskInterfacePtr body_task_iface;
            RbsTaskInterfacePtr foot_l_iface;
            RbsTaskInterfacePtr foot_r_iface;
            JointPositionTaskInterfacePtr joint_pos_iface;
            std::vector<TaskInterfacePtr> task_interfaces;
 
            wbc::ContactForceTaskPtr force_l_task;
            wbc::ContactForceTaskPtr force_r_task;
 
            wbc::ScenePtr scene;
            wbc::RobotModelPtr robot_model;
            wbc::QPSolverPtr solver;
            wbc::types::JointState joint_state;
            wbc::types::RigidBodyState floating_base_state;
            std::vector<wbc::types::Contact> contacts;
            wbc::types::Wrench contact_force_l, contact_force_r;
            bool has_robot_state, has_joint_state;
            int update_rate;
            wbc::types::JointCommand solver_output;
            wbc::HierarchicalQP qp;
            wbc::JointIntegrator joint_integrator;
            bool integrate_from_current_state;
            std::vector<double> p_gain_stance;
            std::vector<double> d_gain_stance;
            std::vector<double> p_gain_swing;
            std::vector<double> d_gain_swing;
            std::vector<double> p_gain;
            std::vector<double> d_gain;
 
            JointCommandMsg solver_output_msg;
            JointCommandPublisher::SharedPtr solver_output_publisher;
            std::unique_ptr<RTJointCommandPublisher> rt_solver_output_publisher;
 
            TimingStatsMsg timing_stats;
            TimingStatsPublisher::SharedPtr timing_stats_publisher;
            std::unique_ptr<RTTimingStatsPublisher> rt_timing_stats_publisher;
 
            DoubleArraySubscription joint_weight_subscriber;
            RTDoubleArrayBuffer rt_joint_weight_buffer;
            DoubleArrayMsgPtr joint_weight_msg;
 
            RobotStateSubscription robot_state_subscriber;
            RTRobotStateBuffer rt_robot_state_buffer;
            RobotStateMsgPtr robot_state_msg;
 
            ContactsSubscription contacts_subscriber;
            RTContactsBuffer rt_contacts_buffer;
            ContactsMsgPtr contacts_msg;
 
            JointStateSubscription joint_state_subscriber;
            RTJointStateBuffer rt_joint_state_buffer;
            JointStateMsgPtr joint_state_msg;
 
            rclcpp::Time stamp;
            rclcpp::TimerBase::SharedPtr timer;
 
            std::shared_ptr<biped_controller::ParamListener> param_listener;
            biped_controller::Params params;
 
            std::vector<int> joint_idx_map;
            std::vector<int> joint_idx_map_left_leg;
            std::vector<int> joint_idx_map_right_leg;
 
    };
}
 
#endif