diff --git a/VirtualRobot/IK/ConstrainedHierarchicalIK.cpp b/VirtualRobot/IK/ConstrainedHierarchicalIK.cpp index e47909a86a7178a397ee4fa8975081c92947fa80..7dbfeda6eb230a58c851f124bd9c930f51af8e27 100644 --- a/VirtualRobot/IK/ConstrainedHierarchicalIK.cpp +++ b/VirtualRobot/IK/ConstrainedHierarchicalIK.cpp @@ -3,7 +3,7 @@ using namespace VirtualRobot; ConstrainedHierarchicalIK::ConstrainedHierarchicalIK(RobotPtr& robot, const RobotNodeSetPtr& nodeSet, float stepSize, int maxIterations, float stall_epsilon, float raise_epsilon) : - ConstrainedIK(robot, maxIterations, stall_epsilon, raise_epsilon), + ConstrainedIK(robot, nodeSet, maxIterations, stall_epsilon, raise_epsilon), nodeSet(nodeSet), stepSize(stepSize) { diff --git a/VirtualRobot/IK/ConstrainedIK.cpp b/VirtualRobot/IK/ConstrainedIK.cpp index 16057cd463dca7daa7b9d748fa907837bd7c13e7..120dfe94ea7bbb9275009db69f714e5bbeb242bc 100644 --- a/VirtualRobot/IK/ConstrainedIK.cpp +++ b/VirtualRobot/IK/ConstrainedIK.cpp @@ -2,14 +2,16 @@ using namespace VirtualRobot; -ConstrainedIK::ConstrainedIK(RobotPtr& robot, int maxIterations, float stall_epsilon, float raise_epsilon) : +ConstrainedIK::ConstrainedIK(RobotPtr& robot, const RobotNodeSetPtr& nodeSet, int maxIterations, float stall_epsilon, float raise_epsilon) : robot(robot), + nodeSet(nodeSet), maxIterations(maxIterations), currentIteration(0), running(false), stallEpsilon(stall_epsilon), raiseEpsilon(raise_epsilon) { + addSeed(eSeedInitial); } void ConstrainedIK::addConstraint(const ConstraintPtr& constraint, int priority, bool hard_constraint) @@ -40,11 +42,23 @@ std::vector<ConstraintPtr> ConstrainedIK::getConstraints() return constraints; } +void ConstrainedIK::addSeed(ConstrainedIK::SeedType type, const Eigen::VectorXf seed) +{ + seeds.push_back(std::pair<SeedType, Eigen::VectorXf>(type, seed)); +} + +void ConstrainedIK::clearSeeds() +{ + seeds.clear(); +} + bool ConstrainedIK::initialize() { Eigen::Matrix4f P; int moves = 0; + nodeSet->getJointValues(initialConfig); + for (auto & constraint : constraints) { constraint->initialize(); @@ -69,6 +83,12 @@ bool ConstrainedIK::initialize() bool ConstrainedIK::solve(bool stepwise) { + if(seeds.size() != 1) + { + // TODO: Implement seeds for IKs other that ConstrainedOptimizationIK + VR_WARNING << "Seeds not supported by this IK implementation" << std::endl; + } + while (currentIteration < maxIterations) { if (!solveStep()) diff --git a/VirtualRobot/IK/ConstrainedIK.h b/VirtualRobot/IK/ConstrainedIK.h index cf2c21b2cbe4f321f01bafbebfa79918b8f9c23c..892d74f55f281e16824d834c22399ed5b919a7a1 100644 --- a/VirtualRobot/IK/ConstrainedIK.h +++ b/VirtualRobot/IK/ConstrainedIK.h @@ -35,12 +35,28 @@ namespace VirtualRobot class VIRTUAL_ROBOT_IMPORT_EXPORT ConstrainedIK : public boost::enable_shared_from_this<ConstrainedIK> { public: - ConstrainedIK(RobotPtr& robot, int maxIterations = 1000, float stall_epsilon = 0.0001, float raise_epsilon = 0.8); + enum SeedType + { + // Zero configuration + eSeedZero, + + // The configuration that was applied when initialize() was called + eSeedInitial, + + // Some other configuration + eSeedOther + }; + + public: + ConstrainedIK(RobotPtr& robot, const RobotNodeSetPtr& nodeSet, int maxIterations = 1000, float stall_epsilon = 0.0001, float raise_epsilon = 0.8); void addConstraint(const ConstraintPtr& constraint, int priority = 0, bool hard_constraint = true); void removeConstraint(const ConstraintPtr& constraint); std::vector<ConstraintPtr> getConstraints(); + void addSeed(SeedType type, const Eigen::VectorXf seed = Eigen::VectorXf()); + void clearSeeds(); + virtual bool initialize(); virtual bool solveStep() = 0; @@ -56,7 +72,12 @@ namespace VirtualRobot std::vector<ConstraintPtr> constraints; std::map<ConstraintPtr, int> priorities; std::map<ConstraintPtr, bool> hardConstraints; + RobotPtr robot; + RobotNodeSetPtr nodeSet; + Eigen::VectorXf initialConfig; + + std::vector<std::pair<SeedType, Eigen::VectorXf>> seeds; int maxIterations; int currentIteration; diff --git a/VirtualRobot/IK/ConstrainedOptimizationIK.cpp b/VirtualRobot/IK/ConstrainedOptimizationIK.cpp index da567d9148b2003ca282c7414b3733f64214c8b2..8d97964000be1564023958018b2b4c363a4689f8 100644 --- a/VirtualRobot/IK/ConstrainedOptimizationIK.cpp +++ b/VirtualRobot/IK/ConstrainedOptimizationIK.cpp @@ -27,17 +27,21 @@ using namespace VirtualRobot; ConstrainedOptimizationIK::ConstrainedOptimizationIK(RobotPtr& robot, const RobotNodeSetPtr& nodeSet, float timeout, float tolerance) : - ConstrainedIK(robot), + ConstrainedIK(robot, nodeSet), nodeSet(nodeSet), timeout(timeout), tolerance(tolerance), maxAttempts(30) { + clearSeeds(); + addSeed(eSeedInitial); + addSeed(eSeedZero); } bool ConstrainedOptimizationIK::initialize() { int size = nodeSet->getSize(); + nodeSet->getJointValues(initialConfig); optimizer.reset(new nlopt::opt(nlopt::LD_SLSQP, size)); @@ -91,20 +95,46 @@ bool ConstrainedOptimizationIK::solve(bool stepwise) int size = nodeSet->getSize(); std::vector<double> x(size); - for(int i = 0; i < size; i++) + + if(attempt >= seeds.size()) { - if(attempt == 0) - { - // First try zero-position - x[i] = 0; - } - else + // Try random configurations + for(int i = 0; i < size; i++) { - // If zero position fails, try random positions float t = (rand()%1000) / 1000.0; x[i] = nodeSet->getNode(i)->getJointLimitLo() + t * (nodeSet->getNode(i)->getJointLimitHi() - nodeSet->getNode(i)->getJointLimitLo()); } } + else + { + switch(seeds[attempt].first) + { + case eSeedZero: + // Try zero configuration + for(int i = 0; i < size; i++) + { + x[i] = 0; + } + break; + + case eSeedInitial: + // Try initial configuration + for(int i = 0; i < size; i++) + { + x[i] = initialConfig(i); + } + break; + + case eSeedOther: + // Try used specified seed + Eigen::VectorXf s = seeds[attempt].second; + for(int i = 0; i < size; i++) + { + x[i] = s(i); + } + break; + } + } double min_f; diff --git a/VirtualRobot/IK/ConstrainedStackedIK.cpp b/VirtualRobot/IK/ConstrainedStackedIK.cpp index be611008777bc847fa73bc51077bd3df8d8e30f7..ed337e6aa2d2731551a820deaa2cba0c3536e085 100644 --- a/VirtualRobot/IK/ConstrainedStackedIK.cpp +++ b/VirtualRobot/IK/ConstrainedStackedIK.cpp @@ -3,7 +3,7 @@ using namespace VirtualRobot; ConstrainedStackedIK::ConstrainedStackedIK(RobotPtr& robot, const RobotNodeSetPtr& nodeSet, float stepSize, int maxIterations, JacobiProvider::InverseJacobiMethod method) : - ConstrainedIK(robot, maxIterations), + ConstrainedIK(robot, nodeSet, maxIterations), nodeSet(nodeSet), method(method), stepSize(stepSize)