// Convert the resulting torque to the body axes
void Propulsion::rotateTorque()
{
tf::Vector3 tempVect(wrenchProp.torque.x, wrenchProp.torque.y, wrenchProp.torque.z);
tempVect = body_to_prop * tempVect;
wrenchProp.torque.x = tempVect.getX();
wrenchProp.torque.y = tempVect.getY();
wrenchProp.torque.z = tempVect.getZ();
// Convert the torque from the motor frame to the body frame
double ratio = parentObj->kinematics.J[0] / (parentObj->kinematics.J[0] + parentObj->kinematics.mass * CGOffset.x*CGOffset.x);
wrenchProp.torque.x = ratio * wrenchProp.torque.x;
ratio = parentObj->kinematics.J[4] / (parentObj->kinematics.J[4] + parentObj->kinematics.mass * CGOffset.y*CGOffset.y);
wrenchProp.torque.y = ratio * wrenchProp.torque.y;
ratio = parentObj->kinematics.J[8] / (parentObj->kinematics.J[8] + parentObj->kinematics.mass * CGOffset.z*CGOffset.z);
wrenchProp.torque.z = ratio * wrenchProp.torque.z;
// Add torque to to force misalignment with CG
// r x F, where r is the distance from CoG to CoL
// Will potentially add the following code in the future, to support shift of CoG mid-flight
// XmlRpc::XmlRpcValue list;
// int i;
// if(!ros::param::getCached("motor/CGOffset", list)) {ROS_FATAL("Invalid parameters for -/motor/CGOffset- in param server!"); ros::shutdown();}
// for (i = 0; i < list.size(); ++i) {
// ROS_ASSERT(list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
// CGOffset[i]=list[i];
// }
wrenchProp.torque.x = wrenchProp.torque.x + CGOffset.y*wrenchProp.force.z - CGOffset.z*wrenchProp.force.y;
wrenchProp.torque.y = wrenchProp.torque.y - CGOffset.x*wrenchProp.force.z + CGOffset.z*wrenchProp.force.x;
wrenchProp.torque.z = wrenchProp.torque.z - CGOffset.y*wrenchProp.force.x + CGOffset.x*wrenchProp.force.y;
}
// Convert the resulting force to the body axes
void Propulsion::rotateForce()
{
tf::Vector3 tempVect(wrenchProp.force.x, wrenchProp.force.y, wrenchProp.force.z);
tempVect = body_to_prop * tempVect; // I'm not sure why this works and not inverted
wrenchProp.force.x = tempVect.getX();
wrenchProp.force.y = tempVect.getY();
wrenchProp.force.z = tempVect.getZ();
}
void Cholesky<T>::operator()(SymmetricMatrix<T>& matrix, LinearVector<T>& vector) const
{
LowerMatrix<T> decomp(matrix);
LinearVector<T> vect(vector);
LinearVector<T> tempVect(vector.getSize());
LinearVector<T> solVect(vector.getSize());
T sum;
for(int k = 0; k < decomp.rowSize(); k++)
{
for(int i = 0; i < k; i++)
{
sum = 0;
for(int j = 0; j < i; j++)
sum += decomp(i,j)*decomp(k,j);
decomp(k,i) = (decomp(k,i)-sum)/decomp(i,i);
}
sum = 0;
for(int j = 0; j < k; j++)
sum += decomp(k,j)*decomp(k,j);
decomp(k,k) = sqrt(decomp(k,k)-sum);
}
std::cout << "The symmetric matrix can be broken down into the 2 following matrices through the use of Cholesky decomposition:" << std::endl;
UpperMatrix<T> transpose(decomp.transpose());
std::cout << decomp << std::endl;
std::cout << transpose << std::endl << std::endl;
for(int j = 0; j < decomp.numRows(); j++)
{
tempVect[j] = vect[j]/decomp(j,j);
for(int i = j+1; i < decomp.numRows(); i++)
vect[i] -= decomp(i,j)*tempVect[j];
}
for(int j = tempVect.getSize()-1; j >= 0; j--)
{
solVect[j] = tempVect[j]/transpose(j,j);
for(int i = 0; i < j; i++)
tempVect[i] -= transpose(i,j)*solVect[j];
}
std::cout << "The solution to the symmetric system is:"<<std::endl;
std::cout << solVect <<std::endl <<std::endl;
}
