Go to the documentation of this file.
25 cf(idx+1) = f_final(1);
26 cf(idx+2) = f_final(2);
27 cf(idx+3) = f_final(3);
28 cf(idx+4) = f_final(4);
29 cf(idx+5) = f_final(5);
58 if(!
this ||
n_links == 1)
return 0;
59 int ret0 = 0, ret1 = 0;
62 return (ret0 > ret1) ? (ret0+1) : (ret1+1);
72 if(!
this ||
n_links == 1)
return 0;
126 if(n_contacts == 0)
return 0;
142 for(
int i=0;
i<org_n_joint;
i++)
155 if(jinfo_save[
i].plink)
delete jinfo_save[
i].
plink;
158 if(jinfo_save)
delete[] jinfo_save;
188 for(
int i=0;
i<org_n_joint;
i++)
196 for(
int i=0;
i<n_contacts;
i++)
210 if(jinfo_save)
delete[] jinfo_save;
299 static fMat33 m11, m12, m22;
307 m22.
mul(scross, scross);
315 n2Jmat.
mul(n2J, n2J);
324 M(
i, j+3) = -m12(
i, j);
325 M(
i+3, j) = m12(
i, j);
326 M(
i+3, j+3) = m22(
i, j);
345 ost <<
"-- pSubChain ->" << endl;
347 else ost <<
"\tsingle link" << endl;
352 ost <<
"\tlinks: " <<
n_links << endl;
356 ost <<
"\trank: " << rank << endl;
int RemoveJoint(Joint *j)
disconnect joint j from its parent
Joint * parent
pointer to the parent joint
Joint * root
Chain information.
int inv_posv(const fMat &)
inverse of positive-definite, symmetric matrix
The class for representing a joint.
std::vector< fVec3 > contact_relvels
fVec3 axis
joint axis in local frame (for 1DOF joints)
std::vector< fVec3 > all_jdot_v
virtual int init(SceneGraph *sg)
Initialize the parameters.
void mul(const fMat33 &mat1, const fMat33 &mat2)
JointType j_type
joint type
int n_root_dof
total DOF in the root side
Class for representing a single link in a schedule tree.
std::vector< double > fric_coefs
Joint * link_side
null in parent side of space joint
void setup_pjoint_virtual(Joint *j)
void setup_pjoint(Joint *j)
Joint * child
pointer to the child joint
@ JROTATE
rotational (1DOF)
std::vector< fMat > all_Jr
int contact_vjoint_index(Joint *_jnt)
fVec3 loc_com
center of mass in local frame
Class for representing "handle"; two pJoint instances are attached to both sides of each joint.
Forward dynamics computation based on Assembly-Disassembly Algorithm.
std::vector< fMat > all_Jv
int TotalCost()
Approximate indicators of total computational cost.
int ConstraintForces(fVec &cf)
Extract the constraint forces.
virtual int clear_contact()
virtual int clear_data()
Clear arrays only; don't delete joints.
joint_list contact_vjoints
int i_joint
index of the joint
Joint * real
pointer to the real (connected) joint; for closed chains.
virtual int clear_data()
Clear arrays only; don't delete joints.
char * name
joint name (including the character name)
void identity()
Identity matrix.
Joint * brother
pointer to the brother joint
void DumpSchedule(ostream &ost)
Dump the schedule information to ost.
std::vector< fVec3 > all_jdot_r
virtual int init(SceneGraph *sg)
Initialize the parameters.
void mul(const fVec3 &vec, double d)
virtual void Clear()
Remove all joints and clear all parameters.
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:04