From f3b8dfdb394810cb8affba545dfbe58c8b36980b 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 a6139c1..7839d7a 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 309e791..076791b 100644 --- a/src/kinematics_plugin.cpp +++ b/src/kinematics_plugin.cpp @@ -582,8 +582,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) { @@ -609,10 +609,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 dd919ba232cc9c180dff6d8ef663d9b85da0ff2e 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 7839d7a..68ad9ac 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 076791b..0410686 100644 --- a/src/kinematics_plugin.cpp +++ b/src/kinematics_plugin.cpp @@ -611,7 +611,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 27b1f188ce47efa2c0acb31e4055983b1123f336 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 68ad9ac..94171f3 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 0410686..d2c3797 100644 --- a/src/kinematics_plugin.cpp +++ b/src/kinematics_plugin.cpp @@ -610,8 +610,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());