arm_movements_by_pose_alg.cpp

Go to the documentation of this file.

#include "arm_movements_by_pose_alg.h"

ArmMovementsByPoseAlgorithm::ArmMovementsByPoseAlgorithm(void)
{
}

ArmMovementsByPoseAlgorithm::~ArmMovementsByPoseAlgorithm(void)
{
}

void ArmMovementsByPoseAlgorithm::config_update(Config& new_cfg, uint32_t level)
{
        this->lock();

        this->hand_size_offset      = new_cfg.hand_size_offset;
        this->wam_security_distance = new_cfg.wam_security_distance;
        this->tf_target_str         = new_cfg.tf_target_str;
        this->robot_wam             = new_cfg.robot_wam;
        
        // save the current configuration
        this->config_=new_cfg;
        
        this->unlock();
}

// Private methods
geometry_msgs::PoseStamped ArmMovementsByPoseAlgorithm::get_pose_from_tf_with_offset(geometry_msgs::PoseStamped pose_orig,
                                                                              std::vector<float> plane_coefs)
{
        pose_orig.pose.position.x += (hand_size_offset * plane_coefs[0]);
        pose_orig.pose.position.y += (hand_size_offset * plane_coefs[1]);
        pose_orig.pose.position.z += (hand_size_offset * plane_coefs[2]);
        
        geometry_msgs::PoseStamped pose_wam;
        
        //tf_listener.waitForTransform(pose_orig.header.frame_id, this->tf_target_str, ros::Time(0), ros::Duration(1));
        ros::Time common_time;
        std::string error_aux;
        tf_listener.getLatestCommonTime(this->tf_target_str, pose_orig.header.frame_id, common_time, &error_aux);
        tf_listener.transformPose(this->tf_target_str, common_time, pose_orig, pose_orig.header.frame_id, pose_wam);

        // security distance
        if (not this->robot_wam && (pose_wam.pose.position.z < wam_security_distance)) {
                throw "Movement is below security distance";
        }
        
        return pose_wam;
}



// ArmMovementsByPoseAlgorithm Public API
std::vector<geometry_msgs::PoseStamped> ArmMovementsByPoseAlgorithm::get_initial_position()
{
        std::vector<geometry_msgs::PoseStamped> movements;
        
        geometry_msgs::PoseStamped pose_st;
        
        if (robot_wam) {
                /* wam
                *   __
                *  /  \
                *  |
                *  T
                */
                pose_st.pose.orientation.x = 0.7071;
                pose_st.pose.orientation.y = 0.7071;
                pose_st.pose.orientation.z = 0;
                pose_st.pose.orientation.w = 0;
                
                pose_st.pose.position.x = 0.5;
                pose_st.pose.position.y = 0;
                pose_st.pose.position.z = 0.4;
                
                pose_st.header.frame_id = "/wam_fk/wam0";
        }
        else {
                // right arm pointing down
                pose_st.pose.orientation.x =  0.7789;
                pose_st.pose.orientation.y = -0.6157;
                pose_st.pose.orientation.z =  0.0867;
                pose_st.pose.orientation.w = -0.0811;
                
                pose_st.pose.position.x = -0.1438;
                pose_st.pose.position.y = -0.2728;
                pose_st.pose.position.z = 0.8070;
                
                // its rotated because the left_arm_action will rotate the movement
                // left arm has its axis inverted! Z-axis is pointing to the arm
                // left arm pointing down
                /*pose.orientation.x = -0.6158;
                pose.orientation.y = 0.7788;
                pose.orientation.z = -0.0813;
                pose.orientation.w = 0.0870;
                
                pose.position.x = -0.1436;
                pose.position.y = 0.2728;
                pose.position.z = 0.8070;*/
                
                pose_st.header.frame_id = "base_footprint";
        }
        
        movements.push_back(pose_st);
        
        return movements;
}


std::vector<geometry_msgs::PoseStamped> ArmMovementsByPoseAlgorithm::get_transformed_poses(std::vector<geometry_msgs::PoseStamped> poses, 
                                                                                    std::vector<float> plane_coefficients)
{
        std::vector<geometry_msgs::PoseStamped> wam_tf_poses;
        
        for (size_t i = 0; i < poses.size(); i++) {
                try {
                        geometry_msgs::PoseStamped pose = get_pose_from_tf_with_offset(poses[i], plane_coefficients);
                        wam_tf_poses.push_back(pose);
                }
                catch (char const* e) {
                        ROS_ERROR_STREAM("SECURITY DISTANCE IS " << wam_security_distance);
                }
        }
        
        return wam_tf_poses;
}


geometry_msgs::Quaternion ArmMovementsByPoseAlgorithm::rotate_pitch(geometry_msgs::Quaternion orientation_msgs, double angle)
{
        using namespace Eigen;

        Quaternion<float> q_orig(orientation_msgs.w, orientation_msgs.x, orientation_msgs.y, orientation_msgs.z);
        Quaternion<float> q_rot(AngleAxis<float>(angle, Vector3f(0,1,0)));
        
        Quaternion<float> q_dest;
        q_dest = q_orig * q_rot;
        orientation_msgs.x = q_dest.x();
        orientation_msgs.y = q_dest.y();
        orientation_msgs.z = q_dest.z();
        orientation_msgs.w = q_dest.w();
        
        return orientation_msgs;
}

geometry_msgs::Quaternion ArmMovementsByPoseAlgorithm::rotate_roll(geometry_msgs::Quaternion orientation_msgs, double angle)
{
        using namespace Eigen;

        Quaternion<float> q_orig(orientation_msgs.w, orientation_msgs.x, orientation_msgs.y, orientation_msgs.z);
        Quaternion<float> q_rot(AngleAxis<float>(angle, Vector3f(1,0,0)));
        
        Quaternion<float> q_dest;
        q_dest = q_orig * q_rot;
        orientation_msgs.x = q_dest.x();
        orientation_msgs.y = q_dest.y();
        orientation_msgs.z = q_dest.z();
        orientation_msgs.w = q_dest.w();
        
        return orientation_msgs;
}

geometry_msgs::Quaternion ArmMovementsByPoseAlgorithm::rotate_yaw(geometry_msgs::Quaternion orientation_msgs, double angle)
{
        using namespace Eigen;

        Quaternion<float> q_orig(orientation_msgs.w, orientation_msgs.x, orientation_msgs.y, orientation_msgs.z);
        Quaternion<float> q_rot(AngleAxis<float>(angle, Vector3f(0,0,1)));
        
        Quaternion<float> q_dest;
        q_dest = q_orig * q_rot;
        
        geometry_msgs::Quaternion result_quaterion_msgs;
        result_quaterion_msgs.x = q_dest.x();
        result_quaterion_msgs.y = q_dest.y();
        result_quaterion_msgs.z = q_dest.z();
        result_quaterion_msgs.w = q_dest.w();
        
        return result_quaterion_msgs;
}