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 bde08db..a1e2161 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());