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; };