robManipulator::Errno mtsIntuitiveResearchKitECM::InverseKinematics(vctDoubleVec & jointSet,
const vctFrm4x4 & cartesianGoal)
{
// re-align desired frame to 4 axis direction to reduce free space
vctDouble3 shaft = cartesianGoal.Translation();
shaft.NormalizedSelf();
const vctDouble3 z = cartesianGoal.Rotation().Column(2).Ref<3>(); // last column of rotation matrix
vctDouble3 axis;
axis.CrossProductOf(z, shaft);
const double angle = acos(vctDotProduct(z, shaft));
const vctMatRot3 reAlign(vctAxAnRot3(axis, angle, VCT_NORMALIZE));
vctFrm4x4 newGoal;
newGoal.Translation().Assign(cartesianGoal.Translation());
newGoal.Rotation().ProductOf(reAlign, cartesianGoal.Rotation());
if (Manipulator.InverseKinematics(jointSet, newGoal) == robManipulator::ESUCCESS) {
// find closest solution mod 2 pi
const double difference = JointGet[3] - jointSet[3];
const double differenceInTurns = nearbyint(difference / (2.0 * cmnPI));
jointSet[3] = jointSet[3] + differenceInTurns * 2.0 * cmnPI;
// make sure we are away from RCM point, this test is
// simplistic
if (jointSet[2] < 40.0 * cmn_mm) {
jointSet[2] = 40.0 * cmn_mm;
}
#if 0
vctFrm4x4 forward = Manipulator.ForwardKinematics(jointSet);
vctDouble3 diff;
diff.DifferenceOf(forward.Translation(), newGoal.Translation());
std::cerr << cmnInternalTo_mm(diff.Norm()) << "mm ";
#endif
return robManipulator::ESUCCESS;
}
return robManipulator::EFAILURE;
}
int main(int argc, char** argv)
{
// ros initialization
ros::init(argc, argv, "dvrk_psm_kinematics");
ros::NodeHandle nh, nh_private("~");
ros::Rate rate(200); // 200 hz rate
// parameter
std::string robot_name;
nh_private.getParam("robot_name", robot_name);
// subscriber
ros::Subscriber sub_psm_cmd =
nh.subscribe("/dvrk_psm/cartesian_pose_command", 1, psm_cmd_pose_cb);
ros::Subscriber sub_mtm_gripper =
nh.subscribe("/dvrk_mtm/gripper_position", 1, mtm_gripper_cb);
ros::Subscriber sub_psm_fb =
nh.subscribe("/dvrk_psm/joint_states", 1, psm_joint_feedback_cb);
ros::Subscriber sub_mode =
nh.subscribe("/dvrk_psm/control_mode", 1, psm_mode_cb);
// publisher
ros::Publisher pub_psm_joint_state_cmd =
nh.advertise<sensor_msgs::JointState>("/dvrk_psm/joint_states_command", 1);
ros::Publisher pub_psm_pose_current =
nh.advertise<geometry_msgs::PoseStamped>("/dvrk_psm/cartesian_pose_current", 1);
ros::Publisher pub_psm_enable_slider =
nh.advertise<sensor_msgs::JointState>("/dvrk_psm/joint_state_publisher/enable_slider", 100);
// --- cisst robManipulator ---
std::string filename = ros::package::getPath("dvrk_kinematics");
filename.append("/config/dvpsm.rob");
robManipulator psm_manip;
robManipulator::Errno result;
result = psm_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 -----
psm_joint_current.SetSize(6); // 6 + 1 (open angle)
psm_joint_current.SetAll(0.0);
psm_joint_command.ForceAssign(psm_joint_current);
sensor_msgs::JointState msg_js;
msg_js.name.clear();
msg_js.name.push_back(robot_name + "_outer_yaw_joint");
msg_js.name.push_back(robot_name + "_outer_pitch_joint_1");
msg_js.name.push_back(robot_name + "_outer_insertion_joint");
msg_js.name.push_back(robot_name + "_outer_roll_joint");
msg_js.name.push_back(robot_name + "_outer_wrist_pitch_joint");
msg_js.name.push_back(robot_name + "_outer_wrist_yaw_joint");
msg_js.name.push_back(robot_name + "_outer_wrist_open_angle_joint_1");
geometry_msgs::PoseStamped msg_pose;
int count = 0;
control_mode = PSM::MODE_RESET; // start with reset_mode
vctFrm4x4 frame6to7;
frame6to7.Assign(0.0, -1.0, 0.0, 0.0,
0.0, 0.0, 1.0, 0.0102,
-1.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 1.0);
// used to compensate joint 4
double j4_compensate = 0;
// ------------ run() --------------------------
while (ros::ok()) {
// --------- Compute current pose & publish ----------
// psm forward kinematics
psm_pose_current = psm_manip.ForwardKinematics(psm_joint_current);
psm_pose_current = psm_pose_current * frame6to7;
psm_pose_current.Rotation().NormalizedSelf();
mtsCISSTToROS(psm_pose_current, msg_pose);
// publish current pose
pub_psm_pose_current.publish(msg_pose);
// ---------- Compute command psm joint positin -----------
vctFrm4x4 pose6;
// 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_TELEOP: take command pose, send to jsp
// control_mode = 4
switch(control_mode)
{
case PSM::MODE_RESET:
psm_joint_command.SetAll(0.0);
psm_joint_command[2] = 0.10;
j4_compensate = 0;
psm_pose_command.Assign(psm_pose_current);
break;
case PSM::MODE_MANUAL:
// do nothing for MANUAL
// controlled using Slifer GUI
j4_compensate = 0;
psm_pose_command.Assign(psm_pose_current);
std::fill(msg_js.position.begin(), msg_js.position.end(), 1);
pub_psm_enable_slider.publish(msg_js);
break;
case PSM::MODE_HOLD:
psm_pose_command.Assign(psm_pose_current);
std::fill(msg_js.position.begin(), msg_js.position.end(), -1);
pub_psm_enable_slider.publish(msg_js);
break;
case PSM::MODE_TELEOP:
// psm_pose_command updated in callback!
cout << "In Teleop" << endl;
pose6 = psm_pose_command * frame6to7.Inverse();
psm_manip.InverseKinematics(psm_joint_command, pose6);
// joint 4 is a special case
if (psm_joint_command_prev[3] - psm_joint_command[3] > 5) {
j4_compensate = 2 * cmnPI;
} else if (psm_joint_command_prev[3] - psm_joint_command[3] < -5) {
j4_compensate = - 2 * cmnPI;
} else {
j4_compensate = 0;
}
psm_joint_command[3] += j4_compensate;
psm_joint_current.Assign(psm_joint_command);
// disable slider
std::fill(msg_js.position.begin(), msg_js.position.end(), -1);
pub_psm_enable_slider.publish(msg_js);
// std::cerr << "PSM_teleop" << std::endl;
break;
default:
ROS_ERROR_STREAM("Should not come here !");
break;
}
psm_joint_command_prev.ForceAssign(psm_joint_command);
// ----- Assign command joint state and Publish --------
// 6 + open angle (gripper)
msg_js.position.resize(psm_joint_command.size() + 1);
std::copy(psm_joint_command.begin(), psm_joint_command.end(), msg_js.position.begin());
msg_js.position[6] = mtm_gripper;
if (control_mode != PSM::MODE_MANUAL && control_mode != PSM::MODE_HOLD) {
pub_psm_joint_state_cmd.publish(msg_js);
}
ros::spinOnce();
rate.sleep();
count++;
}
return 0;
}