Include dependency graph for calc_collar_angles.cpp:Functions | |
| int | calc_collar_angles (vector< double > &thetas, vector< vector< double > > &Hprev, vector< vector< double > > &Pvals, vector< vector< double > > &qvals, vector< double > &firstlink, vector< vector< double > > platepts_glob, vector< vector< double > > platepts_loc) |
| Calculate joint angles between consecutive plates of a collar. | |
| int calc_collar_angles | ( | vector< double > & | thetas, |
| vector< vector< double > > & | Hprev, | ||
| vector< vector< double > > & | Pvals, | ||
| vector< vector< double > > & | qvals, | ||
| vector< double > & | firstlink, | ||
| vector< vector< double > > | platepts_glob, | ||
| vector< vector< double > > | platepts_loc | ||
| ) |
Calculate joint angles between consecutive plates of a collar.
This is based on the scilab code written (in partial_calibs.sce). The collar joint angles are successively calculated (from left to right). The existing collar calibration data is used.
| [out] | thetas | The joint angle values calculated using least squares. The vector passed to the function does not have any size allocated to it. It is defined dynamically inside the function. |
| [in,out] | Hprev | Values obtained from Hprev.txt in the calibs folder. |
| [in] | Pvals | Values obtained from Pvals.txt in the calibs folder |
| [in] | qvals | Values obtained from qvals.txt in the calibs folder |
| [in] | firstlink | Values obtained from firstlink.txt in the calibs folder |
| [in] | platepts_glob | Global positions of the points on the plates of the collar. 1 point per plate. |
| [in] | platepts_loc | Local positions of the points (listed in platepts_glob), values obtained from Plates.txt in collar calibs folder.
H_mat_prev = eye(4,4);
H1 = DHtoF;
hinge(:,1) = H1(1:3,3);
for i = 1:(nb_already_calib-1)
Mat_local = H[origin,rotz(-measured_theta-in-Hprev(i))]*H[Pvals(i,:)',rotR(quarternion-qvals)]
Pvals(i,:) = Mat_local(1:3,4)';
qvals(i,1:3) = rot_to_rodrigues(Mat_local(1:3,1:3))';
init_theta = 0;
theta_up = %pi, theta_low = -%pi;
[fopt,theta(i)] = leastsq(list(gettheta, H1,Mat_local,plate1(:,:,i+1)),'b',theta_low, theta_up,init_theta);
Hprev(4,i) = theta(i);
H_mat_prev = H_mat_prev*homogenous(origin, rotz(theta(i)))*DH_hayati(Hprev(1,i+1), Hprev(2,i+1),Hprev(3,i+1),0);
H1 = DHtoF*H_mat_prev;
hinge(:,i+1)= H1(1:3,3);
end
|
References calc_joint_ang(), angle_data::DH_mat, DHtoFix(), gsl_hom_matrix(), gsl_matrix2double(), gsl_matrixprod(), gsl_rodrigues_to_rot(), gsl_rot_to_rodrigues(), angle_data::Mat_loc, angle_data::Ptglob, angle_data::Ptloc, and rotZ().
Here is the call graph for this function:
1.7.4 
