Commit aacfb2e7 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

[C++] Clean code of lie groups

parent e1891980
......@@ -81,11 +81,11 @@ namespace se3
static void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & lower,
const Eigen::MatrixBase<ConfigR_t> & upper,
const Eigen::MatrixBase<ConfigOut_t> & qout)
{
{
ConfigOut_t& out = const_cast< Eigen::MatrixBase<ConfigOut_t>& > (qout).derived();
LieGroup1::randomConfiguration(lower.template head<LieGroup1::NQ>(), upper.template head<LieGroup1::NQ>(), out.template head<LieGroup1::NQ>());
LieGroup2::randomConfiguration(lower.template tail<LieGroup2::NQ>(), upper.template tail<LieGroup2::NQ>(), out.template tail<LieGroup2::NQ>());
}
}
template <class ConfigL_t, class ConfigR_t>
static bool isSameConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
......
......@@ -309,10 +309,10 @@ namespace se3
protected:
/// Default constructor.
///
///
/// Prevent the construction of derived class.
LieGroupOperationBase() {}
/// Copy constructor
///
/// Prevent the copy of derived class.
......
......@@ -111,9 +111,9 @@ namespace se3
static void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & lower,
const Eigen::MatrixBase<ConfigR_t> & upper,
const Eigen::MatrixBase<ConfigOut_t> & qout)
{
{
R3crossSO3_t::randomConfiguration(lower, upper, qout);
}
}
template <class ConfigL_t, class ConfigR_t>
static bool isSameConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
......@@ -150,7 +150,7 @@ namespace se3
SE3 M1(SE3::Identity()); forwardKinematics(M1, q1);
Motion nu(log6(M0.inverse()*M1)); // TODO: optimize implementation
Tangent_t& out = const_cast< Eigen::MatrixBase<Tangent_t>& >(d).derived();
out.template head<2>() = nu.linear().head<2>();
out(2) = nu.angular()(2);
......@@ -168,7 +168,7 @@ namespace se3
const double& c0 = q(2), s0 = q(3);
Matrix22 R0;
R0 << c0, -s0, s0, c0;
const double& t = vs[2];
const double theta = std::fabs(t);
......@@ -235,9 +235,9 @@ namespace se3
static void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & lower,
const Eigen::MatrixBase<ConfigR_t> & upper,
const Eigen::MatrixBase<ConfigOut_t> & qout)
{
{
R2crossSO1_t::randomConfiguration(lower, upper, qout);
}
}
template <class ConfigL_t, class ConfigR_t>
static bool isSameConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
......@@ -255,7 +255,7 @@ namespace se3
const double& c_theta = q(2),
s_theta = q(3);
M.rotation().topLeftCorner<2,2>() << c_theta, -s_theta, s_theta, c_theta;
M.translation().head<2>() = q.template head<2>();
}
......
......@@ -109,9 +109,9 @@ namespace se3
static void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> &,
const Eigen::MatrixBase<ConfigR_t> &,
const Eigen::MatrixBase<ConfigOut_t> & qout)
{
{
random_impl(qout);
}
}
template <class ConfigL_t, class ConfigR_t>
static bool isSameConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
......@@ -121,7 +121,7 @@ namespace se3
ConstQuaternionMap_t quat1(q0.derived().data());
ConstQuaternionMap_t quat2(q1.derived().data());
return defineSameRotation(quat1,quat2);
return defineSameRotation(quat1,quat2,prec);
}
}; // struct SpecialOrthogonalOperation
......@@ -146,11 +146,8 @@ namespace se3
const Eigen::MatrixBase<ConfigR_t> & q1,
const Eigen::MatrixBase<Tangent_t> & d)
{
const Scalar & c0 = q0(0), & s0 = q0(1);
const Scalar & c1 = q1(0), & s1 = q1(1);
const_cast < Eigen::MatrixBase<Tangent_t>& > (d) [0]
= atan2 (s1*c0 - s0*c1, c0*c1 + s0*s1);
= atan2 (q0(0)*q1(1) - q0(1)*q1(0), q0.dot(q1));
}
template <class ConfigIn_t, class Velocity_t, class ConfigOut_t>
......@@ -180,28 +177,26 @@ namespace se3
const Eigen::MatrixBase<ConfigOut_t>& qout)
{
ConfigOut_t& out = (const_cast< Eigen::MatrixBase<ConfigOut_t>& >(qout)).derived();
const Scalar & c0 = q0(0), & s0 = q0(1);
const Scalar & c1 = q1(0), & s1 = q1(1);
assert ( (q0.norm() - 1) < 1e-8 && "initial configuration not normalized");
assert ( (q1.norm() - 1) < 1e-8 && "final configuration not normalized");
Scalar cosTheta = c0*c1 + s0*s1;
Scalar sinTheta = c0*s1 - s0*c1;
Scalar cosTheta = q0.dot(q1);
Scalar sinTheta = q0(0)*q1(1) - q0(1)*q1(0);
Scalar theta = atan2(sinTheta, cosTheta);
assert (fabs (sin (theta) - sinTheta) < 1e-8);
if (fabs (theta) > 1e-6 && fabs (theta) < PI - 1e-6)
{
out = (sin ((1-u)*theta)/sinTheta) * q0
+ (sin (u*theta)/sinTheta) * q1;
}
out = (sin ((1-u)*theta)/sinTheta) * q0
+ (sin ( u *theta)/sinTheta) * q1;
}
else if (fabs (theta) < 1e-6) // theta = 0
{
out = (1-u) * q0 + u * q1;
}
else // theta = +-PI
{
double theta0 = atan2 (s0, c0);
double theta0 = atan2 (q0(1), q0(0));
out << cos (theta0 + u * theta),
sin (theta0 + u * theta);
}
......@@ -223,9 +218,9 @@ namespace se3
static void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> &,
const Eigen::MatrixBase<ConfigR_t> &,
const Eigen::MatrixBase<ConfigOut_t> & qout)
{
{
random_impl(qout);
}
}
}; // struct SpecialOrthogonal1Operation
} // namespace se3
......
......@@ -71,11 +71,11 @@ namespace se3
static void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit,
const Eigen::MatrixBase<ConfigOut_t> & qout)
{
{
ConfigOut_t& res = const_cast< Eigen::MatrixBase<ConfigOut_t>& > (qout).derived();
for (int i = 0; i < NQ; ++i)
{
if(lower_pos_limit[i] == -std::numeric_limits<Scalar>::infinity() ||
if(lower_pos_limit[i] == -std::numeric_limits<Scalar>::infinity() ||
upper_pos_limit[i] == std::numeric_limits<Scalar>::infinity() )
{
std::ostringstream error;
......@@ -85,7 +85,7 @@ namespace se3
}
res[i] = lower_pos_limit[i] + (( upper_pos_limit[i] - lower_pos_limit[i]) * rand())/RAND_MAX;
}
}
}
}; // struct VectorSpaceOperation
} // namespace se3
......
......@@ -91,6 +91,12 @@ void test_joint_methods(const JointModelBase<JointModel> & jmodel, JointModelCom
BOOST_CHECK(jdata.Dinv.isApprox(jdata_composite.Dinv));
BOOST_CHECK(jdata.UDinv.isApprox(jdata_composite.UDinv));
/// TODO: Remove me. This is for testing purposes.
Eigen::VectorXd qq = q;
Eigen::VectorXd vv = v;
Eigen::VectorXd res(jmodel_composite.nq());
typename se3::IntegrateStep<se3::LieGroupTpl>::ArgsType args(qq, vv, res);
se3::IntegrateStep<se3::LieGroupTpl>::run(jmodel_composite, args);
}
struct TestJointComposite{
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment