pr2_joint_space.cpp

Go to the documentation of this file.

#include <reactive_grasping_pr2/pr2_joint_space.h>

namespace pr2_joint_space {
        
        RightArmModel::RightArmModel(ros::NodeHandle &n, std::string frame) : n_(n) {

                // set constants
                h = 1.f;
                c_u   = 4.f;
                c_xT = 0.3f;
                gain = 1.0f;
                num_joints = 7;
                
                // setup services
                ros::service::waitForService("/pr2_right_arm_kinematics/get_ik_solver_info");
                ros::service::waitForService("/pr2_right_arm_kinematics/get_fk");
                ros::service::waitForService("/pr2_right_arm_kinematics/get_ik");

                fk_info_client_ = 
                        n_.serviceClient<kinematics_msgs::GetKinematicSolverInfo>
                        ("/pr2_right_arm_kinematics/get_ik_solver_info");
                fk_client_ = n_.serviceClient
                        <kinematics_msgs::GetPositionFK>("/pr2_right_arm_kinematics/get_fk");
                ik_client_ = n_.serviceClient
                        <kinematics_msgs::GetPositionIK>("/pr2_right_arm_kinematics/get_ik");

                // and the joint control scheme
                controlled_joints_.resize(num_joints);
                controlled_joints_[0] = "r_shoulder_pan_joint";
                controlled_joints_[1] = "r_shoulder_lift_joint";
                controlled_joints_[2] = "r_upper_arm_roll_joint";
                controlled_joints_[3] = "r_elbow_flex_joint";
                controlled_joints_[4] = "r_forearm_roll_joint";
                controlled_joints_[5] = "r_wrist_flex_joint";
                controlled_joints_[6] = "r_wrist_roll_joint";

                root_frame = frame;
                external_goal_.setZero(3);
                res_.setZero(3);

                if(fk_info_client_.call(request, response)) {
                        for(unsigned int i=0; 
                            i< response.kinematic_solver_info.joint_names.size(); i++) {
                                       
                                ROS_INFO("Joint: %d %s", i,
                                          response.kinematic_solver_info.joint_names[i].c_str());
                        }
                }
                else {
                        
                        ROS_ERROR("Could not call fk_info service");
                }

                // get a joint state mapping 
                joint_mapping_.resize(num_joints);
                for(unsigned int i=0; i < controlled_joints_.size(); ++i) {
                        for(unsigned int j=0; 
                            j< response.kinematic_solver_info.joint_names.size(); j++) {
                                if (response.kinematic_solver_info.joint_names[j].compare(controlled_joints_[i]) == 0) {
                                        joint_mapping_[i] = j;
                                }
                        }
                }
        }


        void RightArmModel::fk(const VectorT &x, VectorT &res) {
                int offset = 0;
                // define the service messages
                fk_request.header.frame_id = root_frame;
                //fk_request.header.stamp = ros::Time::now();
                fk_request.fk_link_names.resize(1);
                fk_request.fk_link_names[0] = "r_wrist_roll_link";

                fk_request.robot_state.joint_state.position.resize
                        (response.kinematic_solver_info.joint_names.size());
                fk_request.robot_state.joint_state.name =  controlled_joints_;
                for(unsigned int i=0; i < joint_mapping_.size(); ++i) {
                        fk_request.robot_state.joint_state.position[i] = x(i+offset);
                }
                
                if(fk_client_.call(fk_request, fk_response)) {
                        if(fk_response.error_code.val == fk_response.error_code.SUCCESS) {
                                res(0) = fk_response.pose_stamped[0].pose.position.x - external_goal_(0);
                                res(1) = fk_response.pose_stamped[0].pose.position.y - external_goal_(1);
                                res(2) = fk_response.pose_stamped[0].pose.position.z - external_goal_(2);
                        }
                        else {
                                ROS_ERROR("Forward kinematics failed");
                        }
                }
                else {
                        ROS_ERROR("Forward kinematics service call failed");
                }
        }

        void RightArmModel::ik(const VectorT &pos, VectorT &x) {
                ik_request.timeout = ros::Duration(2.0);
                ik_request.ik_request.ik_link_name = "r_wrist_roll_link";

                ik_request.ik_request.pose_stamped.header.frame_id = root_frame;
                // TODO fix usage of pos
                ik_request.ik_request.pose_stamped.pose.position.x = external_goal_(0);
                ik_request.ik_request.pose_stamped.pose.position.y = external_goal_(1);
                ik_request.ik_request.pose_stamped.pose.position.z = external_goal_(2);

                ik_request.ik_request.pose_stamped.pose.orientation.x = 0.0;
                ik_request.ik_request.pose_stamped.pose.orientation.y = 0.0;
                ik_request.ik_request.pose_stamped.pose.orientation.z = -0.7;
                ik_request.ik_request.pose_stamped.pose.orientation.w = -0.7;
                ik_request.ik_request.ik_seed_state.joint_state.position.resize(response.kinematic_solver_info.joint_names.size());
                ik_request.ik_request.ik_seed_state.joint_state.name = controlled_joints_;
                std::cout << response.kinematic_solver_info.joint_names.size() << " " << response.kinematic_solver_info.limits.size() << std::endl;
                for(unsigned int i=0; i< response.kinematic_solver_info.joint_names.size(); i++) {
                        ik_request.ik_request.ik_seed_state.joint_state.position[i] = (response.kinematic_solver_info.limits[i].min_position + response.kinematic_solver_info.limits[i].max_position)/2.0;
                        //x(i);
                }
                if(ik_client_.call(ik_request, ik_response)) {
                        if(ik_response.error_code.val == ik_response.error_code.SUCCESS)
                                for(unsigned int i=0; i < ik_response.solution.joint_state.name.size(); i ++)
                                        x(i) = ik_response.solution.joint_state.position[i];
                        else
                                ROS_ERROR("Inverse kinematics failed");
                }
                else {
                        ROS_ERROR("Inverse kinematics service call failed");
                }
        }
        
        void RightArmModel::dynamics(const VectorT &x, const VectorT &u, VectorT &xNext) {
                //int n = x.size();
                //std::cout << xNext.size() << " " << x.size() << " nj " << num_joints << std::endl;
                xNext.setZero(x.size());
                xNext.tail(num_joints) = x.tail(num_joints) + u;
                xNext.block(0, 0, num_joints, 1) = x.block(0, 0, num_joints, 1) + gain * xNext.tail(num_joints);
        }
        
        
        void RightArmModel::dynamicsD(const VectorT &x, const VectorT &u, Deriv &d) {
                int n = x.size();
                int m = u.size();

                d.fx.setIdentity(n, n);
                for(int i = 0; i < m; ++i) {
                        d.fx(i, i) = 1.;
                        d.fx(i, i + m) = 1.;
                }

                d.fu.setZero(n, m);
                for(int i = 0; i < m; ++i) {
                        d.fu(i, i) = 1.;
                        d.fu(i+m, i) = 1.;
                }
        
                // TODO
                d.fxx.resize(n);
                d.fxu.resize(n);
                d.fuu.resize(n);
                for (int i = 0; i < n; ++i) {
                        d.fxx[i].setZero(n,n);
                        d.fxu[i].setZero(n,m);
                        d.fuu[i].setZero(m,m);
                }
                
        }
        


        float RightArmModel::cost(const VectorT &x, const VectorT &u) {
                // cost on distance to goal state
                float goalCost = c_xT * (x - goal_).squaredNorm(); 
                // cost that constrains the velocity
                float totalCost = goalCost + std::pow(c_u, h) * x.tail(7).squaredNorm();
                return totalCost;
        }

        void RightArmModel::setInternalGoal(const VectorT &g) {
                goal_ = g;
        }

        void RightArmModel::setExternalGoal(const VectorT &g) {
                external_goal_ = g;
        }

}