diff --git a/ur_kinematics/CMakeLists.txt b/ur_kinematics/CMakeLists.txt
index a63d597bb118da810e02aeb53c763555fc56af13..014cd44c87f263dab102d8143340022106b8c262 100644
--- a/ur_kinematics/CMakeLists.txt
+++ b/ur_kinematics/CMakeLists.txt
@@ -4,7 +4,7 @@ project(ur_kinematics)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
-find_package(catkin REQUIRED COMPONENTS roscpp geometry_msgs moveit_core pluginlib)
+find_package(catkin REQUIRED COMPONENTS roscpp geometry_msgs moveit_core moveit_ros_planning pluginlib tf_conversions)
## System dependencies are found with CMake's conventions
find_package(Boost REQUIRED COMPONENTS system)
@@ -50,7 +50,7 @@ catkin_package(
INCLUDE_DIRS include
LIBRARIES ur10_kin ur5_kin ur10_moveit_plugin ur5_moveit_plugin
CATKIN_DEPENDS pluginlib
- DEPENDS moveit_core
+ DEPENDS moveit_core
# CATKIN_DEPENDS other_catkin_pkg
# DEPENDS system_lib
)
@@ -82,6 +82,8 @@ set_target_properties(ur10_moveit_plugin PROPERTIES COMPILE_DEFINITIONS "UR10_PA
target_link_libraries(ur10_moveit_plugin
${catkin_LIBRARIES}
${Boost_LIBRARIES}
+ ${moveit_ros_planning_LIBRARIES}
+ /opt/ros/hydro/lib/libmoveit_kdl_kinematics_plugin.so
ur10_kin)
add_library(ur5_moveit_plugin src/ur_moveit_plugin.cpp)
@@ -89,6 +91,8 @@ set_target_properties(ur5_moveit_plugin PROPERTIES COMPILE_DEFINITIONS "UR5_PARA
target_link_libraries(ur5_moveit_plugin
${catkin_LIBRARIES}
${Boost_LIBRARIES}
+ ${moveit_ros_planning_LIBRARIES}
+ /opt/ros/hydro/lib/libmoveit_kdl_kinematics_plugin.so
ur5_kin)
## Declare a cpp executable
diff --git a/ur_kinematics/include/ur_kinematics/ur_kin.h b/ur_kinematics/include/ur_kinematics/ur_kin.h
index 2f5f45204d1dc196402c1729da253e2855c43313..ee5a0465f033f2b2eb526832b1e548375131ae92 100644
--- a/ur_kinematics/include/ur_kinematics/ur_kin.h
+++ b/ur_kinematics/include/ur_kinematics/ur_kin.h
@@ -86,6 +86,8 @@ namespace ur_kinematics {
// @param T The 4x4 end effector pose in row-major ordering
void forward(const double* q, double* T);
+ // @param q The 6 joint values
+ // @param Ti The 4x4 link i pose in row-major ordering. If NULL, nothing is stored.
void forward_all(const double* q, double* T1, double* T2, double* T3,
double* T4, double* T5, double* T6);
diff --git a/ur_kinematics/include/ur_kinematics/ur_moveit_plugin.h b/ur_kinematics/include/ur_kinematics/ur_moveit_plugin.h
index a4b556aed064680942c38bc6762442efe4378955..9e0b5f608103688f889b3aabf763fb85634c2238 100644
--- a/ur_kinematics/include/ur_kinematics/ur_moveit_plugin.h
+++ b/ur_kinematics/include/ur_kinematics/ur_moveit_plugin.h
@@ -2,84 +2,74 @@
#include <ros/ros.h>
#include <moveit/kinematics_base/kinematics_base.h>
#include <moveit_msgs/KinematicSolverInfo.h>
+#include <moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h>
namespace ur_kinematics
{
-class URKinematicsPlugin : public kinematics::KinematicsBase
+class URKinematicsPlugin : public kdl_kinematics_plugin::KDLKinematicsPlugin
{
public:
- /**
+/**
* @brief Plugin-able interface to the ur kinematics
*/
URKinematicsPlugin();
+//
+// //~URKinematicsPlugin();
- //~URKinematicsPlugin();
-
- virtual bool getPositionIK(const geometry_msgs::Pose &ik_pose,
- const std::vector<double> &ik_seed_state,
- std::vector<double> &solution,
- moveit_msgs::MoveItErrorCodes &error_code,
- const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
-
- virtual bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
- const std::vector<double> &ik_seed_state,
- double timeout,
- std::vector<double> &solution,
- moveit_msgs::MoveItErrorCodes &error_code,
- const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
-
- virtual bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
- const std::vector<double> &ik_seed_state,
- double timeout,
- const std::vector<double> &consistency_limits,
- std::vector<double> &solution,
- moveit_msgs::MoveItErrorCodes &error_code,
- const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
-
- virtual bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
- const std::vector<double> &ik_seed_state,
- double timeout,
- std::vector<double> &solution,
- const IKCallbackFn &solution_callback,
- moveit_msgs::MoveItErrorCodes &error_code,
- const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
-
- virtual bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
- const std::vector<double> &ik_seed_state,
- double timeout,
- const std::vector<double> &consistency_limits,
- std::vector<double> &solution,
- const IKCallbackFn &solution_callback,
- moveit_msgs::MoveItErrorCodes &error_code,
- const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
-
- virtual bool getPositionFK(const std::vector<std::string> &link_names,
- const std::vector<double> &joint_angles,
- std::vector<geometry_msgs::Pose> &poses) const;
-
- /**
+//
+// virtual bool getPositionFK(const std::vector<std::string> &link_names,
+// const std::vector<double> &joint_angles,
+// std::vector<geometry_msgs::Pose> &poses) const;
+//
+/**
* @brief Initialization function for the kinematics
* @return True if initialization was successful, false otherwise
*/
- virtual bool initialize(const std::string& robot_description,
- const std::string& group_name,
- const std::string& base_frame,
- const std::string& tip_frame,
- double search_discretization);
-
- /**
-* @brief Return all the joint names in the order they are used internally
-*/
- const std::vector<std::string>& getJointNames() const;
+virtual bool initialize(const std::string& robot_description,
+ const std::string& group_name,
+ const std::string& base_frame,
+ const std::string& tip_frame,
+ double search_discretization);
+//
+// /**
+// * @brief Return all the joint names in the order they are used internally
+// */
+// const std::vector<std::string>& getJointNames() const;
+//
+// /**
+// * @brief Return all the link names in the order they are represented internally
+// */
+// const std::vector<std::string>& getLinkNames() const;
+//
+protected:
/**
-* @brief Return all the link names in the order they are represented internally
+* @brief Given a desired pose of the end-effector, search for the joint angles required to reach it.
+* This particular method is intended for "searching" for a solutions by stepping through the redundancy
+* (or other numerical routines).
+* @param ik_pose the desired pose of the link
+* @param ik_seed_state an initial guess solution for the inverse kinematics
+* @param timeout The amount of time (in seconds) available to the solver
+* @param solution the solution vector
+* @param solution_callback A callback solution for the IK solution
+* @param error_code an error code that encodes the reason for failure or success
+* @param check_consistency Set to true if consistency check needs to be performed
+* @param redundancy The index of the redundant joint
+* @param consistency_limit The returned solutuion will contain a value for the redundant joint in the range [seed_state(redundancy_limit)-consistency_limit,seed_state(redundancy_limit)+consistency_limit]
+* @return True if a valid solution was found, false otherwise
*/
- const std::vector<std::string>& getLinkNames() const;
-
-protected:
+ bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
+ const std::vector<double> &ik_seed_state,
+ double timeout,
+ std::vector<double> &solution,
+ const IKCallbackFn &solution_callback,
+ moveit_msgs::MoveItErrorCodes &error_code,
+ const std::vector<double> &consistency_limits,
+ const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
- moveit_msgs::KinematicSolverInfo fk_solver_info_, ik_solver_info_;
+ std::vector<double> ik_weights_;
+//
+// moveit_msgs::KinematicSolverInfo fk_solver_info_, ik_solver_info_;
};
}
diff --git a/ur_kinematics/package.xml b/ur_kinematics/package.xml
index 14b9a67dd8edfad680c9ad9f1c79bca86162a253..2891b86c05e453212f3eeba09c7aa561f02cdb24 100644
--- a/ur_kinematics/package.xml
+++ b/ur_kinematics/package.xml
@@ -47,14 +47,18 @@
<buildtool_depend>catkin</buildtool_depend>
<build_depend>moveit_core</build_depend>
+ <build_depend>moveit_ros_planning</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>pluginlib</build_depend>
+ <build_depend>tf_conversions</build_depend>
<run_depend>moveit_core</run_depend>
+ <run_depend>moveit_ros_planning</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>geometry_msgs</run_depend>
<run_depend>pluginlib</run_depend>
+ <run_depend>tf_conversions</run_depend>
<!-- The export tag contains other, unspecified, tags -->
diff --git a/ur_kinematics/src/ur_kin.cpp b/ur_kinematics/src/ur_kin.cpp
index d10759dab9997e8e328eccc8d5b0805343f85d89..8c92ea3a13341b8c6f208e6fe5826563e5761adc 100644
--- a/ur_kinematics/src/ur_kin.cpp
+++ b/ur_kinematics/src/ur_kin.cpp
@@ -45,107 +45,119 @@ namespace ur_kinematics {
double s23 = sin(q23), c23 = cos(q23);
double s234 = sin(q234), c234 = cos(q234);
- *T1 = c1; T1++;
- *T1 = 0; T1++;
- *T1 = s1; T1++;
- *T1 = 0; T1++;
- *T1 = s1; T1++;
- *T1 = 0; T1++;
- *T1 = -c1; T1++;
- *T1 = 0; T1++;
- *T1 = 0; T1++;
- *T1 = 1; T1++;
- *T1 = 0; T1++;
- *T1 =d1; T1++;
- *T1 = 0; T1++;
- *T1 = 0; T1++;
- *T1 = 0; T1++;
- *T1 = 1; T1++;
-
- *T2 = c1*c2; T2++;
- *T2 = -c1*s2; T2++;
- *T2 = s1; T2++;
- *T2 =a2*c1*c2; T2++;
- *T2 = c2*s1; T2++;
- *T2 = -s1*s2; T2++;
- *T2 = -c1; T2++;
- *T2 =a2*c2*s1; T2++;
- *T2 = s2; T2++;
- *T2 = c2; T2++;
- *T2 = 0; T2++;
- *T2 = d1 + a2*s2; T2++;
- *T2 = 0; T2++;
- *T2 = 0; T2++;
- *T2 = 0; T2++;
- *T2 = 1; T2++;
-
- *T3 = c23*c1; T3++;
- *T3 = -s23*c1; T3++;
- *T3 = s1; T3++;
- *T3 =c1*(a3*c23 + a2*c2); T3++;
- *T3 = c23*s1; T3++;
- *T3 = -s23*s1; T3++;
- *T3 = -c1; T3++;
- *T3 =s1*(a3*c23 + a2*c2); T3++;
- *T3 = s23; T3++;
- *T3 = c23; T3++;
- *T3 = 0; T3++;
- *T3 = d1 + a3*s23 + a2*s2; T3++;
- *T3 = 0; T3++;
- *T3 = 0; T3++;
- *T3 = 0; T3++;
- *T3 = 1; T3++;
-
- *T4 = c234*c1; T4++;
- *T4 = s1; T4++;
- *T4 = s234*c1; T4++;
- *T4 =c1*(a3*c23 + a2*c2) + d4*s1; T4++;
- *T4 = c234*s1; T4++;
- *T4 = -c1; T4++;
- *T4 = s234*s1; T4++;
- *T4 =s1*(a3*c23 + a2*c2) - d4*c1; T4++;
- *T4 = s234; T4++;
- *T4 = 0; T4++;
- *T4 = -c234; T4++;
- *T4 = d1 + a3*s23 + a2*s2; T4++;
- *T4 = 0; T4++;
- *T4 = 0; T4++;
- *T4 = 0; T4++;
- *T4 = 1; T4++;
-
- *T5 = s1*s5 + c234*c1*c5; T5++;
- *T5 = -s234*c1; T5++;
- *T5 = c5*s1 - c234*c1*s5; T5++;
- *T5 =c1*(a3*c23 + a2*c2) + d4*s1 + d5*s234*c1; T5++;
- *T5 = c234*c5*s1 - c1*s5; T5++;
- *T5 = -s234*s1; T5++;
- *T5 = - c1*c5 - c234*s1*s5; T5++;
- *T5 =s1*(a3*c23 + a2*c2) - d4*c1 + d5*s234*s1; T5++;
- *T5 = s234*c5; T5++;
- *T5 = c234; T5++;
- *T5 = -s234*s5; T5++;
- *T5 = d1 + a3*s23 + a2*s2 - d5*c234; T5++;
- *T5 = 0; T5++;
- *T5 = 0; T5++;
- *T5 = 0; T5++;
- *T5 = 1; T5++;
-
- *T6 = c6*(s1*s5 + c234*c1*c5) - s234*c1*s6; T6++;
- *T6 = - s6*(s1*s5 + c234*c1*c5) - s234*c1*c6; T6++;
- *T6 = c5*s1 - c234*c1*s5; T6++;
- *T6 =d6*(c5*s1 - c234*c1*s5) + c1*(a3*c23 + a2*c2) + d4*s1 + d5*s234*c1; T6++;
- *T6 = - c6*(c1*s5 - c234*c5*s1) - s234*s1*s6; T6++;
- *T6 = s6*(c1*s5 - c234*c5*s1) - s234*c6*s1; T6++;
- *T6 = - c1*c5 - c234*s1*s5; T6++;
- *T6 =s1*(a3*c23 + a2*c2) - d4*c1 - d6*(c1*c5 + c234*s1*s5) + d5*s234*s1; T6++;
- *T6 = c234*s6 + s234*c5*c6; T6++;
- *T6 = c234*c6 - s234*c5*s6; T6++;
- *T6 = -s234*s5; T6++;
- *T6 = d1 + a3*s23 + a2*s2 - d5*c234 - d6*s234*s5; T6++;
- *T6 = 0; T6++;
- *T6 = 0; T6++;
- *T6 = 0; T6++;
- *T6 = 1; T6++;
+ if(T1 != NULL) {
+ *T1 = c1; T1++;
+ *T1 = 0; T1++;
+ *T1 = s1; T1++;
+ *T1 = 0; T1++;
+ *T1 = s1; T1++;
+ *T1 = 0; T1++;
+ *T1 = -c1; T1++;
+ *T1 = 0; T1++;
+ *T1 = 0; T1++;
+ *T1 = 1; T1++;
+ *T1 = 0; T1++;
+ *T1 =d1; T1++;
+ *T1 = 0; T1++;
+ *T1 = 0; T1++;
+ *T1 = 0; T1++;
+ *T1 = 1; T1++;
+ }
+
+ if(T2 != NULL) {
+ *T2 = c1*c2; T2++;
+ *T2 = -c1*s2; T2++;
+ *T2 = s1; T2++;
+ *T2 =a2*c1*c2; T2++;
+ *T2 = c2*s1; T2++;
+ *T2 = -s1*s2; T2++;
+ *T2 = -c1; T2++;
+ *T2 =a2*c2*s1; T2++;
+ *T2 = s2; T2++;
+ *T2 = c2; T2++;
+ *T2 = 0; T2++;
+ *T2 = d1 + a2*s2; T2++;
+ *T2 = 0; T2++;
+ *T2 = 0; T2++;
+ *T2 = 0; T2++;
+ *T2 = 1; T2++;
+ }
+
+ if(T3 != NULL) {
+ *T3 = c23*c1; T3++;
+ *T3 = -s23*c1; T3++;
+ *T3 = s1; T3++;
+ *T3 =c1*(a3*c23 + a2*c2); T3++;
+ *T3 = c23*s1; T3++;
+ *T3 = -s23*s1; T3++;
+ *T3 = -c1; T3++;
+ *T3 =s1*(a3*c23 + a2*c2); T3++;
+ *T3 = s23; T3++;
+ *T3 = c23; T3++;
+ *T3 = 0; T3++;
+ *T3 = d1 + a3*s23 + a2*s2; T3++;
+ *T3 = 0; T3++;
+ *T3 = 0; T3++;
+ *T3 = 0; T3++;
+ *T3 = 1; T3++;
+ }
+
+ if(T4 != NULL) {
+ *T4 = c234*c1; T4++;
+ *T4 = s1; T4++;
+ *T4 = s234*c1; T4++;
+ *T4 =c1*(a3*c23 + a2*c2) + d4*s1; T4++;
+ *T4 = c234*s1; T4++;
+ *T4 = -c1; T4++;
+ *T4 = s234*s1; T4++;
+ *T4 =s1*(a3*c23 + a2*c2) - d4*c1; T4++;
+ *T4 = s234; T4++;
+ *T4 = 0; T4++;
+ *T4 = -c234; T4++;
+ *T4 = d1 + a3*s23 + a2*s2; T4++;
+ *T4 = 0; T4++;
+ *T4 = 0; T4++;
+ *T4 = 0; T4++;
+ *T4 = 1; T4++;
+ }
+
+ if(T5 != NULL) {
+ *T5 = s1*s5 + c234*c1*c5; T5++;
+ *T5 = -s234*c1; T5++;
+ *T5 = c5*s1 - c234*c1*s5; T5++;
+ *T5 =c1*(a3*c23 + a2*c2) + d4*s1 + d5*s234*c1; T5++;
+ *T5 = c234*c5*s1 - c1*s5; T5++;
+ *T5 = -s234*s1; T5++;
+ *T5 = - c1*c5 - c234*s1*s5; T5++;
+ *T5 =s1*(a3*c23 + a2*c2) - d4*c1 + d5*s234*s1; T5++;
+ *T5 = s234*c5; T5++;
+ *T5 = c234; T5++;
+ *T5 = -s234*s5; T5++;
+ *T5 = d1 + a3*s23 + a2*s2 - d5*c234; T5++;
+ *T5 = 0; T5++;
+ *T5 = 0; T5++;
+ *T5 = 0; T5++;
+ *T5 = 1; T5++;
+ }
+
+ if(T6 != NULL) {
+ *T6 = c6*(s1*s5 + c234*c1*c5) - s234*c1*s6; T6++;
+ *T6 = - s6*(s1*s5 + c234*c1*c5) - s234*c1*c6; T6++;
+ *T6 = c5*s1 - c234*c1*s5; T6++;
+ *T6 =d6*(c5*s1 - c234*c1*s5) + c1*(a3*c23 + a2*c2) + d4*s1 + d5*s234*c1; T6++;
+ *T6 = - c6*(c1*s5 - c234*c5*s1) - s234*s1*s6; T6++;
+ *T6 = s6*(c1*s5 - c234*c5*s1) - s234*c6*s1; T6++;
+ *T6 = - c1*c5 - c234*s1*s5; T6++;
+ *T6 =s1*(a3*c23 + a2*c2) - d4*c1 - d6*(c1*c5 + c234*s1*s5) + d5*s234*s1; T6++;
+ *T6 = c234*s6 + s234*c5*c6; T6++;
+ *T6 = c234*c6 - s234*c5*s6; T6++;
+ *T6 = -s234*s5; T6++;
+ *T6 = d1 + a3*s23 + a2*s2 - d5*c234 - d6*s234*s5; T6++;
+ *T6 = 0; T6++;
+ *T6 = 0; T6++;
+ *T6 = 0; T6++;
+ *T6 = 1; T6++;
+ }
}
int inverse(const double* T, double* q_sols, double q6_des) {
diff --git a/ur_kinematics/src/ur_moveit_plugin.cpp b/ur_kinematics/src/ur_moveit_plugin.cpp
index a141c413ed12e9bac28b06624998cf01ad3691c5..9be5a4fdd41dd3fdf2b8eb396fbef8b830e07658 100644
--- a/ur_kinematics/src/ur_moveit_plugin.cpp
+++ b/ur_kinematics/src/ur_moveit_plugin.cpp
@@ -1,473 +1,22 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2012, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Willow Garage nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *********************************************************************/
-/* Author: Sachin Chitta, David Lu!!, Ugo Cupcic */
-
-#include <moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h>
-#include <class_loader/class_loader.h>
-
-//#include <tf/transform_datatypes.h>
+#include <ur_kinematics/ur_moveit_plugin.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <ros/ros.h>
+#include <pluginlib/class_list_macros.h>
+#include <ur_kinematics/ur_kin.h>
#include <tf_conversions/tf_kdl.h>
-#include <kdl_parser/kdl_parser.hpp>
-
-// URDF, SRDF
-#include <urdf_model/model.h>
-#include <srdfdom/model.h>
-
-#include <moveit/rdf_loader/rdf_loader.h>
-
-//register KDLKinematics as a KinematicsBase implementation
-CLASS_LOADER_REGISTER_CLASS(kdl_kinematics_plugin::KDLKinematicsPlugin, kinematics::KinematicsBase)
-
-namespace kdl_kinematics_plugin
-{
-
- KDLKinematicsPlugin::KDLKinematicsPlugin():active_(false) {}
-
-void KDLKinematicsPlugin::getRandomConfiguration(KDL::JntArray &jnt_array, bool lock_redundancy) const
-{
- std::vector<double> jnt_array_vector(dimension_, 0.0);
- state_->setToRandomPositions(joint_model_group_);
- state_->copyJointGroupPositions(joint_model_group_, &jnt_array_vector[0]);
- for (std::size_t i = 0; i < dimension_; ++i)
- {
- if (lock_redundancy)
- if (isRedundantJoint(i))
- continue;
- jnt_array(i) = jnt_array_vector[i];
- }
-}
+//#include <moveit/kdl_kinematics_plugin.cpp>
-bool KDLKinematicsPlugin::isRedundantJoint(unsigned int index) const
-{
- for (std::size_t j=0; j < redundant_joint_indices_.size(); ++j)
- if (redundant_joint_indices_[j] == index)
- return true;
- return false;
-}
-
-void KDLKinematicsPlugin::getRandomConfiguration(const KDL::JntArray &seed_state,
- const std::vector<double> &consistency_limits,
- KDL::JntArray &jnt_array,
- bool lock_redundancy) const
-{
- std::vector<double> values(dimension_, 0.0);
- std::vector<double> near(dimension_, 0.0);
- for (std::size_t i = 0 ; i < dimension_; ++i)
- near[i] = seed_state(i);
-
- // Need to resize the consistency limits to remove mimic joints
- std::vector<double> consistency_limits_mimic;
- for(std::size_t i = 0; i < dimension_; ++i)
- {
- if(!mimic_joints_[i].active)
- continue;
- consistency_limits_mimic.push_back(consistency_limits[i]);
- }
-
- joint_model_group_->getVariableRandomPositionsNearBy(state_->getRandomNumberGenerator(), values, near, consistency_limits_mimic);
-
- for (std::size_t i = 0; i < dimension_; ++i)
- {
- bool skip = false;
- if (lock_redundancy)
- for (std::size_t j = 0; j < redundant_joint_indices_.size(); ++j)
- if (redundant_joint_indices_[j] == i)
- {
- skip = true;
- break;
- }
- if (skip)
- continue;
- jnt_array(i) = values[i];
- }
-}
-
-bool KDLKinematicsPlugin::checkConsistency(const KDL::JntArray& seed_state,
- const std::vector<double> &consistency_limits,
- const KDL::JntArray& solution) const
-{
- for (std::size_t i = 0; i < dimension_; ++i)
- if (fabs(seed_state(i) - solution(i)) > consistency_limits[i])
- return false;
- return true;
-}
+//register URKinematicsPlugin as a KinematicsBase implementation
+PLUGINLIB_EXPORT_CLASS(ur_kinematics::URKinematicsPlugin,kinematics::KinematicsBase);
-bool KDLKinematicsPlugin::initialize(const std::string &robot_description,
- const std::string& group_name,
- const std::string& base_frame,
- const std::string& tip_frame,
- double search_discretization)
+namespace ur_kinematics
{
- setValues(robot_description, group_name, base_frame, tip_frame, search_discretization);
-
- ros::NodeHandle private_handle("~");
- rdf_loader::RDFLoader rdf_loader(robot_description_);
- const boost::shared_ptr<srdf::Model> &srdf = rdf_loader.getSRDF();
- const boost::shared_ptr<urdf::ModelInterface>& urdf_model = rdf_loader.getURDF();
-
- if (!urdf_model || !srdf)
- {
- ROS_ERROR_NAMED("kdl","URDF and SRDF must be loaded for KDL kinematics solver to work.");
- return false;
- }
-
- robot_model_.reset(new robot_model::RobotModel(urdf_model, srdf));
+ URKinematicsPlugin::URKinematicsPlugin()
+ : kdl_kinematics_plugin::KDLKinematicsPlugin(), ik_weights_(6) {}
+// //URKinematicsPlugin::~URKinematicsPlugin() {}
- robot_model::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup(group_name);
- if (!joint_model_group)
- return false;
-
- if(!joint_model_group->isChain())
- {
- ROS_ERROR_NAMED("kdl","Group '%s' is not a chain", group_name.c_str());
- return false;
- }
- if(!joint_model_group->isSingleDOFJoints())
- {
- ROS_ERROR_NAMED("kdl","Group '%s' includes joints that have more than 1 DOF", group_name.c_str());
- return false;
- }
-
- KDL::Tree kdl_tree;
-
- if (!kdl_parser::treeFromUrdfModel(*urdf_model, kdl_tree))
- {
- ROS_ERROR_NAMED("kdl","Could not initialize tree object");
- return false;
- }
- if (!kdl_tree.getChain(base_frame_, getTipFrame(), kdl_chain_))
- {
- ROS_ERROR_NAMED("kdl","Could not initialize chain object");
- return false;
- }
-
- dimension_ = joint_model_group->getActiveJointModels().size() + joint_model_group->getMimicJointModels().size();
- for (std::size_t i=0; i < joint_model_group->getJointModels().size(); ++i)
- {
- if(joint_model_group->getJointModels()[i]->getType() == moveit::core::JointModel::REVOLUTE || joint_model_group->getJointModels()[i]->getType() == moveit::core::JointModel::PRISMATIC)
- {
- ik_chain_info_.joint_names.push_back(joint_model_group->getJointModelNames()[i]);
- const std::vector<moveit_msgs::JointLimits> &jvec = joint_model_group->getJointModels()[i]->getVariableBoundsMsg();
- ik_chain_info_.limits.insert(ik_chain_info_.limits.end(), jvec.begin(), jvec.end());
- }
- }
-
- fk_chain_info_.joint_names = ik_chain_info_.joint_names;
- fk_chain_info_.limits = ik_chain_info_.limits;
-
- if(!joint_model_group->hasLinkModel(getTipFrame()))
- {
- ROS_ERROR_NAMED("kdl","Could not find tip name in joint group '%s'", group_name.c_str());
- return false;
- }
- ik_chain_info_.link_names.push_back(getTipFrame());
- fk_chain_info_.link_names = joint_model_group->getLinkModelNames();
-
- joint_min_.resize(ik_chain_info_.limits.size());
- joint_max_.resize(ik_chain_info_.limits.size());
-
- for(unsigned int i=0; i < ik_chain_info_.limits.size(); i++)
- {
- joint_min_(i) = ik_chain_info_.limits[i].min_position;
- joint_max_(i) = ik_chain_info_.limits[i].max_position;
- }
-
- // Get Solver Parameters
- int max_solver_iterations;
- double epsilon;
- bool position_ik;
-
- private_handle.param("max_solver_iterations", max_solver_iterations, 500);
- private_handle.param("epsilon", epsilon, 1e-5);
- private_handle.param(group_name+"/position_only_ik", position_ik, false);
- ROS_DEBUG_NAMED("kdl","Looking in private handle: %s for param name: %s",
- private_handle.getNamespace().c_str(),
- (group_name+"/position_only_ik").c_str());
-
- if(position_ik)
- ROS_INFO_NAMED("kdl","Using position only ik");
-
- num_possible_redundant_joints_ = kdl_chain_.getNrOfJoints() - joint_model_group->getMimicJointModels().size() - (position_ik? 3:6);
-
- // Check for mimic joints
- bool has_mimic_joints = joint_model_group->getMimicJointModels().size() > 0;
- std::vector<unsigned int> redundant_joints_map_index;
-
- std::vector<JointMimic> mimic_joints;
- unsigned int joint_counter = 0;
- for (std::size_t i = 0; i < kdl_chain_.getNrOfSegments(); ++i)
- {
- const robot_model::JointModel *jm = robot_model_->getJointModel(kdl_chain_.segments[i].getJoint().getName());
-
- //first check whether it belongs to the set of active joints in the group
- if (jm->getMimic() == NULL && jm->getVariableCount() > 0)
- {
- JointMimic mimic_joint;
- mimic_joint.reset(joint_counter);
- mimic_joint.joint_name = kdl_chain_.segments[i].getJoint().getName();
- mimic_joint.active = true;
- mimic_joints.push_back(mimic_joint);
- ++joint_counter;
- continue;
- }
- if (joint_model_group->hasJointModel(jm->getName()))
- {
- if (jm->getMimic() && joint_model_group->hasJointModel(jm->getMimic()->getName()))
- {
- JointMimic mimic_joint;
- mimic_joint.reset(joint_counter);
- mimic_joint.joint_name = kdl_chain_.segments[i].getJoint().getName();
- mimic_joint.offset = jm->getMimicOffset();
- mimic_joint.multiplier = jm->getMimicFactor();
- mimic_joints.push_back(mimic_joint);
- continue;
- }
- }
- }
- for (std::size_t i = 0; i < mimic_joints.size(); ++i)
- {
- if(!mimic_joints[i].active)
- {
- const robot_model::JointModel* joint_model = joint_model_group->getJointModel(mimic_joints[i].joint_name)->getMimic();
- for(std::size_t j=0; j < mimic_joints.size(); ++j)
- {
- if(mimic_joints[j].joint_name == joint_model->getName())
- {
- mimic_joints[i].map_index = mimic_joints[j].map_index;
- }
- }
- }
- }
- mimic_joints_ = mimic_joints;
-
- // Setup the joint state groups that we need
- state_.reset(new robot_state::RobotState(robot_model_));
- state_2_.reset(new robot_state::RobotState(robot_model_));
-
- // Store things for when the set of redundant joints may change
- position_ik_ = position_ik;
- joint_model_group_ = joint_model_group;
- max_solver_iterations_ = max_solver_iterations;
- epsilon_ = epsilon;
-
- active_ = true;
- ROS_DEBUG_NAMED("kdl","KDL solver initialized");
- return true;
-}
-
-bool KDLKinematicsPlugin::setRedundantJoints(const std::vector<unsigned int> &redundant_joints)
-{
- if(num_possible_redundant_joints_ < 0)
- {
- ROS_ERROR_NAMED("kdl","This group cannot have redundant joints");
- return false;
- }
- if(redundant_joints.size() > num_possible_redundant_joints_)
- {
- ROS_ERROR_NAMED("kdl","This group can only have %d redundant joints", num_possible_redundant_joints_);
- return false;
- }
- /*
- XmlRpc::XmlRpcValue joint_list;
- if(private_handle.getParam(group_name+"/redundant_joints", joint_list))
- {
- ROS_ASSERT(joint_list.getType() == XmlRpc::XmlRpcValue::TypeArray);
- std::vector<std::string> redundant_joints;
- for (std::size_t i = 0; i < joint_list.size(); ++i)
- {
- ROS_ASSERT(joint_list[i].getType() == XmlRpc::XmlRpcValue::TypeString);
- redundant_joints.push_back(static_cast<std::string>(joint_list[i]));
- ROS_INFO_NAMED("kdl","Designated joint: %s as redundant joint", redundant_joints.back().c_str());
- }
- }
- */
- std::vector<unsigned int> redundant_joints_map_index;
- unsigned int counter = 0;
- for(std::size_t i=0; i < dimension_; ++i)
- {
- bool is_redundant_joint = false;
- for(std::size_t j=0; j < redundant_joints.size(); ++j)
- {
- if(i == redundant_joints[j])
- {
- is_redundant_joint = true;
- counter++;
- break;
- }
- }
- if(!is_redundant_joint)
- {
- // check for mimic
- if(mimic_joints_[i].active)
- {
- redundant_joints_map_index.push_back(counter);
- counter++;
- }
- }
- }
- for(std::size_t i=0; i < redundant_joints_map_index.size(); ++i)
- ROS_DEBUG_NAMED("kdl","Redundant joint map index: %d %d", (int) i, (int) redundant_joints_map_index[i]);
-
- redundant_joints_map_index_ = redundant_joints_map_index;
- redundant_joint_indices_ = redundant_joints;
- return true;
-}
-
-int KDLKinematicsPlugin::getJointIndex(const std::string &name) const
-{
- for (unsigned int i=0; i < ik_chain_info_.joint_names.size(); i++) {
- if (ik_chain_info_.joint_names[i] == name)
- return i;
- }
- return -1;
-}
-
-int KDLKinematicsPlugin::getKDLSegmentIndex(const std::string &name) const
-{
- int i=0;
- while (i < (int)kdl_chain_.getNrOfSegments()) {
- if (kdl_chain_.getSegment(i).getName() == name) {
- return i+1;
- }
- i++;
- }
- return -1;
-}
-
-bool KDLKinematicsPlugin::timedOut(const ros::WallTime &start_time, double duration) const
-{
- return ((ros::WallTime::now()-start_time).toSec() >= duration);
-}
-
-bool KDLKinematicsPlugin::getPositionIK(const geometry_msgs::Pose &ik_pose,
- const std::vector<double> &ik_seed_state,
- std::vector<double> &solution,
- moveit_msgs::MoveItErrorCodes &error_code,
- const kinematics::KinematicsQueryOptions &options) const
-{
- const IKCallbackFn solution_callback = 0;
- std::vector<double> consistency_limits;
-
- return searchPositionIK(ik_pose,
- ik_seed_state,
- default_timeout_,
- solution,
- solution_callback,
- error_code,
- consistency_limits,
- options);
-}
-
-bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
- const std::vector<double> &ik_seed_state,
- double timeout,
- std::vector<double> &solution,
- moveit_msgs::MoveItErrorCodes &error_code,
- const kinematics::KinematicsQueryOptions &options) const
-{
- const IKCallbackFn solution_callback = 0;
- std::vector<double> consistency_limits;
-
- return searchPositionIK(ik_pose,
- ik_seed_state,
- timeout,
- solution,
- solution_callback,
- error_code,
- consistency_limits,
- options);
-}
-
-bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
- const std::vector<double> &ik_seed_state,
- double timeout,
- const std::vector<double> &consistency_limits,
- std::vector<double> &solution,
- moveit_msgs::MoveItErrorCodes &error_code,
- const kinematics::KinematicsQueryOptions &options) const
-{
- const IKCallbackFn solution_callback = 0;
- return searchPositionIK(ik_pose,
- ik_seed_state,
- timeout,
- solution,
- solution_callback,
- error_code,
- consistency_limits,
- options);
-}
-
-bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
- const std::vector<double> &ik_seed_state,
- double timeout,
- std::vector<double> &solution,
- const IKCallbackFn &solution_callback,
- moveit_msgs::MoveItErrorCodes &error_code,
- const kinematics::KinematicsQueryOptions &options) const
-{
- std::vector<double> consistency_limits;
- return searchPositionIK(ik_pose,
- ik_seed_state,
- timeout,
- solution,
- solution_callback,
- error_code,
- consistency_limits,
- options);
-}
-
-bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
- const std::vector<double> &ik_seed_state,
- double timeout,
- const std::vector<double> &consistency_limits,
- std::vector<double> &solution,
- const IKCallbackFn &solution_callback,
- moveit_msgs::MoveItErrorCodes &error_code,
- const kinematics::KinematicsQueryOptions &options) const
-{
- return searchPositionIK(ik_pose,
- ik_seed_state,
- timeout,
- solution,
- solution_callback,
- error_code,
- consistency_limits,
- options);
-}
-
-bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
+bool URKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
const std::vector<double> &ik_seed_state,
double timeout,
std::vector<double> &solution,
@@ -476,178 +25,95 @@ bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
const std::vector<double> &consistency_limits,
const kinematics::KinematicsQueryOptions &options) const
{
- ros::WallTime n1 = ros::WallTime::now();
- if(!active_)
- {
- ROS_ERROR_NAMED("kdl","kinematics not active");
+ //double homo_ik_pose[4][4];
+
+ KDL::Frame kdl_ik_pose;
+ tf::poseMsgToKDL(ik_pose, kdl_ik_pose);
+ double homo_ik_pose[4][4];
+ kdl_ik_pose.Make4x4((double*) homo_ik_pose);
+ for(int i=0; i<3; i++) homo_ik_pose[i][3] *= 1000;
+ double q_ik_sols[8][6];
+ uint16_t num_sols = inverse((double*) homo_ik_pose, (double*) q_ik_sols, ik_seed_state[5]);
+ if(num_sols <= 0) {
+ // NO SOLUTION
error_code.val = error_code.NO_IK_SOLUTION;
return false;
}
-
- if(ik_seed_state.size() != dimension_)
- {
- ROS_ERROR_STREAM_NAMED("kdl","Seed state must have size " << dimension_ << " instead of size " << ik_seed_state.size());
- error_code.val = error_code.NO_IK_SOLUTION;
- return false;
- }
-
- if(!consistency_limits.empty() && consistency_limits.size() != dimension_)
- {
- ROS_ERROR_STREAM_NAMED("kdl","Consistency limits be empty or must have size " << dimension_ << " instead of size " << consistency_limits.size());
- error_code.val = error_code.NO_IK_SOLUTION;
- return false;
- }
-
- KDL::JntArray jnt_seed_state(dimension_);
- KDL::JntArray jnt_pos_in(dimension_);
- KDL::JntArray jnt_pos_out(dimension_);
-
- KDL::ChainFkSolverPos_recursive fk_solver(kdl_chain_);
- KDL::ChainIkSolverVel_pinv_mimic ik_solver_vel(kdl_chain_, joint_model_group_->getMimicJointModels().size(), redundant_joint_indices_.size(), position_ik_);
- KDL::ChainIkSolverPos_NR_JL_Mimic ik_solver_pos(kdl_chain_, joint_min_, joint_max_, fk_solver, ik_solver_vel, max_solver_iterations_, epsilon_, position_ik_);
- ik_solver_vel.setMimicJoints(mimic_joints_);
- ik_solver_pos.setMimicJoints(mimic_joints_);
-
- if ((redundant_joint_indices_.size() > 0) && !ik_solver_vel.setRedundantJointsMapIndex(redundant_joints_map_index_))
- {
- ROS_ERROR_NAMED("kdl","Could not set redundant joints");
- return false;
- }
-
- if(options.lock_redundant_joints)
- {
- ik_solver_vel.lockRedundantJoints();
- }
-
- solution.resize(dimension_);
-
- KDL::Frame pose_desired;
- tf::poseMsgToKDL(ik_pose, pose_desired);
-
- ROS_DEBUG_STREAM_NAMED("kdl","searchPositionIK2: Position request pose is " <<
- ik_pose.position.x << " " <<
- ik_pose.position.y << " " <<
- ik_pose.position.z << " " <<
- ik_pose.orientation.x << " " <<
- ik_pose.orientation.y << " " <<
- ik_pose.orientation.z << " " <<
- ik_pose.orientation.w);
- //Do the IK
- for(unsigned int i=0; i < dimension_; i++)
- jnt_seed_state(i) = ik_seed_state[i];
- jnt_pos_in = jnt_seed_state;
-
- unsigned int counter(0);
- while(1)
- {
- // ROS_DEBUG_NAMED("kdl","Iteration: %d, time: %f, Timeout: %f",counter,(ros::WallTime::now()-n1).toSec(),timeout);
- counter++;
- if(timedOut(n1,timeout))
- {
- ROS_DEBUG_NAMED("kdl","IK timed out");
- error_code.val = error_code.TIMED_OUT;
- ik_solver_vel.unlockRedundantJoints();
- return false;
- }
- int ik_valid = ik_solver_pos.CartToJnt(jnt_pos_in, pose_desired, jnt_pos_out);
- ROS_DEBUG_NAMED("kdl","IK valid: %d", ik_valid);
- if(!consistency_limits.empty())
- {
- getRandomConfiguration(jnt_seed_state, consistency_limits, jnt_pos_in, options.lock_redundant_joints);
- if( (ik_valid < 0 && !options.return_approximate_solution) || !checkConsistency(jnt_seed_state, consistency_limits, jnt_pos_out))
- {
- ROS_DEBUG_NAMED("kdl","Could not find IK solution: does not match consistency limits");
- continue;
- }
+ double min_weighted_diff = 1e9;
+ int min_diff_index = -1;
+ for(uint16_t i=0; i<num_sols; i++) {
+ double cur_weighted_diff = 0;
+ for(uint16_t j=0; j<6; j++) {
+ cur_weighted_diff += ik_weights_[j] * std::fabs(q_ik_sols[i][j] - ik_seed_state[j]);
}
- else
- {
- getRandomConfiguration(jnt_pos_in, options.lock_redundant_joints);
- ROS_DEBUG_NAMED("kdl","New random configuration");
- for(unsigned int j=0; j < dimension_; j++)
- ROS_DEBUG_NAMED("kdl","%d %f", j, jnt_pos_in(j));
-
- if(ik_valid < 0 && !options.return_approximate_solution)
- {
- ROS_DEBUG_NAMED("kdl","Could not find IK solution");
- continue;
- }
- }
- ROS_DEBUG_NAMED("kdl","Found IK solution");
- for(unsigned int j=0; j < dimension_; j++)
- solution[j] = jnt_pos_out(j);
- if(!solution_callback.empty())
- solution_callback(ik_pose,solution,error_code);
- else
- error_code.val = error_code.SUCCESS;
-
- if(error_code.val == error_code.SUCCESS)
- {
- ROS_DEBUG_STREAM_NAMED("kdl","Solved after " << counter << " iterations");
- ik_solver_vel.unlockRedundantJoints();
- return true;
+ if(cur_weighted_diff < min_weighted_diff) {
+ min_weighted_diff = cur_weighted_diff;
+ min_diff_index = i;
}
}
- ROS_DEBUG_NAMED("kdl","An IK that satisifes the constraints and is collision free could not be found");
- error_code.val = error_code.NO_IK_SOLUTION;
- ik_solver_vel.unlockRedundantJoints();
- return false;
-}
-
-bool KDLKinematicsPlugin::getPositionFK(const std::vector<std::string> &link_names,
- const std::vector<double> &joint_angles,
- std::vector<geometry_msgs::Pose> &poses) const
-{
- ros::WallTime n1 = ros::WallTime::now();
- if(!active_)
- {
- ROS_ERROR_NAMED("kdl","kinematics not active");
- return false;
- }
- poses.resize(link_names.size());
- if(joint_angles.size() != dimension_)
- {
- ROS_ERROR_NAMED("kdl","Joint angles vector must have size: %d",dimension_);
- return false;
- }
-
- KDL::Frame p_out;
- geometry_msgs::PoseStamped pose;
- tf::Stamped<tf::Pose> tf_pose;
-
- KDL::JntArray jnt_pos_in(dimension_);
- for(unsigned int i=0; i < dimension_; i++)
- {
- jnt_pos_in(i) = joint_angles[i];
- }
-
- KDL::ChainFkSolverPos_recursive fk_solver(kdl_chain_);
+ solution.resize(6);
+ std::copy(q_ik_sols[min_diff_index], q_ik_sols[min_diff_index+1], solution.begin());
- bool valid = true;
- for(unsigned int i=0; i < poses.size(); i++)
- {
- ROS_DEBUG_NAMED("kdl","End effector index: %d",getKDLSegmentIndex(link_names[i]));
- if(fk_solver.JntToCart(jnt_pos_in,p_out,getKDLSegmentIndex(link_names[i])) >=0)
- {
- tf::poseKDLToMsg(p_out,poses[i]);
- }
- else
- {
- ROS_ERROR_NAMED("kdl","Could not compute FK for %s",link_names[i].c_str());
- valid = false;
- }
- }
- return valid;
-}
-
-const std::vector<std::string>& KDLKinematicsPlugin::getJointNames() const
-{
- return ik_chain_info_.joint_names;
+ if(!solution_callback.empty())
+ solution_callback(ik_pose,solution,error_code);
+ else
+ error_code.val = error_code.SUCCESS;
+ return true;
}
-
-const std::vector<std::string>& KDLKinematicsPlugin::getLinkNames() const
-{
- return ik_chain_info_.link_names;
+//
+// bool URKinematicsPlugin::getPositionFK(const std::vector<std::string> &link_names,
+// const std::vector<double> &joint_angles,
+// std::vector<geometry_msgs::Pose> &poses) const
+// {
+// // double q[6];
+// // for(int i=0; i<6; i++)
+// // q[i] = joint_angles[i];
+// // double T[6][16];
+// // forward_all(q, T[0], T[1], T[2], T[3], T[4], T[5]);
+// // for(int i=0; i<6; i++) {
+// // aa
+// // }
+// }
+//
+ /**
+* @brief Initialization function for the kinematics
+* @return True if initialization was successful, false otherwise
+*/
+bool URKinematicsPlugin::initialize(const std::string& robot_description,
+ const std::string& group_name,
+ const std::string& base_frame,
+ const std::string& tip_frame,
+ double search_discretization)
+{
+ ros::NodeHandle private_handle("~");
+ if(private_handle.hasParam("ik_weights")) {
+ private_handle.getParam("ik_weights", ik_weights_);
+ } else {
+ ik_weights_[0] = 1.0;
+ ik_weights_[1] = 1.0;
+ ik_weights_[2] = 0.1;
+ ik_weights_[3] = 0.1;
+ ik_weights_[4] = 0.3;
+ ik_weights_[5] = 0.3;
+ }
+
+ return kdl_kinematics_plugin::KDLKinematicsPlugin::initialize(
+ robot_description, group_name, base_frame, tip_frame, search_discretization);
+}
+//
+// /**
+// * @brief Return all the joint names in the order they are used internally
+// */
+// const std::vector<std::string>& URKinematicsPlugin::getJointNames() const
+// {
+// return fk_solver_info_.joint_names;
+// }
+//
+// /**
+// * @brief Return all the link names in the order they are represented internally
+// */
+// const std::vector<std::string>& URKinematicsPlugin::getLinkNames() const
+// {
+// return fk_solver_info_.link_names;
+// }
}
-
-} // namespace
diff --git a/ur_kinematics/src/ur_moveit_plugin.cpp.bak b/ur_kinematics/src/ur_moveit_plugin.cpp.bak
deleted file mode 100644
index e69bab96f6d60dd7aebb50db043bfa345023568b..0000000000000000000000000000000000000000
--- a/ur_kinematics/src/ur_moveit_plugin.cpp.bak
+++ /dev/null
@@ -1,119 +0,0 @@
-
-#include <ur_kinematics/ur_moveit_plugin.h>
-#include <geometry_msgs/PoseStamped.h>
-#include <ros/ros.h>
-#include <pluginlib/class_list_macros.h>
-#include <ur_kinematics/ur_kin.h>
-
-//register URKinematicsPlugin as a KinematicsBase implementation
-PLUGINLIB_EXPORT_CLASS(ur_kinematics::URKinematicsPlugin,kinematics::KinematicsBase);
-
-namespace ur_kinematics
-{
- URKinematicsPlugin::URKinematicsPlugin() {}
- //URKinematicsPlugin::~URKinematicsPlugin() {}
-
- bool URKinematicsPlugin::getPositionIK(const geometry_msgs::Pose &ik_pose,
- const std::vector<double> &ik_seed_state,
- std::vector<double> &solution,
- moveit_msgs::MoveItErrorCodes &error_code,
- const kinematics::KinematicsQueryOptions &options) const
- {}
-
- bool URKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
- const std::vector<double> &ik_seed_state,
- double timeout,
- std::vector<double> &solution,
- moveit_msgs::MoveItErrorCodes &error_code,
- const kinematics::KinematicsQueryOptions &options) const
- {}
-
- bool URKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
- const std::vector<double> &ik_seed_state,
- double timeout,
- const std::vector<double> &consistency_limits,
- std::vector<double> &solution,
- moveit_msgs::MoveItErrorCodes &error_code,
- const kinematics::KinematicsQueryOptions &options) const
- {}
-
- bool URKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
- const std::vector<double> &ik_seed_state,
- double timeout,
- std::vector<double> &solution,
- const IKCallbackFn &solution_callback,
- moveit_msgs::MoveItErrorCodes &error_code,
- const kinematics::KinematicsQueryOptions &options) const
- {}
-
- bool URKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
- const std::vector<double> &ik_seed_state,
- double timeout,
- const std::vector<double> &consistency_limits,
- std::vector<double> &solution,
- const IKCallbackFn &solution_callback,
- moveit_msgs::MoveItErrorCodes &error_code,
- const kinematics::KinematicsQueryOptions &options) const
- {}
-
- bool URKinematicsPlugin::getPositionFK(const std::vector<std::string> &link_names,
- const std::vector<double> &joint_angles,
- std::vector<geometry_msgs::Pose> &poses) const
- {
- double q[6];
- for(int i=0; i<6; i++)
- q[i] = joint_angles[i];
- double T[6][16];
- forward_all(q, T[0], T[1], T[2], T[3], T[4], T[5]);
- for(int i=0; i<6; i++) {
- aa
- }
- }
-
- /**
-* @brief Initialization function for the kinematics
-* @return True if initialization was successful, false otherwise
-*/
- bool URKinematicsPlugin::initialize(const std::string& robot_description,
- const std::string& group_name,
- const std::string& base_frame,
- const std::string& tip_frame,
- double search_discretization)
- {
- setValues(robot_description, group_name, base_frame, tip_frame, search_discretization);
-
- fk_solver_info_.joint_names.push_back("shoulder_pan_joint");
- fk_solver_info_.joint_names.push_back("shoulder_lift_joint");
- fk_solver_info_.joint_names.push_back("elbow_joint");
- fk_solver_info_.joint_names.push_back("wrist_1_joint");
- fk_solver_info_.joint_names.push_back("wrist_2_joint");
- fk_solver_info_.joint_names.push_back("wrist_3_joint");
-
- fk_solver_info_.link_names.push_back("base_link");
- fk_solver_info_.link_names.push_back("shoulder_link");
- fk_solver_info_.link_names.push_back("upper_arm_link");
- fk_solver_info_.link_names.push_back("forearm_link");
- fk_solver_info_.link_names.push_back("wrist_1_link");
- fk_solver_info_.link_names.push_back("wrist_2_link");
- fk_solver_info_.link_names.push_back("wrist_3_link");
- fk_solver_info_.link_names.push_back("ee_link");
-
- return true;
- }
-
- /**
-* @brief Return all the joint names in the order they are used internally
-*/
- const std::vector<std::string>& URKinematicsPlugin::getJointNames() const
- {
- return fk_solver_info_.joint_names;
- }
-
- /**
-* @brief Return all the link names in the order they are represented internally
-*/
- const std::vector<std::string>& URKinematicsPlugin::getLinkNames() const
- {
- return fk_solver_info_.link_names;
- }
-}
diff --git a/ur_kinematics/src/ur_moveit_plugin.cpp.bak2 b/ur_kinematics/src/ur_moveit_plugin.cpp.bak2
deleted file mode 100644
index b667cf7ad9eb8ed35d047bdd53f03857460d4997..0000000000000000000000000000000000000000
--- a/ur_kinematics/src/ur_moveit_plugin.cpp.bak2
+++ /dev/null
@@ -1,609 +0,0 @@
-
-#include <ur_kinematics/ur_moveit_plugin.h>
-#include <geometry_msgs/PoseStamped.h>
-#include <ros/ros.h>
-#include <pluginlib/class_list_macros.h>
-#include <ur_kinematics/ur_kin.h>
-
-//register URKinematicsPlugin as a KinematicsBase implementation
-PLUGINLIB_EXPORT_CLASS(ur_kinematics::URKinematicsPlugin,kinematics::KinematicsBase);
-
-namespace ur_kinematics
-{
- KDLKinematicsPlugin::KDLKinematicsPlugin():active_(false) {}
-
-void KDLKinematicsPlugin::getRandomConfiguration(KDL::JntArray &jnt_array, bool lock_redundancy) const
-{
- std::vector<double> jnt_array_vector(dimension_, 0.0);
- state_->setToRandomPositions(joint_model_group_);
- state_->copyJointGroupPositions(joint_model_group_, &jnt_array_vector[0]);
- for (std::size_t i = 0; i < dimension_; ++i)
- {
- if (lock_redundancy)
- if (isRedundantJoint(i))
- continue;
- jnt_array(i) = jnt_array_vector[i];
- }
-}
-
-bool KDLKinematicsPlugin::isRedundantJoint(unsigned int index) const
-{
- for (std::size_t j=0; j < redundant_joint_indices_.size(); ++j)
- if (redundant_joint_indices_[j] == index)
- return true;
- return false;
-}
-
-void KDLKinematicsPlugin::getRandomConfiguration(const KDL::JntArray &seed_state,
- const std::vector<double> &consistency_limits,
- KDL::JntArray &jnt_array,
- bool lock_redundancy) const
-{
- std::vector<double> values(dimension_, 0.0);
- std::vector<double> near(dimension_, 0.0);
- for (std::size_t i = 0 ; i < dimension_; ++i)
- near[i] = seed_state(i);
-
- // Need to resize the consistency limits to remove mimic joints
- std::vector<double> consistency_limits_mimic;
- for(std::size_t i = 0; i < dimension_; ++i)
- {
- if(!mimic_joints_[i].active)
- continue;
- consistency_limits_mimic.push_back(consistency_limits[i]);
- }
-
- joint_model_group_->getVariableRandomPositionsNearBy(state_->getRandomNumberGenerator(), values, near, consistency_limits_mimic);
-
- for (std::size_t i = 0; i < dimension_; ++i)
- {
- bool skip = false;
- if (lock_redundancy)
- for (std::size_t j = 0; j < redundant_joint_indices_.size(); ++j)
- if (redundant_joint_indices_[j] == i)
- {
- skip = true;
- break;
- }
- if (skip)
- continue;
- jnt_array(i) = values[i];
- }
-}
-
-bool KDLKinematicsPlugin::checkConsistency(const KDL::JntArray& seed_state,
- const std::vector<double> &consistency_limits,
- const KDL::JntArray& solution) const
-{
- for (std::size_t i = 0; i < dimension_; ++i)
- if (fabs(seed_state(i) - solution(i)) > consistency_limits[i])
- return false;
- return true;
-}
-
-bool KDLKinematicsPlugin::initialize(const std::string &robot_description,
- const std::string& group_name,
- const std::string& base_frame,
- const std::string& tip_frame,
- double search_discretization)
-{
- setValues(robot_description, group_name, base_frame, tip_frame, search_discretization);
-
- ros::NodeHandle private_handle("~");
- rdf_loader::RDFLoader rdf_loader(robot_description_);
- const boost::shared_ptr<srdf::Model> &srdf = rdf_loader.getSRDF();
- const boost::shared_ptr<urdf::ModelInterface>& urdf_model = rdf_loader.getURDF();
-
- if (!urdf_model || !srdf)
- {
- ROS_ERROR_NAMED("kdl","URDF and SRDF must be loaded for KDL kinematics solver to work.");
- return false;
- }
-
- robot_model_.reset(new robot_model::RobotModel(urdf_model, srdf));
-
- robot_model::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup(group_name);
- if (!joint_model_group)
- return false;
-
- if(!joint_model_group->isChain())
- {
- ROS_ERROR_NAMED("kdl","Group '%s' is not a chain", group_name.c_str());
- return false;
- }
- if(!joint_model_group->isSingleDOFJoints())
- {
- ROS_ERROR_NAMED("kdl","Group '%s' includes joints that have more than 1 DOF", group_name.c_str());
- return false;
- }
-
- KDL::Tree kdl_tree;
-
- if (!kdl_parser::treeFromUrdfModel(*urdf_model, kdl_tree))
- {
- ROS_ERROR_NAMED("kdl","Could not initialize tree object");
- return false;
- }
- if (!kdl_tree.getChain(base_frame_, getTipFrame(), kdl_chain_))
- {
- ROS_ERROR_NAMED("kdl","Could not initialize chain object");
- return false;
- }
-
- dimension_ = joint_model_group->getActiveJointModels().size() + joint_model_group->getMimicJointModels().size();
- for (std::size_t i=0; i < joint_model_group->getJointModels().size(); ++i)
- {
- if(joint_model_group->getJointModels()[i]->getType() == moveit::core::JointModel::REVOLUTE || joint_model_group->getJointModels()[i]->getType() == moveit::core::JointModel::PRISMATIC)
- {
- ik_chain_info_.joint_names.push_back(joint_model_group->getJointModelNames()[i]);
- const std::vector<moveit_msgs::JointLimits> &jvec = joint_model_group->getJointModels()[i]->getVariableBoundsMsg();
- ik_chain_info_.limits.insert(ik_chain_info_.limits.end(), jvec.begin(), jvec.end());
- }
- }
-
- fk_chain_info_.joint_names = ik_chain_info_.joint_names;
- fk_chain_info_.limits = ik_chain_info_.limits;
-
- if(!joint_model_group->hasLinkModel(getTipFrame()))
- {
- ROS_ERROR_NAMED("kdl","Could not find tip name in joint group '%s'", group_name.c_str());
- return false;
- }
- ik_chain_info_.link_names.push_back(getTipFrame());
- fk_chain_info_.link_names = joint_model_group->getLinkModelNames();
-
- joint_min_.resize(ik_chain_info_.limits.size());
- joint_max_.resize(ik_chain_info_.limits.size());
-
- for(unsigned int i=0; i < ik_chain_info_.limits.size(); i++)
- {
- joint_min_(i) = ik_chain_info_.limits[i].min_position;
- joint_max_(i) = ik_chain_info_.limits[i].max_position;
- }
-
- // Get Solver Parameters
- int max_solver_iterations;
- double epsilon;
- bool position_ik;
-
- private_handle.param("max_solver_iterations", max_solver_iterations, 500);
- private_handle.param("epsilon", epsilon, 1e-5);
- private_handle.param(group_name+"/position_only_ik", position_ik, false);
- ROS_DEBUG_NAMED("kdl","Looking in private handle: %s for param name: %s",
- private_handle.getNamespace().c_str(),
- (group_name+"/position_only_ik").c_str());
-
- if(position_ik)
- ROS_INFO_NAMED("kdl","Using position only ik");
-
- num_possible_redundant_joints_ = kdl_chain_.getNrOfJoints() - joint_model_group->getMimicJointModels().size() - (position_ik? 3:6);
-
- // Check for mimic joints
- bool has_mimic_joints = joint_model_group->getMimicJointModels().size() > 0;
- std::vector<unsigned int> redundant_joints_map_index;
-
- std::vector<JointMimic> mimic_joints;
- unsigned int joint_counter = 0;
- for (std::size_t i = 0; i < kdl_chain_.getNrOfSegments(); ++i)
- {
- const robot_model::JointModel *jm = robot_model_->getJointModel(kdl_chain_.segments[i].getJoint().getName());
-
- //first check whether it belongs to the set of active joints in the group
- if (jm->getMimic() == NULL && jm->getVariableCount() > 0)
- {
- JointMimic mimic_joint;
- mimic_joint.reset(joint_counter);
- mimic_joint.joint_name = kdl_chain_.segments[i].getJoint().getName();
- mimic_joint.active = true;
- mimic_joints.push_back(mimic_joint);
- ++joint_counter;
- continue;
- }
- if (joint_model_group->hasJointModel(jm->getName()))
- {
- if (jm->getMimic() && joint_model_group->hasJointModel(jm->getMimic()->getName()))
- {
- JointMimic mimic_joint;
- mimic_joint.reset(joint_counter);
- mimic_joint.joint_name = kdl_chain_.segments[i].getJoint().getName();
- mimic_joint.offset = jm->getMimicOffset();
- mimic_joint.multiplier = jm->getMimicFactor();
- mimic_joints.push_back(mimic_joint);
- continue;
- }
- }
- }
- for (std::size_t i = 0; i < mimic_joints.size(); ++i)
- {
- if(!mimic_joints[i].active)
- {
- const robot_model::JointModel* joint_model = joint_model_group->getJointModel(mimic_joints[i].joint_name)->getMimic();
- for(std::size_t j=0; j < mimic_joints.size(); ++j)
- {
- if(mimic_joints[j].joint_name == joint_model->getName())
- {
- mimic_joints[i].map_index = mimic_joints[j].map_index;
- }
- }
- }
- }
- mimic_joints_ = mimic_joints;
-
- // Setup the joint state groups that we need
- state_.reset(new robot_state::RobotState(robot_model_));
- state_2_.reset(new robot_state::RobotState(robot_model_));
-
- // Store things for when the set of redundant joints may change
- position_ik_ = position_ik;
- joint_model_group_ = joint_model_group;
- max_solver_iterations_ = max_solver_iterations;
- epsilon_ = epsilon;
-
- active_ = true;
- ROS_DEBUG_NAMED("kdl","KDL solver initialized");
- return true;
-}
-
-bool KDLKinematicsPlugin::setRedundantJoints(const std::vector<unsigned int> &redundant_joints)
-{
- if(num_possible_redundant_joints_ < 0)
- {
- ROS_ERROR_NAMED("kdl","This group cannot have redundant joints");
- return false;
- }
- if(redundant_joints.size() > num_possible_redundant_joints_)
- {
- ROS_ERROR_NAMED("kdl","This group can only have %d redundant joints", num_possible_redundant_joints_);
- return false;
- }
- /*
- XmlRpc::XmlRpcValue joint_list;
- if(private_handle.getParam(group_name+"/redundant_joints", joint_list))
- {
- ROS_ASSERT(joint_list.getType() == XmlRpc::XmlRpcValue::TypeArray);
- std::vector<std::string> redundant_joints;
- for (std::size_t i = 0; i < joint_list.size(); ++i)
- {
- ROS_ASSERT(joint_list[i].getType() == XmlRpc::XmlRpcValue::TypeString);
- redundant_joints.push_back(static_cast<std::string>(joint_list[i]));
- ROS_INFO_NAMED("kdl","Designated joint: %s as redundant joint", redundant_joints.back().c_str());
- }
- }
- */
- std::vector<unsigned int> redundant_joints_map_index;
- unsigned int counter = 0;
- for(std::size_t i=0; i < dimension_; ++i)
- {
- bool is_redundant_joint = false;
- for(std::size_t j=0; j < redundant_joints.size(); ++j)
- {
- if(i == redundant_joints[j])
- {
- is_redundant_joint = true;
- counter++;
- break;
- }
- }
- if(!is_redundant_joint)
- {
- // check for mimic
- if(mimic_joints_[i].active)
- {
- redundant_joints_map_index.push_back(counter);
- counter++;
- }
- }
- }
- for(std::size_t i=0; i < redundant_joints_map_index.size(); ++i)
- ROS_DEBUG_NAMED("kdl","Redundant joint map index: %d %d", (int) i, (int) redundant_joints_map_index[i]);
-
- redundant_joints_map_index_ = redundant_joints_map_index;
- redundant_joint_indices_ = redundant_joints;
- return true;
-}
-
-int KDLKinematicsPlugin::getJointIndex(const std::string &name) const
-{
- for (unsigned int i=0; i < ik_chain_info_.joint_names.size(); i++) {
- if (ik_chain_info_.joint_names[i] == name)
- return i;
- }
- return -1;
-}
-
-int KDLKinematicsPlugin::getKDLSegmentIndex(const std::string &name) const
-{
- int i=0;
- while (i < (int)kdl_chain_.getNrOfSegments()) {
- if (kdl_chain_.getSegment(i).getName() == name) {
- return i+1;
- }
- i++;
- }
- return -1;
-}
-
-bool KDLKinematicsPlugin::timedOut(const ros::WallTime &start_time, double duration) const
-{
- return ((ros::WallTime::now()-start_time).toSec() >= duration);
-}
-
-bool KDLKinematicsPlugin::getPositionIK(const geometry_msgs::Pose &ik_pose,
- const std::vector<double> &ik_seed_state,
- std::vector<double> &solution,
- moveit_msgs::MoveItErrorCodes &error_code,
- const kinematics::KinematicsQueryOptions &options) const
-{
- const IKCallbackFn solution_callback = 0;
- std::vector<double> consistency_limits;
-
- return searchPositionIK(ik_pose,
- ik_seed_state,
- default_timeout_,
- solution,
- solution_callback,
- error_code,
- consistency_limits,
- options);
-}
-
-bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
- const std::vector<double> &ik_seed_state,
- double timeout,
- std::vector<double> &solution,
- moveit_msgs::MoveItErrorCodes &error_code,
- const kinematics::KinematicsQueryOptions &options) const
-{
- const IKCallbackFn solution_callback = 0;
- std::vector<double> consistency_limits;
-
- return searchPositionIK(ik_pose,
- ik_seed_state,
- timeout,
- solution,
- solution_callback,
- error_code,
- consistency_limits,
- options);
-}
-
-bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
- const std::vector<double> &ik_seed_state,
- double timeout,
- const std::vector<double> &consistency_limits,
- std::vector<double> &solution,
- moveit_msgs::MoveItErrorCodes &error_code,
- const kinematics::KinematicsQueryOptions &options) const
-{
- const IKCallbackFn solution_callback = 0;
- return searchPositionIK(ik_pose,
- ik_seed_state,
- timeout,
- solution,
- solution_callback,
- error_code,
- consistency_limits,
- options);
-}
-
-bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
- const std::vector<double> &ik_seed_state,
- double timeout,
- std::vector<double> &solution,
- const IKCallbackFn &solution_callback,
- moveit_msgs::MoveItErrorCodes &error_code,
- const kinematics::KinematicsQueryOptions &options) const
-{
- std::vector<double> consistency_limits;
- return searchPositionIK(ik_pose,
- ik_seed_state,
- timeout,
- solution,
- solution_callback,
- error_code,
- consistency_limits,
- options);
-}
-
-bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
- const std::vector<double> &ik_seed_state,
- double timeout,
- const std::vector<double> &consistency_limits,
- std::vector<double> &solution,
- const IKCallbackFn &solution_callback,
- moveit_msgs::MoveItErrorCodes &error_code,
- const kinematics::KinematicsQueryOptions &options) const
-{
- return searchPositionIK(ik_pose,
- ik_seed_state,
- timeout,
- solution,
- solution_callback,
- error_code,
- consistency_limits,
- options);
-}
-
-bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
- const std::vector<double> &ik_seed_state,
- double timeout,
- std::vector<double> &solution,
- const IKCallbackFn &solution_callback,
- moveit_msgs::MoveItErrorCodes &error_code,
- const std::vector<double> &consistency_limits,
- const kinematics::KinematicsQueryOptions &options) const
-{
- ros::WallTime n1 = ros::WallTime::now();
- if(!active_)
- {
- ROS_ERROR_NAMED("kdl","kinematics not active");
- error_code.val = error_code.NO_IK_SOLUTION;
- return false;
- }
-
- if(ik_seed_state.size() != dimension_)
- {
- ROS_ERROR_STREAM_NAMED("kdl","Seed state must have size " << dimension_ << " instead of size " << ik_seed_state.size());
- error_code.val = error_code.NO_IK_SOLUTION;
- return false;
- }
-
- if(!consistency_limits.empty() && consistency_limits.size() != dimension_)
- {
- ROS_ERROR_STREAM_NAMED("kdl","Consistency limits be empty or must have size " << dimension_ << " instead of size " << consistency_limits.size());
- error_code.val = error_code.NO_IK_SOLUTION;
- return false;
- }
-
- KDL::JntArray jnt_seed_state(dimension_);
- KDL::JntArray jnt_pos_in(dimension_);
- KDL::JntArray jnt_pos_out(dimension_);
-
- KDL::ChainFkSolverPos_recursive fk_solver(kdl_chain_);
- KDL::ChainIkSolverVel_pinv_mimic ik_solver_vel(kdl_chain_, joint_model_group_->getMimicJointModels().size(), redundant_joint_indices_.size(), position_ik_);
- KDL::ChainIkSolverPos_NR_JL_Mimic ik_solver_pos(kdl_chain_, joint_min_, joint_max_, fk_solver, ik_solver_vel, max_solver_iterations_, epsilon_, position_ik_);
- ik_solver_vel.setMimicJoints(mimic_joints_);
- ik_solver_pos.setMimicJoints(mimic_joints_);
-
- if ((redundant_joint_indices_.size() > 0) && !ik_solver_vel.setRedundantJointsMapIndex(redundant_joints_map_index_))
- {
- ROS_ERROR_NAMED("kdl","Could not set redundant joints");
- return false;
- }
-
- if(options.lock_redundant_joints)
- {
- ik_solver_vel.lockRedundantJoints();
- }
-
- solution.resize(dimension_);
-
- KDL::Frame pose_desired;
- tf::poseMsgToKDL(ik_pose, pose_desired);
-
- ROS_DEBUG_STREAM_NAMED("kdl","searchPositionIK2: Position request pose is " <<
- ik_pose.position.x << " " <<
- ik_pose.position.y << " " <<
- ik_pose.position.z << " " <<
- ik_pose.orientation.x << " " <<
- ik_pose.orientation.y << " " <<
- ik_pose.orientation.z << " " <<
- ik_pose.orientation.w);
- //Do the IK
- for(unsigned int i=0; i < dimension_; i++)
- jnt_seed_state(i) = ik_seed_state[i];
- jnt_pos_in = jnt_seed_state;
-
- unsigned int counter(0);
- while(1)
- {
- // ROS_DEBUG_NAMED("kdl","Iteration: %d, time: %f, Timeout: %f",counter,(ros::WallTime::now()-n1).toSec(),timeout);
- counter++;
- if(timedOut(n1,timeout))
- {
- ROS_DEBUG_NAMED("kdl","IK timed out");
- error_code.val = error_code.TIMED_OUT;
- ik_solver_vel.unlockRedundantJoints();
- return false;
- }
- int ik_valid = ik_solver_pos.CartToJnt(jnt_pos_in, pose_desired, jnt_pos_out);
- ROS_DEBUG_NAMED("kdl","IK valid: %d", ik_valid);
- if(!consistency_limits.empty())
- {
- getRandomConfiguration(jnt_seed_state, consistency_limits, jnt_pos_in, options.lock_redundant_joints);
- if( (ik_valid < 0 && !options.return_approximate_solution) || !checkConsistency(jnt_seed_state, consistency_limits, jnt_pos_out))
- {
- ROS_DEBUG_NAMED("kdl","Could not find IK solution: does not match consistency limits");
- continue;
- }
- }
- else
- {
- getRandomConfiguration(jnt_pos_in, options.lock_redundant_joints);
- ROS_DEBUG_NAMED("kdl","New random configuration");
- for(unsigned int j=0; j < dimension_; j++)
- ROS_DEBUG_NAMED("kdl","%d %f", j, jnt_pos_in(j));
-
- if(ik_valid < 0 && !options.return_approximate_solution)
- {
- ROS_DEBUG_NAMED("kdl","Could not find IK solution");
- continue;
- }
- }
- ROS_DEBUG_NAMED("kdl","Found IK solution");
- for(unsigned int j=0; j < dimension_; j++)
- solution[j] = jnt_pos_out(j);
- if(!solution_callback.empty())
- solution_callback(ik_pose,solution,error_code);
- else
- error_code.val = error_code.SUCCESS;
-
- if(error_code.val == error_code.SUCCESS)
- {
- ROS_DEBUG_STREAM_NAMED("kdl","Solved after " << counter << " iterations");
- ik_solver_vel.unlockRedundantJoints();
- return true;
- }
- }
- ROS_DEBUG_NAMED("kdl","An IK that satisifes the constraints and is collision free could not be found");
- error_code.val = error_code.NO_IK_SOLUTION;
- ik_solver_vel.unlockRedundantJoints();
- return false;
-}
-
-bool KDLKinematicsPlugin::getPositionFK(const std::vector<std::string> &link_names,
- const std::vector<double> &joint_angles,
- std::vector<geometry_msgs::Pose> &poses) const
-{
- ros::WallTime n1 = ros::WallTime::now();
- if(!active_)
- {
- ROS_ERROR_NAMED("kdl","kinematics not active");
- return false;
- }
- poses.resize(link_names.size());
- if(joint_angles.size() != dimension_)
- {
- ROS_ERROR_NAMED("kdl","Joint angles vector must have size: %d",dimension_);
- return false;
- }
-
- KDL::Frame p_out;
- geometry_msgs::PoseStamped pose;
- tf::Stamped<tf::Pose> tf_pose;
-
- KDL::JntArray jnt_pos_in(dimension_);
- for(unsigned int i=0; i < dimension_; i++)
- {
- jnt_pos_in(i) = joint_angles[i];
- }
-
- KDL::ChainFkSolverPos_recursive fk_solver(kdl_chain_);
-
- bool valid = true;
- for(unsigned int i=0; i < poses.size(); i++)
- {
- ROS_DEBUG_NAMED("kdl","End effector index: %d",getKDLSegmentIndex(link_names[i]));
- if(fk_solver.JntToCart(jnt_pos_in,p_out,getKDLSegmentIndex(link_names[i])) >=0)
- {
- tf::poseKDLToMsg(p_out,poses[i]);
- }
- else
- {
- ROS_ERROR_NAMED("kdl","Could not compute FK for %s",link_names[i].c_str());
- valid = false;
- }
- }
- return valid;
-}
-
-const std::vector<std::string>& KDLKinematicsPlugin::getJointNames() const
-{
- return ik_chain_info_.joint_names;
-}
-
-const std::vector<std::string>& KDLKinematicsPlugin::getLinkNames() const
-{
- return ik_chain_info_.link_names;
-}
-}