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.cpp
21 * Author: Scott McMillan
22 * Summary: Class declaration for contact modelling (contacts with terrain).
23 * : Used to be class EndEffector, but now is a component of RigidBody
24 * : so that contact forces on each body can be computed.
25 *****************************************************************************/
26
27 #include <dm.h>
28 #include <dmForce.hpp>
29 #include <dmContactModel.hpp>
30 #include <dmEnvironment.hpp>
31
32 //============================================================================
33 // class dmContactModel : public dmForce
34 //============================================================================
35
36 //----------------------------------------------------------------------------
dmContactModel()37 dmContactModel::dmContactModel() :
38 dmForce(),
39 m_reset_flag(true),
40 m_num_contact_points(0),
41 m_contact_flag(NULL),
42 m_sliding_flag(NULL),
43 m_contact_pos(NULL),
44 m_initial_contact_pos(NULL),
45 m_contact_flag_stored(NULL),
46 m_sliding_flag_stored(NULL),
47 m_initial_contact_pos_stored(NULL)
48 {
49 }
50
51 //----------------------------------------------------------------------------
~dmContactModel()52 dmContactModel::~dmContactModel()
53 {
54 if (m_num_contact_points)
55 {
56 delete[]m_contact_flag;
57 delete[]m_sliding_flag;
58
59 delete[]m_contact_pos;
60 delete[]m_initial_contact_pos;
61
62 delete[]m_contact_flag_stored;
63 delete[]m_sliding_flag_stored;
64 delete[]m_initial_contact_pos_stored;
65 }
66 }
67
68 //----------------------------------------------------------------------------
69 // Summary: set the list of contact points in local CS
70 // Parameters: num_contact_points - number of points to be initialized
71 // contact_pts - Contact_Locations in local coordinate system
72 // Returns: none
73 //----------------------------------------------------------------------------
setContactPoints(unsigned int num_contact_points,CartesianVector * contact_pos)74 void dmContactModel::setContactPoints(unsigned int num_contact_points,
75 CartesianVector *contact_pos)
76 {
77 m_reset_flag = true;
78
79 // delete any previous contact points
80 if (m_num_contact_points)
81 {
82 m_num_contact_points = 0;
83 delete[]m_contact_flag;
84 delete[]m_sliding_flag;
85 delete[]m_contact_pos;
86 delete[]m_initial_contact_pos;
87 delete[]m_contact_flag_stored;
88 delete[]m_sliding_flag_stored;
89 delete[]m_initial_contact_pos_stored;
90 }
91
92 if (num_contact_points)
93 {
94 m_num_contact_points = num_contact_points;
95
96 /* FIXME - need to error check this allocation */
97 m_contact_flag = new bool[m_num_contact_points];
98 m_sliding_flag = new bool[m_num_contact_points];
99
100 m_contact_pos = new CartesianVector[m_num_contact_points];
101 m_initial_contact_pos = new CartesianVector[m_num_contact_points];
102
103 m_contact_flag_stored = new bool[m_num_contact_points];
104 m_sliding_flag_stored = new bool[m_num_contact_points];
105 m_initial_contact_pos_stored = new CartesianVector[m_num_contact_points];
106
107 for (unsigned int i = 0; i < m_num_contact_points; i++)
108 {
109 m_contact_flag[i] = false;
110 m_sliding_flag[i] = false;
111
112 m_contact_pos[i][0] = contact_pos[i][0];
113 m_contact_pos[i][1] = contact_pos[i][1];
114 m_contact_pos[i][2] = contact_pos[i][2];
115 }
116 }
117 }
118
119 //----------------------------------------------------------------------------
120 // Summary: get the i-th contact point
121 // Parameters:
122 // Returns: true if returned point is valid
123 //----------------------------------------------------------------------------
getContactPoint(unsigned int index,CartesianVector pos) const124 bool dmContactModel::getContactPoint(unsigned int index,
125 CartesianVector pos) const
126 {
127 if (index < m_num_contact_points)
128 {
129 pos[0] = m_contact_pos[index][0];
130 pos[1] = m_contact_pos[index][1];
131 pos[2] = m_contact_pos[index][2];
132 return true;
133 }
134
135 return false;
136 }
137
138 //----------------------------------------------------------------------------
139 // Summary: get the contact state of the i-th contact point
140 // : (for implementation of a contact sensor).
141 // Parameters:
142 // Returns: true if in contact, false otherwise
143 //----------------------------------------------------------------------------
getContactState(unsigned int index) const144 bool dmContactModel::getContactState(unsigned int index) const
145 {
146 if (index < m_num_contact_points)
147 return m_contact_flag[index];
148 else
149 {
150 cerr << "ERROR: Contact point index out of range" << endl;
151 return false;
152 }
153 }
154
155
156 //----------------------------------------------------------------------------
157 // Summary: Saves (pushes) the current contact state
158 // Parameters: none
159 // Returns: none
160 //----------------------------------------------------------------------------
pushState()161 void dmContactModel::pushState()
162 {
163 for (unsigned int i = 0; i < m_num_contact_points; i++)
164 {
165 m_contact_flag_stored[i] = m_contact_flag[i];
166 m_sliding_flag_stored[i] = m_sliding_flag[i];
167
168 m_initial_contact_pos_stored[i][0] = m_initial_contact_pos[i][0];
169 m_initial_contact_pos_stored[i][1] = m_initial_contact_pos[i][1];
170 m_initial_contact_pos_stored[i][2] = m_initial_contact_pos[i][2];
171 }
172 }
173
174 //----------------------------------------------------------------------------
175 // Summary: Restores (pops) the contact state from storage
176 // Parameters: none
177 // Returns: none
178 //----------------------------------------------------------------------------
popState()179 void dmContactModel::popState()
180 {
181 for (unsigned int i = 0; i < m_num_contact_points; i++)
182 {
183 m_contact_flag[i] = m_contact_flag_stored[i];
184 m_sliding_flag[i] = m_sliding_flag_stored[i];
185
186 m_initial_contact_pos[i][0] = m_initial_contact_pos_stored[i][0];
187 m_initial_contact_pos[i][1] = m_initial_contact_pos_stored[i][1];
188 m_initial_contact_pos[i][2] = m_initial_contact_pos_stored[i][2];
189 }
190 }
191
192
193 //----------------------------------------------------------------------------
194 // Summary: Compute the external force due to contact with terrain stored in
195 // a dmEnvironment object
196 // Parameters: val - struct containing position and orientation of the rigid
197 // body wrt the inertial CS, and the spatial velocity
198 // with respect to the body's CS.
199 // Returns: f_contact - spatial contact force exerted on the body wrt to the
200 // body's CS
201 //----------------------------------------------------------------------------
computeForce(const dmABForKinStruct & val,SpatialVector f_contact)202 void dmContactModel::computeForce(const dmABForKinStruct &val,
203 SpatialVector f_contact)
204 {
205 register int j;
206 Float ground_elevation;
207
208 for (j = 0; j < 6; j++)
209 {
210 f_contact[j] = 0.0;
211 }
212 if (dmEnvironment::getEnvironment() == NULL)
213 {
214 return;
215 }
216
217 for (unsigned int i = 0; i < m_num_contact_points; i++)
218 {
219 for (j = 0; j < 3; j++) // compute the contact pos.
220 { // wrt ICS.
221
222 current_pos[j] = val.p_ICS[j] +
223 val.R_ICS[j][0]*m_contact_pos[i][0] +
224 val.R_ICS[j][1]*m_contact_pos[i][1] +
225 val.R_ICS[j][2]*m_contact_pos[i][2];
226 }
227
228 ground_elevation =
229 (dmEnvironment::getEnvironment())->getGroundElevation(current_pos,
230 normal);
231
232 if (current_pos[2] > ground_elevation) // No contact
233 {
234 // Reset flags.
235 if (m_contact_flag[i] == true)
236 {
237 m_contact_flag[i] = false;
238 m_boundary_flag = true;
239 }
240 m_sliding_flag[i] = false;
241
242 // Store last position
243 m_initial_contact_pos[i][0] = current_pos[0];
244 m_initial_contact_pos[i][1] = current_pos[1];
245 m_initial_contact_pos[i][2] = current_pos[2];
246 }
247 else // Contact!
248 {
249 if (!m_contact_flag[i] || m_reset_flag) // set initial contact Pos.
250 {
251 #ifdef DEBUG
252 cout << "Contact " << flush;
253 #endif
254 m_initial_contact_pos[i][0] = current_pos[0];
255 m_initial_contact_pos[i][1] = current_pos[1];
256 m_initial_contact_pos[i][2] = ground_elevation;
257 }
258
259 if (m_contact_flag[i] == false)
260 {
261 m_contact_flag[i] = true;
262 m_boundary_flag = true;
263 }
264
265 // End-effector linear velocity and "spring" displacement wrt ICS.
266 crossproduct(&val.v[0], m_contact_pos[i], vnC_pos);
267 vnC_pos[0] += val.v[3];
268 vnC_pos[1] += val.v[4];
269 vnC_pos[2] += val.v[5];
270
271 for (j = 0; j < 3; j++)
272 {
273 veC_pos[j] = val.R_ICS[j][0]*vnC_pos[0] +
274 val.R_ICS[j][1]*vnC_pos[1] +
275 val.R_ICS[j][2]*vnC_pos[2];
276 peC_pos[j] = current_pos[j] - m_initial_contact_pos[i][j];
277 }
278
279 // Magnitudes of normal components of velocity and delta position.
280 vtemp = veC_pos[0]*normal[0] +
281 veC_pos[1]*normal[1] +
282 veC_pos[2]*normal[2];
283 ptemp = peC_pos[0]*normal[0] +
284 peC_pos[1]*normal[1] +
285 peC_pos[2]*normal[2];
286
287 // Magnitude of Normal force.
288 fe_normal_mag = -(dmEnvironment::getEnvironment())->
289 getGroundNormalDamperConstant()*vtemp -
290 (dmEnvironment::getEnvironment())->
291 getGroundNormalSpringConstant()*ptemp;
292
293 if (fe_normal_mag < 0.0) // Invalid...trying to suck into ground.
294 {
295 fe[0] = fe[1] = fe[2] = 0.0;
296 }
297 else
298 {
299 for (j = 0; j < 3; j++)
300 {
301 fe_normal[j] = normal[j]*fe_normal_mag;
302 }
303
304 // Planar forces for sticking contact.
305 for (j = 0; j < 3; j++)
306 {
307 v_planar[j] = veC_pos[j] - normal[j]*vtemp;
308 p_planar[j] = peC_pos[j] - normal[j]*ptemp;
309 fe_planar[j] = -(dmEnvironment::getEnvironment())->
310 getGroundPlanarDamperConstant()*v_planar[j] -
311 (dmEnvironment::getEnvironment())->
312 getGroundPlanarSpringConstant()*p_planar[j];
313 }
314 fe_planar_mag = sqrt(fe_planar[0]*fe_planar[0] +
315 fe_planar[1]*fe_planar[1] +
316 fe_planar[2]*fe_planar[2]);
317
318 // Check to see whether it should start sticking.
319 if (m_sliding_flag[i])
320 {
321 // if sliding, is it going slow enough to stick.
322 //Float speedSquared = (v_planar[0]*v_planar[0] +
323 // v_planar[1]*v_planar[1] +
324 // v_planar[2]*v_planar[2]);
325
326 // Stick if speed less than a threshhold or
327 // static planar force is less than the kinetic one.
328 if ( //(speedSquared < 0.005) ||
329 (fe_planar_mag < (fe_normal_mag*
330 (dmEnvironment::getEnvironment())->
331 getGroundKineticFrictionCoeff())))
332 {
333 m_sliding_flag[i] = false;
334 m_boundary_flag = true;
335 }
336 }
337 // Check to see whether it should start sliding.
338 else
339 {
340 if (fe_planar_mag > (fe_normal_mag*
341 (dmEnvironment::getEnvironment())->
342 getGroundStaticFrictionCoeff()))
343 {
344 //slippage!
345 m_sliding_flag[i] = true;
346 m_boundary_flag = true;
347 }
348 }
349
350 // if sliding recompute a smaller planar force
351 if (m_sliding_flag[i])
352 {
353 temp = (fe_normal_mag/fe_planar_mag)*
354 (dmEnvironment::getEnvironment())->
355 getGroundKineticFrictionCoeff();
356 fe_planar[0] *= temp;
357 fe_planar[1] *= temp;
358 fe_planar[2] *= temp;
359
360 m_initial_contact_pos[i][0] = current_pos[0];
361 m_initial_contact_pos[i][1] = current_pos[1];
362 m_initial_contact_pos[i][2] = ground_elevation;
363 }
364
365 // Add normal and planar forces.
366 for (j = 0; j < 3; j++)
367 {
368 fe[j] = fe_normal[j] + fe_planar[j];
369 }
370 }
371
372 // Compute Contact Force at link CS
373 for (j = 0; j < 3; j++)
374 {
375 fn[j] = val.R_ICS[0][j]*fe[0] +
376 val.R_ICS[1][j]*fe[1] +
377 val.R_ICS[2][j]*fe[2];
378 }
379 crossproduct(m_contact_pos[i], fn, nn);
380
381 // Accumulate for multiple contact points.
382 for (j = 0; j < 3; j++)
383 {
384 f_contact[j] += nn[j];
385 f_contact[j + 3] += fn[j];
386 }
387 }
388 }
389 m_reset_flag = false;
390 }
391