Commit 3807421b authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

[C++] Planar joints uses (cos,sin) instead of angle.

parent 3917b447
......@@ -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 ]
......@@ -427,15 +430,19 @@ namespace se3
const double s_coeff = st * 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(2) = c0 * ct - s0 * st;
res(3) = s0 * ct + c0 * st;
return res;
}
else
{
res.head<2>() += R0*q_dot.head<2>();
res[2] += t;
res.head<2>() = q.head<2>() + R0*q_dot.head<2>();
// TODO: theta is small. Use a 1st order approx
double ct,st; SINCOS (theta, &st, &ct);
res(2) = c0 * ct - s0 * st;
res(3) = s0 * ct + c0 * st;
}
return res;
......
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