From 0daaaacc4d19745719d7de73ed538d4a00f6b1ed Mon Sep 17 00:00:00 2001 From: DevonAsh Date: Tue, 21 Jun 2016 19:48:01 +0000 Subject: [PATCH 1/4] Removed prefix code which prevented the plugin from being used for multiple arms on one robot. The prefix code does nothing anyway, all it does is check/confirm that the chain contains links for a UR robot, which one can assume outside of this code that it is being used for a UR robot --- ur_kinematics/src/ur_moveit_plugin.cpp | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/ur_kinematics/src/ur_moveit_plugin.cpp b/ur_kinematics/src/ur_moveit_plugin.cpp index 160558603..72fdb472d 100644 --- a/ur_kinematics/src/ur_moveit_plugin.cpp +++ b/ur_kinematics/src/ur_moveit_plugin.cpp @@ -323,7 +323,7 @@ bool URKinematicsPlugin::initialize(const std::string &robot_description, max_solver_iterations_ = max_solver_iterations; epsilon_ = epsilon; - private_handle.param("arm_prefix", arm_prefix_, ""); + /* private_handle.param("arm_prefix", arm_prefix_, ""); ur_joint_names_.push_back(arm_prefix_ + "shoulder_pan_joint"); ur_joint_names_.push_back(arm_prefix_ + "shoulder_lift_joint"); @@ -362,10 +362,13 @@ bool URKinematicsPlugin::initialize(const std::string &robot_description, } last_ur_joint_ind = cur_ur_joint_ind; } - // if successful, the kinematic chain includes a serial chain of the UR joints + // if successful, the kinematic chain includes a serial chain of the UR joints */ - kdl_tree.getChain(getBaseFrame(), ur_link_names_.front(), kdl_base_chain_); - kdl_tree.getChain(ur_link_names_.back(), getTipFrame(), kdl_tip_chain_); + // Change this to use the link names provided by model, not by hard coded naming. (base_frame provided as arg) + ROS_INFO("Using base frame %s from model", base_frame.c_str()); + ROS_INFO("Using tip frame %s from model", tip_frame.c_str()); + kdl_tree.getChain(getBaseFrame(), base_frame, kdl_base_chain_); + kdl_tree.getChain(tip_frame, getTipFrame(), kdl_tip_chain_); // weights for redundant solution selection ik_weights_.resize(6); From 692f4bcde4ed02682809ecc3228ec6e5d2494c8e Mon Sep 17 00:00:00 2001 From: DevonAsh Date: Tue, 21 Jun 2016 22:00:16 +0000 Subject: [PATCH 2/4] It now is agnostic to how many arms there are - and the plugin works regardless of prefixes --- ur_kinematics/src/ur_moveit_plugin.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/ur_kinematics/src/ur_moveit_plugin.cpp b/ur_kinematics/src/ur_moveit_plugin.cpp index 72fdb472d..6227c9eb6 100644 --- a/ur_kinematics/src/ur_moveit_plugin.cpp +++ b/ur_kinematics/src/ur_moveit_plugin.cpp @@ -323,8 +323,13 @@ bool URKinematicsPlugin::initialize(const std::string &robot_description, max_solver_iterations_ = max_solver_iterations; epsilon_ = epsilon; - /* private_handle.param("arm_prefix", arm_prefix_, ""); + private_handle.param("arm_prefix", arm_prefix_, ""); + // Load the joint names from the group model + ur_joint_names_ = joint_model_group->getJointModelNames(); + ur_link_names_ = joint_model_group->getLinkModelNames(); + + /* ur_joint_names_.push_back(arm_prefix_ + "shoulder_pan_joint"); ur_joint_names_.push_back(arm_prefix_ + "shoulder_lift_joint"); ur_joint_names_.push_back(arm_prefix_ + "elbow_joint"); @@ -340,7 +345,7 @@ bool URKinematicsPlugin::initialize(const std::string &robot_description, ur_link_names_.push_back(arm_prefix_ + "wrist_1_link"); // 5 ur_link_names_.push_back(arm_prefix_ + "wrist_2_link"); // 6 ur_link_names_.push_back(arm_prefix_ + "wrist_3_link"); // 7 - ur_link_names_.push_back(arm_prefix_ + "ee_link"); // 8 + ur_link_names_.push_back(arm_prefix_ + "ee_link"); // 8 */ ur_joint_inds_start_ = getJointIndex(ur_joint_names_[0]); From ed5a8347521ce2a23f74d13cc917180887a22286 Mon Sep 17 00:00:00 2001 From: Devon Ash Date: Thu, 23 Jun 2016 20:10:12 +0000 Subject: [PATCH 3/4] Update ur_moveit_plugin.cpp --- ur_kinematics/src/ur_moveit_plugin.cpp | 18 ------------------ 1 file changed, 18 deletions(-) diff --git a/ur_kinematics/src/ur_moveit_plugin.cpp b/ur_kinematics/src/ur_moveit_plugin.cpp index 6227c9eb6..49b700faa 100644 --- a/ur_kinematics/src/ur_moveit_plugin.cpp +++ b/ur_kinematics/src/ur_moveit_plugin.cpp @@ -329,24 +329,6 @@ bool URKinematicsPlugin::initialize(const std::string &robot_description, ur_joint_names_ = joint_model_group->getJointModelNames(); ur_link_names_ = joint_model_group->getLinkModelNames(); - /* - ur_joint_names_.push_back(arm_prefix_ + "shoulder_pan_joint"); - ur_joint_names_.push_back(arm_prefix_ + "shoulder_lift_joint"); - ur_joint_names_.push_back(arm_prefix_ + "elbow_joint"); - ur_joint_names_.push_back(arm_prefix_ + "wrist_1_joint"); - ur_joint_names_.push_back(arm_prefix_ + "wrist_2_joint"); - ur_joint_names_.push_back(arm_prefix_ + "wrist_3_joint"); - - ur_link_names_.push_back(arm_prefix_ + "base_link"); // 0 - ur_link_names_.push_back(arm_prefix_ + "ur_base_link"); // 1 - ur_link_names_.push_back(arm_prefix_ + "shoulder_link"); // 2 - ur_link_names_.push_back(arm_prefix_ + "upper_arm_link"); // 3 - ur_link_names_.push_back(arm_prefix_ + "forearm_link"); // 4 - ur_link_names_.push_back(arm_prefix_ + "wrist_1_link"); // 5 - ur_link_names_.push_back(arm_prefix_ + "wrist_2_link"); // 6 - ur_link_names_.push_back(arm_prefix_ + "wrist_3_link"); // 7 - ur_link_names_.push_back(arm_prefix_ + "ee_link"); // 8 */ - ur_joint_inds_start_ = getJointIndex(ur_joint_names_[0]); // check to make sure the serial chain is properly defined in the model From 4b8a17acd4d1413b10871fe3f714bcdbaa17f632 Mon Sep 17 00:00:00 2001 From: Devon Ash Date: Tue, 12 Jul 2016 13:36:27 -0400 Subject: [PATCH 4/4] Update ur_moveit_plugin.cpp --- ur_kinematics/src/ur_moveit_plugin.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ur_kinematics/src/ur_moveit_plugin.cpp b/ur_kinematics/src/ur_moveit_plugin.cpp index 49b700faa..f163fc5f8 100644 --- a/ur_kinematics/src/ur_moveit_plugin.cpp +++ b/ur_kinematics/src/ur_moveit_plugin.cpp @@ -349,7 +349,7 @@ bool URKinematicsPlugin::initialize(const std::string &robot_description, } last_ur_joint_ind = cur_ur_joint_ind; } - // if successful, the kinematic chain includes a serial chain of the UR joints */ + // if successful, the kinematic chain includes a serial chain of the UR joints // Change this to use the link names provided by model, not by hard coded naming. (base_frame provided as arg) ROS_INFO("Using base frame %s from model", base_frame.c_str());