///////////////////////////////
//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();}
}
vector2 v3tov2(vector3 vec)
{
return (vector2) { .x = vec.x, .y = vec.y };
}
vector2 doProjectPoint(projector tr, vector3 p)
{
transformer_matrix tm = { .matrix = {{ 0.0f }} };
for (int i = 0; i<2; ++i) {
float t = vector3_dot(vector3_norm(
vector3_sub(tr.dir,
vector3_mul(tr.axis[1-i],
vector3_dot(tr.axis[1-i],
tr.dir)))),
vector3_cross(tr.axis[0], tr.axis[1]));
float k = sqrtf(1.0f - sqr(t))/t;
for(int j = 0; j<3; ++j) {
tm.matrix[i][j] = vector3_dot(axis[j], tr.axis[i]);
tm.matrix[i][j] += sqrtf(1.0f - sqr(tm.matrix[i][j])) * k *
vector3_dot(axis[j], vector3_cross(tr.axis[0], tr.axis[1]));
}
tm.matrix[i][3] = 0;
}
return v3tov2(doTransformMatrix(tm, vector3_sub(p, tr.base)));
}
/// \brief Returns the infinite line that is the intersection of \p plane and \p other.
inline DoubleLine plane3_intersect_plane3(const Plane3& plane, const Plane3& other)
{
DoubleLine line;
line.direction = vector3_cross(plane.normal(), other.normal());
switch(vector3_largest_absolute_component_index(line.direction))
{
case 0:
line.origin.x() = 0;
line.origin.y() = (-other.dist() * plane.normal().z() - -plane.dist() * other.normal().z()) / line.direction.x();
line.origin.z() = (-plane.dist() * other.normal().y() - -other.dist() * plane.normal().y()) / line.direction.x();
break;
case 1:
line.origin.x() = (-plane.dist() * other.normal().z() - -other.dist() * plane.normal().z()) / line.direction.y();
line.origin.y() = 0;
line.origin.z() = (-other.dist() * plane.normal().x() - -plane.dist() * other.normal().x()) / line.direction.y();
break;
case 2:
line.origin.x() = (-other.dist() * plane.normal().y() - -plane.dist() * other.normal().y()) / line.direction.z();
line.origin.y() = (-plane.dist() * other.normal().x() - -other.dist() * plane.normal().x()) / line.direction.z();
line.origin.z() = 0;
break;
default:
break;
}
return line;
}
int moto_ray_intersect_cylinder_dist(MotoRay *self,
float *dist, float a[3], float b[3], float radius)
{
const float z[3] = {0, 0, 1};
float c[3], tmp;
vector3_dif(c, a, b);
float height = vector3_length(c);
c[0] /= height;
c[1] /= height;
c[2] /= height;
float cross[3];
vector3_cross(cross, c, z);
float cos0 = vector3_dot(c, z);
float sin0 = acos(cos0);
float m[16], im[16], t[16], tmpm[16];
matrix44_rotate_from_axis_sincos(m, sin0, cos0, cross[0], cross[1], cross[2]);
matrix44_translate(t, a[0], b[0], c[0]);
matrix44_mult(tmpm, t, m);
matrix44_inverse(im, tmpm, m, tmp);
MotoRay r;
moto_ray_set_transformed(&r, self, im);
*dist = 1;
return 1;
}
/// \brief Keep the value of \p infinity as small as possible to improve precision in Winding_Clip.
void Winding_createInfinite(FixedWinding& winding, const Plane3& plane, double infinity)
{
double max = -infinity;
int x = -1;
for (int i=0 ; i<3; i++)
{
double d = fabs(plane.normal()[i]);
if (d > max)
{
x = i;
max = d;
}
}
if(x == -1)
{
globalErrorStream() << "invalid plane\n";
return;
}
DoubleVector3 vup = g_vector3_identity;
switch (x)
{
case 0:
case 1:
vup[2] = 1;
break;
case 2:
vup[0] = 1;
break;
}
vector3_add(vup, vector3_scaled(plane.normal(), -vector3_dot(vup, plane.normal())));
vector3_normalise(vup);
DoubleVector3 org = vector3_scaled(plane.normal(), plane.dist());
DoubleVector3 vright = vector3_cross(vup, plane.normal());
vector3_scale(vup, infinity);
vector3_scale(vright, infinity);
// project a really big axis aligned box onto the plane
DoubleLine r1, r2, r3, r4;
r1.origin = vector3_added(vector3_subtracted(org, vright), vup);
r1.direction = vector3_normalised(vright);
winding.push_back(FixedWindingVertex(r1.origin, r1, c_brush_maxFaces));
r2.origin = vector3_added(vector3_added(org, vright), vup);
r2.direction = vector3_normalised(vector3_negated(vup));
winding.push_back(FixedWindingVertex(r2.origin, r2, c_brush_maxFaces));
r3.origin = vector3_subtracted(vector3_added(org, vright), vup);
r3.direction = vector3_normalised(vector3_negated(vright));
winding.push_back(FixedWindingVertex(r3.origin, r3, c_brush_maxFaces));
r4.origin = vector3_subtracted(vector3_subtracted(org, vright), vup);
r4.direction = vector3_normalised(vup);
winding.push_back(FixedWindingVertex(r4.origin, r4, c_brush_maxFaces));
}
/* TODO: Needs to be optimized? */
int moto_ray_intersect_triangle(MotoRay *self,
MotoIntersection *intersection,
float A[3], float B[3], float C[3])
{
float tmp;
float normal[3], v1[3], v2[3];
vector3_dif(v1, B, A);
vector3_dif(v2, C, A);
vector3_cross(normal, v1, v2);
vector3_normalize(normal, tmp);
if( ! moto_ray_intersect_plane(self, intersection, A, normal))
return 0;
float n1[3], n2[3], n3[3], oA[3], oB[3], oC[3];
vector3_dif(oA, A, self->pos);
vector3_dif(oB, B, self->pos);
vector3_dif(oC, C, self->pos);
if(vector3_dot(self->dir, normal) < 0) /* faceforward */
{
vector3_cross(n1, oC, oA);
vector3_normalize(n1, tmp);
if(vector3_dot(n1, self->dir) > 0)
return 0;
vector3_cross(n2, oB, oC);
vector3_normalize(n2, tmp);
if(vector3_dot(n2, self->dir) > 0)
return 0;
vector3_cross(n3, oA, oB);
vector3_normalize(n3, tmp);
if(vector3_dot(n3, self->dir) > 0)
return 0;
}
else
{
vector3_cross(n1, oA, oC);
vector3_normalize(n1, tmp);
if(vector3_dot(n1, self->dir) > 0)
return 0;
vector3_cross(n2, oC, oB);
vector3_normalize(n2, tmp);
if(vector3_dot(n2, self->dir) > 0)
return 0;
vector3_cross(n3, oB, oA);
vector3_normalize(n3, tmp);
if(vector3_dot(n3, self->dir) > 0)
return 0;
}
return 1;
}
void vector3_test()
{
vector3 zero = {0,0,0};
vector3 one = {1,1,1};
vector3 y = {0,1,0};
vector3 half = {0.5,0.5,0.5};
vector3 a;
vector3_invert(&a, &one);
vector3_subtract(&a, &one, &a);
vector3_add(&a, &a, &one);
vector3_print(&a);
vector3_multiply(&a, &one, 0.5);
vector3_divide(&a, &a, 2);
vector3_print(&a);
vector3_reflect(&a, &one, &y);
vector3_print(&a);
vector3_scalar_sub(&a, &zero, -0.5);
vector3_scalar_add(&a, &a, 0.5);
vector3_print(&a);
vector3_cross(&a, &one, &y);
vector3_print(&a);
srand(3);
vector3_random(&a);
vector3_print(&a);
printf("%.2f %.2f\n",
vector3_dot(&half, &y), vector3_angle(&half, &y));
printf("%.2f %.2f\n",
vector3_distance(&one, &y), vector3_distancesq(&one, &y));
vector3_copy(&a, &one);
printf("%.2f %.2f\n",
vector3_length(&one), vector3_length(vector3_normalize(&a)) );
}
// 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;
}
#include "plfig.h"
line3 getIntersection(plfig f1, plfig f2)
{
const line3 line3z = { .src = (vector3) { .x = 0.0f, .y = 0.0f, .z=0.0f },
.dest = (vector3) { .x = 0.0f, .y = 0.0f, .z=0.0f } };
vector3 n1 = vector3_norm(vector3_cross(line3ToVector3(f1->line),
line3ToVector3(f1->next->line)));
vector3 n2 = vector3_norm(vector3_cross(line3ToVector3(f2->line),
line3ToVector3(f2->next->line)));
vector3 s = vector3_cross(n1, n2);
if (vector3_dot(s, s) < eps)
return line3z;
s = vector3_norm(s);
float mi = __builtin_inff(), ma = -__builtin_inff();
vector3 dir = vector3_cross(n1, s);
vector3 p = vector3_add(f1->line.src,
vector3_mul(dir,
vector3_dot(vector3_sub(f2->line.src,
f1->line.src),
n2) / vector3_dot(dir, n2)));
float pp = vector3_dot(s, p);
figure3 *f = f1;
while (f) {
ma = max(ma, vector3_dot(f->line.src, s) - pp);
mi = min(mi, vector3_dot(f->line.src, s) - pp);
f = f->next;
}
f = f2;
Vector3 testCross1(const Vector3& a, const Vector3& b)
{
return vector3_cross(a, b);
}
inline Vector3 triangle_cross (const BasicVector3<Element>& x, const BasicVector3<Element> y, const BasicVector3<
Element>& z)
{
return vector3_cross(y - x, z - x);
}
