/** Add a new sample to the path. If the sample time is less than the first time,
* it is added at the end. If it is greater than the last time, it is appended
* to the path. The sample is ignored if it has a time in between the first and
* last times of the path.
*/
void
CurvePlot::addSample(const CurvePlotSample& sample)
{
bool addToBack = false;
if (m_samples.empty() || sample.t > m_samples.back().t)
{
addToBack = true;
}
else if (sample.t < m_samples.front().t)
{
addToBack = false;
}
else
{
// Sample falls within range of current samples; discard it
return;
}
if (addToBack)
m_samples.push_back(sample);
else
m_samples.push_front(sample);
if (m_samples.size() > 1)
{
// Calculate a bounding radius for this segment. No point on the curve will
// be further from the start point than the bounding radius.
if (addToBack)
{
const CurvePlotSample& lastSample = m_samples[m_samples.size() - 2];
double dt = sample.t - lastSample.t;
Matrix4d coeff = cubicHermiteCoefficients(zeroExtend(lastSample.position),
zeroExtend(sample.position),
zeroExtend(lastSample.velocity * dt),
zeroExtend(sample.velocity * dt));
Vector4d extents = coeff.cwiseAbs() * Vector4d(0.0, 1.0, 1.0, 1.0);
m_samples[m_samples.size() - 1].boundingRadius = extents.norm();
}
else
{
const CurvePlotSample& nextSample = m_samples[1];
double dt = nextSample.t - sample.t;
Matrix4d coeff = cubicHermiteCoefficients(zeroExtend(sample.position),
zeroExtend(nextSample.position),
zeroExtend(sample.velocity * dt),
zeroExtend(nextSample.velocity * dt));
Vector4d extents = coeff.cwiseAbs() * Vector4d(0.0, 1.0, 1.0, 1.0);
m_samples[1].boundingRadius = extents.norm();
}
}
}
void rigidBodyDynamics::setState(VectorXd x, Vector4d q) {
_r = x.segment<3>(0);
_v = x.segment<3>(3);
_a = x.segment<3>(6);
_w = x.segment<3>(9);
_qref = q / q.norm();
}
void rigidBodyConstraintParseQuat(const mxArray* pm, Vector4d &quat)
{
if(!mxIsNumeric(pm))
{
mexErrMsgIdAndTxt("Drake:rigidBodyConstraintParseQuat:BadInputs","The input argument 1 should be a 4x1 double vector");
}
if(!(mxGetM(pm) == 4 && mxGetN(pm) == 1))
{
mexErrMsgIdAndTxt("Drake:rigidBodyConstraintParseQuat:BadInputs","The input argument 1 should be of size 4x1");
}
memcpy(quat.data(),mxGetPr(pm),sizeof(double)*4);
for(int i = 0;i<4;i++)
{
if((mxIsInf(quat(i)))||(mxIsNaN(quat(i))))
{
mexErrMsgIdAndTxt("Drake:rigidBodyConstraintParseQuat:BadInputs","The input argument 1 cannot have entry equal to NaN or Inf");
}
}
double quat_norm = quat.norm();
if(quat_norm==0.0)
{
mexErrMsgIdAndTxt("Drake:rigidBodyConstraintParseQuat:BadInputs","The Input argument 1 must be a nonzero vector");
}
quat = quat/quat_norm;
}
bool Rotation3::FromQuaternion(const Vector4d& quat) {
if (fabs(quat.norm() - 1.0) > 1e-6) {
std::cout << "Rotation3::FromQuaternion: invalid quaternion" << std::endl;
return false;
}
quat_ = quat;
return true;
}
Vector4d rigidBodyDynamics::mrp2quaternion(Vector3d mrp) const{
Vector4d dq;
dq << 8*mrp / (16 + mrp.transpose() * mrp), (16 - mrp.transpose() * mrp) / (16+mrp.transpose() * mrp);
// std::cout << "MRP: " << mrp.transpose() << std::endl;
// std::cout << "dq_prenorm: " << dq.transpose() << std::endl;
dq /=dq.norm();
// std::cout << "dq_postnorm: " << dq.transpose() << std::endl;
return dq;
}
Vector4d rigidBodyDynamics::quaternionFromRot(Matrix3d& R) const{
Vector4d q;
double div1, div2, div3, div4;
double numerical_limit = 1.0e-4;
if (abs(R.determinant()-1) > numerical_limit ) {
std::cerr << "R does not have a determinant of +1" << std::endl;
} else {
div1 = 0.5*sqrt(1+R(0,0)+R(1,1)+R(2,2));
div2 = 0.5*sqrt(1+R(0,0)-R(1,1)-R(2,2));
div3 = 0.5*sqrt(1-R(0,0)-R(1,1)+R(2,2));
div4 = 0.5*sqrt(1-R(0,0)+R(1,1)-R(2,2));
//if (div1 > div2 && div1 > div3 && div1 > div4) {
if (fabs(div1) > numerical_limit) {
q(3) = div1;
q(0) = 0.25*(R(1,2)-R(2,1))/q(3);
q(1) = 0.25*(R(2,0)-R(0,2))/q(3);
q(2) = 0.25*(R(0,1)-R(1,0))/q(3);
} else if (fabs(div2) > numerical_limit) {
//} else if (div2 > div1 && div2 > div3 && div2 > div4) {
q(0) = div2;
q(1) = 0.25*(R(0,1)+R(1,0))/q(0);
q(2) = 0.25*(R(0,2)+R(2,0))/q(0);
q(3) = 0.25*(R(1,2)+R(2,1))/q(0);
} else if (fabs(div3) > numerical_limit) {
//} else if (div3 > div1 && div3 > div2 && div3 > div4) {
q(2) = div3;
q(0) = 0.25*(R(0,2)+R(2,0))/q(2);
q(1) = 0.25*(R(1,2)+R(2,1))/q(2);
q(3) = 0.25*(R(0,1)-R(1,0))/q(2);
//} else {
} else if (fabs(div4) > numerical_limit) {
q(1) = div4;
q(0) = 0.25*(R(0,1)+R(1,0))/q(1);
q(2) = 0.25*(R(1,2)+R(2,1))/q(1);
q(3) = 0.25*(R(2,0)-R(0,2))/q(1);
} else {
std::cerr << "quaternionFromRot didn't convert: [" << div1 << ", " << div2 << ", " << div3 << ", " << div4 << std::endl;
std::cerr << "Rotation Matrix: " << R << std::endl;
}
}
/*
if (q(3) < 0) {
q *= -1;
}
*/
q /=q.norm();
return q;
}
Vector4d rigidBodyDynamics::quaternionMultiplication(Vector4d& q1, Vector4d& q2) const {
//q1 \mult q2
Matrix4d qm;
Vector4d result;
qm << q1(3), q1(2), -q1(1), q1(0),
-q1(2), q1(3), q1(0), q1(1),
q1(1), -q1(0), q1(3), q1(2),
-q1(0), -q1(1), -q1(2), q1(3);
result = qm*q2;
result /= result.norm();
return result;
}
Eigen::Matrix<typename Derived::Scalar, 4, 1> rpy2quat(const Eigen::MatrixBase<Derived>& rpy)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Eigen::MatrixBase<Derived>, 3);
auto rpy_2 = (rpy / 2.0).array();
auto s = rpy_2.sin();
auto c = rpy_2.cos();
Vector4d q;
q << c(0)*c(1)*c(2) + s(0)*s(1)*s(2),
s(0)*c(1)*c(2) - c(0)*s(1)*s(2),
c(0)*s(1)*c(2) + s(0)*c(1)*s(2),
c(0)*c(1)*s(2) - s(0)*s(1)*c(2);
q /= q.norm() + std::numeric_limits<typename Derived::Scalar>::epsilon();
return q;
}
bool scso3bracket_tests()
{
bool failed = false;
vector<Vector4d> vecs;
Vector4d tmp;
tmp << 0,0,0,0;
vecs.push_back(tmp);
tmp << 1,0,0,0;
vecs.push_back(tmp);
tmp << 1,0,0,0.1;
vecs.push_back(tmp);
tmp << 0,1,0,0.1;
vecs.push_back(tmp);
tmp << 0,0,1,-0.1;
vecs.push_back(tmp);
tmp << -1,1,0,-0.1;
vecs.push_back(tmp);
tmp << 20,-1,0,2;
vecs.push_back(tmp);
for (unsigned int i=0; i<vecs.size(); ++i)
{
Vector4d resDiff = vecs[i] - ScSO3::vee(ScSO3::hat(vecs[i]));
if (resDiff.norm()>SMALL_EPS)
{
cerr << "Hat-vee Test" << endl;
cerr << "Test case: " << i << endl;
cerr << resDiff.transpose() << endl;
cerr << endl;
}
for (unsigned int j=0; j<vecs.size(); ++j)
{
Vector4d res1 = ScSO3::lieBracket(vecs[i],vecs[j]);
Matrix3d hati = ScSO3::hat(vecs[i]);
Matrix3d hatj = ScSO3::hat(vecs[j]);
Vector4d res2 = ScSO3::vee(hati*hatj-hatj*hati);
Vector4d resDiff = res1-res2;
if (resDiff.norm()>SMALL_EPS)
{
cerr << "ScSO3 Lie Bracket Test" << endl;
cerr << "Test case: " << i << ", " <<j<< endl;
cerr << vecs[i].transpose() << endl;
cerr << vecs[j].transpose() << endl;
cerr << resDiff.transpose() << endl;
cerr << endl;
failed = true;
}
}
Vector4d omega = vecs[i];
Matrix3d exp_x = ScSO3::exp(omega).matrix();
Matrix3d expmap_hat_x = (ScSO3::hat(omega)).exp();
Matrix3d DiffR = exp_x-expmap_hat_x;
double nrm = DiffR.norm();
if (isnan(nrm) || nrm>SMALL_EPS)
{
cerr << "expmap(hat(x)) - exp(x)" << endl;
cerr << "Test case: " << i << endl;
cerr << exp_x <<endl;
cerr << expmap_hat_x <<endl;
cerr << DiffR <<endl;
cerr << endl;
failed = true;
}
}
return failed;
}
