Skip to content
Snippets Groups Projects
Commit 80385f76 authored by acpaquette's avatar acpaquette Committed by Jesse Mapel
Browse files

GetRotation Implementation (#72)

* Added implementetion to function based getRotation

* Added getAngularVelocity and associated tests.
Also added input test for getRotation
parent 6bb562a8
No related branches found
No related tags found
No related merge requests found
......@@ -34,10 +34,9 @@ namespace ale {
std::vector<double> times,
double time, interpolation interp);
std::vector<double> getRotation(std::string from, std::string to,
std::vector<double> coefficients, double time);
std::vector<double> getAngularVelocity(std::string from, std::string to,
std::vector<double> coefficients, double time);
std::vector<double> getRotation(std::vector<std::vector<double>> coeffs, double time);
std::vector<double> getAngularVelocity(std::vector<std::vector<double>> coeffs, double time);
double evaluatePolynomial(std::vector<double> coeffs, double time, int d);
double interpolate(std::vector<double> points, std::vector<double> times, double time, interpolation interp, int d);
std::string load(std::string filename);
......
......@@ -174,16 +174,52 @@ namespace ale {
}
// Rotation Function Functions
vector<double> getRotation(string from, string to,
vector<double> coefficients, double time) {
vector<double> coordinate = {0.0, 0.0, 0.0};
return coordinate;
std::vector<double> getRotation(vector<vector<double>> coeffs, double time) {
if (coeffs.size() != 3) {
throw invalid_argument("Invalid input coefficients, expected three vectors.");
}
vector<double> getAngularVelocity(string from, string to,
vector<double> coefficients, double time) {
vector<double> coordinate = {0.0, 0.0, 0.0};
return coordinate;
vector<double> rotation = {0.0, 0.0, 0.0};
rotation[0] = evaluatePolynomial(coeffs[0], time, 0); // X
rotation[1] = evaluatePolynomial(coeffs[1], time, 0); // Y
rotation[2] = evaluatePolynomial(coeffs[2], time, 0); // Z
Eigen::Quaterniond quat;
quat = Eigen::AngleAxisd(rotation[0] * M_PI / 180, Eigen::Vector3d::UnitZ())
* Eigen::AngleAxisd(rotation[1] * M_PI / 180, Eigen::Vector3d::UnitX())
* Eigen::AngleAxisd(rotation[2] * M_PI / 180, Eigen::Vector3d::UnitZ());
quat.normalize();
vector<double> rotationQ = {quat.w(), quat.x(), quat.y(), quat.z()};
return rotationQ;
}
vector<double> getAngularVelocity(vector<vector<double>> coeffs, double time) {
if (coeffs.size() != 3) {
throw invalid_argument("Invalid input coefficients, expected three vectors.");
}
double phi = evaluatePolynomial(coeffs[0], time, 0); // X
double theta = evaluatePolynomial(coeffs[1], time, 0); // Y
double psi = evaluatePolynomial(coeffs[2], time, 0); // Z
double phi_dt = evaluatePolynomial(coeffs[0], time, 1);
double theta_dt = evaluatePolynomial(coeffs[1], time, 1);
double psi_dt = evaluatePolynomial(coeffs[2], time, 1);
Eigen::Quaterniond quat1, quat2;
quat1 = Eigen::AngleAxisd(phi * M_PI / 180, Eigen::Vector3d::UnitZ());
quat2 = Eigen::AngleAxisd(theta * M_PI / 180, Eigen::Vector3d::UnitX());
Eigen::Vector3d velocity = phi_dt * Eigen::Vector3d::UnitZ();
velocity += theta_dt * (quat1 * Eigen::Vector3d::UnitX());
velocity += psi_dt * (quat1 * quat2 * Eigen::Vector3d::UnitZ());
return {velocity[0], velocity[1], velocity[2]};
}
// Polynomial evaluation helper function
......
......@@ -236,6 +236,48 @@ TEST(VelocityCoeffTest, InvalidInput) {
EXPECT_THROW(ale::getVelocity(invalid_coeffs_sizes, valid_time), invalid_argument);
}
TEST(RotationCoeffTest, ZeroOrderPolynomial) {
double time = 1.0;
vector<vector<double>> coeffs = {{90},
{0},
{0}};
vector<double> coordinate = ale::getRotation(coeffs, time);
ASSERT_EQ(4, coordinate.size());
EXPECT_DOUBLE_EQ(1 / sqrt(2), coordinate[0]);
EXPECT_DOUBLE_EQ(0, coordinate[1]);
EXPECT_DOUBLE_EQ(0, coordinate[2]);
EXPECT_DOUBLE_EQ(1 / sqrt(2), coordinate[3]);
}
TEST(RotationCoeffTest, InvalidInput) {
double time = 1.0;
vector<vector<double>> coeffs = {{90},
{0}};
EXPECT_THROW(ale::getRotation(coeffs, time), invalid_argument);
}
TEST(AngularVelocityCoeffTest, DefaultAngularExample) {
vector<vector<double>> coeffs = {{0, 90}, {0, 90}, {0, 90}};
double time = 1.0;
vector<double> av = ale::getAngularVelocity(coeffs, time);
ASSERT_EQ(3, av.size());
EXPECT_DOUBLE_EQ(90, av[0]);
EXPECT_DOUBLE_EQ(90, av[1]);
EXPECT_DOUBLE_EQ(90, av[2]);
}
TEST(AngularVelocityCoeffTest, InvalidInput) {
vector<vector<double>> coeffs = {{0, 90}, {0, 90}};
double time = 2.0;
EXPECT_THROW(ale::getAngularVelocity(coeffs, time), invalid_argument);
}
TEST(RotationInterpTest, ExampleGetRotation) {
// simple test, only checks if API hit correctly and output is normalized
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please to comment