1 /***************************************************************************** 2 * DynaMechs: A Multibody Dynamic Simulation Library 3 * 4 * Copyright (C) 1994-2001 Scott McMillan All Rights Reserved. 5 * 6 * This library is free software; you can redistribute it and/or 7 * modify it under the terms of the GNU General Public License 8 * as published by the Free Software Foundation; either version 2 9 * of the License, or (at your option) any later version. 10 * 11 * This library is distributed in the hope that it will be useful, 12 * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 * General Public License for more details. 15 * 16 * You should have received a copy of the GNU Library General Public 17 * License along with this library; if not, write to the Free 18 * Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. 19 ***************************************************************************** 20 * File: dmContactModel.hpp 21 * Author: Scott McMillan 22 * Summary: Class definition for contact modelling (contacts with Env). 23 * : Used to be class EndEffector, but now is a component of RigidBody 24 * : as a dmForce component, so that contact forces on each body can be 25 * : computed. 26 *****************************************************************************/ 27 28 #ifndef _DM_CONTACT_MODEL_HPP 29 #define _DM_CONTACT_MODEL_HPP 30 31 #include <dm.h> 32 #include <dmEnvironment.hpp> 33 #include <dmForce.hpp> 34 35 //============================================================================ 36 37 /** 38 39 A {\tt dmContactModel} object is subclassed from the {\tt dmForce} and computes 40 the compliant forces of contact with a prismatic terrain. It is instantiated 41 and ``assigned'' to a {\tt dmRigidBody} object using the {\tt 42 dmRigidBody::addForce()} function. This class queries the static {\tt 43 dmEnvironment} object about terrain and computes forces of contact between it 44 and the contact points defined in this class for the particular {\tt 45 dmRigidBody} object. {\em This class is one of those that could use a bit of 46 work. The whole interaction between robot (more specifically, rigid bodies) 47 and environment has not been completely thought through yet, with the dmForce 48 class it has taken one major step forward, though.} 49 50 The default constructor instantiates an empty object. A call to the 51 {\tt setContactPoints} function initializes the contact points that 52 will be set. Its first parameter, {\tt num\_contact\_points}, is the 53 number of points (3D vertices) that are to be allocated, and the 54 second is an array of {\tt num\_contact\_points} {\tt 55 CartesianVector}s that specify the constant locations of the contact 56 points relative to the rigid body's coordinate system. The function 57 {\tt getNumContactPoints} can be used to query the size of the array, 58 and {\tt getContactPoint} can get the values of a particular vertex by 59 index (0 for the first, 1 for the second, etc.). The latter returns 60 true if the index is within range; otherwise, it returns false. The 61 {\tt getContactState} function can be used to query whether an 62 individual contact point is currently in contact. 63 64 The {\tt computeForce} is the main part of this class and computes the 65 compliant contact force exerted on the rigid body. It is called by the {\tt 66 ABForwardAcceleration} functions of all {\tt dmRefMember} objects and the {\tt 67 dmABBackwardDynamics} functions of the {\tt dmLink} objects that are derived 68 from the {\tt dmRigidBody} class. The {\tt ABForwardKinematics} functions 69 computes the position and orientation of the rigid body with respect to the 70 inertial coordinate system, and the spatial velocity with respect to the body's 71 coordinate system and sets the {\tt p\_ICS}, {\tt R\_ICS}, and {\tt v} elements 72 of the {\tt dmABForKinStruct} struct ({\tt val}). This struct is used at the 73 beginning of the subsequent pass of the AB algorthm when it calls this class's 74 {\tt computeForce} to compute the resultant spatial force exerted at all of the 75 contact points and returns it in {\tt force} parameter (with respect to the 76 body's coordinate system). 77 78 In the current implementation of the {\tt computeForce} function, an initial 79 contact location is set to maintain information from iteration to iteration and 80 compute spring-like forces from the initial contact point. If the state of the 81 system is manually changed while some of these points are in contact with the 82 terrain, large forces can result on the next iteration of the simulation. The 83 {\tt reset} function is used to overcome this potential problem by resetting 84 all contact points. {\em Also note that this iteration to iteration dependency 85 is really only valid assuming a simple single step integration method like 86 Euler. I do not know how valid this is with multistep methods like 87 Runge-Kutte, but it seems to work so far.} 88 89 See also {\tt dmEnvironment}, {\tt dmRigidBody}, {\tt dmLoadFile\_dm}*/ 90 91 //============================================================================ 92 93 class DM_DLL_API dmContactModel : public dmForce 94 { 95 public: 96 /// 97 dmContactModel(); 98 /// 99 virtual ~dmContactModel(); 100 101 /// 102 void setContactPoints(unsigned int num_contact_points, 103 CartesianVector *contact_pos); 104 /// getNumContactPoints() const105 unsigned int getNumContactPoints() const {return m_num_contact_points;} 106 /// 107 bool getContactPoint(unsigned int index, CartesianVector pos) const; 108 /// 109 bool getContactState(unsigned int index) const; 110 111 /// 112 void computeForce(const dmABForKinStruct &val, SpatialVector force); 113 /// reset()114 inline void reset() { m_reset_flag = true;} 115 116 /// 117 void pushState(); 118 /// 119 void popState(); 120 121 // rendering functions (for future expansion/visualization): 122 /// 123 void draw() const; 124 125 private: 126 // not implemented 127 dmContactModel(const dmContactModel &); 128 dmContactModel &operator=(const dmContactModel &); 129 130 private: 131 bool m_reset_flag; 132 133 //dmEnvironment *m_env; 134 135 unsigned int m_num_contact_points; 136 bool *m_contact_flag; 137 bool *m_sliding_flag; 138 CartesianVector *m_contact_pos; // list of point locations to be checked 139 CartesianVector *m_initial_contact_pos; // ICS 1st pnt of contact for each 140 141 // variables to store contact state for backing out of a step 142 // when using variable stepsize integrators. 143 bool *m_contact_flag_stored; 144 bool *m_sliding_flag_stored; 145 CartesianVector *m_initial_contact_pos_stored; 146 147 148 // temporary variables only used by computeContactForce 149 Float ptemp, vtemp, temp; 150 Float fe_normal_mag, fe_planar_mag; 151 CartesianVector normal, current_pos; 152 CartesianVector peC_pos, veC_pos, vnC_pos, fe, fn, nn; 153 CartesianVector p_planar, v_planar; 154 CartesianVector fe_normal, fe_planar; 155 }; 156 157 #endif 158