Skip to content
Snippets Groups Projects
Commit 67aa1b9a authored by Kelsey's avatar Kelsey
Browse files

IK working.

parent 6b2369b9
Branches
Tags
No related merge requests found
Expand all Hide whitespace changes
Inline Side-by-side
Showing
with 273 additions and 1523 deletions
ur_kinematics/CMakeLists.txt
+ 6
− 2
View file @ 67aa1b9a
...@@ -4,7 +4,7 @@ project(ur_kinematics) ...@@ -4,7 +4,7 @@ project(ur_kinematics)
## Find catkin macros and libraries ## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages ## 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 ## System dependencies are found with CMake's conventions
find_package(Boost REQUIRED COMPONENTS system) find_package(Boost REQUIRED COMPONENTS system)
...@@ -50,7 +50,7 @@ catkin_package( ...@@ -50,7 +50,7 @@ catkin_package(
INCLUDE_DIRS include INCLUDE_DIRS include
LIBRARIES ur10_kin ur5_kin ur10_moveit_plugin ur5_moveit_plugin LIBRARIES ur10_kin ur5_kin ur10_moveit_plugin ur5_moveit_plugin
CATKIN_DEPENDS pluginlib CATKIN_DEPENDS pluginlib
DEPENDS moveit_core DEPENDS moveit_core
# CATKIN_DEPENDS other_catkin_pkg # CATKIN_DEPENDS other_catkin_pkg
# DEPENDS system_lib # DEPENDS system_lib
) )
...@@ -82,6 +82,8 @@ set_target_properties(ur10_moveit_plugin PROPERTIES COMPILE_DEFINITIONS "UR10_PA ...@@ -82,6 +82,8 @@ set_target_properties(ur10_moveit_plugin PROPERTIES COMPILE_DEFINITIONS "UR10_PA
target_link_libraries(ur10_moveit_plugin target_link_libraries(ur10_moveit_plugin
${catkin_LIBRARIES} ${catkin_LIBRARIES}
${Boost_LIBRARIES} ${Boost_LIBRARIES}
${moveit_ros_planning_LIBRARIES}
/opt/ros/hydro/lib/libmoveit_kdl_kinematics_plugin.so
ur10_kin) ur10_kin)
add_library(ur5_moveit_plugin src/ur_moveit_plugin.cpp) 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 ...@@ -89,6 +91,8 @@ set_target_properties(ur5_moveit_plugin PROPERTIES COMPILE_DEFINITIONS "UR5_PARA
target_link_libraries(ur5_moveit_plugin target_link_libraries(ur5_moveit_plugin
${catkin_LIBRARIES} ${catkin_LIBRARIES}
${Boost_LIBRARIES} ${Boost_LIBRARIES}
${moveit_ros_planning_LIBRARIES}
/opt/ros/hydro/lib/libmoveit_kdl_kinematics_plugin.so
ur5_kin) ur5_kin)
## Declare a cpp executable ## Declare a cpp executable
......
ur_kinematics/include/ur_kinematics/ur_kin.h
+ 2
− 0
View file @ 67aa1b9a
...@@ -86,6 +86,8 @@ namespace ur_kinematics { ...@@ -86,6 +86,8 @@ namespace ur_kinematics {
// @param T The 4x4 end effector pose in row-major ordering // @param T The 4x4 end effector pose in row-major ordering
void forward(const double* q, double* T); 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, void forward_all(const double* q, double* T1, double* T2, double* T3,
double* T4, double* T5, double* T6); double* T4, double* T5, double* T6);
......
ur_kinematics/include/ur_kinematics/ur_moveit_plugin.h
+ 52
− 62
View file @ 67aa1b9a
...@@ -2,84 +2,74 @@ ...@@ -2,84 +2,74 @@
#include <ros/ros.h> #include <ros/ros.h>
#include <moveit/kinematics_base/kinematics_base.h> #include <moveit/kinematics_base/kinematics_base.h>
#include <moveit_msgs/KinematicSolverInfo.h> #include <moveit_msgs/KinematicSolverInfo.h>
#include <moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h>
namespace ur_kinematics namespace ur_kinematics
{ {
class URKinematicsPlugin : public kinematics::KinematicsBase class URKinematicsPlugin : public kdl_kinematics_plugin::KDLKinematicsPlugin
{ {
public: public:
/** /**
* @brief Plugin-able interface to the ur kinematics * @brief Plugin-able interface to the ur kinematics
*/ */
URKinematicsPlugin(); URKinematicsPlugin();
//
// //~URKinematicsPlugin();
//~URKinematicsPlugin(); //
// virtual bool getPositionFK(const std::vector<std::string> &link_names,
virtual bool getPositionIK(const geometry_msgs::Pose &ik_pose, // const std::vector<double> &joint_angles,
const std::vector<double> &ik_seed_state, // std::vector<geometry_msgs::Pose> &poses) const;
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;
/**
* @brief Initialization function for the kinematics * @brief Initialization function for the kinematics
* @return True if initialization was successful, false otherwise * @return True if initialization was successful, false otherwise
*/ */
virtual bool initialize(const std::string& robot_description, virtual bool initialize(const std::string& robot_description,
const std::string& group_name, const std::string& group_name,
const std::string& base_frame, const std::string& base_frame,
const std::string& tip_frame, const std::string& tip_frame,
double search_discretization); double search_discretization);
//
/** // /**
* @brief Return all the joint names in the order they are used internally // * @brief Return all the joint names in the order they are used internally
*/ // */
const std::vector<std::string>& getJointNames() const; // 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; bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
const std::vector<double> &ik_seed_state,
protected: 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_;
}; };
} }
ur_kinematics/package.xml
+ 4
− 0
View file @ 67aa1b9a
...@@ -47,14 +47,18 @@ ...@@ -47,14 +47,18 @@
<buildtool_depend>catkin</buildtool_depend> <buildtool_depend>catkin</buildtool_depend>
<build_depend>moveit_core</build_depend> <build_depend>moveit_core</build_depend>
<build_depend>moveit_ros_planning</build_depend>
<build_depend>roscpp</build_depend> <build_depend>roscpp</build_depend>
<build_depend>geometry_msgs</build_depend> <build_depend>geometry_msgs</build_depend>
<build_depend>pluginlib</build_depend> <build_depend>pluginlib</build_depend>
<build_depend>tf_conversions</build_depend>
<run_depend>moveit_core</run_depend> <run_depend>moveit_core</run_depend>
<run_depend>moveit_ros_planning</run_depend>
<run_depend>roscpp</run_depend> <run_depend>roscpp</run_depend>
<run_depend>geometry_msgs</run_depend> <run_depend>geometry_msgs</run_depend>
<run_depend>pluginlib</run_depend> <run_depend>pluginlib</run_depend>
<run_depend>tf_conversions</run_depend>
<!-- The export tag contains other, unspecified, tags --> <!-- The export tag contains other, unspecified, tags -->
......
ur_kinematics/src/ur_kin.cpp
+ 113
− 101
View file @ 67aa1b9a
...@@ -45,107 +45,119 @@ namespace ur_kinematics { ...@@ -45,107 +45,119 @@ namespace ur_kinematics {
double s23 = sin(q23), c23 = cos(q23); double s23 = sin(q23), c23 = cos(q23);
double s234 = sin(q234), c234 = cos(q234); double s234 = sin(q234), c234 = cos(q234);
*T1 = c1; T1++; if(T1 != NULL) {
*T1 = 0; T1++; *T1 = c1; T1++;
*T1 = s1; T1++; *T1 = 0; T1++;
*T1 = 0; T1++; *T1 = s1; T1++;
*T1 = s1; T1++; *T1 = 0; T1++;
*T1 = 0; T1++; *T1 = s1; T1++;
*T1 = -c1; T1++; *T1 = 0; T1++;
*T1 = 0; T1++; *T1 = -c1; T1++;
*T1 = 0; T1++; *T1 = 0; T1++;
*T1 = 1; T1++; *T1 = 0; T1++;
*T1 = 0; T1++; *T1 = 1; T1++;
*T1 =d1; T1++; *T1 = 0; T1++;
*T1 = 0; T1++; *T1 =d1; T1++;
*T1 = 0; T1++; *T1 = 0; T1++;
*T1 = 0; T1++; *T1 = 0; T1++;
*T1 = 1; T1++; *T1 = 0; T1++;
*T1 = 1; T1++;
*T2 = c1*c2; T2++; }
*T2 = -c1*s2; T2++;
*T2 = s1; T2++; if(T2 != NULL) {
*T2 =a2*c1*c2; T2++; *T2 = c1*c2; T2++;
*T2 = c2*s1; T2++; *T2 = -c1*s2; T2++;
*T2 = -s1*s2; T2++; *T2 = s1; T2++;
*T2 = -c1; T2++; *T2 =a2*c1*c2; T2++;
*T2 =a2*c2*s1; T2++; *T2 = c2*s1; T2++;
*T2 = s2; T2++; *T2 = -s1*s2; T2++;
*T2 = c2; T2++; *T2 = -c1; T2++;
*T2 = 0; T2++; *T2 =a2*c2*s1; T2++;
*T2 = d1 + a2*s2; T2++; *T2 = s2; T2++;
*T2 = 0; T2++; *T2 = c2; T2++;
*T2 = 0; T2++; *T2 = 0; T2++;
*T2 = 0; T2++; *T2 = d1 + a2*s2; T2++;
*T2 = 1; T2++; *T2 = 0; T2++;
*T2 = 0; T2++;
*T3 = c23*c1; T3++; *T2 = 0; T2++;
*T3 = -s23*c1; T3++; *T2 = 1; T2++;
*T3 = s1; T3++; }
*T3 =c1*(a3*c23 + a2*c2); T3++;
*T3 = c23*s1; T3++; if(T3 != NULL) {
*T3 = -s23*s1; T3++; *T3 = c23*c1; T3++;
*T3 = -c1; T3++; *T3 = -s23*c1; T3++;
*T3 =s1*(a3*c23 + a2*c2); T3++; *T3 = s1; T3++;
*T3 = s23; T3++; *T3 =c1*(a3*c23 + a2*c2); T3++;
*T3 = c23; T3++; *T3 = c23*s1; T3++;
*T3 = 0; T3++; *T3 = -s23*s1; T3++;
*T3 = d1 + a3*s23 + a2*s2; T3++; *T3 = -c1; T3++;
*T3 = 0; T3++; *T3 =s1*(a3*c23 + a2*c2); T3++;
*T3 = 0; T3++; *T3 = s23; T3++;
*T3 = 0; T3++; *T3 = c23; T3++;
*T3 = 1; T3++; *T3 = 0; T3++;
*T3 = d1 + a3*s23 + a2*s2; T3++;
*T4 = c234*c1; T4++; *T3 = 0; T3++;
*T4 = s1; T4++; *T3 = 0; T3++;
*T4 = s234*c1; T4++; *T3 = 0; T3++;
*T4 =c1*(a3*c23 + a2*c2) + d4*s1; T4++; *T3 = 1; T3++;
*T4 = c234*s1; T4++; }
*T4 = -c1; T4++;
*T4 = s234*s1; T4++; if(T4 != NULL) {
*T4 =s1*(a3*c23 + a2*c2) - d4*c1; T4++; *T4 = c234*c1; T4++;
*T4 = s234; T4++; *T4 = s1; T4++;
*T4 = 0; T4++; *T4 = s234*c1; T4++;
*T4 = -c234; T4++; *T4 =c1*(a3*c23 + a2*c2) + d4*s1; T4++;
*T4 = d1 + a3*s23 + a2*s2; T4++; *T4 = c234*s1; T4++;
*T4 = 0; T4++; *T4 = -c1; T4++;
*T4 = 0; T4++; *T4 = s234*s1; T4++;
*T4 = 0; T4++; *T4 =s1*(a3*c23 + a2*c2) - d4*c1; T4++;
*T4 = 1; T4++; *T4 = s234; T4++;
*T4 = 0; T4++;
*T5 = s1*s5 + c234*c1*c5; T5++; *T4 = -c234; T4++;
*T5 = -s234*c1; T5++; *T4 = d1 + a3*s23 + a2*s2; T4++;
*T5 = c5*s1 - c234*c1*s5; T5++; *T4 = 0; T4++;
*T5 =c1*(a3*c23 + a2*c2) + d4*s1 + d5*s234*c1; T5++; *T4 = 0; T4++;
*T5 = c234*c5*s1 - c1*s5; T5++; *T4 = 0; T4++;
*T5 = -s234*s1; T5++; *T4 = 1; T4++;
*T5 = - c1*c5 - c234*s1*s5; T5++; }
*T5 =s1*(a3*c23 + a2*c2) - d4*c1 + d5*s234*s1; T5++;
*T5 = s234*c5; T5++; if(T5 != NULL) {
*T5 = c234; T5++; *T5 = s1*s5 + c234*c1*c5; T5++;
*T5 = -s234*s5; T5++; *T5 = -s234*c1; T5++;
*T5 = d1 + a3*s23 + a2*s2 - d5*c234; T5++; *T5 = c5*s1 - c234*c1*s5; T5++;
*T5 = 0; T5++; *T5 =c1*(a3*c23 + a2*c2) + d4*s1 + d5*s234*c1; T5++;
*T5 = 0; T5++; *T5 = c234*c5*s1 - c1*s5; T5++;
*T5 = 0; T5++; *T5 = -s234*s1; T5++;
*T5 = 1; T5++; *T5 = - c1*c5 - c234*s1*s5; T5++;
*T5 =s1*(a3*c23 + a2*c2) - d4*c1 + d5*s234*s1; T5++;
*T6 = c6*(s1*s5 + c234*c1*c5) - s234*c1*s6; T6++; *T5 = s234*c5; T5++;
*T6 = - s6*(s1*s5 + c234*c1*c5) - s234*c1*c6; T6++; *T5 = c234; T5++;
*T6 = c5*s1 - c234*c1*s5; T6++; *T5 = -s234*s5; T5++;
*T6 =d6*(c5*s1 - c234*c1*s5) + c1*(a3*c23 + a2*c2) + d4*s1 + d5*s234*c1; T6++; *T5 = d1 + a3*s23 + a2*s2 - d5*c234; T5++;
*T6 = - c6*(c1*s5 - c234*c5*s1) - s234*s1*s6; T6++; *T5 = 0; T5++;
*T6 = s6*(c1*s5 - c234*c5*s1) - s234*c6*s1; T6++; *T5 = 0; T5++;
*T6 = - c1*c5 - c234*s1*s5; T6++; *T5 = 0; T5++;
*T6 =s1*(a3*c23 + a2*c2) - d4*c1 - d6*(c1*c5 + c234*s1*s5) + d5*s234*s1; T6++; *T5 = 1; T5++;
*T6 = c234*s6 + s234*c5*c6; T6++; }
*T6 = c234*c6 - s234*c5*s6; T6++;
*T6 = -s234*s5; T6++; if(T6 != NULL) {
*T6 = d1 + a3*s23 + a2*s2 - d5*c234 - d6*s234*s5; T6++; *T6 = c6*(s1*s5 + c234*c1*c5) - s234*c1*s6; T6++;
*T6 = 0; T6++; *T6 = - s6*(s1*s5 + c234*c1*c5) - s234*c1*c6; T6++;
*T6 = 0; T6++; *T6 = c5*s1 - c234*c1*s5; T6++;
*T6 = 0; T6++; *T6 =d6*(c5*s1 - c234*c1*s5) + c1*(a3*c23 + a2*c2) + d4*s1 + d5*s234*c1; T6++;
*T6 = 1; 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) { int inverse(const double* T, double* q_sols, double q6_des) {
......
ur_kinematics/src/ur_moveit_plugin.cpp
+ 96
− 630
View file @ 67aa1b9a
This diff is collapsed.
ur_kinematics/src/ur_moveit_plugin.cpp.bak deleted 100644 → 0
+ 0
− 119
View file @ 6b2369b9
#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;
}
}
ur_kinematics/src/ur_moveit_plugin.cpp.bak2 deleted 100644 → 0
+ 0
− 609
View file @ 6b2369b9
This diff is collapsed.
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment