Skip to content

Commit

Permalink
Merge pull request #1 from mechwiz/master
Browse files Browse the repository at this point in the history
implement PR TAMS-Group#30
  • Loading branch information
dudasdavid authored Jul 9, 2021
2 parents 3dd845a + 27b1f18 commit e2111cc
Show file tree
Hide file tree
Showing 2 changed files with 12 additions and 5 deletions.
3 changes: 3 additions & 0 deletions include/bio_ik/robot_info.h
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,7 @@ class RobotInfo
std::vector<VariableInfo> variables;
std::vector<size_t> activeVariables;
std::vector<moveit::core::JointModel::JointType> variable_joint_types;
std::vector<bool> has_bounds;
moveit::core::RobotModelConstPtr robot_model;

__attribute__((always_inline)) static inline double clamp2(double v, double lo, double hi)
Expand Down Expand Up @@ -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);
has_bounds.push_back(bounded);
}

for(size_t ivar = 0; ivar < model->getVariableCount(); ivar++)
Expand All @@ -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 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; }
};
Expand Down
14 changes: 9 additions & 5 deletions src/kinematics_plugin.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand All @@ -609,10 +609,14 @@ struct BioIKKinematicsPlugin : kinematics::KinematicsBase {
v = hi;
}
state[ivar] = v;
}

// wrap angles
robot_model->enforcePositionBounds(state.data());
// 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());
}
}

// map result to jointgroup variables
{
Expand Down

0 comments on commit e2111cc

Please sign in to comment.