From edb027e8564650760c30295f611e78eca8d09d9e Mon Sep 17 00:00:00 2001
From: ipa-fxm <felix.messmer@ipa.fhg.de>
Date: Fri, 6 Mar 2015 15:26:26 +0100
Subject: [PATCH] crop ik solutions wrt joint_limits
---
ur10_moveit_config/config/kinematics.yaml | 3 +-
ur5_moveit_config/config/kinematics.yaml | 3 +-
ur_kinematics/src/ur_moveit_plugin.cpp | 51 +++++++++++++++++++++--
3 files changed, 49 insertions(+), 8 deletions(-)
diff --git a/ur10_moveit_config/config/kinematics.yaml b/ur10_moveit_config/config/kinematics.yaml
index c45b664..e37d336 100644
--- a/ur10_moveit_config/config/kinematics.yaml
+++ b/ur10_moveit_config/config/kinematics.yaml
@@ -1,4 +1,3 @@
-## the UR10KinematicsPlugin only works with the unlimited UR10, i.e. limited:=false
#manipulator:
# kinematics_solver: ur_kinematics/UR10KinematicsPlugin
# kinematics_solver_search_resolution: 0.005
@@ -8,4 +7,4 @@ manipulator:
kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
kinematics_solver_search_resolution: 0.005
kinematics_solver_timeout: 0.005
- kinematics_solver_attempts: 3
\ No newline at end of file
+ kinematics_solver_attempts: 3
diff --git a/ur5_moveit_config/config/kinematics.yaml b/ur5_moveit_config/config/kinematics.yaml
index 7b3d7af..e2fa9b1 100644
--- a/ur5_moveit_config/config/kinematics.yaml
+++ b/ur5_moveit_config/config/kinematics.yaml
@@ -1,4 +1,3 @@
-## the UR5KinematicsPlugin only works with the unlimited UR5, i.e. limited:=false
#manipulator:
# kinematics_solver: ur_kinematics/UR5KinematicsPlugin
# kinematics_solver_search_resolution: 0.005
@@ -8,4 +7,4 @@ manipulator:
kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
kinematics_solver_search_resolution: 0.005
kinematics_solver_timeout: 0.005
- kinematics_solver_attempts: 3
\ No newline at end of file
+ kinematics_solver_attempts: 3
diff --git a/ur_kinematics/src/ur_moveit_plugin.cpp b/ur_kinematics/src/ur_moveit_plugin.cpp
index 56ca34f..1605586 100644
--- a/ur_kinematics/src/ur_moveit_plugin.cpp
+++ b/ur_kinematics/src/ur_moveit_plugin.cpp
@@ -645,14 +645,57 @@ bool URKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
// Do the analytic IK
num_sols = inverse((double*) homo_ik_pose, (double*) q_ik_sols,
jnt_pos_test(ur_joint_inds_start_+5));
+
+
+ uint16_t num_valid_sols;
+ std::vector< std::vector<double> > q_ik_valid_sols;
+ for(uint16_t i=0; i<num_sols; i++)
+ {
+ bool valid = true;
+ std::vector< double > valid_solution;
+ valid_solution.assign(6,0.0);
+
+ for(uint16_t j=0; j<6; j++)
+ {
+ if((q_ik_sols[i][j] <= ik_chain_info_.limits[j].max_position) && (q_ik_sols[i][j] >= ik_chain_info_.limits[j].min_position))
+ {
+ valid_solution[j] = q_ik_sols[i][j];
+ valid = true;
+ continue;
+ }
+ else if ((q_ik_sols[i][j] > ik_chain_info_.limits[j].max_position) && (q_ik_sols[i][j]-2*M_PI > ik_chain_info_.limits[j].min_position))
+ {
+ valid_solution[j] = q_ik_sols[i][j]-2*M_PI;
+ valid = true;
+ continue;
+ }
+ else if ((q_ik_sols[i][j] < ik_chain_info_.limits[j].min_position) && (q_ik_sols[i][j]+2*M_PI < ik_chain_info_.limits[j].max_position))
+ {
+ valid_solution[j] = q_ik_sols[i][j]+2*M_PI;
+ valid = true;
+ continue;
+ }
+ else
+ {
+ valid = false;
+ break;
+ }
+ }
+
+ if(valid)
+ {
+ q_ik_valid_sols.push_back(valid_solution);
+ }
+ }
+
// use weighted absolute deviations to determine the solution closest the seed state
std::vector<idx_double> weighted_diffs;
- for(uint16_t i=0; i<num_sols; i++) {
+ for(uint16_t i=0; i<q_ik_valid_sols.size(); i++) {
double cur_weighted_diff = 0;
for(uint16_t j=0; j<6; j++) {
// solution violates the consistency_limits, throw it out
- double abs_diff = std::fabs(ik_seed_state[ur_joint_inds_start_+j] - q_ik_sols[i][j]);
+ double abs_diff = std::fabs(ik_seed_state[ur_joint_inds_start_+j] - q_ik_valid_sols[i][j]);
if(!consistency_limits.empty() && abs_diff > consistency_limits[ur_joint_inds_start_+j]) {
cur_weighted_diff = std::numeric_limits<double>::infinity();
break;
@@ -669,7 +712,7 @@ bool URKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
printf(" q %1.2f, %1.2f, %1.2f, %1.2f, %1.2f, %1.2f\n", ik_seed_state[1], ik_seed_state[2], ik_seed_state[3], ik_seed_state[4], ik_seed_state[5], ik_seed_state[6]);
for(uint16_t i=0; i<weighted_diffs.size(); i++) {
int cur_idx = weighted_diffs[i].first;
- printf("diff %f, i %d, q %1.2f, %1.2f, %1.2f, %1.2f, %1.2f, %1.2f\n", weighted_diffs[i].second, cur_idx, q_ik_sols[cur_idx][0], q_ik_sols[cur_idx][1], q_ik_sols[cur_idx][2], q_ik_sols[cur_idx][3], q_ik_sols[cur_idx][4], q_ik_sols[cur_idx][5]);
+ printf("diff %f, i %d, q %1.2f, %1.2f, %1.2f, %1.2f, %1.2f, %1.2f\n", weighted_diffs[i].second, cur_idx, q_ik_valid_sols[cur_idx][0], q_ik_valid_sols[cur_idx][1], q_ik_valid_sols[cur_idx][2], q_ik_valid_sols[cur_idx][3], q_ik_valid_sols[cur_idx][4], q_ik_valid_sols[cur_idx][5]);
}
printf("end\n");
#endif
@@ -682,7 +725,7 @@ bool URKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
// copy the best solution to the output
int cur_idx = weighted_diffs[i].first;
- std::copy(q_ik_sols[cur_idx], q_ik_sols[cur_idx+1], solution.begin() + ur_joint_inds_start_);
+ solution = q_ik_valid_sols[cur_idx];
// see if this solution passes the callback function test
if(!solution_callback.empty())
--
GitLab