PoseConstraint: Added method for continuous target updates

git-svn-id: http://svn.code.sf.net/p/simox/code/trunk@803 042f3d55-54a8-47e9-b7fb-15903f145c44

VirtualRobot/IK/constraints/PoseConstraint.cpp

@@ -17,7 +17,9 @@ PoseConstraint::PoseConstraint(const RobotPtr &robot, const RobotNodeSetPtr &nod
     nodeSet(nodeSet),
     eef(eef),
     target(target),
-    cartesianSelection(cartesianSelection)
+    cartesianSelection(cartesianSelection),
+    tolerancePosition(tolerancePosition),
+    toleranceRotation(toleranceRotation)
 {
     ik.reset(new DifferentialIK(nodeSet));
     ik->setGoal(target, eef, cartesianSelection, tolerancePosition, toleranceRotation);
@@ -112,5 +114,6 @@ std::string PoseConstraint::getConstraintType()
 void PoseConstraint::updateTarget(const Eigen::Matrix4f &newTarget)
 {
     target = newTarget;
+    ik->setGoal(target, eef, cartesianSelection, tolerancePosition, toleranceRotation);
 }
 

VirtualRobot/IK/constraints/PoseConstraint.h

@@ -62,6 +62,9 @@ namespace VirtualRobot
             DifferentialIKPtr ik;
             IKSolver::CartesianSelection cartesianSelection;
 
+            float tolerancePosition;
+            float toleranceRotation;
+
             SceneObjectSetPtr visualizationNodeSet;
     };