joint.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 __CT_JOINT_H__
00022 #define __CT_JOINT_H__
00023 
00024 #include "csphyzik/phyztype.h"
00025 #include "csphyzik/math3d.h"
00026 #include "csphyzik/ctvspat.h"
00027 
00028 class ctArticulatedBody;
00029 class ctForce;
00030 
00032 #define JOINT_STATESIZE 2
00033 
00039 class ctJoint
00040 {
00041 public:
00043   static double joint_friction;
00044 
00045   ctJoint ()
00046     : inboard_offset(0), outboard_offset(0), joint_axis(0)
00047   { q = qv = qa = 0; }
00048 
00049   ctJoint ( ctArticulatedBody *in, ctVector3 &in_offset,
00050             ctArticulatedBody *out, ctVector3 &out_offset );
00051 
00053   virtual void calc_vw( ctVector3 &v, ctVector3 &w ) = 0;
00059   virtual void calc_coriolus ( const ctVector3 &r, const ctVector3 &w_f,
00060                                ctSpatialVector &c ) = 0;
00061 
00062   virtual ctSpatialVector get_spatial_joint_axis() = 0;
00063 
00068   virtual real get_actuator_magnitude( real external_f, real inertail_comp );
00069 
00070   virtual int get_state_size(){ return JOINT_STATESIZE; }// = 0;
00071   virtual int set_state( real *sa );
00072   virtual int get_state( const real *sa );
00073   virtual int set_delta_state( real *state_array );
00074 
00076   void update_link_RF ();
00077 
00079   ctVector3 get_r ();
00080 
00081   ctArticulatedBody *inboard;
00086   ctVector3 inboard_offset;
00087   ctArticulatedBody *outboard;
00088 
00093   ctVector3 outboard_offset;
00094 
00096   ctVector3 joint_axis;
00098   real q, qv, qa;
00099 
00100 protected:
00102 
00103 
00104 };
00105 
00108 class ctPrismaticJoint : public ctJoint
00109 {
00110 public:
00111   virtual void calc_vw ( ctVector3 &v, ctVector3& /*w*/ )
00112   { v = v + joint_axis *qv; }
00113 
00114   virtual void calc_coriolus ( const ctVector3 &r, const ctVector3 &w_f,
00115                               ctSpatialVector &c );
00116 
00117   virtual ctSpatialVector get_spatial_joint_axis ();
00118 
00119 };
00120 
00122 class ctRevoluteJoint : public ctJoint
00123 {
00124 public:
00125   ctRevoluteJoint ( ctArticulatedBody *in, ctVector3 &in_offset,
00126                     ctArticulatedBody *out, ctVector3 &out_offset,
00127                     ctVector3 &paxis );
00128 
00129   virtual void calc_vw ( ctVector3 &v, ctVector3 &w )
00130   {
00131     w = w + joint_axis*qv;
00132     v = v + ( joint_axis % outboard_offset )*qv;
00133   }
00134 
00135   virtual void calc_coriolus ( const ctVector3 &r, const ctVector3 &w_f,
00136                                ctSpatialVector &c );
00137 
00138   virtual ctSpatialVector get_spatial_joint_axis ();
00139 };
00140 
00147 class ctConstrainedRevoluteJoint : public ctRevoluteJoint
00148 {
00149 public:
00150   ctConstrainedRevoluteJoint ( ctArticulatedBody *in, ctVector3 &in_offset,
00151                                ctArticulatedBody *out, ctVector3 &out_offset,
00152                                ctVector3 &paxis );
00153 
00158   virtual real get_actuator_magnitude ( real external_f, real inertail_comp );
00159 
00160   void set_constraint ( real pmax_angle, real pmin_angle )
00161   { max_angle = pmax_angle; min_angle = pmin_angle; }
00162 
00163   void set_spring_constant( real pk )
00164   { k = pk; }
00165 
00166   void set_damping_constant( real pk )
00167   { damping_k = pk; }
00168 
00169   real max_angle;
00170   real min_angle;
00171 
00173 
00175   real k;
00177   real damping_k;
00178 };
00179 
00180 #endif // __CT_JOINT_H__