Repository navigation
Hotfix (#75) - #76
Hotfix (#75)#76
Conversation
…obian returning a wrong size matrix for intermediate links - raw_pose_jacobian_by_chain() now follows pose_jacobian_by_chain() from MATLAB. The Jacobian has one column per joint up to the requested link. - Fixed the out-of-bounds read when copying the Jacobian of a partial chain. - Added raw_pose_jacobian_by_chain(q, to_ith_chain) for the complete chains, used by pose_jacobian(q).
|
Hi @juanjqo, thanks for the careful fix. I built this branch and checked it against the repro in #75, and it looks good to merge. What I verified.
Two notes
This is the code in case someone wants to check it further. #include <dqrobotics/DQ.h>
#include <dqrobotics/robot_modeling/DQ_SerialWholeBody.h>
#include <dqrobotics/robot_modeling/DQ_HolonomicBase.h>
#include <dqrobotics/robots/FrankaEmikaPandaRobot.h>
#include <iostream>
#include <cmath>
using namespace DQ_robotics;
static double fd_check(DQ_SerialWholeBody& robot, const VectorXd& q, int ith, int ncols_expected)
{
MatrixXd J;
if (ith < 0) J = robot.pose_jacobian(q);
else J = robot.pose_jacobian(q, ith);
std::cout << "ith=" << ith << " -> " << J.rows() << "x" << J.cols();
if (J.cols() != ncols_expected) { std::cout << " WRONG SIZE (expected " << ncols_expected << ")\n"; return 1e9; }
const double h = 1e-6;
double max_err = 0.0;
VectorXd qp = q, qm = q;
for (int j = 0; j < J.cols(); j++)
{
qp = q; qm = q;
qp(j) += h; qm(j) -= h;
VectorXd vec_p, vec_m;
if (ith < 0) vec_p = robot.fkm(qp).vec8(); else vec_p = robot.fkm(qp, ith).vec8();
if (ith < 0) vec_m = robot.fkm(qm).vec8(); else vec_m = robot.fkm(qm, ith).vec8();
VectorXd fd = (vec_p - vec_m) / (2 * h);
max_err = std::max(max_err, (fd - J.col(j)).lpNorm<Eigen::Infinity>());
}
std::cout << " max|J_fd - J| = " << max_err << (max_err < 1e-5 ? " OK" : " MISMATCH") << "\n";
return max_err;
}
int main()
{
{
std::cout << "=== two Pandas ===\n";
DQ_SerialWholeBody robot(std::make_shared<DQ_SerialManipulatorMDH>(FrankaEmikaPandaRobot::kinematics()));
robot.add(std::make_shared<DQ_SerialManipulatorMDH>(FrankaEmikaPandaRobot::kinematics()));
const int n = robot.get_dim_configuration_space();
VectorXd q(n);
for (int i = 0; i < n; i++) q(i) = 0.1 * (i + 1) - 0.7;
double worst = 0;
for (int ith : {0,1,2,5,6,7,8,10,13}) worst = std::max(worst, fd_check(robot, q, ith, ith + 1));
worst = std::max(worst, fd_check(robot, q, -1, n));
std::cout << "worst err (2 pandas): " << worst << "\n";
}
{
std::cout << "=== holonomic base + Panda ===\n";
DQ_SerialWholeBody robot(std::make_shared<DQ_HolonomicBase>());
robot.add(std::make_shared<DQ_SerialManipulatorMDH>(FrankaEmikaPandaRobot::kinematics()));
const int n = robot.get_dim_configuration_space(); // 3 + 7
VectorXd q(n);
for (int i = 0; i < n; i++) q(i) = 0.1 * (i + 1) - 0.7;
double worst = 0;
for (int ith : {2,3,4,9}) worst = std::max(worst, fd_check(robot, q, ith, ith + 1));
worst = std::max(worst, fd_check(robot, q, -1, n));
std::cout << "worst err (base+panda): " << worst << "\n";
}
return 0;
}Output === two Pandas ===
ith=0 -> 8x1 max|J_fd - J| = 0.00986385 MISMATCH
ith=1 -> 8x2 max|J_fd - J| = 0.00729175 MISMATCH
ith=2 -> 8x3 max|J_fd - J| = 0.00995404 MISMATCH
ith=5 -> 8x6 max|J_fd - J| = 0.00804811 MISMATCH
ith=6 -> 8x7 max|J_fd - J| = 0.0102614 MISMATCH
ith=7 -> 8x8 max|J_fd - J| = 0.528007 MISMATCH
ith=8 -> 8x9 max|J_fd - J| = 0.685762 MISMATCH
ith=10 -> 8x11 max|J_fd - J| = 0.695775 MISMATCH
ith=13 -> 8x14 max|J_fd - J| = 0.681906 MISMATCH
ith=-1 -> 8x14 max|J_fd - J| = 1.563e-10 OK
worst err (2 pandas): 0.695775
=== holonomic base + Panda ===
ith=2 -> 8x3 max|J_fd - J| = 0.146237 MISMATCH
ith=3 -> 8x4 max|J_fd - J| = 4.24694e-11 OK
ith=4 -> 8x5 max|J_fd - J| = 2.92643e-11 OK
ith=9 -> 8x10 max|J_fd - J| = 1.34305e-10 OK
ith=-1 -> 8x10 max|J_fd - J| = 1.34305e-10 OK
worst err (base+panda): 0.146237 |
|
Thanks, @mmmarinho. I'll investigate the potential bug in the MATLAB version. Kind regards, Juancho |
|
Thanks, @mmmarinho!
This should be very easy to fix.
This will indeed require a bit more investigation. @juanjqo, after you submit a pull request on MATLAB, please assign me as the reviewer. Please also ensure that the existing examples run without issues after the modifications. Kind regards, |
Hi @mmmarinho, this PR fixes #75.
Modifications:
raw_pose_jacobian_by_chain()now followspose_jacobian_by_chain()from MATLAB. The Jacobian has one column per joint up toith, as in MATLAB.raw_pose_jacobian_by_chain(q, to_ith_chain)for complete chains.pose_jacobian(q)uses it, in the same way that MATLAB'spose_jacobian(q)callspose_jacobian_by_chain(q).11:58:35: Starting /home/juanjqo/Documents/dqrobotics_tests/cpp/serial_whole_body/build/terminal/serial_whole_body... pose_jacobian(q, 0) (8x1) = 0.1478 0 0 0.4777 -0.07953 0.006102 -0.01973 0.0246 pose_jacobian(q, 1) (8x2) = 0.1848 0.1848 -0.01767 0.01767 -0.3531 0.3531 0.3014 0.3014 -0.04946 -0.05092 0.06643 -0.05116 -0.01539 -0.009506 0.01619 0.04535 pose_jacobian(q, 2) (8x3) = 0.2323 0.1086 0.2323 0.1231 0.04836 -0.1231 -0.01235 0.482 0.01235 0.4252 -0.05931 0.4252 -0.143 0.01725 -0.1329 0.009697 0.0003862 0.009487 -0.01651 0.00286 -0.0186 0.07486 0.05514 0.07588 pose_jacobian(q, 7) (8x8) = -0.03276 0.01493 0.2094 0.1621 0.07541 0.2513 -0.03276 0.1237 0.2928 0.002449 0.2831 -0.03832 0.2954 -0.03695 -0.2928 -3.362e-18 0.4016 -0.05444 0.3536 -0.024 0.3959 -0.04006 -0.4016 8.584e-19 -0.04355 -0.4968 -0.03106 0.4708 0.01825 0.4288 -0.04355 0.4845 -0.003975 0.01944 -0.003866 -0.168 -0.022 -0.3092 0.0185 -0.1613 -0.1181 0.02179 -0.1033 0.00559 -0.1131 0.02838 0.1153 -0.000986 0.08769 0.01964 0.0871 0.005716 0.08967 0.06803 -0.08754 -0.0197 0.01725 -0.001461 0.02451 0.05861 -0.02454 0.19 0.01838 0.04119Kind regards,
Juancho