Go to the documentation of this file.
32 double b_mass, c_mass;
33 fVec3 b_com(0,0,0), c_com(0,0,0);
37 int is_target =
false;
45 if(my_chname && !strcmp(my_chname, charname))
53 static fVec3 abs_com_pos, my_com;
67 ms = c_com - (c_mass *
abs_pos);
76 J(0,
i_dof) = msXaxis(0);
77 J(1,
i_dof) = msXaxis(1);
78 J(2,
i_dof) = msXaxis(2);
84 J(0,
i_dof) = msXaxis(0);
85 J(1,
i_dof) = msXaxis(1);
86 J(2,
i_dof) = msXaxis(2);
102 J(0,
i_dof+3+
i) = msXatt(0,
i);
103 J(1,
i_dof+3+
i) = msXatt(1,
i);
104 J(2,
i_dof+3+
i) = msXatt(2,
i);
115 com.add(c_com, b_com);
116 return c_mass + b_mass;
152 static fVec3 axis0, pp, lin;
155 lin.
cross(axis0, pp);
197 for(
int i=0;
i<3;
a+=6,
i++)
202 lin.
cross(axis0, pp);
226 for(
i=0;
i<3;
a+=6,
i++)
231 lin.
cross(axis0, pp);
263 static fVec save_acc, zero_acc;
264 static fVec3 space_acc;
337 static fVec3 x(1, 0, 0), y(0, 1, 0), z(0, 0, 1);
368 static fVec3 x(1, 0, 0), y(0, 1, 0), z(0, 0, 1);
395 static fVec3 x(1, 0, 0), y(0, 1, 0), z(0, 0, 1);
457 static fVec3 x(1, 0, 0), y(0, 1, 0), z(0, 0, 1);
539 static fVec3 out1, out2;
541 static fVec3 jk_axis, my_axis, d_jk_axis, dp, dJ;
550 d_jk_axis.
cross(my_axis, jk_axis);
553 out1.
cross(d_jk_axis, dp);
557 Jt.
cross(my_axis, dp);
565 Jk.
cross(my_axis, dp);
568 out2.
cross(jk_axis, dJ);
570 J[loc_index](0, k_index) = out1(0) + out2(0);
571 J[loc_index](1, k_index) = out1(1) + out2(1);
572 J[loc_index](2, k_index) = out1(2) + out2(2);
573 J[loc_index](3, k_index) = d_jk_axis(0);
574 J[loc_index](4, k_index) = d_jk_axis(1);
575 J[loc_index](5, k_index) = d_jk_axis(2);
581 static fVec3 jk_axis, my_axis, d_jk_axis;
591 d_jk_axis.
cross(my_axis, jk_axis);
593 J[loc_index](0, k_index) = d_jk_axis(0);
594 J[loc_index](1, k_index) = d_jk_axis(1);
595 J[loc_index](2, k_index) = d_jk_axis(2);
603 static fVec3 jk_axis, my_axis, out1;
606 out1.
cross(jk_axis, my_axis);
607 J[loc_index](0, k_index) = out1(0);
608 J[loc_index](1, k_index) = out1(1);
609 J[loc_index](2, k_index) = out1(2);
int t_given
torque or motion controlled
Joint * parent
pointer to the parent joint
fVec3 abs_pos
absolute position
Joint * root
Chain information.
double com_jacobian(fMat &J, fVec3 &com, const char *chname)
void cross(const fVec3 &vec1, const fVec3 &vec2)
Cross product.
void cross(const fVec3 &p)
Sets spectial matrices.
The class for representing a joint.
int calc_jacobian_2_rotate_sub(fMat *J, Joint *target, Joint *j1)
fVec3 axis
joint axis in local frame (for 1DOF joints)
int calc_jacobian_2_sphere_sub(fMat *J, Joint *target, Joint *j1)
int SetJointAcc(const fVec &accs)
void mul(const fMat33 &mat1, const fMat33 &mat2)
JointType j_type
joint type
int calc_jacobian_sphere(fMat &J, Joint *target)
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.
int CalcJdot(fVec &jdot)
Jdot x thdot.
int calc_jacobian(fMat &J, Joint *target)
Joint * child
pointer to the child joint
int calc_jacobian_slide(fMat &J, Joint *target)
@ JROTATE
rotational (1DOF)
@ JSPHERE
spherical (3DOF)
int n_dof
degrees of freedom (0/1/3/6)
int calc_jacobian_2_rotate_rotate(fMat *J, Joint *target, Joint *j1, const fVec3 &axis1, int index1, const fVec3 &axis2, int index2)
int isAscendant(Joint *target)
Identifies whether the target joint is a direct ascendant.
Matrix of generic size. The elements are stored in a one-dimensional array in row-major order.
int GetJointAcc(fVec &accs)
int calc_jacobian_2_slide_rotate(fMat *J, Joint *target, Joint *jk, const fVec3 &k_axis, int k_index, const fVec3 &loc_axis, int loc_index)
int resize(int i, int j)
Changes the matrix size.
void zero()
Creates a zero matrix.
void set(double *v)
Set element values from array or three values.
int calc_jacobian_2_slide_sub(fMat *J, Joint *target, Joint *j1)
fVec3 loc_com
center of mass in local frame
int calc_jacobian_2_rotate_slide(fMat *J, Joint *target, Joint *jk, const fVec3 &k_axis, int k_index, const fVec3 &loc_axis, int loc_index)
double ComJacobian(fMat &J, fVec3 &com, const char *chname=0)
Computes the com Jacobian.
char * CharName() const
Returns the character name.
Classes for defining open/closed kinematic chains.
int CalcJacobian2(fMat *J2)
2nd-order derivatives of the Jacobian matrix.
fMat33 abs_att
absolute orientation
void sub(const fVec3 &vec1, const fVec3 &vec2)
void zero()
Creates a zero vector.
int i_dof
index in all DOF
void resize(int i)
Change the size.
int calc_jacobian_2_free_sub(fMat *J, Joint *target, Joint *j1)
int calc_jacobian_rotate(fMat &J, Joint *target)
fVec3 loc_ang_acc
angular acceleration in local frame
Joint * brother
pointer to the brother joint
int calc_jacobian_2(fMat *J, Joint *target)
int calc_jacobian_free(fMat &J, Joint *target)
void zero()
Creates a zero vector.
fVec3 loc_lin_acc
linear acceleration in local frame
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