From 9e8074b6392efe5003b1b2881045eaab0a957003 Mon Sep 17 00:00:00 2001
From: Romano Hauser <romano.hauser@ost.ch>
Date: Mon, 25 Mar 2024 11:09:34 +0100
Subject: [PATCH] add bool state isRelationFixed in getJacobianMatrix
---
include/TrafoMatrix.hpp | 4 ++--
src/TrafoMatrix.cpp | 12 ++++++++----
2 files changed, 10 insertions(+), 6 deletions(-)
diff --git a/include/TrafoMatrix.hpp b/include/TrafoMatrix.hpp
index 6c29134..ba265fc 100644
--- a/include/TrafoMatrix.hpp
+++ b/include/TrafoMatrix.hpp
@@ -55,11 +55,11 @@ namespace tf {
////////////////////////////////////////////////////////////////////////////////
//// Jacobi Matrix for TrafoMatrix
- virtual Matrix<6,6,double> getJacobiMatrix();
+ virtual Matrix<6,6,double> getJacobiMatrix(bool isRelationFixed = true );
/////////////////////////////////////////////////////////////////////////////////
//// Jacobi Matrix for TrafoMatrix as static method for general use:
- static Matrix<6,6,double> getJacobiMatrix(Matrix<4,4,double> _tf);
+ static Matrix<6,6,double> getJacobiMatrix(Matrix<4,4,double> _tf, bool isRelationFixed= true );
/////////////////////////////////////////////////////////////////////////////////
//// inverse matrix
diff --git a/src/TrafoMatrix.cpp b/src/TrafoMatrix.cpp
index 4fc38ff..c51cc12 100644
--- a/src/TrafoMatrix.cpp
+++ b/src/TrafoMatrix.cpp
@@ -100,7 +100,7 @@ namespace tf {
////////////////////////////////////////////////////////////////////////////////
//// Jacobi Matrix for TrafoMatrix
- eeros::math::Matrix<6,6,double> TrafoMatrix::getJacobiMatrix(){
+ eeros::math::Matrix<6,6,double> TrafoMatrix::getJacobiMatrix(bool isRelationFixed ){
eeros::math::Matrix<6,6,double> jacobian;
jacobian.zero();
@@ -114,7 +114,9 @@ namespace tf {
for(int j=0; j<3; j++){ // row
jacobian(j,i)= get(j,i); // links oben
jacobian(j+3,i+3)= get(j,i); // rechts unten
- jacobian(j, i+3) = crossP(j); // rechts oben
+ if (isRelationFixed){
+ jacobian(j, i+3) = crossP(j); // rechts oben
+ }
}
}
return jacobian;
@@ -124,7 +126,7 @@ namespace tf {
/////////////////////////////////////////////////////////////////////////////////
//// Jacobi Matrix for TrafoMatrix as static method for general use:
- eeros::math::Matrix<6,6,double> TrafoMatrix::getJacobiMatrix(eeros::math::Matrix<4,4,double> _tf){
+ eeros::math::Matrix<6,6,double> TrafoMatrix::getJacobiMatrix(eeros::math::Matrix<4,4,double> _tf, bool isRelationFixed ){
eeros::math::Matrix<6,6,double> jacobian;
jacobian.zero();
@@ -138,7 +140,9 @@ namespace tf {
for(int j=0; j<3; j++){ // row
jacobian(j,i)= _tf(j,i); // links oben
jacobian(j+3,i+3)= _tf(j,i); // rechts unten
- jacobian(j, i+3) = crossP(j); // rechts oben
+ if (isRelationFixed){
+ jacobian(j, i+3) = crossP(j); // rechts oben
+ }
}
}
return jacobian;
--
GitLab