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;
}
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
}