std::pair<fcl::Vec3f, fcl::Matrix3f> Kinematics::getPoseOfLinkN(const std::vector<double> &joint_angles, const int &n) const {
Eigen::MatrixXd res = Eigen::MatrixXd::Identity(4, 4);
Eigen::MatrixXd init_trans(4, 4);
init_trans << 1.0, 0.0, 0.0, joint_origins_[0][0],
0.0, 1.0, 0.0, joint_origins_[0][1],
0.0, 0.0, 1.0, joint_origins_[0][2],
0.0, 0.0, 0.0, 1.0;
std::vector<Eigen::MatrixXd> transformations;
transformations.push_back(init_trans);
for (unsigned int i = 0; i < n; i++) {
transformations.push_back(getTransformationMatr(joint_angles[i], 0.0, links_[i][0], joint_origins_[i + 1][3]));
}
if (n != 0) {
transformations.push_back(getTransformationMatr(joint_angles[n], 0.0, 0.0, 0.0));
}
else {
transformations.push_back(getTransformationMatr(joint_angles[0], 0.0, 0.0, 0.0));
}
for (int i = transformations.size() - 1; i >= 0; i--) {
res = transformations[i] * res;
}
fcl::Vec3f r_vec = fcl::Vec3f(res(0, 3), res(1, 3), res(2, 3));
fcl::Matrix3f r_matr = fcl::Matrix3f(res(0, 0), res(0, 1), res(0, 2),
res(1, 0), res(1, 1), res(1, 2),
res(2, 0), res(2, 1), res(2, 2));
auto p = std::make_pair(r_vec, r_matr);
return p;
}
/*@ requires \true;
@ behavior a :
@ ensures 0<=\result<=1;
*/
int main(){
cpt=3;
status=0;
/*@ loop invariant i :
@ 0<=status<=1
@ && 0<=cpt<=3
@ && (cpt==0 ==> status==0);
*/
while (cpt>0) {
status=init_trans();
if (status && (status=commit_trans())) goto label_ok;
cpt--;
}
return 0;
label_ok:
return 1;
}
