Fail to compute the pose task Jacobian using pinocchio::Jlog6

[zzzbbs]

Following the guidance of scaron , I firstly calculate J_log by

        VectorXd q_whole_body = VectorXd::Zero(20);
        Pose pose_1_base, pose_2_base;
        model.forwardKinematics("left_arm_end_effector_mount_link", q_whole_body, pose_1_base);
        Pose pose_chain_base;
        model.forwardKinematics("left_arm_base_link", q_whole_body, pose_chain_base);
        q_whole_body[9] = EPS_4;
        model.forwardKinematics("left_arm_end_effector_mount_link", q_whole_body, pose_2_base);

        Pose err_pose_ee = pose_1_base.inverse() * pose_2_base;  
        Twist err_twist_ee = pinocchio::log6(err_pose_ee);
        Vector6d err_vec_base = err_twist_ee.toVector();

        pinocchio::Data::Matrix6 Jlog;
        pinocchio::Jlog6(err_pose_ee, Jlog);

Then I try to calculate b_J_{0b} by:

pinocchio::computeFrameJacobian(pin_model_, pin_data_, q_whole_body_, link_index, pinocchio::LOCAL_WORLD_ALIGNED, jac);
Pose pose = pose_1_base;
pose.translation() = Vector3d::Zero();
MatrixXd J_0b_local  = pose.toActionMatrixInverse() * jac;

Finally I used the calculated result to get J_pose_task by:
J_pose_task = -Jlog * err_pose_ee.inverse().toActionMatrix() * J_0b_local;

Then I check the result of J_pose_task and the calculated twist, I found that the twist are just the opposite of the corresponding columns of J_pose_task. Could you help me on this issue and suggest me how to debug it? Thanks and looking forward to your reply!

[zzzbbs]

Here is the result of the program.

[stephane-caron]

Presently I don't have the bandwidth to look at your code, but you can find a reference implementation in pink.FrameTask.compute_jacobian. For debugging, you could print out the intermediate matrices of your code versus the reference ones, and check each step of the calculation this way. Hoping this helps!

[zzzbbs]

Thank you very much for your reply!
I have checked the provided python package. I found the calculation process seems similar, but my code don't work well. Here is the simplified version of my code. My goal is using the joint velocity q_dot to calculate the twist_{bt} after twist_{bt} = Jq_dot. But my calculated twist_{bt} is in the opposite direction to the actual twist_{bt}.

        Pose Trans_bt = pose_start_base.inverse() * pose_end_base;  // T_{bt} = T_{b0} * T_{0t}
        Twist twist_bt = pinocchio::log6(pose_start_base.inverse()*pose_end_base); // twist_{bt}
        Vector6d twist_bt_vec = twist_bt.toVector();

        pinocchio::Data::Matrix6 Jlog;
        pinocchio::Jlog6(pose_end_base.inverse() * pose_start_base, Jlog); //calculate Jlog6(T_{tb})

        MatrixXd jacobian_local;
        //calculate b_J_{0b} using pinocchio::computeFrameJacobian(pin_model_, pin_data_, q_whole_body_, link_index, pinocchio::LOCAL, jac);
        model.jacobian("left_arm_end_effector_mount_link", jacobian_local); 
        MatrixXd jacobian_log = -Jlog * jacobian_local; // calculate - Jlog6(T_{tb}) * b_J_{0b}
        Vector6d cal_twist_bt_vec = jacobian_log * q_vel;
       

[zzzbbs]

I found a python example in #1963 below:

    iMd = data.oMi[JOINT_ID].actInv(oMdes)
    err = pinocchio.log(iMd).vector  # in joint frame
    ...
    J = pinocchio.computeJointJacobian(model, data, q, JOINT_ID)
    J = pinocchio.Jlog6(iMd.inverse()) @ J

which is different from the cpp example in the calculation of pose task jacobian.( https://github.com/stack-of-tasks/pinocchio/blob/master/examples/inverse-kinematics.cpp)

 const pinocchio::SE3 iMd = data.oMi[JOINT_ID].actInv(oMdes);
    err = pinocchio::log6(iMd).toVector(); // in joint frame
    pinocchio::computeJointJacobian(model, data, q, JOINT_ID, J); // J in joint frame
    pinocchio::Data::Matrix6 Jlog;
    pinocchio::Jlog6(iMd.inverse(), Jlog);
    J = -Jlog * J;