Go to the documentation of this file.
33 #define N_FRIC_CONE_DIV 8
71 PSIM_TAG_LAMBDA = 100,
126 void dump(ostream& ost);
175 void dump(ostream& ost);
365 void send_inertia(
int dest);
373 void send_acc(
int dest);
381 void send_force(
int dest);
410 void dump(ostream& ost);
419 int assign_processes(
int start_rank,
int end_rank);
420 int create_types(
int* n_proc_joints,
int** lengths, MPI_Aint** disps, MPI_Datatype** oldtypes);
423 MPI_Datatype parent_lambda_type;
424 MPI_Datatype parent_acc_type;
425 MPI_Datatype parent_force_type;
447 :
virtual public Chain
468 cone_dir[
i](0) = cos(ang);
469 cone_dir[
i](1) = sin(ang);
470 cone_dir[
i](2) = 0.0;
474 inertia_wait_time = 0.0;
476 force_wait_time = 0.0;
487 delete joint_info[
i].pjoints[0];
488 delete joint_info[
i].pjoints[1];
489 if(joint_info[
i].plink)
delete joint_info[
i].plink;
493 if(subchains)
delete subchains;
495 if(all_acc_types)
delete[] all_acc_types;
497 cerr <<
"[" << rank <<
"] inertia_wait_time = " << inertia_wait_time << endl;
498 cerr <<
"[" << rank <<
"] acc_wait_time = " << acc_wait_time << endl;
499 cerr <<
"[" << rank <<
"] force_wait_time = " << force_wait_time << endl;
504 virtual void Clear();
517 int Update(
double timestep, std::vector<class SDContactPair*>& sdContactPairs);
523 int Schedule(
Joint** joints);
534 void DumpSchedule(ostream& ost);
542 int ConstraintForces(
fVec& cf);
545 _pjoints[0] = joint_info[_joint->
i_joint].pjoints[0];
546 _pjoints[1] = joint_info[_joint->
i_joint].pjoints[1];
553 virtual int init(SceneGraph* sg);
558 virtual int clear_contact();
562 void default_schedule_virtual(
Joint* j);
563 void setup_pjoint(
Joint* j);
564 void setup_pjoint_virtual(
Joint* j);
567 void update_position();
568 void update_velocity();
572 void update_collision();
591 for(joint_list::iterator j=contact_vjoints.begin(); j!=contact_vjoints.end(); count++, j++)
593 if(_jnt == *j)
return count;
607 double inertia_wait_time;
608 double acc_wait_time;
609 double force_wait_time;
613 MPI_Datatype* all_acc_types;
int t_given
torque or motion controlled
pSubChain(pSim *_sim, pSubChain *_p, pJoint *_p0, pJoint *_p1)
void identity()
Creates an identity matrix (only for square matrices).
pSubChain * subchain
the subchain which only contains the link associated with this pjoint
The class for representing a joint.
pJoint(Joint *_joint, Joint *_link_side)
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.
int calc_contact_force(double timestep)
void col_disassembly_body()
The class representing the whole mechanism. May contain multiple characters.
JointType j_type
joint type
void calc_acc(const fVec3 &g0)
Class for representing a single link in a schedule tree.
void GetPJoint(Joint *_joint, pJoint *_pjoints[2])
std::vector< double > fric_coefs
Joint * link_side
null in parent side of space joint
std::vector< class pJoint * > p_joint_list
@ JROTATE
rotational (1DOF)
@ JSPHERE
spherical (3DOF)
int n_dof
degrees of freedom (0/1/3/6)
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.
std::vector< fMat > all_Jr
int contact_vjoint_index(Joint *_jnt)
Class for representing "handle"; two pJoint instances are attached to both sides of each joint.
pSim()
Default constructor.
3x3 matrix and 3-element vector classes.
int get_outer_index(pJoint *pj)
std::vector< fMat > all_Jv
#define N_FRIC_CONE_DIV
Number of vertices of the friction cone approximation.
Classes for defining open/closed kinematic chains.
void zero()
Creates a zero vector.
joint_list contact_vjoints
int i_joint
index of the joint
int parent_side
link is in parent side?
virtual int clear_data()
Clear arrays only; don't delete joints.
void resize(int i)
Change the size.
std::vector< Joint * > joint_list
pSubChain(pSim *_sim, pSubChain *_p, pLink *_pl)
Node for schedule tree; represents a partial chain.
std::list< class pSubChain * > subchain_list
int * outer_joints_origin
std::vector< fVec3 > all_jdot_r
Main class for forward dynamics computation.
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