Commit 358d9364 authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #364 from jmirabel/devel

Planar joint uses cosine and sine of the angle.
parents 1ae3842e 14dac0e4
......@@ -269,7 +269,7 @@ namespace se3
struct traits<JointPlanar>
{
enum {
NQ = 3,
NQ = 4,
NV = 3
};
typedef double Scalar;
......@@ -330,7 +330,8 @@ namespace se3
{
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigVector_t,V);
double c_theta,s_theta; SINCOS (q_joint(2), &s_theta, &c_theta);
const double& c_theta = q_joint(2),
s_theta = q_joint(3);
M.rotation().topLeftCorner<2,2>() << c_theta, -s_theta, s_theta, c_theta;
M.translation().head<2>() = q_joint.template head<2>();
......@@ -341,7 +342,8 @@ namespace se3
{
Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q = qs.segment<NQ>(idx_q ());
double c_theta,s_theta; SINCOS (q(2), &s_theta, &c_theta);
const double& c_theta = q(2),
s_theta = q(3);
data.M.rotation ().topLeftCorner <2,2> () << c_theta, -s_theta, s_theta, c_theta;
data.M.translation ().head <2> () = q.head<2> ();
......@@ -353,9 +355,10 @@ namespace se3
const Eigen::VectorXd & vs ) const
{
Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q = qs.segment<NQ> (idx_q ());
Eigen::VectorXd::ConstFixedSegmentReturnType<NV>::Type & q_dot = vs.segment<NQ> (idx_v ());
Eigen::VectorXd::ConstFixedSegmentReturnType<NV>::Type & q_dot = vs.segment<NV> (idx_v ());
double c_theta,s_theta; SINCOS (q(2), &s_theta, &c_theta);
const double& c_theta = q(2),
s_theta = q(3);
data.M.rotation ().topLeftCorner <2,2> () << c_theta, -s_theta, s_theta, c_theta;
data.M.translation ().head <2> () = q.head<2> ();
......@@ -393,14 +396,14 @@ namespace se3
typedef Eigen::Matrix<double, 2, 2> Matrix22;
typedef Eigen::Matrix<double, 2, 1> Vector2;
double c0,s0; SINCOS (q(2), &s0, &c0);
const double& c0 = q(2), s0 = q(3);
Matrix22 R0;
R0 << c0, -s0, s0, c0;
const double& t = q_dot[2];
const double theta = std::fabs(t);
ConfigVector_t res(q);
ConfigVector_t res;
if(theta > 1e-14)
{
// q_dot = [ x, y, t ]
......@@ -412,8 +415,8 @@ namespace se3
// else : S = -Sp
// S / t = Sp / |t|
// S * S = - I2
// R = I2 + ( 1 - ct) / |t| * S + ( 1 - st / t ) * S * S
// = ( 1 - ct) / |t| * S + st / t * I2
// R = I2 + ( 1 - ct) / |t| * S + ( 1 - st / |t| ) * S * S
// = ( 1 - ct) / |t| * S + st / |t| * I2
//
// Ru = exp3 (w)
// tu = R * v = (1 - ct) / |t| * S * v + st / t * v
......@@ -421,21 +424,25 @@ namespace se3
// M0 * Mu = ( R0 * Ru, R0 * tu + t0 )
Eigen::VectorXd::ConstFixedSegmentReturnType<2>::Type v = vs.segment<2>(idx_v());
double ct,st; SINCOS (theta, &st, &ct);
Eigen::Matrix<Scalar,2,1> cst;
SINCOS (t, &cst[1], &cst[0]);
const double inv_theta = 1/theta;
const double c_coeff = (1.-ct) * inv_theta;
const double s_coeff = st * inv_theta;
const double c_coeff = (1.-cst[0]) * inv_theta;
const double s_coeff = std::fabs(cst[1]) * inv_theta;
const Vector2 Sp_v (-v[1], v[0]);
if (t > 0) res.head<2>() += R0 * (s_coeff * v + c_coeff * Sp_v);
else res.head<2>() += R0 * (s_coeff * v - c_coeff * Sp_v);
res[2] += t;
if (t > 0) res.head<2>() = q.head<2>() + R0 * (s_coeff * v + c_coeff * Sp_v);
else res.head<2>() = q.head<2>() + R0 * (s_coeff * v - c_coeff * Sp_v);
res.tail<2>() = R0 * cst;
return res;
}
else
{
res.head<2>() += R0*q_dot.head<2>();
res[2] += t;
// cos(t) ~ 1 - t^2 / 2
// sin(t) ~ t
res.head<2>() = q.head<2>() + R0*q_dot.head<2>();
res(2) = c0 * 1 - s0 * t;
res(3) = s0 * 1 + c0 * t;
}
return res;
......@@ -459,14 +466,17 @@ namespace se3
ConfigVector_t random_impl() const
{
ConfigVector_t result(ConfigVector_t::Random());
ConfigVector_t result;
result.head<2>().setRandom();
const Scalar angle = PI * ((Scalar)(-1 + 2 * rand()) / (Scalar) RAND_MAX);
SINCOS(angle, &result[3], &result[2]);
return result;
}
ConfigVector_t randomConfiguration_impl(const ConfigVector_t & lower_pos_limit, const ConfigVector_t & upper_pos_limit ) const throw (std::runtime_error)
{
ConfigVector_t result;
for (int i = 0; i < result.size(); ++i)
for (int i = 0; i < 2; ++i)
{
if(lower_pos_limit[i] == -std::numeric_limits<double>::infinity() ||
upper_pos_limit[i] == std::numeric_limits<double>::infinity() )
......@@ -478,6 +488,8 @@ namespace se3
}
result[i] = lower_pos_limit[i] + ( upper_pos_limit[i] - lower_pos_limit[i]) * rand()/RAND_MAX;
}
const Scalar angle = PI * ((Scalar)(-1 + 2 * rand()) / (Scalar) RAND_MAX);
SINCOS(angle, &result[3], &result[2]);
return result;
}
......@@ -493,7 +505,7 @@ namespace se3
TangentVector_t res;
res.head<2>() = nu.linear().head<2>();
res(2) = q_1(2) - q_0(2);
res(2) = nu.angular()(2);
return res;
}
......@@ -505,7 +517,7 @@ namespace se3
ConfigVector_t neutralConfiguration_impl() const
{
ConfigVector_t q;
q << 0, 0, 0;
q << 0, 0, 1, 0;
return q;
}
......
......@@ -49,7 +49,7 @@ namespace se3
/// \brief Log: SO3 -> so3.
///
/// Pseudo-inverse of log from SO3 -> { v \in so3, ||v|| < 2pi }.
/// Pseudo-inverse of log from \f$ SO3 -> { v \in so3, ||v|| \le pi } \f$.
///
/// \param[in] R The rotation matrix.
///
......@@ -60,6 +60,9 @@ namespace se3
{
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(D, 3, 3);
Eigen::AngleAxis<typename D::Scalar> angleAxis(R);
assert(0 <= angleAxis.angle() && angleAxis.angle() <= 2 * M_PI);
if (angleAxis.angle() > M_PI)
return -(2*M_PI - angleAxis.angle()) * angleAxis.axis();
return angleAxis.axis() * angleAxis.angle();
}
......
......@@ -182,26 +182,26 @@ BOOST_AUTO_TEST_CASE ( simple_boxes )
std::cout << geomData;
Eigen::VectorXd q(model.nq);
q << 0, 0, 0,
0, 0, 0 ;
q << 0, 0, 1, 0,
0, 0, 1, 0 ;
se3::updateGeometryPlacements(model, data, geomModel, geomData, q);
BOOST_CHECK(computeCollision(geomModel,geomData,0) == true);
q << 2, 0, 0,
0, 0, 0 ;
q << 2, 0, 1, 0,
0, 0, 1, 0 ;
se3::updateGeometryPlacements(model, data, geomModel, geomData, q);
BOOST_CHECK(computeCollision(geomModel,geomData,0) == false);
q << 0.99, 0, 0,
0, 0, 0 ;
q << 0.99, 0, 1, 0,
0, 0, 1, 0 ;
se3::updateGeometryPlacements(model, data, geomModel, geomData, q);
BOOST_CHECK(computeCollision(geomModel,geomData,0) == true);
q << 1.01, 0, 0,
0, 0, 0 ;
q << 1.01, 0, 1, 0,
0, 0, 1, 0 ;
se3::updateGeometryPlacements(model, data, geomModel, geomData, q);
BOOST_CHECK(computeCollision(geomModel,geomData,0) == false);
......
......@@ -323,7 +323,7 @@ BOOST_AUTO_TEST_CASE ( neutral_configuration_test )
0,
0,0,0,
0,0,0,
0,0,0;
0,0,1,0;
BOOST_CHECK_MESSAGE(model.neutralConfiguration.isApprox(expected, 1e-12), "neutral configuration - wrong results");
......
......@@ -599,6 +599,7 @@ BOOST_AUTO_TEST_CASE (vsFreeFlyer)
using namespace se3;
typedef Eigen::Matrix <double, 3, 1> Vector3;
typedef Eigen::Matrix <double, 6, 1> Vector6;
typedef Eigen::Matrix <double, 4, 1> VectorPl;
typedef Eigen::Matrix <double, 7, 1> VectorFF;
typedef Eigen::Matrix <double, 3, 3> Matrix3;
......@@ -613,7 +614,7 @@ BOOST_AUTO_TEST_CASE (vsFreeFlyer)
Data dataPlanar(modelPlanar);
Data dataFreeFlyer(modelFreeflyer);
Eigen::VectorXd q = Eigen::VectorXd::Ones (modelPlanar.nq);q[2] = PI /2;
VectorPl q; q << 1, 1, 0, 1; // Angle is PI /2;
VectorFF qff; qff << 1, 1, 0, 0, 0, sqrt(2)/2, sqrt(2)/2 ;
Eigen::VectorXd v = Eigen::VectorXd::Ones (modelPlanar.nv);
Vector6 vff; vff << 1, 1, 0, 0, 0, 1;
......
Supports Markdown
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