Skip to content

Commit

Permalink
Fix the tests
Browse files Browse the repository at this point in the history
  • Loading branch information
@Kanma
Kanma committed Oct 17, 2024
1 parent 4a6047b commit 6b9d6e1
Show file tree
Hide file tree
Showing 3 changed files with 91 additions and 91 deletions.
14 changes: 7 additions & 7 deletions src/gafro/algebra/InnerProductCayleyTable.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,12 +32,12 @@ namespace gafro
template <int b1, int b2>
struct Inner
{
template <int i, int j, int lhs, int rhs, int sign>
template <int i, int j, int lhs, int rhs, int sign_>
constexpr static std::pair<int, int> next()
{
if constexpr (j == Metric::dim - 1 && i < Metric::dim - 1)
{
return recurse<i + 1, 0, lhs, rhs, sign>();
return recurse<i + 1, 0, lhs, rhs, sign_>();
}
else if constexpr (j == Metric::dim - 1 && i == Metric::dim - 1)
{
Expand All @@ -47,15 +47,15 @@ namespace gafro
return std::make_pair(lhs ^ rhs, 0);
}

return std::make_pair(lhs ^ rhs, sign);
return std::make_pair(lhs ^ rhs, sign_);
}
else
{
return recurse<i, j + 1, lhs, rhs, sign>();
return recurse<i, j + 1, lhs, rhs, sign_>();
}
}

template <int i, int j, int lhs, int rhs, int sign>
template <int i, int j, int lhs, int rhs, int sign_>
constexpr static std::pair<int, int> recurse()
{
if (b1 == 0 || b2 == 0)
Expand All @@ -65,7 +65,7 @@ namespace gafro

if constexpr ((Metric::template get<i, j>() != 0.0) && (((b1 & (1 << i)) != 0) && ((b2 & (1 << j)) != 0)))
{
constexpr int s = sign * Metric::template get<i, j>() *
constexpr int s = sign_ * Metric::template get<i, j>() *
math::pown<math::positive<Algebra<Metric>::BladeBitmap::template getRightShifts<(1 << i), lhs>()>() +
math::positive<Algebra<Metric>::BladeBitmap::template getLeftShifts<(1 << j), rhs>()>()>();

Expand All @@ -75,7 +75,7 @@ namespace gafro
return next<i, j, lhs2, rhs2, s>();
}

return next<i, j, lhs, rhs, sign>();
return next<i, j, lhs, rhs, sign_>();
}

constexpr static std::pair<int, double> result = recurse<0, 0, b1, b2, 1>();
Expand Down
2 changes: 1 addition & 1 deletion src/gafro/physics/Twist.hxx
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,7 +59,7 @@ namespace gafro
template <class T>
Wrench<T> Twist<T>::commute(const Wrench<T> &wrench) const
{
return Base::commute(wrench);
return Base::commute(wrench).template cast<Wrench<T>>();
}

template <class T>
Expand Down
166 changes: 83 additions & 83 deletions tests/robots/Manipulator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -220,59 +220,59 @@ TEST_CASE("Manipulator configuration 1", "[Manipulator]")
{
SECTION("Point")
{
// Eigen::Vector<double, 3> position({ 0.0, M_PI / 2.0, 0.0 });

// Point<double> primitive(1.0, 0.0, 0.0);
// SandwichProduct<Point<double>, Motor<double>>::Type::template Matrix<1, 3> jacobian =
// manipulator.getEEPrimitiveJacobian(position, primitive);

// REQUIRE(jacobian.getCoefficient(0, 0).get<blades::e1>() == Approx(-2.0));
// REQUIRE(jacobian.getCoefficient(0, 0).get<blades::e2>() == Approx(-1.0));
// REQUIRE(jacobian.getCoefficient(0, 0).get<blades::e3>() == Approx(0.0).margin(1e-6));
// REQUIRE(jacobian.getCoefficient(0, 0).get<blades::ei>() == Approx(-1.0));
// REQUIRE(jacobian.getCoefficient(0, 0).get<blades::e0>() == Approx(0.0).margin(1e-6));

// REQUIRE(jacobian.getCoefficient(0, 1).get<blades::e1>() == Approx(-1.0));
// REQUIRE(jacobian.getCoefficient(0, 1).get<blades::e2>() == Approx(-1.0));
// REQUIRE(jacobian.getCoefficient(0, 1).get<blades::e3>() == Approx(0.0).margin(1e-6));
// REQUIRE(jacobian.getCoefficient(0, 1).get<blades::ei>() == Approx(-2.0));
// REQUIRE(jacobian.getCoefficient(0, 1).get<blades::e0>() == Approx(0.0).margin(1e-6));

// REQUIRE(jacobian.getCoefficient(0, 2).get<blades::e1>() == Approx(-1.0));
// REQUIRE(jacobian.getCoefficient(0, 2).get<blades::e2>() == Approx(0.0).margin(1e-6));
// REQUIRE(jacobian.getCoefficient(0, 2).get<blades::e3>() == Approx(0.0).margin(1e-6));
// REQUIRE(jacobian.getCoefficient(0, 2).get<blades::ei>() == Approx(1.0));
// REQUIRE(jacobian.getCoefficient(0, 2).get<blades::e0>() == Approx(0.0).margin(1e-6));
Eigen::Vector<double, 3> position({ 0.0, M_PI / 2.0, 0.0 });

Point<double> primitive(1.0, 0.0, 0.0);
SandwichProduct<Point<double>, Motor<double>>::Type::template Matrix<1, 3> jacobian =
manipulator.getEEPrimitiveJacobian(position, primitive);

REQUIRE(jacobian.getCoefficient(0, 0).get<blades::e1>() == Approx(-2.0));
REQUIRE(jacobian.getCoefficient(0, 0).get<blades::e2>() == Approx(-1.0));
REQUIRE(jacobian.getCoefficient(0, 0).get<blades::e3>() == Approx(0.0).margin(1e-6));
REQUIRE(jacobian.getCoefficient(0, 0).get<blades::ei>() == Approx(-1.0));
REQUIRE(jacobian.getCoefficient(0, 0).get<blades::e0>() == Approx(0.0).margin(1e-6));

REQUIRE(jacobian.getCoefficient(0, 1).get<blades::e1>() == Approx(-1.0));
REQUIRE(jacobian.getCoefficient(0, 1).get<blades::e2>() == Approx(-1.0));
REQUIRE(jacobian.getCoefficient(0, 1).get<blades::e3>() == Approx(0.0).margin(1e-6));
REQUIRE(jacobian.getCoefficient(0, 1).get<blades::ei>() == Approx(-2.0));
REQUIRE(jacobian.getCoefficient(0, 1).get<blades::e0>() == Approx(0.0).margin(1e-6));

REQUIRE(jacobian.getCoefficient(0, 2).get<blades::e1>() == Approx(-1.0));
REQUIRE(jacobian.getCoefficient(0, 2).get<blades::e2>() == Approx(0.0).margin(1e-6));
REQUIRE(jacobian.getCoefficient(0, 2).get<blades::e3>() == Approx(0.0).margin(1e-6));
REQUIRE(jacobian.getCoefficient(0, 2).get<blades::ei>() == Approx(1.0));
REQUIRE(jacobian.getCoefficient(0, 2).get<blades::e0>() == Approx(0.0).margin(1e-6));
}

SECTION("Line")
{
// Eigen::Vector<double, 3> position({ 0.0, M_PI / 2.0, 0.0 });

// Line<double> primitive(Point<double>(0.0, 0.0, 0.0), Point<double>(1.0, 0.0, 0.0));
// SandwichProduct<Line<double>, Motor<double>>::Type::template Matrix<1, 3> jacobian =
// manipulator.getEEPrimitiveJacobian(position, primitive);

// REQUIRE(jacobian.getCoefficient(0, 0).get<blades::e12i>() == Approx(1.0));
// REQUIRE(jacobian.getCoefficient(0, 0).get<blades::e13i>() == Approx(0.0).margin(1e-6));
// REQUIRE(jacobian.getCoefficient(0, 0).get<blades::e23i>() == Approx(0.0).margin(1e-6));
// REQUIRE(jacobian.getCoefficient(0, 0).get<blades::e01i>() == Approx(-1.0));
// REQUIRE(jacobian.getCoefficient(0, 0).get<blades::e02i>() == Approx(0.0).margin(1e-6));
// REQUIRE(jacobian.getCoefficient(0, 0).get<blades::e03i>() == Approx(0.0).margin(1e-6));

// REQUIRE(jacobian.getCoefficient(0, 1).get<blades::e12i>() == Approx(2.0));
// REQUIRE(jacobian.getCoefficient(0, 1).get<blades::e13i>() == Approx(0.0).margin(1e-6));
// REQUIRE(jacobian.getCoefficient(0, 1).get<blades::e23i>() == Approx(0.0).margin(1e-6));
// REQUIRE(jacobian.getCoefficient(0, 1).get<blades::e01i>() == Approx(-1.0));
// REQUIRE(jacobian.getCoefficient(0, 1).get<blades::e02i>() == Approx(0.0).margin(1e-6));
// REQUIRE(jacobian.getCoefficient(0, 1).get<blades::e03i>() == Approx(0.0).margin(1e-6));

// REQUIRE(jacobian.getCoefficient(0, 2).get<blades::e12i>() == Approx(2.0));
// REQUIRE(jacobian.getCoefficient(0, 2).get<blades::e13i>() == Approx(0.0).margin(1e-6));
// REQUIRE(jacobian.getCoefficient(0, 2).get<blades::e23i>() == Approx(0.0).margin(1e-6));
// REQUIRE(jacobian.getCoefficient(0, 2).get<blades::e01i>() == Approx(-1.0));
// REQUIRE(jacobian.getCoefficient(0, 2).get<blades::e02i>() == Approx(0.0).margin(1e-6));
// REQUIRE(jacobian.getCoefficient(0, 2).get<blades::e03i>() == Approx(0.0).margin(1e-6));
Eigen::Vector<double, 3> position({ 0.0, M_PI / 2.0, 0.0 });

Line<double> primitive(Point<double>(0.0, 0.0, 0.0), Point<double>(1.0, 0.0, 0.0));
SandwichProduct<Line<double>, Motor<double>>::Type::template Matrix<1, 3> jacobian =
manipulator.getEEPrimitiveJacobian(position, primitive);

REQUIRE(jacobian.getCoefficient(0, 0).get<blades::e12i>() == Approx(1.0));
REQUIRE(jacobian.getCoefficient(0, 0).get<blades::e13i>() == Approx(0.0).margin(1e-6));
REQUIRE(jacobian.getCoefficient(0, 0).get<blades::e23i>() == Approx(0.0).margin(1e-6));
REQUIRE(jacobian.getCoefficient(0, 0).get<blades::e01i>() == Approx(-1.0));
REQUIRE(jacobian.getCoefficient(0, 0).get<blades::e02i>() == Approx(0.0).margin(1e-6));
REQUIRE(jacobian.getCoefficient(0, 0).get<blades::e03i>() == Approx(0.0).margin(1e-6));

REQUIRE(jacobian.getCoefficient(0, 1).get<blades::e12i>() == Approx(2.0));
REQUIRE(jacobian.getCoefficient(0, 1).get<blades::e13i>() == Approx(0.0).margin(1e-6));
REQUIRE(jacobian.getCoefficient(0, 1).get<blades::e23i>() == Approx(0.0).margin(1e-6));
REQUIRE(jacobian.getCoefficient(0, 1).get<blades::e01i>() == Approx(-1.0));
REQUIRE(jacobian.getCoefficient(0, 1).get<blades::e02i>() == Approx(0.0).margin(1e-6));
REQUIRE(jacobian.getCoefficient(0, 1).get<blades::e03i>() == Approx(0.0).margin(1e-6));

REQUIRE(jacobian.getCoefficient(0, 2).get<blades::e12i>() == Approx(2.0));
REQUIRE(jacobian.getCoefficient(0, 2).get<blades::e13i>() == Approx(0.0).margin(1e-6));
REQUIRE(jacobian.getCoefficient(0, 2).get<blades::e23i>() == Approx(0.0).margin(1e-6));
REQUIRE(jacobian.getCoefficient(0, 2).get<blades::e01i>() == Approx(-1.0));
REQUIRE(jacobian.getCoefficient(0, 2).get<blades::e02i>() == Approx(0.0).margin(1e-6));
REQUIRE(jacobian.getCoefficient(0, 2).get<blades::e03i>() == Approx(0.0).margin(1e-6));
}
}

Expand Down Expand Up @@ -623,44 +623,44 @@ TEST_CASE("Manipulator configuration 2", "[Manipulator]")
{
Eigen::Vector<double, 2> position({ 0.0, M_PI / 2.0 });

// Point<double> primitive(1.0, 0.0, 0.0);
// SandwichProduct<Point<double>, Motor<double>>::Type::template Matrix<1, 2> jacobian =
// manipulator.getEEPrimitiveJacobian(position, primitive);

// REQUIRE(jacobian.getCoefficient(0, 0).get<blades::e1>() == Approx(-2.0));
// REQUIRE(jacobian.getCoefficient(0, 0).get<blades::e2>() == Approx(-1.0));
// REQUIRE(jacobian.getCoefficient(0, 0).get<blades::e3>() == Approx(0.0));
// REQUIRE(jacobian.getCoefficient(0, 0).get<blades::ei>() == Approx(-1.0));
// REQUIRE(jacobian.getCoefficient(0, 0).get<blades::e0>() == Approx(0.0));

// REQUIRE(jacobian.getCoefficient(0, 1).get<blades::e1>() == Approx(-1.0));
// REQUIRE(jacobian.getCoefficient(0, 1).get<blades::e2>() == Approx(-1.0));
// REQUIRE(jacobian.getCoefficient(0, 1).get<blades::e3>() == Approx(0.0));
// REQUIRE(jacobian.getCoefficient(0, 1).get<blades::ei>() == Approx(-2.0));
// REQUIRE(jacobian.getCoefficient(0, 1).get<blades::e0>() == Approx(0.0));
Point<double> primitive(1.0, 0.0, 0.0);
SandwichProduct<Point<double>, Motor<double>>::Type::template Matrix<1, 2> jacobian =
manipulator.getEEPrimitiveJacobian(position, primitive);

REQUIRE(jacobian.getCoefficient(0, 0).get<blades::e1>() == Approx(-2.0));
REQUIRE(jacobian.getCoefficient(0, 0).get<blades::e2>() == Approx(-1.0));
REQUIRE(jacobian.getCoefficient(0, 0).get<blades::e3>() == Approx(0.0));
REQUIRE(jacobian.getCoefficient(0, 0).get<blades::ei>() == Approx(-1.0));
REQUIRE(jacobian.getCoefficient(0, 0).get<blades::e0>() == Approx(0.0));

REQUIRE(jacobian.getCoefficient(0, 1).get<blades::e1>() == Approx(-1.0));
REQUIRE(jacobian.getCoefficient(0, 1).get<blades::e2>() == Approx(-1.0));
REQUIRE(jacobian.getCoefficient(0, 1).get<blades::e3>() == Approx(0.0));
REQUIRE(jacobian.getCoefficient(0, 1).get<blades::ei>() == Approx(-2.0));
REQUIRE(jacobian.getCoefficient(0, 1).get<blades::e0>() == Approx(0.0));
}

SECTION("Line")
{
// Eigen::Vector<double, 2> position({ 0.0, M_PI / 2.0 });

// Line<double> primitive(Point<double>(0.0, 0.0, 0.0), Point<double>(1.0, 0.0, 0.0));
// SandwichProduct<Line<double>, Motor<double>>::Type::template Matrix<1, 2> jacobian =
// manipulator.getEEPrimitiveJacobian(position, primitive);

// REQUIRE(jacobian.getCoefficient(0, 0).get<blades::e12i>() == Approx(1.0));
// REQUIRE(jacobian.getCoefficient(0, 0).get<blades::e13i>() == Approx(0.0).margin(1e-6));
// REQUIRE(jacobian.getCoefficient(0, 0).get<blades::e23i>() == Approx(0.0).margin(1e-6));
// REQUIRE(jacobian.getCoefficient(0, 0).get<blades::e01i>() == Approx(-1.0));
// REQUIRE(jacobian.getCoefficient(0, 0).get<blades::e02i>() == Approx(0.0).margin(1e-6));
// REQUIRE(jacobian.getCoefficient(0, 0).get<blades::e03i>() == Approx(0.0).margin(1e-6));

// REQUIRE(jacobian.getCoefficient(0, 1).get<blades::e12i>() == Approx(2.0));
// REQUIRE(jacobian.getCoefficient(0, 1).get<blades::e13i>() == Approx(0.0).margin(1e-6));
// REQUIRE(jacobian.getCoefficient(0, 1).get<blades::e23i>() == Approx(0.0).margin(1e-6));
// REQUIRE(jacobian.getCoefficient(0, 1).get<blades::e01i>() == Approx(-1.0));
// REQUIRE(jacobian.getCoefficient(0, 1).get<blades::e02i>() == Approx(0.0).margin(1e-6));
// REQUIRE(jacobian.getCoefficient(0, 1).get<blades::e03i>() == Approx(0.0).margin(1e-6));
Eigen::Vector<double, 2> position({ 0.0, M_PI / 2.0 });

Line<double> primitive(Point<double>(0.0, 0.0, 0.0), Point<double>(1.0, 0.0, 0.0));
SandwichProduct<Line<double>, Motor<double>>::Type::template Matrix<1, 2> jacobian =
manipulator.getEEPrimitiveJacobian(position, primitive);

REQUIRE(jacobian.getCoefficient(0, 0).get<blades::e12i>() == Approx(1.0));
REQUIRE(jacobian.getCoefficient(0, 0).get<blades::e13i>() == Approx(0.0).margin(1e-6));
REQUIRE(jacobian.getCoefficient(0, 0).get<blades::e23i>() == Approx(0.0).margin(1e-6));
REQUIRE(jacobian.getCoefficient(0, 0).get<blades::e01i>() == Approx(-1.0));
REQUIRE(jacobian.getCoefficient(0, 0).get<blades::e02i>() == Approx(0.0).margin(1e-6));
REQUIRE(jacobian.getCoefficient(0, 0).get<blades::e03i>() == Approx(0.0).margin(1e-6));

REQUIRE(jacobian.getCoefficient(0, 1).get<blades::e12i>() == Approx(2.0));
REQUIRE(jacobian.getCoefficient(0, 1).get<blades::e13i>() == Approx(0.0).margin(1e-6));
REQUIRE(jacobian.getCoefficient(0, 1).get<blades::e23i>() == Approx(0.0).margin(1e-6));
REQUIRE(jacobian.getCoefficient(0, 1).get<blades::e01i>() == Approx(-1.0));
REQUIRE(jacobian.getCoefficient(0, 1).get<blades::e02i>() == Approx(0.0).margin(1e-6));
REQUIRE(jacobian.getCoefficient(0, 1).get<blades::e03i>() == Approx(0.0).margin(1e-6));
}
}

Expand Down

0 comments on commit 6b9d6e1

Please sign in to comment.