Esempio n. 1
0
int main(int argc, char** argv)
{
    // ros initialization
    ros::init(argc, argv, "dvrk_mtm_logic");
    ros::NodeHandle nh;
    ros::Rate rate(200);  // 100 hz rate

    // subscriber
    ros::Subscriber sub_mtm_cmd =
            nh.subscribe("/dvrk_mtm/cartesian_pose_command", 1, mtm_cmd_pose_cb);
    ros::Subscriber sub_mtm_fb =
            nh.subscribe("/dvrk_mtm/joint_states", 1, mtm_joint_feedback_cb);
    ros::Subscriber sub_mode =
            nh.subscribe("/dvrk_mtm/control_mode", 1, mtm_mode_cb);

    // publisher
    ros::Publisher pub_mtm_joint_state =
            nh.advertise<sensor_msgs::JointState>("/dvrk_mtm/joint_states_command", 1);
    ros::Publisher pub_mtm_pose =
            nh.advertise<geometry_msgs::PoseStamped>("/dvrk_mtm/cartesian_pose_current", 1);
    ros::Publisher pub_mtm_enable_slider =
            nh.advertise<sensor_msgs::JointState>("/dvrk_mtm/joint_state_publisher/enable_slider", 100);


    // cisst robManipulator
    std::string filename = ros::package::getPath("dvrk_kinematics");
    filename.append("/config/dvmtm.rob");
    robManipulator mtm_manip;
    robManipulator::Errno result;
    result = mtm_manip.LoadRobot(filename);
    if (result == robManipulator::EFAILURE) {
        ROS_ERROR("failed to load manipulator config file: %s", filename.c_str());
    } else {
        ROS_INFO("loaded psm manipulator");
    }

    // initialize joint current/command/msg_js
    mtm_joint_current.SetSize(7);  // 7 joints
    mtm_joint_current.SetAll(0.0);

    mtm_joint_command.ForceAssign(mtm_joint_current);
    sensor_msgs::JointState msg_full_js;
    msg_full_js.name.clear();
    msg_full_js.name.push_back("right_outer_yaw_joint");
    msg_full_js.name.push_back("right_shoulder_pitch_joint");
    msg_full_js.name.push_back("right_elbow_pitch_joint");
    msg_full_js.name.push_back("right_wrist_platform_joint");
    msg_full_js.name.push_back("right_wrist_pitch_joint");
    msg_full_js.name.push_back("right_wrist_yaw_joint");
    msg_full_js.name.push_back("right_wrist_roll_joint");

    geometry_msgs::PoseStamped msg_pose;

    int counter = 0;
    // ------------ run() --------------------------
    while (ros::ok()) {

        // --------- Compute current pose & publish ----------
        // psm forward kinematics
        mtm_pose_current = mtm_manip.ForwardKinematics(mtm_joint_current);

        mtsCISSTToROS(mtm_pose_current, msg_pose);
//        if (count % 10 == 0) {
//            std::cerr << mtm_pose_current << std::endl << std::endl;
//        }

        // publish current pose
        pub_mtm_pose.publish(msg_pose);

        // MODE_RESET: send HOME joint position
        // MODE_MANUAL: controled by JSP GUI, not sending anything to jsp
        // MODE_HOLD: disable JSP GUI, not sending anything to jsp
        // MODE_CLUTCH: enable J1-3 slider, J4-7 follow
        // MODE_TELEOP: take command pose, send to jsp

        // ----- control mode -------
        switch (control_mode)
        {
        case MTM::MODE_RESET:
            mtm_joint_command.SetAll(0.0);
            mtm_joint_command[3] = cmnPI_2;
            mtm_joint_command[4] = cmnPI_2;
            break;
        case MTM::MODE_MANUAL:
            mtm_joint_command.ForceAssign(mtm_joint_current);
            std::fill(msg_full_js.position.begin(), msg_full_js.position.end(), 1);
            pub_mtm_enable_slider.publish(msg_full_js);
            break;
        case MTM::MODE_HOLD:
            mtm_joint_command.ForceAssign(mtm_joint_current);
            std::fill(msg_full_js.position.begin(), msg_full_js.position.end(), -1);
            pub_mtm_enable_slider.publish(msg_full_js);
            break;
        case MTM::MODE_CLUTCH:
            mtm_joint_command.ForceAssign(mtm_joint_current);
            std::fill(msg_full_js.position.begin(), msg_full_js.position.begin()+3, 1);
            std::fill(msg_full_js.position.begin()+3, msg_full_js.position.end(), -1);
            pub_mtm_enable_slider.publish(msg_full_js);
            break;
        case MTM::MODE_TELEOP:
            // teleop
            std::fill(msg_full_js.position.begin(), msg_full_js.position.end(), 1);
            pub_mtm_enable_slider.publish(msg_full_js);
            break;
        default:
            break;
        }

        // copy to msg
        msg_full_js.position.resize(mtm_joint_command.size());
        std::copy(mtm_joint_command.begin(), mtm_joint_command.end(), msg_full_js.position.begin());
        if (control_mode == MTM::MODE_RESET) {
            // publish cmd joint state
            pub_mtm_joint_state.publish(msg_full_js);
        }

        // MODE_HOLE && MODE_CLUTCH
        if (control_mode == MTM::MODE_HOLD || control_mode == MTM::MODE_CLUTCH) {
            // inverse kinematics
            mtm_joint_command.ForceAssign(mtm_joint_current);
            mtm_manip.InverseKinematics(mtm_joint_command, mtm_pose_command);

            // now copy to msg_rot_js rotation only
            sensor_msgs::JointState msg_rot_js;
            msg_rot_js.name.clear();
            msg_rot_js.name.push_back("right_wrist_platform_joint");
            msg_rot_js.name.push_back("right_wrist_pitch_joint");
            msg_rot_js.name.push_back("right_wrist_yaw_joint");
            msg_rot_js.name.push_back("right_wrist_roll_joint");
            msg_rot_js.position.resize(4); // only last 4 joints
            std::copy(mtm_joint_command.begin()+3, mtm_joint_command.end(), msg_rot_js.position.begin());
            pub_mtm_joint_state.publish(msg_rot_js);
        }

        ros::spinOnce();
        rate.sleep();
        counter++;
    }

    return 0;
}
Esempio n. 2
0
void mtsTeleop::Run(void)
{
    // increment counter
    counter_++;

    // cisst process queued commands
    ProcessQueuedCommands();

    // refresh data
    ros::spinOnce();

    // publish
#if 1
    if (counter_%10 == 0) {
        //            std::cerr << "clutch = " << is_clutch_pressed_ << std::endl;
        //            std::cerr << " mtm = " << std::endl
        //                      << mtm_pose_cur_ << std::endl;
    }
#endif

    vctMatRot3 mtm2psm;
    mtm2psm.Assign(-1.0, 0.0, 0.0,
                   0.0, -1.0, 0.0,
                   0.0, 0.0, 1.0);

    vctMatRot3 mtm02psm0;
    mtm02psm0.Assign(1.0, 0.0, 0.0,
                     0.0, 0.0, 1.0,
                     0.0, -1.0, 0.0);

    vctMatRot3 psm6tomtm7;
    psm6tomtm7.Assign(1.0, 0.0, 0.0,
                      0.0, 0.0, -1.0,
                      0.0, 1.0, 0.0);

    if (is_enabled_) {

        // assign current psm pose
        psm_pose_cmd_.Assign(psm_pose_cur_);

        // translation
        double scale = 0.2;
        vct3 mtm_tra = mtm_pose_cur_.Translation() - mtm_pose_pre_.Translation();
        vct3 psm_tra = scale * mtm_tra;

        psm_pose_cmd_.Translation() = psm_pose_cmd_.Translation() + mtm2psm * psm_tra;

        // rotation
        vctMatRot3 psm_motion_rot;
        //psm_motion_rot = mtm2psm * mtm_pose_cur_.Rotation() * psm6tomtm7;
        psm_motion_rot = mtm02psm0* mtm_pose_cur_.Rotation() * psm6tomtm7;
        psm_pose_cmd_.Rotation().FromNormalized(psm_motion_rot);

//        std::cerr << " teleop enabled " << counter_ << std::endl;

    } else {
        // In this mode, MTM orientation follows PSM orientation

        // keep current pose
        psm_pose_cmd_.Assign(psm_pose_cur_);

        // mtm needs to follow psm orientation
        mtm_pose_cmd_.Assign(mtm_pose_cur_);
        vctMatRot3 mtm_rot_cmd;
        mtm_rot_cmd = mtm02psm0.Inverse() * psm_pose_cur_.Rotation() * psm6tomtm7.Inverse();
        mtm_pose_cmd_.Rotation().FromNormalized(mtm_rot_cmd);
    }

    mtsCISSTToROS(psm_pose_cmd_, msg_psm_pose_);
    pub_psm_pose_.publish(msg_psm_pose_);
    mtsCISSTToROS(mtm_pose_cmd_, msg_mtm_pose_);
    pub_mtm_pose_.publish(msg_mtm_pose_);

    // save current pose as previous
    mtm_pose_pre_.Assign(mtm_pose_cur_);

#if 0
    if (counter_%10 == 0) {
        std::cerr << " mtm = " << std::endl
                  << mtm_pose_cur_ << std::endl;
        std::cerr << "psm = " << std::endl
                  << psm_pose_cur_ << std::endl << std::endl;
    }
#endif

}