Contact Points

[aaronbest123]

I am trying to use this library to simulate gait and I need to find when the foot contacts the ground. I think I should be using the add_contacts_point! but I cannot seem to implement it correctly. This is the current implementation that I am trying is below, would really appreciate any advice on implementing contact:

contact_point = RigidBodyDynamics.Spatial.Point3D(LAnkle_2_Foot,SVector(0.0,0.0,0.0));
add_contact_point!(RFoot, contact_point)