From 2dab23119a4f0956908116d7e3767bf478517dfa Mon Sep 17 00:00:00 2001 From: Michael Wiznitzer Date: Wed, 24 Feb 2021 21:21:05 -0500 Subject: [PATCH 1/3] don't wrap joint angles for continuous joints --- include/bio_ik/robot_info.h | 3 +++ src/kinematics_plugin.cpp | 14 +++++++++----- 2 files changed, 12 insertions(+), 5 deletions(-) diff --git a/include/bio_ik/robot_info.h b/include/bio_ik/robot_info.h index a6139c14..7839d7aa 100644 --- a/include/bio_ik/robot_info.h +++ b/include/bio_ik/robot_info.h @@ -56,6 +56,7 @@ class RobotInfo std::vector variables; std::vector activeVariables; std::vector variable_joint_types; + std::vector continuous_joints; moveit::core::RobotModelConstPtr robot_model; __attribute__((always_inline)) static inline double clamp2(double v, double lo, double hi) @@ -97,6 +98,7 @@ class RobotInfo info.max_velocity_rcp = info.max_velocity > 0.0 ? 1.0 / info.max_velocity : 0.0; variables.push_back(info); + continuous_joints.push_back(!bounded); } for(size_t ivar = 0; ivar < model->getVariableCount(); ivar++) @@ -120,6 +122,7 @@ class RobotInfo inline bool isRevolute(size_t variable_index) const { return variable_joint_types[variable_index] == moveit::core::JointModel::REVOLUTE; } inline bool isPrismatic(size_t variable_index) const { return variable_joint_types[variable_index] == moveit::core::JointModel::PRISMATIC; } + inline bool isContinuous(size_t variable_index) const { return continuous_joints[variable_index]; } inline double getMaxVelocity(size_t i) const { return variables[i].max_velocity; } inline double getMaxVelocityRcp(size_t i) const { return variables[i].max_velocity_rcp; } }; diff --git a/src/kinematics_plugin.cpp b/src/kinematics_plugin.cpp index be721c7d..82ed25e2 100644 --- a/src/kinematics_plugin.cpp +++ b/src/kinematics_plugin.cpp @@ -583,8 +583,8 @@ struct BioIKKinematicsPlugin : kinematics::KinematicsBase { if (robot_info.isRevolute(ivar) && robot_model->getMimicJointModels().empty()) { auto r = problem.initial_guess[ivar]; - auto lo = robot_info.getMin(ivar); - auto hi = robot_info.getMax(ivar); + auto lo = robot_info.getClipMin(ivar); + auto hi = robot_info.getClipMax(ivar); // move close to initial guess if (r < v - M_PI || r > v + M_PI) { @@ -610,10 +610,14 @@ struct BioIKKinematicsPlugin : kinematics::KinematicsBase { v = hi; } state[ivar] = v; - } - // wrap angles - robot_model->enforcePositionBounds(state.data()); + // wrap angles that are not continuous + if(!robot_info.isContinuous(ivar)) + { + auto* joint_model = robot_model->getJointOfVariable(ivar); + joint_model->enforcePositionBounds(state.data() + ivar, joint_model->getVariableBounds()); + } + } // map result to jointgroup variables { From 9f6db75bcf245470c2cf57a42fcc96b2780184a7 Mon Sep 17 00:00:00 2001 From: Michael Wiznitzer Date: Thu, 25 Feb 2021 13:44:28 -0500 Subject: [PATCH 2/3] use hasBounds terminology instead of isContinuous --- include/bio_ik/robot_info.h | 6 +++--- src/kinematics_plugin.cpp | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/include/bio_ik/robot_info.h b/include/bio_ik/robot_info.h index 7839d7aa..68ad9acd 100644 --- a/include/bio_ik/robot_info.h +++ b/include/bio_ik/robot_info.h @@ -56,7 +56,7 @@ class RobotInfo std::vector variables; std::vector activeVariables; std::vector variable_joint_types; - std::vector continuous_joints; + std::vector has_bounds; moveit::core::RobotModelConstPtr robot_model; __attribute__((always_inline)) static inline double clamp2(double v, double lo, double hi) @@ -98,7 +98,7 @@ class RobotInfo info.max_velocity_rcp = info.max_velocity > 0.0 ? 1.0 / info.max_velocity : 0.0; variables.push_back(info); - continuous_joints.push_back(!bounded); + has_bounds.push_back(!bounded); } for(size_t ivar = 0; ivar < model->getVariableCount(); ivar++) @@ -122,7 +122,7 @@ class RobotInfo inline bool isRevolute(size_t variable_index) const { return variable_joint_types[variable_index] == moveit::core::JointModel::REVOLUTE; } inline bool isPrismatic(size_t variable_index) const { return variable_joint_types[variable_index] == moveit::core::JointModel::PRISMATIC; } - inline bool isContinuous(size_t variable_index) const { return continuous_joints[variable_index]; } + inline bool hasBounds(size_t variable_index) const { return has_bounds[variable_index]; } inline double getMaxVelocity(size_t i) const { return variables[i].max_velocity; } inline double getMaxVelocityRcp(size_t i) const { return variables[i].max_velocity_rcp; } }; diff --git a/src/kinematics_plugin.cpp b/src/kinematics_plugin.cpp index 82ed25e2..bde08db4 100644 --- a/src/kinematics_plugin.cpp +++ b/src/kinematics_plugin.cpp @@ -612,7 +612,7 @@ struct BioIKKinematicsPlugin : kinematics::KinematicsBase { state[ivar] = v; // wrap angles that are not continuous - if(!robot_info.isContinuous(ivar)) + if(!robot_info.hasBounds(ivar)) { auto* joint_model = robot_model->getJointOfVariable(ivar); joint_model->enforcePositionBounds(state.data() + ivar, joint_model->getVariableBounds()); From 2f06f2cdc51d5dad52761ecf2ac447b6de5f079c Mon Sep 17 00:00:00 2001 From: Michael Wiznitzer Date: Thu, 25 Feb 2021 14:23:51 -0500 Subject: [PATCH 3/3] word logic better --- include/bio_ik/robot_info.h | 2 +- src/kinematics_plugin.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/include/bio_ik/robot_info.h b/include/bio_ik/robot_info.h index 68ad9acd..94171f3c 100644 --- a/include/bio_ik/robot_info.h +++ b/include/bio_ik/robot_info.h @@ -98,7 +98,7 @@ class RobotInfo info.max_velocity_rcp = info.max_velocity > 0.0 ? 1.0 / info.max_velocity : 0.0; variables.push_back(info); - has_bounds.push_back(!bounded); + has_bounds.push_back(bounded); } for(size_t ivar = 0; ivar < model->getVariableCount(); ivar++) diff --git a/src/kinematics_plugin.cpp b/src/kinematics_plugin.cpp index bde08db4..a1e21614 100644 --- a/src/kinematics_plugin.cpp +++ b/src/kinematics_plugin.cpp @@ -611,8 +611,8 @@ struct BioIKKinematicsPlugin : kinematics::KinematicsBase { } state[ivar] = v; - // wrap angles that are not continuous - if(!robot_info.hasBounds(ivar)) + // wrap angles that are bounded + if(robot_info.hasBounds(ivar)) { auto* joint_model = robot_model->getJointOfVariable(ivar); joint_model->enforcePositionBounds(state.data() + ivar, joint_model->getVariableBounds());