Public Member Functions | Protected Member Functions | Protected Attributes | Private Attributes | Friends
Joint Class Reference

The class for representing a joint. More...

#include <chain.h>

List of all members.

Public Member Functions

int CalcJacobian (fMat &J)
 The Jacobian matrix of position/orientation w.r.t. the joint values.
int CalcJacobian2 (fMat *J2)
 2nd-order derivatives of the Jacobian matrix.
int CalcJdot (fVec &jdot)
 Jdot x thdot.
char * CharName () const
 Returns the character name.
int DescendantDOF ()
 Total DOF of the descendants (end link side).
int DescendantNumJoints ()
 Total number of joints of the descendants (end link side).
int isAscendant (Joint *target)
 Identifies whether the target joint is a direct ascendant.
int isDescendant (Joint *target)
 Identifies whether the target joint is a direct descendant.
 Joint ()
 Joint (const char *name, JointType jt=JUNKNOWN, const fVec3 &rpos=0.0, const fMat33 &ratt=1.0, AxisIndex _axis_index=AXIS_NULL, int t_given=true)
 Joint (JointData *jdata, const char *charname)
double Scale ()
 Returns the scale applied to the joint.
void SetFixedJointType (const fVec3 &rpos, const fMat33 &ratt)
 Set joint type to fixed.
void SetFreeJointType (const fVec3 &rpos, const fMat33 &ratt)
 Set joint type to free.
void SetJointData (JointData *jdata, const char *charname)
int SetJointOrientation (const fMat33 &r)
 Set relative orientation.
int SetJointPosition (const fVec3 &p)
 Set relative position.
void SetName (const char *_name, const char *_charname=0)
 Change the joint name.
void SetRotateJointType (const fVec3 &rpos, const fMat33 &ratt, AxisIndex ai)
 Set joint type to rotational.
void SetSlideJointType (const fVec3 &rpos, const fMat33 &ratt, AxisIndex ai)
 Set joint type to prismatic.
void SetSphereJointType (const fVec3 &rpos, const fMat33 &ratt)
 Set joint type to spherical.
double TotalMass ()
 Returns the total mass of the child links.
void UpdateJointType ()
 ~Joint ()
1DOF joints

Obtain/set the joint value/velocity/acceleration/force of a 1-DOF joint.

int SetJointValue (double _q)
int SetJointVel (double _qd)
int SetJointAcc (double _qdd)
int GetJointValue (double &_q)
int GetJointVel (double &_qd)
int GetJointAcc (double &_qdd)
int SetJointForce (double _tau)
int GetJointForce (double &_tau)
3DOF joints

Obtain/set the joint value/velocity/acceleration/force of a 3-DOF joint.

int SetJointValue (const fMat33 &r)
int SetJointValue (const fEulerPara &ep)
int SetJointVel (const fVec3 &rd)
int SetJointAcc (const fVec3 &rdd)
int SetJointAcc (double ax, double ay, double az)
int GetJointValue (fMat33 &r)
int GetJointVel (fVec3 &rd)
int GetJointAcc (fVec3 &rdd)
int SetJointForce (const fVec3 &_n3)
int GetJointForce (fVec3 &_n3)
6DOF joints

Obtain/set the joint value/velocity/acceleration/force of a 6-DOF joint.

int SetJointValue (const fVec3 &p, const fMat33 &r)
int SetJointValue (const fVec3 &p, const fEulerPara &ep)
int SetJointVel (const fVec3 &pd, const fVec3 &rd)
int SetJointAcc (const fVec3 &pdd, const fVec3 &rdd)
int SetJointAcc (double lx, double ly, double lz, double ax, double ay, double az)
int GetJointValue (fVec3 &p, fMat33 &r)
int GetJointVel (fVec3 &pd, fVec3 &rd)
int GetJointAcc (fVec3 &pdd, fVec3 &rdd)
int SetJointForce (const fVec3 &_f3, const fVec3 &_n3)
int GetJointForce (fVec3 &_f3, fVec3 &_n3)

Public Attributes

User-specified joint parameters
Jointparent
 pointer to the parent joint
Jointbrother
 pointer to the brother joint
Jointchild
 pointer to the child joint
Jointreal
 pointer to the real (connected) joint; for closed chains.
fVec3 rpos_real
 relative position in the real joint frame
fMat33 ratt_real
 relative orientation in the real joint frame
char * name
 joint name (including the character name)
char * basename
 joint base name (without the character name)
char * realname
 name of the real joint (for closed chains)
JointType j_type
 joint type
fVec3 axis
 joint axis in local frame (for 1DOF joints)
fVec3 init_pos
 origin of the joint value (for prismatic joints)
fMat33 init_att
 origin of the joint value (for rotational joints)
fVec3 rel_pos
 (initial) position in parent joint's frame (for 0/3/6 DOF joints)
fMat33 rel_att
 (initial) orientation in parent joint's frame (for 0/3/6 DOF joints)
fEulerPara rel_ep
 Euler parameter representation of rel_att (for 0/3/6 DOF joints)
double mass
 mass
fMat33 inertia
 intertia
fVec3 loc_com
 center of mass in local frame
double gear_ratio
double rotor_inertia
int t_given
 torque or motion controlled
Variables automatically set or computed
int n_dof
 degrees of freedom (0/1/3/6)
int n_thrust
 DOF for motion controlled joints.
int n_root_dof
 total DOF in the root side
int i_value
 index in all joint values
int i_dof
 index in all DOF
int i_thrust
 index in all motion controlled joints
int i_joint
 index of the joint
double q
 joint value (for 1DOF joints)
double qd
 joint velocity (for 1DOF joints)
double qdd
 joint acceleration (for 1DOF joints)
fVec3 rel_lin_vel
fVec3 rel_ang_vel
fVec3 rel_lin_acc
fVec3 rel_ang_acc
double tau
 joint force/torque (for 1DOF joints)
fVec3 tau_f
 joint force for 6DOF joints
fVec3 tau_n
 joint torque for 3 and 6 DOF joints
fVec3 ext_force
 external force
fVec3 ext_moment
 external moment around the local frame
results of forward kinematics computation
fVec3 abs_pos
 absolute position
fMat33 abs_att
 absolute orientation
fVec3 loc_lin_vel
 linear velocity in local frame
fVec3 loc_ang_vel
 angular velocity in local frame
fVec3 loc_com_vel
 com velocity in local frame
fVec3 loc_lin_acc
 linear acceleration in local frame
fVec3 loc_ang_acc
 angular acceleration in local frame
fVec3 loc_com_acc
 com acceleration in local frame
Results of inverse dynamics computation

force/moment applied to joints/links

fVec3 joint_f
fVec3 total_f
fVec3 joint_n
fVec3 total_n
Velocities and accelerations in parent joint's frame for integration
fVec3 p_lin_vel
fEulerPara p_ep_dot
fVec3 p_ang_vel
fVec3 p_lin_acc
fVec3 p_ang_acc

Protected Member Functions

void _init_virtual ()
void add_child (Joint *j)
void calc_acceleration ()
int calc_jacobian (fMat &J, Joint *target)
int calc_jacobian_2 (fMat *J, Joint *target)
int calc_jacobian_2_free_sub (fMat *J, Joint *target, Joint *j1)
int calc_jacobian_2_rotate_rotate (fMat *J, Joint *target, Joint *j1, const fVec3 &axis1, int index1, const fVec3 &axis2, int index2)
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)
int calc_jacobian_2_rotate_sub (fMat *J, Joint *target, Joint *j1)
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 calc_jacobian_2_slide_sub (fMat *J, Joint *target, Joint *j1)
int calc_jacobian_2_sphere_sub (fMat *J, Joint *target, Joint *j1)
int calc_jacobian_free (fMat &J, Joint *target)
int calc_jacobian_rotate (fMat &J, Joint *target)
int calc_jacobian_slide (fMat &J, Joint *target)
int calc_jacobian_sphere (fMat &J, Joint *target)
void calc_joint_force (fVec &tau)
void calc_position ()
void calc_velocity ()
void clear ()
void clear_data ()
int clear_ext_force ()
int clear_joint_force ()
double com_jacobian (fMat &J, fVec3 &com, const char *chname)
int descendant_dof ()
int descendant_num_joints ()
Jointfind_joint (const char *n, const char *charname)
Jointfind_joint (int _id)
int get_joint_acc (fVec &accs)
int get_joint_force (fVec &forces)
void get_joint_list (Joint **joints)
void get_joint_name_list (char **jnames)
int get_joint_value (fVec &values)
int get_joint_vel (fVec &vels)
void init ()
void init_arrays ()
void init_virtual ()
void inv_dyn ()
void inv_dyn_1 ()
void inv_dyn_2 ()
int is_descendant (Joint *cur, Joint *target)
int post_integrate ()
int pre_integrate ()
int remove_child (Joint *j)
int save (ostream &ost, int indent, const char *charname) const
int save_xml (ostream &ost, int indent, const char *charname) const
int set_joint_acc (const fVec &accs)
int set_joint_force (const fVec &forces)
int set_joint_value (const fVec &values)
int set_joint_vel (const fVec &vels)
double total_com (fVec3 &com, const char *chname)
double total_mass ()

Protected Attributes

Chainchain

Private Attributes

double cur_scale

Friends

class Chain

Detailed Description

The class for representing a joint.

Definition at line 538 of file chain.h.


Constructor & Destructor Documentation

Definition at line 121 of file chain.cpp.

Joint::Joint ( const char *  name,
JointType  jt = JUNKNOWN,
const fVec3 rpos = 0.0,
const fMat33 ratt = 1.0,
AxisIndex  _axis_index = AXIS_NULL,
int  t_given = true 
)

Definition at line 130 of file chain.cpp.

Joint::Joint ( JointData jdata,
const char *  charname 
)

Definition at line 172 of file chain.cpp.

Definition at line 292 of file chain.cpp.


Member Function Documentation

void Joint::_init_virtual ( ) [protected]

Definition at line 278 of file init.cpp.

void Joint::add_child ( Joint j) [protected]

Definition at line 157 of file edit.cpp.

void Joint::calc_acceleration ( ) [protected]

Definition at line 80 of file fk.cpp.

int Joint::calc_jacobian ( fMat J,
Joint target 
) [protected]

Definition at line 127 of file jacobi.cpp.

int Joint::calc_jacobian_2 ( fMat J,
Joint target 
) [protected]

Definition at line 310 of file jacobi.cpp.

int Joint::calc_jacobian_2_free_sub ( fMat J,
Joint target,
Joint j1 
) [protected]

Definition at line 454 of file jacobi.cpp.

int Joint::calc_jacobian_2_rotate_rotate ( fMat J,
Joint target,
Joint j1,
const fVec3 axis1,
int  index1,
const fVec3 axis2,
int  index2 
) [protected]

Definition at line 537 of file jacobi.cpp.

int Joint::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 
) [protected]

Definition at line 599 of file jacobi.cpp.

int Joint::calc_jacobian_2_rotate_sub ( fMat J,
Joint target,
Joint j1 
) [protected]

Definition at line 334 of file jacobi.cpp.

int Joint::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 
) [protected]

Definition at line 579 of file jacobi.cpp.

int Joint::calc_jacobian_2_slide_sub ( fMat J,
Joint target,
Joint j1 
) [protected]

Definition at line 365 of file jacobi.cpp.

int Joint::calc_jacobian_2_sphere_sub ( fMat J,
Joint target,
Joint j1 
) [protected]

Definition at line 392 of file jacobi.cpp.

int Joint::calc_jacobian_free ( fMat J,
Joint target 
) [protected]

Definition at line 220 of file jacobi.cpp.

int Joint::calc_jacobian_rotate ( fMat J,
Joint target 
) [protected]

Definition at line 150 of file jacobi.cpp.

int Joint::calc_jacobian_slide ( fMat J,
Joint target 
) [protected]

Definition at line 172 of file jacobi.cpp.

int Joint::calc_jacobian_sphere ( fMat J,
Joint target 
) [protected]

Definition at line 192 of file jacobi.cpp.

void Joint::calc_joint_force ( fVec tau) [protected]

Definition at line 73 of file id.cpp.

void Joint::calc_position ( ) [protected]

Definition at line 21 of file fk.cpp.

void Joint::calc_velocity ( ) [protected]

Definition at line 46 of file fk.cpp.

The Jacobian matrix of position/orientation w.r.t. the joint values.

Definition at line 119 of file jacobi.cpp.

2nd-order derivatives of the Jacobian matrix.

Definition at line 296 of file jacobi.cpp.

int Joint::CalcJdot ( fVec jdot)

Jdot x thdot.

Definition at line 261 of file jacobi.cpp.

char* Joint::CharName ( ) const [inline]

Returns the character name.

Definition at line 648 of file chain.h.

void Joint::clear ( void  ) [protected]

Definition at line 226 of file chain.cpp.

void Joint::clear_data ( ) [protected]

Definition at line 160 of file vary.cpp.

int Joint::clear_ext_force ( ) [protected]

Definition at line 934 of file joint.cpp.

int Joint::clear_joint_force ( ) [protected]

Definition at line 918 of file joint.cpp.

double Joint::com_jacobian ( fMat J,
fVec3 com,
const char *  chname 
) [protected]

Definition at line 30 of file jacobi.cpp.

int Joint::descendant_dof ( ) [protected]

Definition at line 446 of file chain.cpp.

Definition at line 458 of file chain.cpp.

Total DOF of the descendants (end link side).

Definition at line 441 of file chain.cpp.

Total number of joints of the descendants (end link side).

Definition at line 453 of file chain.cpp.

Joint * Joint::find_joint ( const char *  n,
const char *  charname 
) [protected]

Definition at line 396 of file chain.cpp.

Joint * Joint::find_joint ( int  _id) [protected]

Definition at line 420 of file chain.cpp.

int Joint::get_joint_acc ( fVec accs) [protected]

Definition at line 221 of file joint.cpp.

int Joint::get_joint_force ( fVec forces) [protected]

Definition at line 877 of file joint.cpp.

void Joint::get_joint_list ( Joint **  joints) [protected]

Definition at line 381 of file chain.cpp.

void Joint::get_joint_name_list ( char **  jnames) [protected]

Definition at line 354 of file chain.cpp.

int Joint::get_joint_value ( fVec values) [protected]

Definition at line 131 of file joint.cpp.

int Joint::get_joint_vel ( fVec vels) [protected]

Definition at line 179 of file joint.cpp.

int Joint::GetJointAcc ( double &  _qdd)

Definition at line 282 of file joint.cpp.

Definition at line 321 of file joint.cpp.

int Joint::GetJointAcc ( fVec3 pdd,
fVec3 rdd 
)

Definition at line 362 of file joint.cpp.

int Joint::GetJointForce ( double &  _tau)

Definition at line 796 of file joint.cpp.

Definition at line 822 of file joint.cpp.

int Joint::GetJointForce ( fVec3 _f3,
fVec3 _n3 
)

Definition at line 849 of file joint.cpp.

int Joint::GetJointValue ( double &  _q)

Definition at line 256 of file joint.cpp.

Definition at line 295 of file joint.cpp.

int Joint::GetJointValue ( fVec3 p,
fMat33 r 
)

Definition at line 334 of file joint.cpp.

int Joint::GetJointVel ( double &  _qd)

Definition at line 269 of file joint.cpp.

Definition at line 308 of file joint.cpp.

int Joint::GetJointVel ( fVec3 pd,
fVec3 rd 
)

Definition at line 348 of file joint.cpp.

void Joint::init ( ) [protected]

Definition at line 135 of file init.cpp.

void Joint::init_arrays ( ) [protected]

Definition at line 204 of file init.cpp.

void Joint::init_virtual ( ) [protected]

Definition at line 271 of file init.cpp.

void Joint::inv_dyn ( ) [protected]

Definition at line 23 of file id.cpp.

void Joint::inv_dyn_1 ( ) [protected]

Definition at line 32 of file id.cpp.

void Joint::inv_dyn_2 ( ) [protected]

Definition at line 46 of file id.cpp.

int Joint::is_descendant ( Joint cur,
Joint target 
) [protected]

Definition at line 470 of file chain.cpp.

int Joint::isAscendant ( Joint target)

Identifies whether the target joint is a direct ascendant.

Definition at line 479 of file chain.cpp.

int Joint::isDescendant ( Joint target)

Identifies whether the target joint is a direct descendant.

Definition at line 465 of file chain.cpp.

int Joint::post_integrate ( ) [protected]

Definition at line 309 of file integ.cpp.

int Joint::pre_integrate ( ) [protected]

Definition at line 281 of file integ.cpp.

int Joint::remove_child ( Joint j) [protected]

Definition at line 43 of file edit.cpp.

int Joint::save ( ostream &  ost,
int  indent,
const char *  charname 
) const [protected]
int Joint::save_xml ( ostream &  ost,
int  indent,
const char *  charname 
) const [protected]
double Joint::Scale ( ) [inline]

Returns the scale applied to the joint.

Definition at line 643 of file chain.h.

int Joint::set_joint_acc ( const fVec accs) [protected]

Definition at line 508 of file joint.cpp.

int Joint::set_joint_force ( const fVec forces) [protected]

Definition at line 757 of file joint.cpp.

int Joint::set_joint_value ( const fVec values) [protected]

Definition at line 426 of file joint.cpp.

int Joint::set_joint_vel ( const fVec vels) [protected]

Definition at line 467 of file joint.cpp.

Set joint type to fixed.

Definition at line 56 of file joint.cpp.

void Joint::SetFreeJointType ( const fVec3 rpos,
const fMat33 ratt 
)

Set joint type to free.

Definition at line 80 of file joint.cpp.

int Joint::SetJointAcc ( double  _qdd)

Definition at line 580 of file joint.cpp.

Definition at line 639 of file joint.cpp.

int Joint::SetJointAcc ( double  ax,
double  ay,
double  az 
)

Definition at line 652 of file joint.cpp.

int Joint::SetJointAcc ( const fVec3 pdd,
const fVec3 rdd 
)

Definition at line 711 of file joint.cpp.

int Joint::SetJointAcc ( double  lx,
double  ly,
double  lz,
double  ax,
double  ay,
double  az 
)

Definition at line 725 of file joint.cpp.

void Joint::SetJointData ( JointData jdata,
const char *  charname 
)

Definition at line 301 of file chain.cpp.

int Joint::SetJointForce ( double  _tau)

Definition at line 783 of file joint.cpp.

Definition at line 809 of file joint.cpp.

int Joint::SetJointForce ( const fVec3 _f3,
const fVec3 _n3 
)

Definition at line 835 of file joint.cpp.

Set relative orientation.

Definition at line 385 of file joint.cpp.

Set relative position.

Definition at line 379 of file joint.cpp.

int Joint::SetJointValue ( double  _q)

Definition at line 538 of file joint.cpp.

Definition at line 598 of file joint.cpp.

Definition at line 612 of file joint.cpp.

Definition at line 667 of file joint.cpp.

Definition at line 682 of file joint.cpp.

int Joint::SetJointVel ( double  _qd)

Definition at line 562 of file joint.cpp.

Definition at line 626 of file joint.cpp.

int Joint::SetJointVel ( const fVec3 pd,
const fVec3 rd 
)

Definition at line 697 of file joint.cpp.

void Joint::SetName ( const char *  _name,
const char *  _charname = 0 
) [inline]

Change the joint name.

Definition at line 655 of file chain.h.

Set joint type to rotational.

Definition at line 30 of file joint.cpp.

void Joint::SetSlideJointType ( const fVec3 rpos,
const fMat33 ratt,
AxisIndex  ai 
)

Set joint type to prismatic.

Definition at line 43 of file joint.cpp.

Set joint type to spherical.

Definition at line 68 of file joint.cpp.

double Joint::total_com ( fVec3 com,
const char *  chname 
) [protected]

Definition at line 137 of file fk.cpp.

double Joint::total_mass ( ) [protected]

Definition at line 22 of file joint.cpp.

double Joint::TotalMass ( )

Returns the total mass of the child links.

Definition at line 16 of file joint.cpp.

Definition at line 92 of file joint.cpp.


Friends And Related Function Documentation

friend class Chain [friend]

Definition at line 540 of file chain.h.


Member Data Documentation

absolute orientation

Definition at line 742 of file chain.h.

absolute position

Definition at line 741 of file chain.h.

joint axis in local frame (for 1DOF joints)

Definition at line 696 of file chain.h.

joint base name (without the character name)

Definition at line 692 of file chain.h.

pointer to the brother joint

Definition at line 684 of file chain.h.

Chain* Joint::chain [protected]

Definition at line 769 of file chain.h.

pointer to the child joint

Definition at line 685 of file chain.h.

double Joint::cur_scale [private]

Definition at line 843 of file chain.h.

external force

Definition at line 736 of file chain.h.

external moment around the local frame

Definition at line 737 of file chain.h.

Definition at line 707 of file chain.h.

index in all DOF

Definition at line 720 of file chain.h.

index of the joint

Definition at line 722 of file chain.h.

index in all motion controlled joints

Definition at line 721 of file chain.h.

index in all joint values

Definition at line 719 of file chain.h.

intertia

Definition at line 705 of file chain.h.

origin of the joint value (for rotational joints)

Definition at line 698 of file chain.h.

origin of the joint value (for prismatic joints)

Definition at line 697 of file chain.h.

joint type

Definition at line 694 of file chain.h.

Definition at line 755 of file chain.h.

Definition at line 756 of file chain.h.

angular acceleration in local frame

Definition at line 747 of file chain.h.

angular velocity in local frame

Definition at line 744 of file chain.h.

center of mass in local frame

Definition at line 706 of file chain.h.

com acceleration in local frame

Definition at line 748 of file chain.h.

com velocity in local frame

Definition at line 745 of file chain.h.

linear acceleration in local frame

Definition at line 746 of file chain.h.

linear velocity in local frame

Definition at line 743 of file chain.h.

double Joint::mass

mass

Definition at line 704 of file chain.h.

degrees of freedom (0/1/3/6)

Definition at line 715 of file chain.h.

total DOF in the root side

Definition at line 717 of file chain.h.

DOF for motion controlled joints.

Definition at line 716 of file chain.h.

char* Joint::name

joint name (including the character name)

Definition at line 691 of file chain.h.

Definition at line 765 of file chain.h.

Definition at line 763 of file chain.h.

Definition at line 762 of file chain.h.

Definition at line 764 of file chain.h.

Definition at line 761 of file chain.h.

pointer to the parent joint

Definition at line 683 of file chain.h.

double Joint::q

joint value (for 1DOF joints)

Definition at line 724 of file chain.h.

double Joint::qd

joint velocity (for 1DOF joints)

Definition at line 725 of file chain.h.

double Joint::qdd

joint acceleration (for 1DOF joints)

Definition at line 726 of file chain.h.

relative orientation in the real joint frame

Definition at line 689 of file chain.h.

pointer to the real (connected) joint; for closed chains.

Definition at line 687 of file chain.h.

name of the real joint (for closed chains)

Definition at line 693 of file chain.h.

Definition at line 731 of file chain.h.

Definition at line 729 of file chain.h.

(initial) orientation in parent joint's frame (for 0/3/6 DOF joints)

Definition at line 701 of file chain.h.

Euler parameter representation of rel_att (for 0/3/6 DOF joints)

Definition at line 702 of file chain.h.

Definition at line 730 of file chain.h.

Definition at line 728 of file chain.h.

(initial) position in parent joint's frame (for 0/3/6 DOF joints)

Definition at line 700 of file chain.h.

Definition at line 708 of file chain.h.

relative position in the real joint frame

Definition at line 688 of file chain.h.

torque or motion controlled

Definition at line 709 of file chain.h.

double Joint::tau

joint force/torque (for 1DOF joints)

Definition at line 733 of file chain.h.

joint force for 6DOF joints

Definition at line 734 of file chain.h.

joint torque for 3 and 6 DOF joints

Definition at line 735 of file chain.h.

Definition at line 755 of file chain.h.

Definition at line 756 of file chain.h.


The documentation for this class was generated from the following files:


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Apr 11 2019 03:30:21