1#define BIORBD_API_EXPORTS
2#include "RigidBody/SoftContactSphere.h"
4#include "Utils/String.h"
6using namespace BIORBD_NAMESPACE;
10 m_radius(std::make_shared<utils::Scalar>(-1)),
12 m_damping(std::make_shared<utils::Scalar>(-1)),
13 m_muStatic(std::make_shared<utils::Scalar>(0.8)),
22 m_radius(std::make_shared<utils::Scalar>(-1)),
24 m_damping(std::make_shared<utils::Scalar>(-1)),
25 m_muStatic(std::make_shared<utils::Scalar>(0.8)),
38 const utils::Scalar &x,
39 const utils::Scalar &y,
40 const utils::Scalar &z,
41 const utils::Scalar &
radius,
60 const utils::Scalar &
radius,
78 const utils::Scalar &x,
79 const utils::Scalar &y,
80 const utils::Scalar &z,
81 const utils::Scalar &
radius,
103 const utils::Scalar &
radius,
207 utils::Scalar normalVelocity = dx.dot(normal);
209 (*
m_radius * normal).cross(angularVelocity);
212 utils::Scalar delta = -((x - plane).dot(normal) - *
m_radius);
213 utils::Scalar deltaDot = -normalVelocity;
216 utils::Scalar eps(1e-16);
217 utils::Scalar bv(50);
218 utils::Scalar bd(300);
219 utils::Scalar fslope =
220 (0.5 + 0.5 * std::tanh(bd * delta) + eps) *
221 (0.5 + 0.5 * std::tanh(bv * (deltaDot + 2. / 3. / *
m_damping) + eps));
225 utils::Scalar deltaSquaredRooted(std::sqrt(delta * delta));
226 utils::Scalar forceFactor =
228 std::sqrt(deltaSquaredRooted * deltaSquaredRooted * deltaSquaredRooted);
231 utils::Scalar fHC = forceFactor * (1. + 1.5 * *
m_damping * deltaDot);
232 utils::Scalar normalForce = fHC * fslope;
234 utils::Scalar tangentVelocityNorm(
235 std::sqrt(tangentVelocity.squaredNorm() + 1e-5));
239 utils::Scalar forceFriction =
240 normalForce * *
m_muDynamic * std::tanh(4. * frictionVelocity) +
242 ((0.25 * frictionVelocity * frictionVelocity + 0.75) *
243 (0.25 * frictionVelocity * frictionVelocity + 0.75)) +
247 return normalForce * normal +
248 forceFriction * -tangentVelocity / tangentVelocityNorm;
255 utils::Scalar delta = -((x - plane).dot(normal) - *
m_radius);
257 return (x - plane) - delta * normal;
std::shared_ptr< std::pair< utils::Vector3d, utils::Vector3d > > m_contactPlane
SoftContactNode()
Construct a contact node.
A point attached to a contact node, generally speaking a skin marker.
utils::Scalar damping() const
Return the value of the damping.
std::shared_ptr< utils::Scalar > m_damping
The damping factor of the contact sphere.
SoftContactSphere DeepCopy() const
Deep copy of the contact node.
utils::Scalar muStatic() const
Return the value of the muStatic.
SoftContactSphere()
Construct a contact node.
utils::Scalar radius() const
Return the value of the radius.
void setTransitionVelocity(const utils::Scalar &transitionVelocity)
Set a new value for the transitionVelocity.
virtual utils::Vector3d applicationPoint(const utils::Vector3d &x) const
Get the application point relative to the plane.
std::shared_ptr< utils::Scalar > m_muViscous
Vicous coefficient of friction (mu).
std::shared_ptr< utils::Scalar > m_transitionVelocity
std::shared_ptr< utils::Scalar > m_muStatic
Static coefficient of friction (mu).
utils::Scalar muViscous() const
Return the value of the muViscous.
void setType()
Set the type of the contact node.
void setDamping(const utils::Scalar &damping)
Set a new value for the damping.
utils::Scalar transitionVelocity() const
Return the value of the transitionVelocity.
std::shared_ptr< utils::Scalar > m_radius
The radius of the contact sphere.
std::shared_ptr< utils::Scalar > m_muDynamic
Dynamic coefficient of friction (mu).
void setMuStatic(const utils::Scalar &muStatic)
Set a new value for the muStatic.
void setMuDynamic(const utils::Scalar &muDynamic)
Set a new value for the muDynamic.
void setRadius(const utils::Scalar &radius)
Set a new value for the radius.
utils::Scalar muDynamic() const
Return the value of the muDynamic.
virtual utils::Vector3d computeForce(const utils::Vector3d &x, const utils::Vector3d &dx, const utils::Vector3d &angularVelocity) const
Get the force in a spatial vector at the center of mass of the underlying segment.
void setStiffness(const utils::Scalar &stiffness)
Set a new value for the stiffness.
utils::Scalar stiffness() const
Return the value of the stiffness.
std::shared_ptr< utils::Scalar > m_stiffness
The stiffness of the contact sphere.
void setMuViscous(const utils::Scalar &muViscous)
Set a new value for the muViscous.
const String & name() const
Return the name of the node.
std::shared_ptr< NODE_TYPE > m_typeOfNode
The type of the node.
Wrapper around the std::string class with augmented functionality.
Wrapper around Eigen Vector3d and attach it to a parent.
Vector3d DeepCopy() const
Deep copy of a 3D vector.