/////////////////////////////// //State derivatives calculation void Kinematics::calcDerivatives() { // variable declaration geometry_msgs::Vector3 tempVect; double Reb[9]; // create transformation matrix from state orientation quaternion quat2rotmtx (parentObj->states.pose.orientation, Reb); // create position derivatives from earth velocity posDot = Reb*parentObj->states.velocity.linear; if (isnan(posDot)) {ROS_FATAL("NaN member in position derivative vector"); ros::shutdown();} // create body velocity derivatives from acceleration, angular rotation and body velocity linearAcc = (1.0/mass)*forceInput; if (isnan(linearAcc)) {ROS_FATAL("NaN member in linear acceleration vector"); ros::shutdown();} geometry_msgs::Vector3 corriolisAcc; vector3_cross(-parentObj->states.velocity.angular, parentObj->states.velocity.linear, &corriolisAcc); if (isnan(corriolisAcc)) {ROS_FATAL("NaN member in corriolis acceleration vector"); ros::shutdown();} speedDot = linearAcc + corriolisAcc; if (isnan(speedDot)) {ROS_FATAL("NaN member in velocity derivative vector"); ros::shutdown();} // create angular derivatives quaternion from angular rates quatDot.w = 1.0; quatDot.x = parentObj->states.velocity.angular.x*0.5*parentObj->dt; quatDot.y = parentObj->states.velocity.angular.y*0.5*parentObj->dt; quatDot.z = parentObj->states.velocity.angular.z*0.5*parentObj->dt; if (isnan(quatDot)) {ROS_FATAL("NaN member in quaternion derivative vector"); ros::shutdown();} // create angular rate derivatives from torque vector3_cross(parentObj->states.velocity.angular, J*parentObj->states.velocity.angular, &tempVect); tempVect = -tempVect+torqueInput; rateDot = Jinv*tempVect; if (isnan(rateDot)) {ROS_FATAL("NaN member in angular velocity derivative vector"); ros::shutdown();} }
// Force calculation function geometry_msgs::Vector3 Gravity::getForce() { double Reb[9]; quat2rotmtx(parentObj->states.pose.orientation,Reb); g = parentObj->environment.gravity; geometry_msgs::Vector3 gravVect; gravVect.z = parentObj->kinematics.mass*g; wrenchGrav.force = Reb/gravVect; // Printouts // std::cout << wrenchGrav.force.x << " "; return wrenchGrav.force; }
// 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; }