bool testRawDataAcces() {
bool passed = true;
Eigen::Matrix<Scalar, 4, 1> raw = {0, 1, 0, 0};
Eigen::Map<RxSO3Type const> map_of_const_rxso3(raw.data());
SOPHUS_TEST_APPROX(passed, map_of_const_rxso3.quaternion().coeffs().eval(),
raw, Constants<Scalar>::epsilon());
SOPHUS_TEST_EQUAL(passed, map_of_const_rxso3.quaternion().coeffs().data(),
raw.data());
Eigen::Map<RxSO3Type const> const_shallow_copy = map_of_const_rxso3;
SOPHUS_TEST_EQUAL(passed, const_shallow_copy.quaternion().coeffs().eval(),
map_of_const_rxso3.quaternion().coeffs().eval());
Eigen::Matrix<Scalar, 4, 1> raw2 = {1, 0, 0, 0};
Eigen::Map<RxSO3Type> map_of_rxso3(raw.data());
Eigen::Quaternion<Scalar> quat;
quat.coeffs() = raw2;
map_of_rxso3.setQuaternion(quat);
SOPHUS_TEST_APPROX(passed, map_of_rxso3.quaternion().coeffs().eval(), raw2,
Constants<Scalar>::epsilon());
SOPHUS_TEST_EQUAL(passed, map_of_rxso3.quaternion().coeffs().data(),
raw.data());
SOPHUS_TEST_NEQ(passed, map_of_rxso3.quaternion().coeffs().data(),
quat.coeffs().data());
Eigen::Map<RxSO3Type> shallow_copy = map_of_rxso3;
SOPHUS_TEST_EQUAL(passed, shallow_copy.quaternion().coeffs().eval(),
map_of_rxso3.quaternion().coeffs().eval());
RxSO3Type const const_so3(quat);
for (int i = 0; i < 4; ++i) {
SOPHUS_TEST_EQUAL(passed, const_so3.data()[i], raw2.data()[i]);
}
RxSO3Type so3(quat);
for (int i = 0; i < 4; ++i) {
so3.data()[i] = raw[i];
}
for (int i = 0; i < 4; ++i) {
SOPHUS_TEST_EQUAL(passed, so3.data()[i], raw.data()[i]);
}
for (int i = 0; i < 10; ++i) {
Matrix3<Scalar> M = Matrix3<Scalar>::Random();
for (Scalar scale : {Scalar(0.01), Scalar(0.99), Scalar(1), Scalar(10)}) {
Matrix3<Scalar> R = makeRotationMatrix(M);
Matrix3<Scalar> sR = scale * R;
SOPHUS_TEST(passed, isScaledOrthogonalAndPositive(sR),
"isScaledOrthogonalAndPositive(sR): % *\n%", scale, R);
Matrix3<Scalar> sR_cols_swapped;
sR_cols_swapped << sR.col(1), sR.col(0), sR.col(2);
SOPHUS_TEST(passed, !isScaledOrthogonalAndPositive(sR_cols_swapped),
"isScaledOrthogonalAndPositive(-sR): % *\n%", scale, R);
}
}
return passed;
}
// returns the local R,t in nd0 that produces nd1
// NOTE: returns a postfix rotation; should return a prefix
void transformN2N(Eigen::Matrix<double,4,1> &trans,
Eigen::Quaternion<double> &qr,
Node &nd0, Node &nd1)
{
Matrix<double,3,4> tfm;
Quaterniond q0,q1;
q0 = nd0.qrot;
transformW2F(tfm,nd0.trans,q0);
trans.head(3) = tfm*nd1.trans;
trans(3) = 1.0;
q1 = nd1.qrot;
qr = q0.inverse()*q1;
qr.normalize();
if (qr.w() < 0)
qr.coeffs() = -qr.coeffs();
}
IGL_INLINE bool igl::snap_to_canonical_view_quat(
const Eigen::Quaternion<Scalarq> & q,
const double threshold,
Eigen::Quaternion<Scalars> & s)
{
return snap_to_canonical_view_quat<Scalars>(
q.coeffs().data(),threshold,s.coeffs().data());
}
void SetTwistHandler::handleSimulation(){
// called when the main script calls: simHandleModule
if(!_initialized){
_initialize();
}
Eigen::Quaternion < simFloat > orientation; //(x,y,z,w)
Eigen::Matrix< simFloat, 3, 1> linVelocity((simFloat)_twistCommands.twist.linear.x,
(simFloat)_twistCommands.twist.linear.y,
(simFloat)_twistCommands.twist.linear.z);
Eigen::Matrix< simFloat, 3, 1> angVelocity((simFloat)_twistCommands.twist.angular.x,
(simFloat)_twistCommands.twist.angular.y,
(simFloat)_twistCommands.twist.angular.z);
// Input velocity is expected to be in body frame but V-Rep expects it to be in world frame.
if(simGetObjectQuaternion(_associatedObjectID, -1, orientation.coeffs().data())!=-1){
linVelocity = orientation*linVelocity;
angVelocity = orientation*angVelocity;
} else {
std::stringstream ss;
ss << "- [" << _associatedObjectName << "] Error getting orientation. " << std::endl;
ConsoleHandler::printInConsole(ss);
}
simResetDynamicObject(_associatedObjectID);
//Set object velocity
if (_isStatic){
Eigen::Matrix<simFloat, 3, 1> position;
simGetObjectPosition(_associatedObjectID, -1, position.data());
const simFloat timeStep = simGetSimulationTimeStep();
position = position + timeStep*linVelocity;
simSetObjectPosition(_associatedObjectID, -1, position.data());
const simFloat angle = timeStep*angVelocity.norm();
if(angle > 1e-6){
orientation = Eigen::Quaternion< simFloat >(Eigen::AngleAxis< simFloat >(timeStep*angVelocity.norm(), angVelocity/angVelocity.norm()))*orientation;
}
simSetObjectQuaternion(_associatedObjectID, -1, orientation.coeffs().data());
} else {
// Apply the linear velocity to the object
if(simSetObjectFloatParameter(_associatedObjectID, 3000, linVelocity[0])
&& simSetObjectFloatParameter(_associatedObjectID, 3001, linVelocity[1])
&& simSetObjectFloatParameter(_associatedObjectID, 3002, linVelocity[2])==-1) {
std::stringstream ss;
ss << "- [" << _associatedObjectName << "] Error setting linear velocity. " << std::endl;;
ConsoleHandler::printInConsole(ss);
}
// Apply the angular velocity to the object
if(simSetObjectFloatParameter(_associatedObjectID, 3020, angVelocity[0])
&& simSetObjectFloatParameter(_associatedObjectID, 3021, angVelocity[1])
&& simSetObjectFloatParameter(_associatedObjectID, 3022, angVelocity[2])==-1) {
std::stringstream ss;
ss << "- [" << _associatedObjectName << "] Error setting angular velocity. " << std::endl;;
ConsoleHandler::printInConsole(ss);
}
}
}
void GetTwistHandler::handleSimulation(){
// called when the main script calls: simHandleModule
if(!_initialized){
_initialize();
}
ros::Time now = ros::Time::now();
const simFloat currentSimulationTime = simGetSimulationTime();
if ((currentSimulationTime-_lastPublishedObjTwistTime) >= 1.0/_ObjTwistFrequency){
Eigen::Quaternion< simFloat > orientation; //(x,y,z,w)
Eigen::Matrix<simFloat, 3, 1> linVelocity;
Eigen::Matrix<simFloat, 3, 1> angVelocity;
bool error = false;
// Get object velocity. If the object is not static simGetVelocity is more accurate.
if (_isStatic){
error = error || simGetObjectVelocity(_associatedObjectID, linVelocity.data(), angVelocity.data()) == -1;
} else {
error = error || simGetVelocity(_associatedObjectID, linVelocity.data(), angVelocity.data()) == -1;
}
// Get object orientation
error = error || simGetObjectQuaternion(_associatedObjectID, -1, orientation.coeffs().data()) == -1;
if(!error){
linVelocity = orientation.conjugate()*linVelocity; // Express linear velocity in body frame
angVelocity = orientation.conjugate()*angVelocity; // Express angular velocity in body frame
// Fill the status msg
geometry_msgs::TwistStamped msg;
msg.twist.linear.x = linVelocity[0];
msg.twist.linear.y = linVelocity[1];
msg.twist.linear.z = linVelocity[2];
msg.twist.angular.x = angVelocity[0];
msg.twist.angular.y = angVelocity[1];
msg.twist.angular.z = angVelocity[2];
msg.header.stamp = now;
_pub.publish(msg);
_lastPublishedObjTwistTime = currentSimulationTime;
} else {
std::stringstream ss;
ss << "- [" << _associatedObjectName << "] Error getting object velocity and/or orientation." << std::endl;;
ConsoleHandler::printInConsole(ss);
}
}
}
/*
* This small example show the conversion between Eigen types and KDL types
*
* The toKDLxxx() function specify the KDL type in the name because it is impossible
* to determine the KDL type at compile time when using a dynamic eigen matrix.
*/
int main(int argc, char* argv[]) {
using namespace KDL;
using namespace std;
cout << "Demonstration of simple conversion routines between KDL and Eigen types \n";
Vector a(1.0,2.0,3.0);
cout << "KDL Vector " << a << "\n";
Eigen::Vector3d a_eigen;
a_eigen = toEigen(a);
cout << "Eigen representation " << a_eigen.transpose() << "\n";
cout << "converted back to KDL " << toKDLVector(a_eigen) << "\n";
cout << endl;
Twist t(Vector(1.0,2.0,3.0),Vector(4.0,5.0,6.0));
cout << "KDL Vector " << t << "\n";
Eigen::Matrix<double,6,1> t_eigen;
t_eigen = toEigen(t);
cout << "Eigen representation " << t_eigen.transpose() << "\n";
cout << "converted back to KDL " << toKDLTwist(t_eigen) << "\n";
cout << endl;
Wrench w(Vector(1.0,2.0,3.0),Vector(4.0,5.0,6.0));
cout << "KDL Wrench " << t << "\n";
Eigen::Matrix<double,6,1> w_eigen;
w_eigen = toEigen(w);
cout << "Eigen representation " << w_eigen.transpose() << "\n";
cout << "converted back to KDL " << toKDLTwist(w_eigen) << "\n";
Rotation R( Rotation::EulerZYX(30*deg2rad,45*deg2rad,0*deg2rad) );
cout << "KDL Rotation " << R << "\n";
Eigen::Matrix<double,3,3> R_eigen;
R_eigen = toEigen( R );
cout << "Eigen 3x3 matrix " << R_eigen << "\n";
cout << "converted back to KDL " << toKDLRotation( R_eigen ) << "\n";
Eigen::Quaternion<double> q;
q = toEigenQuaternion( R );
cout << "Eigen Quaternion " << q.coeffs() << "\n";
cout << "converted back to KDL " << toKDLRotation( q ) << "\n";
cout << endl;
return 0;
}
bool testRawDataAcces() {
bool passed = true;
Eigen::Matrix<Scalar, 7, 1> raw;
raw << 0, 1, 0, 0, 1, 3, 2;
Eigen::Map<Sim3Type const> map_of_const_sim3(raw.data());
SOPHUS_TEST_APPROX(passed, map_of_const_sim3.quaternion().coeffs().eval(),
raw.template head<4>().eval(),
Constants<Scalar>::epsilon());
SOPHUS_TEST_APPROX(passed, map_of_const_sim3.translation().eval(),
raw.template tail<3>().eval(),
Constants<Scalar>::epsilon());
SOPHUS_TEST_EQUAL(passed, map_of_const_sim3.quaternion().coeffs().data(),
raw.data());
SOPHUS_TEST_EQUAL(passed, map_of_const_sim3.translation().data(),
raw.data() + 4);
Eigen::Map<Sim3Type const> const_shallow_copy = map_of_const_sim3;
SOPHUS_TEST_EQUAL(passed, const_shallow_copy.quaternion().coeffs().eval(),
map_of_const_sim3.quaternion().coeffs().eval());
SOPHUS_TEST_EQUAL(passed, const_shallow_copy.translation().eval(),
map_of_const_sim3.translation().eval());
Eigen::Matrix<Scalar, 7, 1> raw2;
raw2 << 1, 0, 0, 0, 3, 2, 1;
Eigen::Map<Sim3Type> map_of_sim3(raw.data());
Eigen::Quaternion<Scalar> quat;
quat.coeffs() = raw2.template head<4>();
map_of_sim3.setQuaternion(quat);
map_of_sim3.translation() = raw2.template tail<3>();
SOPHUS_TEST_APPROX(passed, map_of_sim3.quaternion().coeffs().eval(),
raw2.template head<4>().eval(),
Constants<Scalar>::epsilon());
SOPHUS_TEST_APPROX(passed, map_of_sim3.translation().eval(),
raw2.template tail<3>().eval(),
Constants<Scalar>::epsilon());
SOPHUS_TEST_EQUAL(passed, map_of_sim3.quaternion().coeffs().data(),
raw.data());
SOPHUS_TEST_EQUAL(passed, map_of_sim3.translation().data(), raw.data() + 4);
SOPHUS_TEST_NEQ(passed, map_of_sim3.quaternion().coeffs().data(),
quat.coeffs().data());
Eigen::Map<Sim3Type> shallow_copy = map_of_sim3;
SOPHUS_TEST_EQUAL(passed, shallow_copy.quaternion().coeffs().eval(),
map_of_sim3.quaternion().coeffs().eval());
SOPHUS_TEST_EQUAL(passed, shallow_copy.translation().eval(),
map_of_sim3.translation().eval());
Eigen::Map<Sim3Type> const const_map_of_sim3 = map_of_sim3;
SOPHUS_TEST_EQUAL(passed, const_map_of_sim3.quaternion().coeffs().eval(),
map_of_sim3.quaternion().coeffs().eval());
SOPHUS_TEST_EQUAL(passed, const_map_of_sim3.translation().eval(),
map_of_sim3.translation().eval());
Sim3Type const const_sim3(quat, raw2.template tail<3>().eval());
for (int i = 0; i < 7; ++i) {
SOPHUS_TEST_EQUAL(passed, const_sim3.data()[i], raw2.data()[i]);
}
Sim3Type se3(quat, raw2.template tail<3>().eval());
for (int i = 0; i < 7; ++i) {
SOPHUS_TEST_EQUAL(passed, se3.data()[i], raw2.data()[i]);
}
for (int i = 0; i < 7; ++i) {
SOPHUS_TEST_EQUAL(passed, se3.data()[i], raw.data()[i]);
}
return passed;
}
void ConP2::setJacobians(std::vector<Node,Eigen::aligned_allocator<Node> > &nodes)
{
// node references
Node &nr = nodes[ndr];
Matrix<double,4,1> &tr = nr.trans;
Quaternion<double> &qr = nr.qrot;
Node &n1 = nodes[nd1];
Matrix<double,4,1> &t1 = n1.trans;
Quaternion<double> &q1 = n1.qrot;
// first get the second frame in first frame coords
Eigen::Matrix<double,3,1> pc = nr.w2n * t1;
// Jacobians wrt first frame parameters
// translational part of 0p1 wrt translational vars of p0
// this is just -R0' [from 0t1 = R0'(t1 - t0)]
J0.block<3,3>(0,0) = -nr.w2n.block<3,3>(0,0);
// translational part of 0p1 wrt rotational vars of p0
// dR'/dq * [pw - t]
Eigen::Matrix<double,3,1> pwt;
pwt = (t1-tr).head(3); // transform translations
// dx
Eigen::Matrix<double,3,1> dp = nr.dRdx * pwt; // dR'/dq * [pw - t]
J0.block<3,1>(0,3) = dp;
// dy
dp = nr.dRdy * pwt; // dR'/dq * [pw - t]
J0.block<3,1>(0,4) = dp;
// dz
dp = nr.dRdz * pwt; // dR'/dq * [pw - t]
J0.block<3,1>(0,5) = dp;
// rotational part of 0p1 wrt translation vars of p0 => zero
J0.block<3,3>(3,0).setZero();
// rotational part of 0p1 wrt rotational vars of p0
// from 0q1 = qpmean * s0' * q0' * q1
// dqdx
Eigen::Quaternion<double> qr0, qr1, qrn, qrd;
qr1.coeffs() = q1.coeffs();
qrn.coeffs() = Vector4d(-qpmean.w(),-qpmean.z(),qpmean.y(),qpmean.x()); // qpmean * ds0'/dx
qr0.coeffs() = Vector4d(-qr.x(),-qr.y(),-qr.z(),qr.w());
qr0 = qr0*qr1; // rotate to zero mean
qrd = qpmean*qr0; // for normalization check
qrn = qrn*qr0;
#ifdef NORMALIZE_Q
if (qrd.w() < 0.0)
qrn.vec() = -qrn.vec();
#endif
J0.block<3,1>(3,3) = qrn.vec();
// dqdy
qrn.coeffs() = Vector4d(qpmean.z(),-qpmean.w(),-qpmean.x(),qpmean.y()); // qpmean * ds0'/dy
qrn = qrn*qr0;
#ifdef NORMALIZE_Q
if (qrd.w() < 0.0)
qrn.vec() = -qrn.vec();
#endif
J0.block<3,1>(3,4) = qrn.vec();
// dqdz
qrn.coeffs() = Vector4d(-qpmean.y(),qpmean.x(),-qpmean.w(),qpmean.z()); // qpmean * ds0'/dz
qrn = qrn*qr0;
#ifdef NORMALIZE_Q
if (qrd.w() < 0.0)
qrn.vec() = -qrn.vec();
#endif
J0.block<3,1>(3,5) = qrn.vec();
// transpose
J0t = J0.transpose();
// cout << endl << "J0 " << ndr << endl << J0 << endl;
// Jacobians wrt second frame parameters
// translational part of 0p1 wrt translational vars of p1
// this is just R0' [from 0t1 = R0'(t1 - t0)]
J1.block<3,3>(0,0) = nr.w2n.block<3,3>(0,0);
// translational part of 0p1 wrt rotational vars of p1: zero
J1.block<3,3>(0,3).setZero();
// rotational part of 0p1 wrt translation vars of p0 => zero
J1.block<3,3>(3,0).setZero();
// rotational part of 0p1 wrt rotational vars of p0
// from 0q1 = q0'*s1*q1
Eigen::Quaternion<double> qrc;
qrc.coeffs() = Vector4d(-qr.x(),-qr.y(),-qr.z(),qr.w());
qrc = qpmean*qrc*qr1; // mean' * qr0' * qr1
qrc.normalize();
// cout << endl << "QRC : " << qrc.coeffs().transpose() << endl;
double dq = 1.0e-8;
double wdq = 1.0 - dq*dq;
qr1.coeffs() = Vector4d(dq,0,0,wdq);
// cout << "QRC+x: " << (qrc*qr1).coeffs().transpose() << endl;
// cout << "QRdx: " << ((qrc*qr1).coeffs().transpose() - qrc.coeffs().transpose())/dq << endl;
// dqdx
qrn.coeffs() = Vector4d(1,0,0,0);
qrn = qrc*qrn;
// cout << "J1dx: " << qrn.coeffs().transpose() << endl;
#ifdef NORMALIZE_Q
if (qrc.w() < 0.0)
qrn.vec() = -qrn.vec();
#endif
J1.block<3,1>(3,3) = qrn.vec();
// dqdy
qrn.coeffs() = Vector4d(0,1,0,0);
qrn = qrc*qrn;
#ifdef NORMALIZE_Q
if (qrc.w() < 0.0)
qrn.vec() = -qrn.vec();
#endif
J1.block<3,1>(3,4) = qrn.vec();
// dqdz
qrn.coeffs() = Vector4d(0,0,1,0);
qrn = qrc*qrn;
#ifdef NORMALIZE_Q
if (qrc.w() < 0.0)
qrn.vec() = -qrn.vec();
#endif
J1.block<3,1>(3,5) = qrn.vec();
// transpose
J1t = J1.transpose();
// cout << endl << "J1 " << nd1 << endl << J1 << endl;
};
