diff --git a/VirtualRobot/IK/DifferentialIK.cpp b/VirtualRobot/IK/DifferentialIK.cpp index b1ac7453e465dbe311664e4374242d3611312623..491a4718a09447a7f1550787c52129e37172009f 100644 --- a/VirtualRobot/IK/DifferentialIK.cpp +++ b/VirtualRobot/IK/DifferentialIK.cpp @@ -586,17 +586,27 @@ namespace VirtualRobot return rns->getTCP(); } + Eigen::VectorXf DifferentialIK::getDeltaToGoal(SceneObjectPtr tcp) + { + Eigen::VectorXf result(6, 0.0f); + updateDeltaToGoal(result, tcp); + return result; + } + + Eigen::VectorXf DifferentialIK::getDelta(const Eigen::Matrix4f& current, const Eigen::Matrix4f& goal, IKSolver::CartesianSelection mode) + { + Eigen::VectorXf result(6, 0.0f); + updateDelta(result, current, goal, mode); + return result; + } + void DifferentialIK::updateDeltaToGoal(Eigen::VectorXf &delta, SceneObjectPtr tcp) { if (!tcp) { tcp = getDefaultTCP(); } - VR_ASSERT(tcp); - //IKSolver::CartesianSelection mode = this->modes[tcp]; - //Eigen::Matrix4f current = tcp->getGlobalPose(); - //Eigen::Matrix4f goal = this->targets[tcp]; updateDelta(delta, tcp->getGlobalPose(), this->targets[tcp], this->modes[tcp]); } diff --git a/VirtualRobot/IK/DifferentialIK.h b/VirtualRobot/IK/DifferentialIK.h index 5674abc3eba08c5d5179e751f67ce60ebd775f38..21ab3d8ade9bd7da48d8efcb9e9a9cb214f16551 100644 --- a/VirtualRobot/IK/DifferentialIK.h +++ b/VirtualRobot/IK/DifferentialIK.h @@ -261,9 +261,18 @@ namespace VirtualRobot /*! Returns 6D workspace delta that is used for Jacobi calculation. + Updates the currentDeltaToGoal vector */ virtual void updateDeltaToGoal(Eigen::VectorXf &delta, SceneObjectPtr tcp = SceneObjectPtr()); virtual void updateDelta(Eigen::VectorXf &delta, const Eigen::Matrix4f& current, const Eigen::Matrix4f& goal, IKSolver::CartesianSelection mode = IKSolver::All); + /*! + Returns 6D workspace delta that is used for Jacobi calculation. + */ + /*! + Returns 6D workspace delta that is used for Jacobi calculation. + */ + virtual Eigen::VectorXf getDeltaToGoal(SceneObjectPtr tcp = SceneObjectPtr()); + virtual Eigen::VectorXf getDelta(const Eigen::Matrix4f& current, const Eigen::Matrix4f& goal, IKSolver::CartesianSelection mode = IKSolver::All); //! When considering large errors, the translational part can be cut to this length. Set to <= 0 to ignore cutting (standard)