/* Gets the end effector position for a given set of joint angles */
void Kinematics::getEndEffectorPosition(const std::vector<double> &joint_angles, std::vector<double> &end_effector_position) const {
int n = joint_angles.size();
std::pair<fcl::Vec3f, fcl::Matrix3f> ee_pose = getPoseOfLinkN(joint_angles, n);
end_effector_position.push_back(ee_pose.first[0]);
end_effector_position.push_back(ee_pose.first[1]);
end_effector_position.push_back(ee_pose.first[2]);
}
Eigen::MatrixXd Kinematics::getEndEffectorPose(const std::vector<double> &joint_angles, bool &eigen) {
Eigen::MatrixXd m(4, 4);
Eigen::MatrixXd res = Eigen::MatrixXd::Identity(4, 4);
res(0, 3) = joint_origins_[0][0];
res(1, 3) = joint_origins_[0][1];
res(2, 3) = joint_origins_[0][2];
for (size_t i = 0; i < joint_angles.size() + 1; i++) {
if (i == joint_angles.size()) {
res = getPoseOfLinkN(0.0, res, i);
}
else {
res = getPoseOfLinkN(joint_angles[i], res, i);
}
}
return res;
}
void Kinematics::getEEJacobian(const std::vector<double> &joint_angles, Eigen::MatrixXd &jacobian) {
auto ee_pose = getEndEffectorPose(joint_angles);
Eigen::Vector3d o_e;
o_e << ee_pose.first[0], ee_pose.first[1], ee_pose.first[2];
std::vector<std::pair<fcl::Vec3f, fcl::Matrix3f>> link_poses;
for (size_t i = 0; i < joint_angles.size(); i++) {
auto pose = getPoseOfLinkN(joint_angles, i);
Eigen::VectorXd column_vector(6);
Eigen::Vector3d o_i;
o_i << pose.first[0], pose.first[1], pose.first[2];
Eigen::Vector3d z_i;
z_i << pose.second(0, 2), pose.second(1, 2), pose.second(2, 2);
Eigen::Vector3d upper = z_i.cross(o_e - o_i);
column_vector << upper, z_i;
jacobian.col(i) = column_vector;
}
}
std::pair<fcl::Vec3f, fcl::Matrix3f> Kinematics::getEndEffectorPose(const std::vector<double> &joint_angles) const {
int n = joint_angles.size();
return getPoseOfLinkN(joint_angles, n);
}
void Kinematics::getPositionOfLinkN(const std::vector<double> &joint_angles, const int &n, std::vector<double> &position) const {
std::pair<fcl::Vec3f, fcl::Matrix3f> link_n_pose = getPoseOfLinkN(joint_angles, n);
position.push_back(link_n_pose.first[0]);
position.push_back(link_n_pose.first[1]);
position.push_back(link_n_pose.first[2]);
}
