// Wrench calculation function
geometry_msgs::Vector3 PanosContactPoints::getForce()
{
double Reb[9];
double kFLong, kFLat, kFrictionE, kFrictionX, kFrictionY;
int i, j;
contact = false;
safe = true; // NaN protection flag
geometry_msgs::Vector3 Euler;
geometry_msgs::Quaternion quat = parentObj->states.pose.orientation; // Read vehicle orientation quaternion
quat2rotmtx(quat, Reb); // Construct the rotation matrix
Euler = quat2euler(quat);
geometry_msgs::Wrench tempE, totalE; /** @todo This is a TODO list example */
geometry_msgs::Vector3 dx,we,vpoint,Ve;
totalE.force.x = 0;
totalE.force.y = 0;
totalE.force.z = 0;
totalE.torque.x = 0;
totalE.torque.y = 0;
totalE.torque.z = 0;
// Get velocity in the inertial frame
Ve = parentObj->kinematics.posDot;
// Read vehicle coordinates
uavpos[0]=parentObj->states.pose.position.x;
uavpos[1]=parentObj->states.pose.position.y;
uavpos[2]=parentObj->states.pose.position.z;
// Rotate contact points coordinates from body frame to earth frame
multi_mtx_mtx_3Xn(Reb,pointCoords,cpi_up,contactPtsNo);
// Place the spring ends to the contact points positions
for (i=0;i<3;i++)
{
for (j=0;j<contactPtsNo;j++) {
cpi_up[contactPtsNo*i+j] += uavpos[i]; // Place upper spring end to contact point
if (i==2)
cpi_up[contactPtsNo*i+j] -= len; //Raise the upper spring end to obtain a visually pleasing result in RViz
cpi_down[contactPtsNo*i+j]=cpi_up[contactPtsNo*i+j]; // Place lower spring end to contact point
}
}
// Rotate body angular speeds to earth frame
we = Reb*parentObj->states.velocity.angular;
// Main calculations for each contact point
for (i=0;i<contactPtsNo;i++)
{
cpi_down[i+2*contactPtsNo]+=len; // Place lower spring end "len" below upper spring end
// Calculate force arm
dx.x = (cpi_up[i]-uavpos[0]);
dx.y = (cpi_up[i+contactPtsNo]-uavpos[1]);
dx.z = (cpi_up[i+2*contactPtsNo]-uavpos[2]);
if (cpi_down[i+2*contactPtsNo]>0) // if the lower spring end touches the ground
{
cpi_down[i+2*contactPtsNo]=0; // Force lower spring end to stay at ground level
contact=true;
spp[i]=len+cpi_up[i+2*contactPtsNo]; // Calculate current spring contraction
spd[i]=(spp[i]-sppprev[i])/parentObj->dt; // Calculate current spring contraction spreed
vector3_cross(we,dx, &vpoint); // Contact point Earth-frame speed due to vehicle rotation
vpoint = Ve+vpoint; // Add vehicle velocity to find total contact point velocity
// Calculate spring force. Acts only upon Earth z-axis
// tempE.force.z = -(kspring*spp[i] + mspring*spd[i]);
tempE.force.z = -(springIndex[i]*spp[i] + springIndex[i + contactPtsNo]*spd[i]);
// Make spring force negative or zero, as the spring lower end isn't fixed on the ground
if (tempE.force.z > 0)
tempE.force.z = 0;
/** @todo model full spring contraction (possibly with exponential force curve after full contraction?) */
int tempIndex = materialIndex[i];
// Get longitudinal and lateral friction coefficients for this surface
kFLong = frictForw[tempIndex];
kFLat = frictSide[tempIndex];
if ((inputBrake>0.9) && (i<3)) { // Apply breaks
kFLong = 1.0;
}
// Convert friction coefficients to the Earth frame
double trackAngle = Euler.z - atan2(vpoint.y, vpoint.x);
if (i==2) { // Apply steeering
trackAngle = trackAngle + inputSteer;
}
// Calculate the magnitude of the friction force in the Earth frame
double frictionLong = fabs(sqrt(kFLong*(kFLong*cos(trackAngle)*cos(trackAngle)+kFLat*sin(trackAngle)*sin(trackAngle)))*tempE.force.z);
frictionLong = std::max(frictionLong - 0.05*frictionLong/sqrt(vpoint.x*vpoint.x + vpoint.y*vpoint.y + 0.001), 0.0); //Aply static friction
frictionLong = frictionLong*cos(trackAngle);
double frictionLat = fabs(sqrt(kFLat*(kFLat*sin(trackAngle)*sin(trackAngle)+kFLong*cos(trackAngle)*cos(trackAngle)))*tempE.force.z);
frictionLat = std::max(frictionLat - 0.05*frictionLat/sqrt(vpoint.x*vpoint.x + vpoint.y*vpoint.y + 0.001), 0.0); //Aply static friction
frictionLat = frictionLat*sin(trackAngle);
tempE.force.x = -frictionLong*cos(Euler.z)-frictionLat*sin(Euler.z);
tempE.force.y = -frictionLong*sin(Euler.z)+frictionLat*cos(Euler.z);
// Add current contact point force contribution to the total
totalE.force = totalE.force + tempE.force;
// Calculate current contact point torque
vector3_cross(dx,tempE.force, &tempE.torque);
// Add current contact point torque contribution to the total
totalE.torque = totalE.torque + tempE.torque;
if (i==2) {
// std::cout << kFrictionLong << ',';
// std::cout << trackAngle << ',';
// std::cout << kFrictionE << ',';
// std::cout << tempE.force.x << ',';
// std::cout << tempE.force.y << ',';
// // std::cout << tempB.force.z << ',';
// std::cout << std::endl;
}
}
// If there is no ground contact
else {
// Set spring contraction to zero
spp[i] = 0;
spd[i] = 0;
}
sppprev[i] = spp[i];
}
// Chech for rogue NaN results
if (isnan(totalE.force.x) or isnan(totalE.force.y) or isnan(totalE.force.z)) {
safe = false; /** @todo remove the safe safety, it is not used*/
ROS_FATAL("State NAN in PanosGroundReactions force calculation!");
ros::shutdown();
}
if (isnan(totalE.torque.x) or isnan(totalE.torque.y) or isnan(totalE.torque.z)) {
safe = false;
ROS_FATAL("State NAN in PanosGroundReactions torque calculation!");
ros::shutdown();
}
// If there is a ground contact write the resulting wrench
if (contact and safe) {
wrenchGround.force= Reb/totalE.force;
wrenchGround.torque= Reb/totalE.torque;
}
// Otherwise there is no wrench created
else {
wrenchGround.force.x = 0;
wrenchGround.force.y = 0;
wrenchGround.force.z = 0;
wrenchGround.torque.x = 0;
wrenchGround.torque.y = 0;
wrenchGround.torque.z = 0;
}
return wrenchGround.force;
}
/**
* ahrs_step() takes the values from the IMU and produces the
* new estimated attitude.
*/
void
ahrs_step(
v_t angles_out,
f_t dt,
f_t p,
f_t q,
f_t r,
f_t ax,
f_t ay,
f_t az,
f_t heading
)
{
m_t C;
m_t A;
m_t AT;
v_t X_dot;
m_t temp;
v_t Xm;
v_t Xe;
/* Construct the quaternion omega matrix */
rotate2omega( A, p, q, r );
/* X_dot = AX */
mv_mult( X_dot, A, X, 4, 4 );
/* X += X_dot dt */
v_scale( X_dot, X_dot, dt, 4 );
v_add( X, X_dot, X, 4 );
v_normq( X, X );
/*
printf( "X =" );
Vprint( X, 4 );
printf( "\n" );
*/
/* AT = transpose(A) */
m_transpose( AT, A, 4, 4 );
/* P = A * P * AT + Q */
m_mult( temp, A, P, 4, 4, 4 );
m_mult( P, temp, AT, 4, 4, 4 );
m_add( P, Q, P, 4, 4 );
compute_c( C, X );
/* Compute the euler angles of the measured attitude */
accel2euler(
Xm,
ax,
ay,
az,
heading
);
/*
* Compute the euler angles of the estimated attitude
* Xe = quat2euler( X )
*/
quat2euler( Xe, X );
/* Kalman filter update */
kalman_update(
P,
R,
C,
X,
Xe,
Xm
);
/* Estimated attitude extraction */
quat2euler( angles_out, X );
}
