Go to the documentation of this file.
29 static fMat Jtemp_other;
34 for(
i=0;
i<n_dof;
i++)
36 double*
a = Jtemp.
data() + 6*
i;
37 double*
b = Jtemp_other.
data() + 6*
i;
39 double m1 = *(
a+1) - *(
b+1);
40 double m2 = *(
a+2) - *(
b+2);
41 double m3 = *(
a+3) - *(
b+3);
42 double m4 = *(
a+4) - *(
b+4);
43 double m5 = *(
a+5) - *(
b+5);
58 double*
a =
J.
data() + count;
59 int b_row = Jtemp.
row();
60 double*
b = Jtemp.
data() +
i;
61 for(j=0; j<n_dof;
a+=a_row,
b+=b_row, j++)
77 static fVec3 fb_pos, fb_att;
93 cur_att.
mul(rt, catt);
104 fb(count) = fb_pos(
i);
112 fb(count) = fb_att(
i);
fVec3 abs_pos
absolute position
void set(const fMat33 &mat)
Copies a matrix.
void rotation(const fMat33 &ref, const fMat33 &tgt)
Computes the rotation from tgt to ref.
void mul(const fMat33 &mat1, const fMat33 &mat2)
int CalcJacobian(fMat &J)
The Jacobian matrix of position/orientation w.r.t. the joint values.
double * data() const
Returns the pointer to the first element.
Joint * joint
target joint
int NumDOF()
Total degrees of freedom.
@ HAVE_CONSTRAINT
with constraint
Joint * other_joint
connected to other joint: constrains the relative position and/or orientation w.r....
Matrix of generic size. The elements are stored in a one-dimensional array in row-major order.
int resize(int i, int j)
Changes the matrix size.
void zero()
Creates a zero matrix.
int calc_feedback()
compute the feedback velocity
IK::ConstIndex const_index[6]
with/without constraint for each DOF
fVec3 abs_pos
current constraint position
Inverse kinematics (UTPoser) class.
fMat33 abs_att
absolute orientation
void sub(const fVec3 &vec1, const fVec3 &vec2)
friend fMat33 tran(const fMat33 &m)
Returns the transpose.
fMat33 abs_att
current constraint orientation
int row() const
Returns the number of rows.
int calc_jacobian()
Computes the constraint Jacobian.
fVec fb
Jacobian matrix (n_const x total DOF)
void mul(const fVec3 &vec, double d)
openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Wed Sep 7 2022 02:51:03