//Extracts column i from given matrix and returns a n*1 column vector
DBL_MATRIX extractColumn(DBL_MATRIX& mat, int i)
{
int num_rows = mat.rowCount();
int num_col = mat.columnCount();
if(i >= num_col || i < 0){
MSPP_LOG(logERROR) << "Alignment::extractColumn_(): Index exceeds matrix dimensions." << std::endl;
}
mspp_precondition(i < num_col && i >= 0 , "Alignment::extractColumn_(): Index exceeds matrix dimensions.");
DBL_MATRIX column_vector (num_rows,1);
for(int k = 0; k < num_rows; k++){
column_vector(k,0) = mat(k,i);
}
return column_vector;
}
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;
}
}
