void Racer::applySprings(float seconds)
{
// Applies springs and dampers using the helper function getForce()
hkVector4 force, point;
hkVector4 upVector = drawable->getYhkVector();
hkTransform transform = body->getTransform();
if (wheelFL->touchingGround)
{
force = getForce(&upVector, wheelFL->body, &attachFL, FRONT);
point.setTransformedPos(transform, attachFL);
body->applyForce(seconds, force, point);
}
if (wheelFR->touchingGround)
{
force = getForce(&upVector, wheelFR->body, &attachFR, FRONT);
point.setTransformedPos(transform, attachFR);
body->applyForce(seconds, force, point);
}
if (wheelRL->touchingGround)
{
force = getForce(&upVector, wheelRL->body, &attachRL, REAR);
point.setTransformedPos(transform, attachRL);
body->applyForce(seconds, force, point);
}
if (wheelRR->touchingGround)
{
force = getForce(&upVector, wheelRR->body, &attachRR, REAR);
point.setTransformedPos(transform, attachRR);
body->applyForce(seconds, force, point);
}
}
sf::Vector3f Force::produitVectoriel(Force vect)
{
sf::Vector3f vect3d;
vect3d.x = 0;
vect3d.y = 0;
vect3d.z = getForce().x*vect.getForce().y-getForce().y*vect.getForce().x;
return vect3d;
}
void StrainForce::solve(double time) {
if (nodes.size() < 1)
return;
if (nodes.size() < 2) {
if (auto sys = system.lock()) {
auto node = sys->getNode(nodes.front());
node->applyForce(appliedForce);
return;
}
}
if (!initialized) {
setup();
initialized = true;
}
if (auto sys = system.lock()) {
size_t size = nodes.size()-1;
auto mainNode = sys->getNode(mainId);
mainNode->applyForce(appliedForce);
for (size_t i = 0; i < size; ++i) {
auto node = sys->getNode(otherIds[i]);
auto right = mainNode->getForce() - mainNode->getMass() / node->getMass() * node->getForce();
gsl_vector_set(vecB[0], i, right.x);
gsl_vector_set(vecB[1], i, right.y);
gsl_vector_set(vecB[2], i, right.z);
}
for (int i = 0; i < 3; ++i)
gsl_linalg_LU_solve(matA, permutation, vecB[i], vecX[i]);
for (size_t i = 0; i < size; ++i) {
auto node = sys->getNode(otherIds[i]);
glm::dvec3 f = {
gsl_vector_get(vecX[0], i),
gsl_vector_get(vecX[1], i),
gsl_vector_get(vecX[2], i),
};
node->applyForce(f);
mainNode->applyForce(-f);
}
if(fixedAxe)
for (size_t i = 0; i < nodes.size(); ++i)
{
auto node = sys->getNode(nodes[i]);
node->FixDirection(appliedForce);
}
}
}
void Node::setForceColor(){
float xforce = getForce().x;
float yforce = getForce().y;
float zforce = getForce().z;
float approximate_maximum_speed = 2.0;
float blue = fabs( xforce/approximate_maximum_speed ); //fabs function is Float Absolute value
float red = fabs( yforce/approximate_maximum_speed );
float green = fabs( zforce/approximate_maximum_speed );
this -> setRGBA(red, green, blue, this -> color.a);
}
void Force::MobilityConstantForceImpl::
calcForce(const State& state, Vector_<SpatialVec>& bodyForces,
Vector_<Vec3>& particleForces, Vector& mobilityForces) const
{
const MobilizedBody& mb = m_matter.getMobilizedBody(m_mobodIx);
mb.applyOneMobilityForce(state, m_whichU, getForce(state), mobilityForces);
}
bool ChargeSystem::solve()
{
const double c_smallVelocitiesSq = 0.0001*0.0001;
const int c_maxIters = 1000000;
normalize();
int n = d_particles.size();
std::vector<PhasePoint> newPP( n );
int iter = 0;
do
{
for( int i = 0; i < n; ++i)
{
PhasePoint& pp = d_particles[i];
PhasePoint& newpp = newPP[i];
newpp.d_p = pp.d_p + d_t * pp.d_v;
makeUnit(newpp.d_p);
newpp.d_v = pp.d_v + d_t * getForce(i); // unit mass
newpp.d_v -= (newpp.d_p * newpp.d_v) * newpp.d_p; // dv * dp == 0 always
}
d_particles.swap(newPP);
++iter;
}
while( movement() > c_smallVelocitiesSq && iter < c_maxIters);
return iter < c_maxIters;
}
// void calcForceAllAndWriteBack(const Tpsys & psys,
void calcForceAllAndWriteBack(Tpsys & psys,
const Tdinfo & dinfo) {
setDomainInfoParticleMesh(dinfo);
setParticleParticleMesh(psys);
calcMeshForceOnly();
for(S32 i = 0; i < n_loc_tot_; i++)
psys[i].copyFromForceParticleMesh(getForce(psys[i].getPos()));
}
/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
* wave:
*
* If Waving is set, compute new geometry and redisplay.
*/
void wave(void)
{
if (Waving) {
getForce();
getVelocity();
getPosition();
glutPostRedisplay();
}
}
void main( int argc, char ** argv )
{
Environment env(argc, argv);
WALBERLA_UNUSED(env);
walberla::mpi::MPIManager::instance()->useWorldComm();
//init domain partitioning
auto forest = blockforest::createBlockForest( AABB(0,0,0,20,20,20), // simulation domain
Vector3<uint_t>(2,2,2), // blocks in each direction
Vector3<bool>(false, false, false) // periodicity
);
domain::BlockForestDomain domain(forest);
//init data structures
data::ParticleStorage ps(100);
//initialize particle
auto uid = createSphere(ps, domain);
WALBERLA_LOG_DEVEL_ON_ROOT("uid: " << uid);
//init kernels
mpi::ReduceProperty RP;
mpi::SyncNextNeighbors SNN;
//sync
SNN(ps, domain);
auto pIt = ps.find(uid);
if (pIt != ps.end())
{
pIt->getForceRef() += Vec3(real_c(walberla::mpi::MPIManager::instance()->rank()));
}
RP.operator()<ForceTorqueNotification>(ps);
if (walberla::mpi::MPIManager::instance()->rank() == 0)
{
WALBERLA_CHECK_FLOAT_EQUAL( pIt->getForce(), Vec3(real_t(28)) );
} else
{
WALBERLA_CHECK_FLOAT_EQUAL( pIt->getForce(), Vec3(real_t(walberla::mpi::MPIManager::instance()->rank())) );
}
}
void TriaxialStressController::controlExternalStress(int wall, Vector3r resultantForce, State* p, Real wall_max_vel) {
scene->forces.sync();
Real translation=normal[wall].dot(getForce(scene,wall_id[wall])-resultantForce);
const bool log=false;
if(log) LOG_DEBUG("wall="<<wall<<" actualForce="<<getForce(scene,wall_id[wall])<<", resultantForce="<<resultantForce<<", translation="<<translation);
if (translation!=0) {
if (stiffness[wall]!=0) {
translation /= stiffness[wall];
if(log) TRVAR2(translation,wall_max_vel*scene->dt)
translation = std::min( std::abs(translation), wall_max_vel*scene->dt ) * Mathr::Sign(translation);
}
else translation = wall_max_vel * Mathr::Sign(translation)*scene->dt;
}
previousTranslation[wall] = (1-stressDamping)*translation*normal[wall] + 0.8*previousTranslation[wall];// formula for "steady-flow" evolution with fluctuations
//Don't update position since Newton is doing that starting from bzr2612
//p->se3.position += previousTranslation[wall];
externalWork += previousTranslation[wall].dot(getForce(scene,wall_id[wall]));
//this is important is using VelocityBins. Otherwise the motion is never detected. Related to https://bugs.launchpad.net/yade/+bug/398089
p->vel=previousTranslation[wall]/scene->dt;
//if(log)TRVAR2(previousTranslation,p->se3.position);
}
void Racer::applySprings(float seconds)
{
// Applies springs and dampers using the helper function getForce()
hkVector4 force, point;
D3DXMATRIX transMat;
(body->getTransform()).get4x4ColumnMajor(transMat);
hkVector4 upVector = hkVector4(transMat._21, transMat._22, transMat._23);
if (wheelFL->touchingGround)
{
force = getForce(&upVector, wheelFL->body, &attachFL, FRONT);
point.setTransformedPos(body->getTransform(), attachFL);
body->applyForce(seconds, force, point);
}
if (wheelFR->touchingGround)
{
force = getForce(&upVector, wheelFR->body, &attachFR, FRONT);
point.setTransformedPos(body->getTransform(), attachFR);
body->applyForce(seconds, force, point);
}
if (wheelRL->touchingGround)
{
force = getForce(&upVector, wheelRL->body, &attachRL, REAR);
point.setTransformedPos(body->getTransform(), attachRL);
body->applyForce(seconds, force, point);
}
if (wheelRR->touchingGround)
{
force = getForce(&upVector, wheelRR->body, &attachRR, REAR);
point.setTransformedPos(body->getTransform(), attachRR);
body->applyForce(seconds, force, point);
}
}
void CPHCharacter::get_State(SPHNetState& state)
{
GetPosition(state.position);
m_body_interpolation.GetPosition(state.previous_position,0);
GetVelocity(state.linear_vel);
getForce(state.force);
state.angular_vel.set(0.f,0.f,0.f);
state.quaternion.identity();
state.previous_quaternion.identity();
state.torque.set(0.f,0.f,0.f);
// state.accel = GetAcceleration();
// state.max_velocity = GetMaximumVelocity();
if(!b_exist)
{
state.enabled=false;
return;
}
state.enabled=CPHObject::is_active();//!!dBodyIsEnabled(m_body);
}
void PhysShape::pack(BitStream* stream)
{
if (stream->writeFlag(isEnabled()))
{
bool active = stream->writeFlag(isActive());
if (!active)
{
bool toInactive = active!=mPrevActive;
mPrevActive = active;
if (!stream->writeFlag(toInactive))
return;
}
else
mPrevActive = true;
mathWrite(*stream, getPosition());
mathWrite(*stream, getRotation());
mathWrite(*stream, getForce());
mathWrite(*stream, getTorque());
mathWrite(*stream, getLinVelocity());
mathWrite(*stream, getAngVelocity());
}
}
void Neural_SB::update(float timeElapsed)
{
getInput();
m_pBrain->run( m_Input, m_Output );
b2Vec2 f = getForce();
f *= timeElapsed;
//std::cout << "\tForce : " << f.x << " " << f.y << std::endl;
b2Vec2 vel = m_pMonster->getBody()->GetLinearVelocity();
vel += f; //Mass is ignored
m_pMonster->getBody()->SetLinearVelocity(vel);
//Firing
float bullet_fire = (rand()%100)/100.0;
if( bullet_fire <= m_Output[ o_fire_bullet ] )
m_pMonster->fireBullet();
float shield_fire = (rand()%100)/100.0;
if( shield_fire <= m_Output[ o_fire_shield ] )
m_pMonster->fireShield();
}
void simulation(void) {
//if (true) {
// sewClothModel();
//} else if (stepCount == 200){
// for (int i = 0; i < PTCAMT; i ++) {
// clearForce(i);
// m3dCopyVector3(ClothLnk[i].next_position, ClothLnk[i].position);
// }
//} else {
for (int i = 0; i < PTCAMT; i ++) {
clearForce(i);
getForce(i);
//if ((i>=44 && i<=46)||(i>=54 && i<=56)||(i>=64 && i<=66))
// printf("[%d].force {%.1f, %.1f, %.1f} \n", i, ClothLnk[i].force[0], ClothLnk[i].force[1], ClothLnk[i].force[2]);
//printf(" ClothLnk[%d].force = {%f, %f, %f} \n", i, ClothLnk[i].force[0], ClothLnk[i].force[1], ClothLnk[i].force[2]);
}
//}
for (int i = 0; i < PTCAMT; i ++) {
nextPosition(i);
}
refreshPosition();
stepCount ++;
}
void TriaxialStressController::computeStressStrain()
{
scene->forces.sync();
State* p_bottom=Body::byId(wall_bottom_id,scene)->state.get();
State* p_top=Body::byId(wall_top_id,scene)->state.get();
State* p_left=Body::byId(wall_left_id,scene)->state.get();
State* p_right=Body::byId(wall_right_id,scene)->state.get();
State* p_front=Body::byId(wall_front_id,scene)->state.get();
State* p_back=Body::byId(wall_back_id,scene)->state.get();
height = p_top->se3.position.y() - p_bottom->se3.position.y() - thickness;
width = p_right->se3.position.x() - p_left->se3.position.x() - thickness;
depth = p_front->se3.position.z() - p_back->se3.position.z() - thickness;
meanStress = 0;
if (height0 == 0) height0 = height;
if (width0 == 0) width0 = width;
if (depth0 == 0) depth0 = depth;
strain[0] = log(width0/width);
strain[1] = log(height0/height);
strain[2] = log(depth0/depth);
volumetricStrain=strain[0]+strain[1]+strain[2];
Real invXSurface = 1.f/(height*depth);
Real invYSurface = 1.f/(width*depth);
Real invZSurface = 1.f/(width*height);
force[wall_bottom]=getForce(scene,wall_id[wall_bottom]);
stress[wall_bottom]=force[wall_bottom]*invYSurface;
force[wall_top]= getForce(scene,wall_id[wall_top]);
stress[wall_top]=force[wall_top]*invYSurface;
force[wall_left]= getForce(scene,wall_id[wall_left]);
stress[wall_left]=force[wall_left]*invXSurface;
force[wall_right]= getForce(scene,wall_id[wall_right]);
stress[wall_right]= force[wall_right]*invXSurface;
force[wall_front]= getForce(scene,wall_id[wall_front]);
stress[wall_front]=force[wall_front]*invZSurface;
force[wall_back]= getForce(scene,wall_id[wall_back]);
stress[wall_back]= force[wall_back]*invZSurface;
for (int i=0; i<6; i++) meanStress-=stress[i].dot(normal[i]);
meanStress/=6.;
}
/** \brief Loop for state polling in modes script and polling. Also sends command in script mode. */
void timer_cb(const ros::TimerEvent& ev)
{
// ==== Get state values by built-in commands ====
gripper_response info;
float acc = 0.0;
info.speed = 0.0;
if (g_mode_polling) {
const char * state = systemState();
if (!state)
return;
info.state_text = std::string(state);
info.position = getOpening();
acc = getAcceleration();
info.f_motor = getForce();//getGraspingForce();
} else if (g_mode_script) {
// ==== Call custom measure-and-move command ====
int res = 0;
if (!std::isnan(g_goal_position)) {
ROS_INFO("Position command: pos=%5.1f, speed=%5.1f", g_goal_position, g_speed);
res = script_measure_move(1, g_goal_position, g_speed, info);
} else if (!std::isnan(g_goal_speed)) {
ROS_INFO("Velocity command: speed=%5.1f", g_goal_speed);
res = script_measure_move(2, 0, g_goal_speed, info);
} else
res = script_measure_move(0, 0, 0, info);
if (!std::isnan(g_goal_position))
g_goal_position = NAN;
if (!std::isnan(g_goal_speed))
g_goal_speed = NAN;
if (!res) {
ROS_ERROR("Measure-and-move command failed");
return;
}
// ==== Moving msg ====
if (g_ismoving != info.ismoving) {
std_msgs::Bool moving_msg;
moving_msg.data = info.ismoving;
g_pub_moving.publish(moving_msg);
g_ismoving = info.ismoving;
}
} else
return;
// ==== Status msg ====
wsg_50_common::Status status_msg;
status_msg.status = info.state_text;
status_msg.width = info.position;
status_msg.speed = info.speed;
status_msg.acc = acc;
status_msg.force = info.f_motor;
status_msg.force_finger0 = info.f_finger0;
status_msg.force_finger1 = info.f_finger1;
g_pub_state.publish(status_msg);
// ==== Joint state msg ====
// \todo Use name of node for joint names
sensor_msgs::JointState joint_states;
joint_states.header.stamp = ros::Time::now();;
joint_states.header.frame_id = "summit_xl_wsg50_base_link";
joint_states.name.push_back("summit_xl_wsg50_finger_left_joint");
joint_states.name.push_back("summit_xl_wsg50_finger_right_joint");
joint_states.position.resize(2);
joint_states.position[0] = -info.position/2000.0;
joint_states.position[1] = info.position/2000.0;
joint_states.velocity.resize(2);
joint_states.velocity[0] = info.speed/1000.0;
joint_states.velocity[1] = info.speed/1000.0;
joint_states.effort.resize(2);
joint_states.effort[0] = info.f_motor;
joint_states.effort[1] = info.f_motor;
g_pub_joint.publish(joint_states);
// printf("Timer, last duration: %6.1f\n", ev.profile.last_duration.toSec() * 1000.0);
}
int main() {
MECHANIC_SETTINGS mechSettings;
spoke_data spokeData;
frequency_data frequency;
force_data forceData;
FILE *output = 0;
FILE *input = fopen("workspace/mechanics_inputs.dat", "rb");
if(!input) {
printf("No file found for mechanics input!\n");
return -1;
}
fread(&mechSettings, sizeof(MECHANIC_SETTINGS), 1, input);
fclose(input);
input = fopen("workspace/analyzer_output.dat", "rb");
if(!input) {
printf("No frequency input file found!\n");
return -2;
}
fread(&frequency, sizeof(frequency_data), 1, input);
fclose(input);
printf("Frequency: %lf\n", frequency.frequency);
spokeData.type = mechSettings.type;
spokeData.length = 0.08;
spokeData.elasticity = 196E9;
spokeData.density = 7849;
// correction factor for 14G276 2mm
//spokeData.correction = 1.460418701171875;
//spokeData.correction = 1.475;
//spokeData.correction = 1.450;
spokeData.correction = 1.4694;
printf("Mech settings:\n");
printf("Type: %d\n", mechSettings.type);
printf("Diameter: %f\n", mechSettings.round.diameter);
switch(spokeData.type) {
case MECHANIC_FLAT:
spokeData.majorAxis = mechSettings.flat.width / 1000;
spokeData.minorAxis = mechSettings.flat.height / 1000;
break;
case MECHANIC_ROUND:
spokeData.majorAxis = mechSettings.round.diameter / 1000;
break;
case MECHANIC_OVAL:
spokeData.majorAxis = mechSettings.oval.diameterX / 1000;
spokeData.minorAxis = mechSettings.oval.diameterY / 1000;
break;
case MECHANIC_ROUNDRECT:
spokeData.majorAxis = mechSettings.roundrect.width / 1000;
spokeData.minorAxis = mechSettings.roundrect.height / 1000;
break;
default:
return -3;
}
getForce(&spokeData, &frequency, &forceData);
output = fopen("workspace/result.dat", "wb");
if(!output) {
printf("Couldn't write the result!\n");
return -4;
}
fwrite(&forceData, sizeof(force_data), 1, output);
fclose(output);
return 0;
}
/** \brief Reads gripper responses in auto_update mode. The gripper pushes state messages in regular intervals. */
void read_thread(int interval_ms)
{
ROS_INFO("Thread started");
status_t status;
int res;
bool pub_state = false;
// Prepare messages
wsg_50_common::Status status_msg;
status_msg.status = "";
sensor_msgs::JointState joint_states;
// joint_states.header.frame_id = "wsg_50/palm_link";
joint_states.name.push_back("wsg_50/palm_joint_gripper_left");
joint_states.name.push_back("wsg_50/palm_joint_gripper_right");
joint_states.position.resize(2);
joint_states.velocity.resize(2);
joint_states.effort.resize(2);
// Request automatic updates (error checking is done below)
getOpening(interval_ms);
getSpeed(interval_ms);
getForce(interval_ms);
msg_t msg; msg.id = 0;
msg.data = 0;
msg.len = 0;
int cnt[3] = {0,0,0};
auto time_start = std::chrono::system_clock::now();
// Prepare FMF data reading
std::vector<float> finger_data; finger_data.push_back(0.0);
finger_data.push_back(0.0);
int index = 0;
bool waiting_fmf = false;
unsigned char vResult[4];
while (g_thread)
{
// Receive gripper response
msg_free(&msg);
res = msg_receive( &msg );
if (res < 0 ) //|| msg.len < 2)
{
ROS_ERROR("Gripper response failure");
continue;
}
float val = 0.0;
status = cmd_get_response_status(msg.data);
// Decode float for opening/speed/force
if (msg.id >= 0x43 && msg.id <= 0x45 && msg.len == 6)
{
if (status != E_SUCCESS)
{
ROS_ERROR("Gripper response failure for opening/speed/force\n");
continue;
}
val = convert(&msg.data[2]);
}
// Request force measurement
// if (use_fmf && !waiting_fmf)
// {
// waiting_fmf = true;
// getFingerData_serial(index);
// }
if ((info_fingers[index].type == 1) && !waiting_fmf)
{
waiting_fmf = true;
getFingerData(index, true);
}
// Handle response types
int motion = -1;
switch (msg.id)
{
/*** Opening ***/
case 0x43:
status_msg.width = val;
pub_state = true;
cnt[0]++;
break;
/*** Speed ***/
case 0x44:
status_msg.speed = val;
cnt[1]++;
break;
/*** Force ***/
case 0x45:
status_msg.force = val;
cnt[2]++;
break;
/*** Home ***/
case 0x20:
// Homing command; nothing to do
break;
case 0x22:
// Stop command; nothing to do
break;
/*** Move ***/
case 0x21:
// Move commands are sent from outside this thread
case 0x25:
// Grasping object
case 0x26:
// Releasing object
if (status == E_SUCCESS)
{
status_msg.status = std::string(status_to_str(status));
ROS_INFO("Position reached");
motion = 0;
}
else if (status == E_AXIS_BLOCKED)
{
status_msg.status = std::string(status_to_str(status));
ROS_INFO("Axis blocked");
motion = 0;
}
else if (status == E_CMD_PENDING)
{
status_msg.status = std::string(status_to_str(status));
ROS_INFO("Movement started");
motion = 1;
}
else if (status == E_ALREADY_RUNNING)
{
status_msg.status = std::string(status_to_str(status));
ROS_INFO("Movement error: already running");
}
else if (status == E_CMD_ABORTED)
{
status_msg.status = std::string(status_to_str(status));
ROS_INFO("Movement aborted");
motion = 0;
}
else
{
status_msg.status = std::string(status_to_str(status));
ROS_INFO("Movement error");
motion = 0;
}
break;
/*** Finger Data ***/
case 0x63:
if (status == E_SUCCESS)
{
vResult[0] = msg.data[2];
vResult[1] = msg.data[3];
vResult[2] = msg.data[4];
vResult[3] = msg.data[5];
finger_data[index] = convert(vResult);
}
else
finger_data[index] = 0.0;
waiting_fmf = false;
index = abs(index - 1);
break;
/*** Soft Limits ***/
case 0x34:
if (status == E_SUCCESS) ROS_INFO("New Soft Limits");
break;
case 0x35:
if (status == E_SUCCESS && msg.len == 10)
{
unsigned char limit_minus[4];
unsigned char limit_plus[4];
limit_minus[0] = msg.data[2];
limit_minus[1] = msg.data[3];
limit_minus[2] = msg.data[4];
limit_minus[3] = msg.data[5];
limit_plus[0] = msg.data[6];
limit_plus[1] = msg.data[7];
limit_plus[2] = msg.data[8];
limit_plus[3] = msg.data[9];
ROS_INFO("Soft limits: \nLIMIT MINUS %f\nLIMIT PLUS %f\n", convert(limit_minus), convert(limit_plus));
}
else ROS_INFO("Failed to read soft limits, len %d", msg.len);
break;
case 0x36:
if (status == E_SUCCESS) ROS_INFO("Soft Limits Cleared");
break;
default:
ROS_INFO("Received response 0x%02x (%2dB)\n", msg.id, msg.len);
}
// ***** PUBLISH motion message
if (motion == 0 || motion == 1)
{
std_msgs::Bool moving_msg;
moving_msg.data = motion;
g_pub_moving.publish(moving_msg);
g_ismoving = motion;
}
// ***** PUBLISH state message & joint message
if (pub_state)
{
pub_state = false;
if (info_fingers[0].type == 1)
status_msg.force_finger0 = finger_data[0];
else
status_msg.force_finger0 = status_msg.force/2;
if (info_fingers[1].type == 1)
status_msg.force_finger1 = finger_data[1];
else
status_msg.force_finger1 = status_msg.force/2;
g_pub_state.publish(status_msg);
joint_states.header.stamp = ros::Time::now();;
joint_states.position[0] = -status_msg.width/2000.0;
joint_states.position[1] = status_msg.width/2000.0;
joint_states.velocity[0] = status_msg.speed/1000.0;
joint_states.velocity[1] = status_msg.speed/1000.0;
joint_states.effort[0] = status_msg.force_finger0;
joint_states.effort[1] = status_msg.force_finger1;
g_pub_joint.publish(joint_states);
}
// Check # of received messages regularly
double rate_exp = 1000.0 / (double)interval_ms;
std::string names[3] = { "opening", "speed", "force" };
std::chrono::duration<float> t = std::chrono::system_clock::now() - time_start;
double t_ = t.count();
if (t_ > 5.0)
{
time_start = std::chrono::system_clock::now();
//printf("Infos for %5.1fHz, %5.1fHz, %5.1fHz\n", (double)cnt[0]/t_, (double)cnt[1]/t_, (double)cnt[2]/t_);
std::string info = "Rates for ";
for (int i=0; i<3; i++)
{
double rate_is = (double)cnt[i]/t_;
info += names[i] + ": " + std::to_string((int)rate_is) + "Hz, ";
if (rate_is == 0.0)
ROS_ERROR("Did not receive data for %s", names[i].c_str());
}
ROS_DEBUG_STREAM((info + " expected: " + std::to_string((int)rate_exp) + "Hz").c_str());
cnt[0] = 0; cnt[1] = 0; cnt[2] = 0;
}
}
// Disable automatic updates
getOpening(0);
getSpeed(0);
getForce(0);
ROS_INFO("Thread ended");
}
float Force::produitScalaire(Force vect)
{
return (getForce().x*vect.getForce().x+getForce().y*vect.getForce().y);
}
void TimeStep::operator()()
{
if( numberOfSubIterations_ == 1 )
{
forceEvaluationFunc_();
collisionResponse_.timestep( timeStepSize_ );
synchronizeFunc_();
}
else
{
// during the intermediate time steps of the collision response, the currently acting forces
// (interaction forces, gravitational force, ...) have to remain constant.
// Since they are reset after the call to collision response, they have to be stored explicitly before.
// Then they are set again after each intermediate step.
// generate map from all known bodies (process local) to total forces/torques
// this has to be done on a block-local basis, since the same body could reside on several blocks from this process
using BlockID_T = domain_decomposition::IBlockID::IDType;
std::map< BlockID_T, std::map< walberla::id_t, std::array< real_t, 6 > > > forceTorqueMap;
for( auto blockIt = blockStorage_->begin(); blockIt != blockStorage_->end(); ++blockIt )
{
BlockID_T blockID = blockIt->getId().getID();
auto& blockLocalForceTorqueMap = forceTorqueMap[blockID];
// iterate over local and remote bodies and store force/torque in map
for( auto bodyIt = pe::BodyIterator::begin(*blockIt, bodyStorageID_); bodyIt != pe::BodyIterator::end(); ++bodyIt )
{
auto & f = blockLocalForceTorqueMap[ bodyIt->getSystemID() ];
const auto & force = bodyIt->getForce();
const auto & torque = bodyIt->getTorque();
f = {{force[0], force[1], force[2], torque[0], torque[1], torque[2] }};
}
}
// perform pe time steps
const real_t subTimeStepSize = timeStepSize_ / real_c( numberOfSubIterations_ );
for( uint_t i = 0; i != numberOfSubIterations_; ++i )
{
// in the first iteration, forces are already set
if( i != 0 )
{
for( auto blockIt = blockStorage_->begin(); blockIt != blockStorage_->end(); ++blockIt )
{
BlockID_T blockID = blockIt->getId().getID();
auto& blockLocalForceTorqueMap = forceTorqueMap[blockID];
// re-set stored force/torque on bodies
for( auto bodyIt = pe::BodyIterator::begin(*blockIt, bodyStorageID_); bodyIt != pe::BodyIterator::end(); ++bodyIt )
{
const auto f = blockLocalForceTorqueMap.find( bodyIt->getSystemID() );
if( f != blockLocalForceTorqueMap.end() )
{
const auto & ftValues = f->second;
bodyIt->addForce ( ftValues[0], ftValues[1], ftValues[2] );
bodyIt->addTorque( ftValues[3], ftValues[4], ftValues[5] );
}
}
}
}
// evaluate forces (e.g. lubrication forces)
forceEvaluationFunc_();
collisionResponse_.timestep( subTimeStepSize );
synchronizeFunc_();
}
}
}
const Vector3r& getTorqueUnsynced(Body::id_t id){ return getForce(id);}
/** \brief Reads gripper responses in auto_update mode. The gripper pushes state messages in regular intervals. */
void read_thread(int interval_ms)
{
ROS_INFO("Thread started");
status_t status;
int res;
bool pub_state = false;
double rate_exp = 1000.0 / (double)interval_ms;
std::string names[3] = { "opening", "speed", "force" };
// Prepare messages
wsg_50_common::Status status_msg;
status_msg.status = "UNKNOWN";
sensor_msgs::JointState joint_states;
joint_states.header.frame_id = "wsg50_base_link";
joint_states.name.push_back("wsg50_finger_left_joint");
joint_states.name.push_back("wsg50_finger_right_joint");
joint_states.position.resize(2);
joint_states.velocity.resize(2);
joint_states.effort.resize(2);
// Request automatic updates (error checking is done below)
getOpening(interval_ms);
getSpeed(interval_ms);
getForce(interval_ms);
msg_t msg; msg.id = 0; msg.data = 0; msg.len = 0;
int cnt[3] = {0,0,0};
auto time_start = std::chrono::system_clock::now();
while (g_mode_periodic) {
// Receive gripper response
msg_free(&msg);
res = msg_receive( &msg );
if (res < 0 || msg.len < 2) {
ROS_ERROR("Gripper response failure");
continue;
}
float val = 0.0;
status = cmd_get_response_status(msg.data);
// Decode float for opening/speed/force
if (msg.id >= 0x43 && msg.id <= 0x45 && msg.len == 6) {
if (status != E_SUCCESS) {
ROS_ERROR("Gripper response failure for opening/speed/force\n");
continue;
}
val = convert(&msg.data[2]);
}
// Handle response types
int motion = -1;
switch (msg.id) {
/*** Opening ***/
case 0x43:
status_msg.width = val;
pub_state = true;
cnt[0]++;
break;
/*** Speed ***/
case 0x44:
status_msg.speed = val;
cnt[1]++;
break;
/*** Force ***/
case 0x45:
status_msg.force = val;
cnt[2]++;
break;
/*** Move ***/
// Move commands are sent from outside this thread
case 0x21:
if (status == E_SUCCESS) {
ROS_INFO("Position reached");
motion = 0;
} else if (status == E_AXIS_BLOCKED) {
ROS_INFO("Axis blocked");
motion = 0;
} else if (status == E_CMD_PENDING) {
ROS_INFO("Movement started");
motion = 1;
} else if (status == E_ALREADY_RUNNING) {
ROS_INFO("Movement error: already running");
} else if (status == E_CMD_ABORTED) {
ROS_INFO("Movement aborted");
motion = 0;
} else {
ROS_INFO("Movement error");
motion = 0;
}
break;
/*** Stop ***/
// Stop commands are sent from outside this thread
case 0x22:
// Stop command; nothing to do
break;
default:
ROS_INFO("Received unknown respone 0x%02x (%2dB)\n", msg.id, msg.len);
}
// ***** PUBLISH motion message
if (motion == 0 || motion == 1) {
std_msgs::Bool moving_msg;
moving_msg.data = motion;
g_pub_moving.publish(moving_msg);
g_ismoving = motion;
}
// ***** PUBLISH state message & joint message
if (pub_state) {
pub_state = false;
g_pub_state.publish(status_msg);
joint_states.header.stamp = ros::Time::now();;
joint_states.position[0] = -status_msg.width/2000.0;
joint_states.position[1] = status_msg.width/2000.0;
joint_states.velocity[0] = status_msg.speed/1000.0;
joint_states.velocity[1] = status_msg.speed/1000.0;
joint_states.effort[0] = status_msg.force;
joint_states.effort[1] = status_msg.force;
g_pub_joint.publish(joint_states);
}
// Check # of received messages regularly
std::chrono::duration<float> t = std::chrono::system_clock::now() - time_start;
double t_ = t.count();
if (t_ > 5.0) {
time_start = std::chrono::system_clock::now();
//printf("Infos for %5.1fHz, %5.1fHz, %5.1fHz\n", (double)cnt[0]/t_, (double)cnt[1]/t_, (double)cnt[2]/t_);
std::string info = "Rates for ";
for (int i=0; i<3; i++) {
double rate_is = (double)cnt[i]/t_;
info += names[i] + ": " + std::to_string((int)rate_is) + "Hz, ";
if (rate_is == 0.0)
ROS_ERROR("Did not receive data for %s", names[i].c_str());
}
ROS_DEBUG_STREAM((info + " expected: " + std::to_string((int)rate_exp) + "Hz").c_str());
cnt[0] = 0; cnt[1] = 0; cnt[2] = 0;
}
}
// Disable automatic updates
// TODO: The functions will receive an unexpected response
getOpening(0);
getSpeed(0);
getForce(0);
ROS_INFO("Thread ended");
}
Vector3 MassObj::getAccelaration (const MassObj& mo){
Vector3 f = getForce(mo);
return getAccelaration(f);
}
void PhysicsBody::changed(ConstFieldMaskArg whichField,
UInt32 origin,
BitVector details)
{
Inherited::changed(whichField, origin, details);
//Do not respond to changes that have a Sync origin
if(origin & ChangedOrigin::Sync)
{
return;
}
if(whichField & WorldFieldMask)
{
if(_BodyID != 0)
{
dBodyDestroy(_BodyID);
}
if(getWorld() != NULL)
{
_BodyID = dBodyCreate(getWorld()->getWorldID());
}
}
if( ((whichField & PositionFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetPosition(_BodyID, getPosition().x(),getPosition().y(),getPosition().z());
}
if( ((whichField & RotationFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dMatrix3 rotation;
Vec4f v1 = getRotation()[0];
Vec4f v2 = getRotation()[1];
Vec4f v3 = getRotation()[2];
rotation[0] = v1.x();
rotation[1] = v1.y();
rotation[2] = v1.z();
rotation[3] = 0;
rotation[4] = v2.x();
rotation[5] = v2.y();
rotation[6] = v2.z();
rotation[7] = 0;
rotation[8] = v3.x();
rotation[9] = v3.y();
rotation[10] = v3.z();
rotation[11] = 0;
dBodySetRotation(_BodyID, rotation);
}
if( ((whichField & QuaternionFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dQuaternion q;
q[0]=getQuaternion().w();
q[1]=getQuaternion().x();
q[2]=getQuaternion().y();
q[3]=getQuaternion().z();
dBodySetQuaternion(_BodyID,q);
}
if( ((whichField & LinearVelFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetLinearVel(_BodyID, getLinearVel().x(),getLinearVel().y(),getLinearVel().z());
}
if( ((whichField & AngularVelFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetAngularVel(_BodyID, getAngularVel().x(),getAngularVel().y(),getAngularVel().z());
}
if( ((whichField & ForceFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetForce(_BodyID, getForce().x(),getForce().y(),getForce().z());
}
if( ((whichField & TorqueFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetTorque(_BodyID, getTorque().x(),getTorque().y(),getTorque().z());
}
if( ((whichField & AutoDisableFlagFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetAutoDisableFlag(_BodyID, getAutoDisableFlag());
}
if( ((whichField & AutoDisableLinearThresholdFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetAutoDisableLinearThreshold(_BodyID, getAutoDisableLinearThreshold());
}
if( ((whichField & AutoDisableAngularThresholdFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetAutoDisableAngularThreshold(_BodyID, getAutoDisableAngularThreshold());
}
if( ((whichField & AutoDisableStepsFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetAutoDisableSteps(_BodyID, getAutoDisableSteps());
}
if( ((whichField & AutoDisableTimeFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetAutoDisableTime(_BodyID, getAutoDisableTime());
}
if( ((whichField & FiniteRotationModeFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetFiniteRotationMode(_BodyID, getFiniteRotationMode());
}
if( ((whichField & FiniteRotationModeFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetFiniteRotationMode(_BodyID, getFiniteRotationMode());
}
if( ((whichField & FiniteRotationAxisFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetFiniteRotationAxis(_BodyID, getFiniteRotationAxis().x(),getFiniteRotationAxis().y(),getFiniteRotationAxis().z());
}
if( ((whichField & GravityModeFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetGravityMode(_BodyID, getGravityMode());
}
if( ((whichField & LinearDampingFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetLinearDamping(_BodyID, getLinearDamping());
}
if( ((whichField & AngularDampingFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetAngularDamping(_BodyID, getAngularDamping());
}
if( ((whichField & LinearDampingThresholdFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetLinearDampingThreshold(_BodyID, getLinearDampingThreshold());
}
if( ((whichField & AngularDampingThresholdFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetAngularDampingThreshold(_BodyID, getAngularDampingThreshold());
}
if( ((whichField & MaxAngularSpeedFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
if(getMaxAngularSpeed() >= 0.0)
{
dBodySetMaxAngularSpeed(_BodyID, getMaxAngularSpeed());
}
else
{
dBodySetMaxAngularSpeed(_BodyID, dInfinity);
}
}
if( ((whichField & MassFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dMass TheMass;
dBodyGetMass(_BodyID, &TheMass);
TheMass.mass = getMass();
dBodySetMass(_BodyID, &TheMass);
}
if( ((whichField & MassCenterOfGravityFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
//dMass TheMass;
//dBodyGetMass(_BodyID, &TheMass);
////TheMass.c[0] = getMassCenterOfGravity().x();
////TheMass.c[1] = getMassCenterOfGravity().y();
////TheMass.c[2] = getMassCenterOfGravity().z();
//Vec4f v1 = getMassInertiaTensor()[0];
//Vec4f v2 = getMassInertiaTensor()[1];
//Vec4f v3 = getMassInertiaTensor()[2];
//TheMass.I[0] = v1.x();
//TheMass.I[1] = v1.y();
//TheMass.I[2] = v1.z();
//TheMass.I[3] = getMassCenterOfGravity().x();
//TheMass.I[4] = v2.x();
//TheMass.I[5] = v2.y();
//TheMass.I[6] = v2.z();
//TheMass.I[7] = getMassCenterOfGravity().y();
//TheMass.I[8] = v3.x();
//TheMass.I[9] = v3.y();
//TheMass.I[10] = v3.z();
//TheMass.I[11] = getMassCenterOfGravity().z();
//dBodySetMass(_BodyID, &TheMass);
}
if( ((whichField & MassInertiaTensorFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dMass TheMass;
dBodyGetMass(_BodyID, &TheMass);
Vec4f v1 = getMassInertiaTensor()[0];
Vec4f v2 = getMassInertiaTensor()[1];
Vec4f v3 = getMassInertiaTensor()[2];
TheMass.I[0] = v1.x();
TheMass.I[1] = v1.y();
TheMass.I[2] = v1.z();
TheMass.I[3] = 0;
TheMass.I[4] = v2.x();
TheMass.I[5] = v2.y();
TheMass.I[6] = v2.z();
TheMass.I[7] = 0;
TheMass.I[8] = v3.x();
TheMass.I[9] = v3.y();
TheMass.I[10] = v3.z();
TheMass.I[11] = 0;
dBodySetMass(_BodyID, &TheMass);
}
if( ((whichField & KinematicFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
if(getKinematic())
{
dBodySetKinematic(_BodyID);
}
else
{
dBodySetDynamic(_BodyID);
}
}
}
void checkSphSphLubricationForce()
{
const uint_t timestep (timeloop_->getCurrentTimeStep()+1);
// variables for output of total force - requires MPI-reduction
pe::Vec3 forceSphr1(0);
pe::Vec3 forceSphr2(0);
// temporary variables
real_t gap (0);
// calculate gap between the two spheres
for( auto blockIt = blocks_->begin(); blockIt != blocks_->end(); ++blockIt )
{
for( auto curSphereIt = pe::BodyIterator::begin<pe::Sphere>( *blockIt, bodyStorageID_); curSphereIt != pe::BodyIterator::end<pe::Sphere>(); ++curSphereIt )
{
pe::SphereID sphereI = ( curSphereIt.getBodyID() );
if ( sphereI->getID() == id1_ )
{
for( auto blockIt2 = blocks_->begin(); blockIt2 != blocks_->end(); ++blockIt2 )
{
for( auto oSphereIt = pe::BodyIterator::begin<pe::Sphere>( *blockIt2, bodyStorageID_); oSphereIt != pe::BodyIterator::end<pe::Sphere>(); ++oSphereIt )
{
pe::SphereID sphereJ = ( oSphereIt.getBodyID() );
if ( sphereJ->getID() == id2_ )
{
gap = pe::getSurfaceDistance( sphereI, sphereJ );
break;
}
}
}
break;
}
}
}
WALBERLA_MPI_SECTION()
{
mpi::reduceInplace( gap, mpi::MAX );
}
WALBERLA_ROOT_SECTION()
{
if (gap < real_comparison::Epsilon<real_t>::value )
{
// shadow copies have not been synced yet as the spheres are outside the overlap region
gap = walberla::math::Limits<real_t>::inf();
WALBERLA_LOG_INFO_ON_ROOT( "Spheres still too far apart to calculate gap!" );
} else {
WALBERLA_LOG_INFO_ON_ROOT( "Gap between sphere 1 and 2: " << gap );
}
}
// get force on spheres
for( auto blockIt = blocks_->begin(); blockIt != blocks_->end(); ++blockIt )
{
for( auto bodyIt = pe::BodyIterator::begin<pe::Sphere>( *blockIt, bodyStorageID_); bodyIt != pe::BodyIterator::end<pe::Sphere>(); ++bodyIt )
{
if( bodyIt->getID() == id1_ )
{
forceSphr1 += bodyIt->getForce();
} else if( bodyIt->getID() == id2_ )
{
forceSphr2 += bodyIt->getForce();
}
}
}
// MPI reduction of pe forces over all processes
WALBERLA_MPI_SECTION()
{
mpi::reduceInplace( forceSphr1[0], mpi::SUM );
mpi::reduceInplace( forceSphr1[1], mpi::SUM );
mpi::reduceInplace( forceSphr1[2], mpi::SUM );
mpi::reduceInplace( forceSphr2[0], mpi::SUM );
mpi::reduceInplace( forceSphr2[1], mpi::SUM );
mpi::reduceInplace( forceSphr2[2], mpi::SUM );
}
WALBERLA_LOG_INFO_ON_ROOT("Total force on sphere " << id1_ << " : " << forceSphr1);
WALBERLA_LOG_INFO_ON_ROOT("Total force on sphere " << id2_ << " : " << forceSphr2);
if ( print_ )
{
WALBERLA_ROOT_SECTION()
{
std::ofstream file1;
std::string filename1("Gap_LubricationForceBody1.txt");
file1.open( filename1.c_str(), std::ofstream::app );
file1.setf( std::ios::unitbuf );
file1.precision(15);
file1 << gap << " " << forceSphr1[0] << std::endl;
file1.close();
std::ofstream file2;
std::string filename2("Gap_LubricationForceBody2.txt");
file2.open( filename2.c_str(), std::ofstream::app );
file2.setf( std::ios::unitbuf );
file2.precision(15);
file2 << gap << " " << forceSphr2[0] << std::endl;
file2.close();
}
}
WALBERLA_ROOT_SECTION()
{
if ( timestep == uint_t(1000) )
{
// both force x-components should be the same only with inverted signs
WALBERLA_CHECK_FLOAT_EQUAL ( forceSphr2[0], -forceSphr1[0] );
// according to the formula from Ding & Aidun 2003
// F = 3/2 * M_PI * rho_L * nu_L * relative velocity of both spheres * r * r * 1/gap
// the correct analytically calculated value is 339.2920063998
// in this geometry setup the relative error is 0.1246489711 %
real_t analytical = real_c(3.0)/real_c(2.0) * walberla::math::M_PI * real_c(1.0) * nu_L_ * real_c(2.0) * real_c(vel_[0]) * radius_ * radius_ * real_c(1.0)/gap;
real_t relErr = std::fabs( analytical - forceSphr2[0] ) / analytical * real_c(100.0);
WALBERLA_CHECK_LESS( relErr, real_t(1) );
}
}
}
void checkSphWallLubricationForce()
{
const uint_t timestep (timeloop_->getCurrentTimeStep()+1);
// variables for output of total force - requires MPI-reduction
pe::Vec3 forceSphr1(0);
// temporary variables
real_t gap (0);
for( auto blockIt = blocks_->begin(); blockIt != blocks_->end(); ++blockIt )
{
for( auto curSphereIt = pe::BodyIterator::begin<pe::Sphere>( *blockIt, bodyStorageID_); curSphereIt != pe::BodyIterator::end<pe::Sphere>(); ++curSphereIt )
{
pe::SphereID sphereI = ( curSphereIt.getBodyID() );
if ( sphereI->getID() == id1_ )
{
for( auto globalBodyIt = globalBodyStorage_->begin(); globalBodyIt != globalBodyStorage_->end(); ++globalBodyIt)
{
if( globalBodyIt->getID() == id2_ )
{
pe::PlaneID planeJ = static_cast<pe::PlaneID>( globalBodyIt.getBodyID() );
gap = pe::getSurfaceDistance(sphereI, planeJ);
break;
}
}
break;
}
}
}
WALBERLA_MPI_SECTION()
{
mpi::reduceInplace( gap, mpi::MAX );
}
WALBERLA_ROOT_SECTION()
{
if (gap < real_comparison::Epsilon<real_t>::value )
{
gap = walberla::math::Limits<real_t>::inf();
WALBERLA_LOG_INFO_ON_ROOT( "Sphere still too far from wall to calculate gap!" );
} else {
WALBERLA_LOG_INFO_ON_ROOT( "Gap between sphere and wall: " << gap );
}
}
// get force on sphere
for( auto blockIt = blocks_->begin(); blockIt != blocks_->end(); ++blockIt )
{
for( auto bodyIt = pe::BodyIterator::begin<pe::Sphere>( *blockIt, bodyStorageID_); bodyIt != pe::BodyIterator::end<pe::Sphere>(); ++bodyIt )
{
if( bodyIt->getID() == id1_ )
{
forceSphr1 += bodyIt->getForce();
}
}
}
// MPI reduction of pe forces over all processes
WALBERLA_MPI_SECTION()
{
mpi::reduceInplace( forceSphr1[0], mpi::SUM );
mpi::reduceInplace( forceSphr1[1], mpi::SUM );
mpi::reduceInplace( forceSphr1[2], mpi::SUM );
}
WALBERLA_LOG_INFO_ON_ROOT("Total force on sphere " << id1_ << " : " << forceSphr1);
if ( print_ )
{
WALBERLA_ROOT_SECTION()
{
std::ofstream file1;
std::string filename1("Gap_LubricationForceBody1.txt");
file1.open( filename1.c_str(), std::ofstream::app );
file1.setf( std::ios::unitbuf );
file1.precision(15);
file1 << gap << " " << forceSphr1[0] << std::endl;
file1.close();
}
}
WALBERLA_ROOT_SECTION()
{
if ( timestep == uint_t(26399) )
{
// according to the formula from Ding & Aidun 2003
// F = 6 * M_PI * rho_L * nu_L * relative velocity of both bodies=relative velocity of the sphere * r * r * 1/gap
// the correct analytically calculated value is 339.292006996217
// in this geometry setup the relative error is 0.183515322065561 %
real_t analytical = real_c(6.0) * walberla::math::M_PI * real_c(1.0) * nu_L_ * real_c(-vel_[0]) * radius_ * radius_ * real_c(1.0)/gap;
real_t relErr = std::fabs( analytical - forceSphr1[0] ) / analytical * real_c(100.0);
WALBERLA_CHECK_LESS( relErr, real_t(1) );
}
}
}
float Force::getNorme()
{
float carreX = pow((getForce().x),2);
float carreY = pow((getForce().y),2);
return (float) sqrt(carreX+carreY);
}
