Cheetah Software  1.0
Public Member Functions | Protected Member Functions | Protected Attributes | Private Attributes | List of all members
ContactImpulse< T > Class Template Reference

#include <ContactImpulse.h>

+ Inheritance diagram for ContactImpulse< T >:
+ Collaboration diagram for ContactImpulse< T >:

Public Member Functions

 ContactImpulse (FloatingBaseModel< T > *model)
 
virtual ~ContactImpulse ()
 
virtual void UpdateExternalForces (T K, T D, T dt)
 
virtual void UpdateQdot (FBModelState< T > &state)
 
- Public Member Functions inherited from ContactConstraint< T >
 ContactConstraint (FloatingBaseModel< T > *model)
 
virtual ~ContactConstraint ()
 
void AddCollision (Collision< T > *collision)
 
const vectorAligned< Vec3< T > > & getContactPosList ()
 
const Vec3< T > & getGCForce (size_t idx)
 

Protected Member Functions

void _UpdateVelocity (DVec< T > &qdot)
 
void _UpdateQdotOneDirection (size_t idx, const vectorAligned< D3Mat< T > > &Jc_list, const T *lambda_list, const vectorAligned< DVec< T > > AinvB_list, const T *des_vel_list, const T *min_list, const T *max_list, DVec< T > &qdot)
 
- Protected Member Functions inherited from ContactConstraint< T >
void _groundContactWithOffset (T K, T D)
 
size_t _CheckContact ()
 

Protected Attributes

size_t _iter_lim
 
bool _b_debug
 
T _tol = 1.e-6
 
T _penetration_recover_ratio
 
size_t _nDof
 
size_t _iter_sum
 
- Protected Attributes inherited from ContactConstraint< T >
vectorAligned< Vec2< T > > deflectionRate
 
vectorAligned< Vec2< T > > _tangentialDeflections
 
size_t _nContact
 
size_t _nCollision
 
FloatingBaseModel< T > * _model
 
std::vector< Collision< T > * > _collision_list
 
std::vector< size_t > _idx_list
 
std::vector< T > _cp_resti_list
 
std::vector< T > _cp_mu_list
 
std::vector< T > _cp_penetration_list
 
vectorAligned< Vec3< T > > _cp_force_list
 
vectorAligned< Vec3< T > > _cp_local_force_list
 
vectorAligned< Vec3< T > > _cp_pos_list
 
vectorAligned< Mat3< T > > _cp_frame_list
 

Private Attributes

T _dt
 

Detailed Description

template<typename T>
class ContactImpulse< T >

Definition at line 15 of file ContactImpulse.h.

Constructor & Destructor Documentation

template<typename T >
ContactImpulse< T >::ContactImpulse ( FloatingBaseModel< T > *  model)
inline

Definition at line 17 of file ContactImpulse.h.

References ContactImpulse< T >::_b_debug, ContactImpulse< T >::_iter_lim, and ContactImpulse< T >::_nDof.

17  : ContactConstraint<T>(model) {
18  _iter_lim = 10;
19  _nDof = CC::_model->_nDof;
20  _b_debug = false;
21  }
template<typename T >
virtual ContactImpulse< T >::~ContactImpulse ( )
inlinevirtual

Definition at line 22 of file ContactImpulse.h.

22 {}

Member Function Documentation

template<typename T >
void ContactImpulse< T >::_UpdateQdotOneDirection ( size_t  idx,
const vectorAligned< D3Mat< T > > &  Jc_list,
const T *  lambda_list,
const vectorAligned< DVec< T > >  AinvB_list,
const T *  des_vel_list,
const T *  min_list,
const T *  max_list,
DVec< T > &  qdot 
)
protected

Definition at line 149 of file ContactImpulse.cpp.

152  {
153  T dforce, pre_force, cp_vel, dforce_sum;
154  for (size_t iter(0); iter < _iter_lim; ++iter) {
155  dforce_sum = 0;
156  for (size_t i(0); i < CC::_nContact; ++i) {
157  cp_vel = (Jc_list[i].block(idx, 0, 1, _nDof) * qdot)(0, 0);
158 
159  dforce = (des_vel_list[i] - cp_vel) * lambda_list[i];
160  pre_force = CC::_cp_local_force_list[i][idx];
161  CC::_cp_local_force_list[i][idx] =
162  std::max(min_list[i], std::min(pre_force + dforce, max_list[i]));
163 
164  dforce = CC::_cp_local_force_list[i][idx] - pre_force;
165 
166  qdot += (AinvB_list[i] * dforce);
167  dforce_sum += fabs(dforce);
168  }
169  if (dforce_sum < _tol) {
170  _iter_sum += iter;
171  // printf("dforce sum is small enough: %f\n", dforce_sum);
172  break;
173  }
174  }
175 }
template<typename T >
void ContactImpulse< T >::_UpdateVelocity ( DVec< T > &  qdot)
protected

Definition at line 39 of file ContactImpulse.cpp.

39  {
40  T* lambda_list_x = new T[CC::_nContact];
41  T* lambda_list_y = new T[CC::_nContact];
42  T* lambda_list_z = new T[CC::_nContact];
43 
44  T* des_vel_list_z = new T[CC::_nContact];
45  T* des_vel_list_tan = new T[CC::_nContact];
46 
47  T* min_list_z = new T[CC::_nContact];
48  T* max_list_z = new T[CC::_nContact];
49 
50  T* min_list_tan = new T[CC::_nContact];
51  T* max_list_tan = new T[CC::_nContact];
52 
53  vectorAligned<DVec<T> > AinvB_list_x(CC::_nContact);
54  vectorAligned<DVec<T> > AinvB_list_y(CC::_nContact);
55  vectorAligned<DVec<T> > AinvB_list_z(CC::_nContact);
56 
57  vectorAligned<D3Mat<T> > Jc_list(CC::_nContact);
58  // For check
59  vectorAligned<Vec3<T> > cp_vel_list(CC::_nContact);
60 
61  DVec<T> AinvB;
62  Vec3<T> cp_local_vel;
63  Vec3<T> direction;
64  CC::_model->contactJacobians();
65 
66  // Prepare Matrix and Vector
67  for (size_t i(0); i < CC::_nContact; ++i) {
68  // Lambda & Ainv*J'
69  lambda_list_x[i] = CC::_model->applyTestForce(
70  CC::_idx_list[i], CC::_cp_frame_list[i].template block<3, 1>(0, 0),
71  AinvB_list_x[i]);
72  lambda_list_x[i] = 1. / lambda_list_x[i];
73 
74  lambda_list_y[i] = CC::_model->applyTestForce(
75  CC::_idx_list[i], CC::_cp_frame_list[i].template block<3, 1>(0, 1),
76  AinvB_list_y[i]);
77  lambda_list_y[i] = 1. / lambda_list_y[i];
78 
79  lambda_list_z[i] = CC::_model->applyTestForce(
80  CC::_idx_list[i], CC::_cp_frame_list[i].template block<3, 1>(0, 2),
81  AinvB_list_z[i]);
82  lambda_list_z[i] = 1. / lambda_list_z[i];
83 
84  // Local Velocity
85  cp_local_vel =
86  CC::_cp_frame_list[i].transpose() * CC::_model->_vGC[CC::_idx_list[i]];
87 
88  // Desired Velocity
89  des_vel_list_tan[i] = 0.;
90  if (cp_local_vel[2] < 0.) {
91  des_vel_list_z[i] =
92  -CC::_cp_resti_list[i] * cp_local_vel[2] -
93  _penetration_recover_ratio * CC::_cp_penetration_list[i];
94  } else {
95  des_vel_list_z[i] =
96  std::max(cp_local_vel[2],
97  -_penetration_recover_ratio * CC::_cp_penetration_list[i]);
98  }
99 
100  // Contact Jacobian
101  Jc_list[i] =
102  CC::_cp_frame_list[i].transpose() * CC::_model->_Jc[CC::_idx_list[i]];
103 
104  min_list_z[i] = 0.;
105  max_list_z[i] = 1.e5;
106  }
107 
108  // Update Velocity & Find Impulse Force
109  for (size_t iter(0); iter < _iter_lim; ++iter) {
110  _iter_sum = 0;
111  // Normal (Z) *********************************************************
112  _UpdateQdotOneDirection(2, Jc_list, lambda_list_z, AinvB_list_z,
113  des_vel_list_z, min_list_z, max_list_z, qdot);
114 
115  // X ******************************************************************
116  for (size_t i(0); i < CC::_nContact; ++i) {
117  max_list_tan[i] = CC::_cp_mu_list[i] * CC::_cp_local_force_list[i][2];
118  min_list_tan[i] = -max_list_tan[i];
119  }
120  _UpdateQdotOneDirection(0, Jc_list, lambda_list_x, AinvB_list_x,
121  des_vel_list_tan, min_list_tan, max_list_tan, qdot);
122 
123  // Y ******************************************************************
124  _UpdateQdotOneDirection(1, Jc_list, lambda_list_y, AinvB_list_y,
125  des_vel_list_tan, min_list_tan, max_list_tan, qdot);
126 
127  if (_iter_sum < 5) {
128  // printf("converged: %lu \n", _iter_sum);
129  break;
130  }
131  _iter_sum = 0;
132  }
133 
134  delete[] lambda_list_x;
135  delete[] lambda_list_y;
136  delete[] lambda_list_z;
137 
138  delete[] des_vel_list_z;
139  delete[] des_vel_list_tan;
140 
141  delete[] min_list_z;
142  delete[] max_list_z;
143 
144  delete[] min_list_tan;
145  delete[] max_list_tan;
146 }
ContactImpulse::_UpdateQdotOneDirection
void _UpdateQdotOneDirection(size_t idx, const vectorAligned< D3Mat< T > > &Jc_list, const T *lambda_list, const vectorAligned< DVec< T > > AinvB_list, const T *des_vel_list, const T *min_list, const T *max_list, DVec< T > &qdot)
Definition: ContactImpulse.cpp:149
Vec3
typename Eigen::Matrix< T, 3, 1 > Vec3
Definition: cppTypes.h:26
vectorAligned
typename std::vector< T, Eigen::aligned_allocator< T >> vectorAligned
Definition: cppTypes.h:118
DVec
typename Eigen::Matrix< T, Eigen::Dynamic, 1 > DVec
Definition: cppTypes.h:102
template<typename T >
virtual void ContactImpulse< T >::UpdateExternalForces ( T  K,
T  D,
T  dt 
)
inlinevirtual

Used for spring damper based contact constraint method, therefore almost empty function in this class. Only used to update dt and penetration recovery ratio

Parameters
dt: time step (sec)
(member)penetration_recover_ratio : push contact point if there is penetration. This is fake input and currently set by 0.

Implements ContactConstraint< T >.

Definition at line 32 of file ContactImpulse.h.

References ContactImpulse< T >::_dt, ContactImpulse< T >::_penetration_recover_ratio, and ContactImpulse< T >::UpdateQdot().

32  {
33  (void)K;
34  (void)D;
35  // Set penetration recovery ration here
36  _penetration_recover_ratio = 0.0 / dt;
37  _dt = dt;
38  }

+ Here is the call graph for this function:

template<typename T >
void ContactImpulse< T >::UpdateQdot ( FBModelState< T > &  state)
virtual

Update velocities including floating base and joints.

Parameters
state: full state of a floating system full state
Returns
state : update velocity

Implements ContactConstraint< T >.

Definition at line 5 of file ContactImpulse.cpp.

References FBModelState< T >::bodyVelocity, and FBModelState< T >::qd.

5  {
6  CC::_nContact = CC::_CheckContact();
7  if (CC::_nContact > 0) {
8  DVec<T> qdot(_nDof);
9 
10  for (size_t i(0); i < 6; ++i) {
11  qdot[i] = state.bodyVelocity[i];
12  }
13  for (size_t i(0); i < _nDof - 6; ++i) {
14  qdot[i + 6] = state.qd[i];
15  }
16 
17  _UpdateVelocity(qdot);
18 
19  for (size_t i(0); i < 6; ++i) {
20  state.bodyVelocity[i] = qdot[i];
21  }
22  for (size_t i(0); i < _nDof - 6; ++i) {
23  state.qd[i] = qdot[i + 6];
24  }
25 
26  // Update contact force w.r.t. global frame
27  // This is not neccessary for dynamics, but
28  // the global contact forces are used for
29  // graphics
30  for (size_t i(0); i < CC::_nContact; ++i) {
31  // Divide by dt to convert impulses to forces
32  CC::_cp_force_list[CC::_idx_list[i]] =
33  CC::_cp_frame_list[i] * CC::_cp_local_force_list[i] / _dt;
34  }
35  }
36 }
ContactImpulse::_UpdateVelocity
void _UpdateVelocity(DVec< T > &qdot)
Definition: ContactImpulse.cpp:39
FBModelState::bodyVelocity
SVec< T > bodyVelocity
Definition: FloatingBaseModel.h:42
DVec
typename Eigen::Matrix< T, Eigen::Dynamic, 1 > DVec
Definition: cppTypes.h:102

+ Here is the caller graph for this function:

Member Data Documentation

template<typename T >
bool ContactImpulse< T >::_b_debug
protected

Definition at line 49 of file ContactImpulse.h.

template<typename T >
T ContactImpulse< T >::_dt
private

Definition at line 63 of file ContactImpulse.h.

template<typename T >
size_t ContactImpulse< T >::_iter_lim
protected

Definition at line 48 of file ContactImpulse.h.

template<typename T >
size_t ContactImpulse< T >::_iter_sum
protected

Definition at line 60 of file ContactImpulse.h.

template<typename T >
size_t ContactImpulse< T >::_nDof
protected

Definition at line 52 of file ContactImpulse.h.

template<typename T >
T ContactImpulse< T >::_penetration_recover_ratio
protected

Definition at line 51 of file ContactImpulse.h.

template<typename T >
T ContactImpulse< T >::_tol = 1.e-6
protected

Definition at line 50 of file ContactImpulse.h.

The documentation for this class was generated from the following files: