From 50926143d0d5144032b4abc6a3ff751e0600da2c Mon Sep 17 00:00:00 2001
From: p-kaiser <p-kaiser@042f3d55-54a8-47e9-b7fb-15903f145c44>
Date: Thu, 23 Apr 2015 14:53:23 +0000
Subject: [PATCH] TSR Constraint: Fixed displacement calculation
git-svn-id: http://svn.code.sf.net/p/simox/code/trunk@835 042f3d55-54a8-47e9-b7fb-15903f145c44
---
VirtualRobot/IK/Constraint.cpp | 2 +-
VirtualRobot/IK/constraints/TSRConstraint.cpp | 24 ++++++++++++++-----
2 files changed, 19 insertions(+), 7 deletions(-)
diff --git a/VirtualRobot/IK/Constraint.cpp b/VirtualRobot/IK/Constraint.cpp
index dbf0a1465..42ba844cf 100644
--- a/VirtualRobot/IK/Constraint.cpp
+++ b/VirtualRobot/IK/Constraint.cpp
@@ -3,7 +3,7 @@
using namespace VirtualRobot;
Constraint::Constraint(const RobotNodeSetPtr &nodeSet) :
- JacobiProvider(nodeSet, JacobiProvider::eSVD),
+ JacobiProvider(nodeSet, JacobiProvider::eSVDDamped),
lastError(-1),
lastLastError(-1)
{
diff --git a/VirtualRobot/IK/constraints/TSRConstraint.cpp b/VirtualRobot/IK/constraints/TSRConstraint.cpp
index 399139aa8..6bdaed898 100644
--- a/VirtualRobot/IK/constraints/TSRConstraint.cpp
+++ b/VirtualRobot/IK/constraints/TSRConstraint.cpp
@@ -84,21 +84,33 @@ Eigen::VectorXf TSRConstraint::getError(float stepSize)
Eigen::Matrix4f T_dx;
MathTools::posrpy2eigen4f(target.block<3,1>(0,0), target.block<3,1>(3,0), T_dx);
- float target_global[6];
- MathTools::eigen4f2rpy(transformation * T_dx, target_global);
+ Eigen::Matrix4f P_target = transformation * T_dx;
+ //float target_global[6];
+ //MathTools::eigen4f2rpy(P_target, target_global);
- float e_global[6];
- MathTools::eigen4f2rpy(eef_global * eefOffset, e_global);
+ Eigen::Matrix4f P_eef = eef_global * eefOffset;
+ //float e_global[6];
+ //MathTools::eigen4f2rpy(P_eef, e_global);
- resolveRPYAmbiguities(e_global, target_global);
+ Eigen::Matrix4f P_delta = P_target * P_eef.inverse();
+ Eigen::AngleAxis<float> P_delta_AA;
+ P_delta_AA = P_delta.block<3, 3>(0, 0);
Eigen::VectorXf dx(6);
+ dx.head(3) = Eigen::Vector3f(
+ P_target(0,3) - P_eef(0,3),
+ P_target(1,3) - P_eef(1,3),
+ P_target(2,3) - P_eef(2,3)
+ );
+ dx.tail(3) = P_delta_AA.axis() * P_delta_AA.angle();
+
+ /*resolveRPYAmbiguities(e_global, target_global);
dx << (target_global[0] - e_global[0]),
(target_global[1] - e_global[1]),
(target_global[2] - e_global[2]),
getShortestDistanceForRPYComponent(e_global[3], target_global[3]),
getShortestDistanceForRPYComponent(e_global[4], target_global[4]),
- getShortestDistanceForRPYComponent(e_global[5], target_global[5]);
+ getShortestDistanceForRPYComponent(e_global[5], target_global[5]);*/
return dx * stepSize;
}
--
GitLab