articula.h Source File

00001 /*
00002     Dynamics/Kinematics modeling and simulation library.
00003     Copyright (C) 1999 by Michael Alexander Ewert
00004 
00005     This library is free software; you can redistribute it and/or
00006     modify it under the terms of the GNU Library General Public
00007     License as published by the Free Software Foundation; either
00008     version 2 of the License, or (at your option) any later version.
00009 
00010     This library is distributed in the hope that it will be useful,
00011     but WITHOUT ANY WARRANTY; without even the implied warranty of
00012     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00013     Library General Public License for more details.
00014 
00015     You should have received a copy of the GNU Library General Public
00016     License along with this library; if not, write to the Free
00017     Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
00018 
00019 */
00020 
00021 #ifndef ARTICULATEDBODY_H
00022 #define ARTICULATEDBODY_H
00023 
00024 #include "csphyzik/rigidbod.h"
00025 #include "csphyzik/ctvspat.h"
00026 
00027 #define ABSTATESIZE 0  // all state is in solver and joint
00028 
00029 class ctJoint;
00030 class ctFeatherstoneAlgorithm;
00031 class ctInverseKinematics;
00032 class ctKinematicEntity; 
00033 
00035 // articulated body.  Definition: a handle ( rigid body ) with 0 or more
00036 // articulated bodies attached to it via joints
00037 class ctArticulatedBody : public ctPhysicalEntity
00038 {
00039 public:
00040 
00042   static void set_joint_friction ( double pfrict = DEFAULT_JOINT_FRICTION );
00043 
00044   // debug var
00045   int tag;
00046 
00047   ctArticulatedBody ();
00048   ctArticulatedBody ( ctRigidBody *phandle );
00049 
00050   virtual ~ctArticulatedBody ();
00051 
00053   ctReferenceFrame *get_handle_RF ()
00054   { if ( handle ) return handle->get_RF (); else return NULL; }
00055 
00057   ctRigidBody *get_handle ()
00058   { return handle; }
00059 
00061   void rotate_around_axis ( real ptheta );
00062 
00069   void link_revolute ( ctArticulatedBody *child, ctVector3 &pin_joint_offset,
00070                        ctVector3 &pout_joint_offset, ctVector3 &pjoint_axis );
00071 
00072   void link_joint ( ctJoint *jnt, ctArticulatedBody *child );
00073 
00074   virtual void apply_given_F ( ctForce &frc );
00075 
00080   void apply_forces ( real t );
00081 
00082   virtual void apply_impulse( ctVector3 impulse_point,
00083                               ctVector3 impulse_vector );
00084 
00086   virtual void get_impulse_m_and_I_inv ( real *pm, ctMatrix3 *pI_inv,
00087     const ctVector3 &impulse_point, const ctVector3 &unit_length_impulse_vector );
00088 
00089   virtual void init_state ();
00090 
00091   // methods for state to and from integrator
00092   virtual int get_state_size ();
00093   virtual int set_state ( real *sa );
00094   virtual int get_state ( const real *sa );
00095   virtual int set_delta_state ( real *state_array );
00096 
00102   int set_state_links ( real *sa );
00103   int get_state_links ( const real *sa );
00104   int set_delta_state_links ( real *state_array );
00105 
00107   void make_grounded ()
00108   { is_grounded = true; };
00109 
00110   void make_ungrounded ()
00111   { is_grounded = false; };
00112 
00117   void attach_to_entity ( ctKinematicEntity *pattache )
00118   { is_grounded = true;  attached_to = pattache; };
00119 
00120   void detattached_from_entity ()
00121   { is_grounded = false;  attached_to = NULL; };
00122 
00124   ctFeatherstoneAlgorithm *install_featherstone_solver ();
00125 
00127   ctInverseKinematics *install_IK_solver ();
00128 
00133   void compute_link_velocities();
00134 
00136   void calc_relative_frame ();
00137 
00138   void update_links();
00139 
00140   friend class ctFeatherstoneAlgorithm;
00141   friend class ctInverseKinematics;
00142   friend class ctJoint;
00143 protected:
00145   ctRigidBody *handle;
00147   ctJoint *inboard_joint;
00149   ctLinkList<ctArticulatedBody> outboard_links;
00151   bool is_grounded;
00152 
00153   ctKinematicEntity *attached_to;
00154 
00155   // work variables
00156 
00161   ctMatrix3 T_fg;
00162 
00167   ctVector3 r_fg;
00168 
00173   ctVector3 w_body;
00174   ctVector3 v_body;
00175 
00176 };
00177 
00178 
00179 #endif