[BUG] in DQ_SerialWholeBody::pose_jacobian method #61
Description
Code of Conduct
By submitting this report you automatically agree that you've read and accepted the following conditions.
- Support for DQ Robotics is given voluntarily and it's not the developers' role to educate and/or convince anyone of their vision.
- Any possible response and its timeliness will be based on the relevance, accuracy, and politeness of a request and the following discussion.
- If a DQ Robotics member replies, the user must let them know if their response solves their issue or not.
- Any suggestion/advice/request made by anyone, as well intentioned as they might be, might not be incorporated into DQ Robotics.
Bug description
@dqrobotics/developers
Hi @mmmarinho, the method DQ_SerialWholeBody::pose_jacobian returns a result that is different from the Matlab implementation. I think this is related to the use of raw_methodssince the bug is present only when I set the reference frame to a value different from DQ(1).
Minimal Example
#include <iostream>
#include <dqrobotics/robot_modeling/DQ_SerialWholeBody.h>
#include <dqrobotics/robot_modeling/DQ_SerialManipulatorMDH.h>
#include <dqrobotics/robots/FrankaEmikaPandaRobot.h>
#include <memory>
#include <iomanip>
using namespace DQ_robotics;
using namespace Eigen;
int main()
{
auto arm = std::make_shared<DQ_SerialManipulatorMDH>(FrankaEmikaPandaRobot::kinematics());
DQ_SerialWholeBody robot = DQ_SerialWholeBody(arm);
robot.set_reference_frame(1 + 0.5*E_*k_);
VectorXd q = VectorXd::Zero(robot.get_dim_configuration_space());
MatrixXd J = robot.pose_jacobian(q);
std::cout<<std::fixed<<std::setprecision(4)<<J<<std::endl;
return 0;
}Output
0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000
0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000
0.5000 0.0000 0.5000 -0.0000 0.5000 -0.0000 -0.5000
0.0000 -0.5000 0.0000 0.5000 -0.0000 0.5000 0.0000
-0.0000 -0.0650 -0.0000 -0.0930 -0.0000 -0.2850 -0.0000
-0.2315 0.0000 -0.2315 -0.0000 -0.2315 0.0000 0.2315
0.0000 -0.0117 0.0000 -0.0296 0.0000 0.0117 -0.0000
-0.0117 -0.0000 -0.0117 0.0000 -0.0117 0.0000 -0.0323
Expected behavior (Matlab)
clear all
close all
clc
robot = DQ_SerialWholeBody(FrankaEmikaPandaRobot.kinematics());
robot.set_reference_frame(1 + 0.5*DQ.E*DQ.k);
q = zeros(robot.get_dim_configuration_space());
J = robot.pose_jacobian(q)Expected output
J =
0 0 0 0 0 0 0
0 0 0 0 0 0 0
0.5000 0.0000 0.5000 -0.0000 0.5000 -0.0000 -0.5000
0.0000 -0.5000 0.0000 0.5000 0 0.5000 0
-0.0000 0.1850 -0.0000 -0.3430 -0.0000 -0.5350 0.0000
-0.4815 -0.0000 -0.4815 0.0000 -0.4815 0.0000 0.4815
-0.0000 -0.0117 0.0000 -0.0296 -0.0000 0.0117 0.0000
-0.0117 0.0000 -0.0117 -0.0000 -0.0117 -0.0000 -0.0323
Environment: