wam_controller.h

Go to the documentation of this file.

#ifndef WAM_CONTROLLER_H
#define WAM_CONTROLLER_H

#include <ros/ros.h>
#include <Eigen/Core>
#include "sensor_msgs/JointState.h"
#include "geometry_msgs/PoseStamped.h"


#include "visual_servoing/robot_controller.h"

// Hand services
#include <std_srvs/Empty.h>
#include "wam_srvs/BHandSpreadPos.h"
#include "wam_srvs/BHandGraspPos.h"
#include "wam_srvs/BHandPinchPos.h"
#include "wam_srvs/BHandFingerPos.h"

// Arm services
#include "wam_srvs/Play.h"
#include "wam_srvs/Hold.h"
#include "wam_srvs/JointMove.h"

// Arm topics
#include "wam_msgs/RTJointPos.h"

class WamController : public RobotController {
    public:
        double max_rate = 0.01;
        WamController(std::string robot_ns, ros::NodeHandle nh);

        // Hand calls
        bool initialize();
        bool close_grasp();
        bool open_grasp();
        bool open_spread();
        bool close_spread();
        bool grasp_pos(double radians);
        bool spread_pos(double radians);
        bool pinch_pos(double radians);
        bool finger_pos(double f1, double f2, double f3);
        bool reset();
        bool set_2_finger_grasp();
        bool set_3_finger_grasp();

        // Arm calls
        bool delta_move(const Eigen::VectorXd& delta);
        bool rt_delta_move(const Eigen::VectorXd& delta);
        bool joint_move(const Eigen::VectorXd& target_jnt_state);
        bool move_to_initial_position();
        bool play_motion(std::string motion);
        bool hold_joints(bool hold_joints);

        geometry_msgs::PoseStamped get_pose() {
            return last_pose;
        }

        Eigen::VectorXd get_jnt_state();
        Eigen::VectorXd get_used_jnt_state();

        size_t get_dof() {
            return last_jnt_state.position.size();
        }

        std::vector<int> get_used_joints() {
            return mapping;
        }

        void set_used_joints(std::string);
        void set_used_joints(std::vector<int> new_joints);

        void set_target_joint_pose(Eigen::VectorXd target);
        Eigen::VectorXd map_joints(const Eigen::VectorXd& delta);

    private:
        // Hand Services
        ros::ServiceClient initialize_srvs;
        ros::ServiceClient close_grasp_srvs;
        ros::ServiceClient open_grasp_srvs;
        ros::ServiceClient open_spread_srvs;
        ros::ServiceClient close_spread_srvs;
        ros::ServiceClient grasp_pos_srvs;
        ros::ServiceClient spread_pos_srvs;
        ros::ServiceClient pinch_pos_srvs;
        ros::ServiceClient finger_pos_srvs;

        // Arm Services
        ros::ServiceClient play_srvs;
        ros::ServiceClient joint_move_srvs;
        ros::ServiceClient hold_jnt_srvs;
        ros::Publisher jnt_rt_pub;

        ros::Subscriber pose_sub;
        ros::Subscriber jnt_sub;

        geometry_msgs::PoseStamped last_pose;
        sensor_msgs::JointState last_jnt_state;

        std::vector<int> mapping{0,1,3};

        template <size_t DOF>
        Eigen::VectorXd _get_jnt_state();

        template <size_t DOF>
        Eigen::VectorXd _map_joints(const Eigen::VectorXd& delta);

        void pose_cb(geometry_msgs::PoseStampedConstPtr pose) {
            last_pose = *pose;
        }

        void jnt_state_cb(sensor_msgs::JointStateConstPtr state) {
            last_jnt_state = *state;
        }

};

#endif // WAM_CONTROLLER_H