Repository navigation
Update forward kinematics for continuous joints #245
Description
The current issue with continuous joints is that pinocchio seems to be storing 2 parameters for these joints: sin(theta) and cos(theta). That, however, breaks forward kinematics unless we properly normalize these values. The following snippet works in our unit tests but should now be adapted to be part of the current forward_kinematics function.
auto& pmodel = this->robot_->get_pinocchio_model();
auto& data = this->robot_->get_pinocchio_data();
Eigen::VectorXd q = pinocchio::neutral(pmodel);
const std::size_t idx_q = pmodel.joints[1].idx_q();
const double angle = M_PI / 3.0;
q[idx_q + 0] = std::cos(angle);
q[idx_q + 1] = std::sin(angle);
pinocchio::forwardKinematics(pmodel, data, q);
const auto& M = data.oMi[1];
Eigen::Matrix3d R_exp = Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitZ()).toRotationMatrix();
EXPECT_TRUE(M.rotation().isApprox(R_exp, 1e-9)) << "Rotation should be a pure Rz(pi/3).";
EXPECT_TRUE(M.translation().isZero(1e-9)) << "Translation should remain zero.";Metadata
Metadata
Assignees
Labels
No labels
Activity