本文整理汇总了C++中eigen::Quaterniond类的典型用法代码示例。如果您正苦于以下问题:C++ Quaterniond类的具体用法?C++ Quaterniond怎么用?C++ Quaterniond使用的例子?那么恭喜您, 这里精选的类代码示例或许可以为您提供帮助。
在下文中一共展示了Quaterniond类的20个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于我们的系统推荐出更棒的C++代码示例。
示例1: quat_to_euler
void quat_to_euler(Eigen::Quaterniond q, double& roll, double& pitch, double& yaw) {
const double q0 = q.w();
const double q1 = q.x();
const double q2 = q.y();
const double q3 = q.z();
roll = atan2(2*(q0*q1+q2*q3), 1-2*(q1*q1+q2*q2));
pitch = asin(2*(q0*q2-q3*q1));
yaw = atan2(2*(q0*q3+q1*q2), 1-2*(q2*q2+q3*q3));
}
开发者ID:patmarion,项目名称:oh-distro,代码行数:9,代码来源:testIKMoreConstraintsLCMValkyrie.cpp
示例2: translation
void UpperBodyPlanner::msgPose2Eigen(const geometry_msgs::Pose& input, Eigen::Vector3d& translation, Eigen::Quaterniond& q) {
translation(0) = input.position.x;
translation(1) = input.position.y;
translation(2) = input.position.z;
q.w() = input.orientation.w;
q.x() = input.orientation.x;
q.y() = input.orientation.y;
q.z() = input.orientation.z;
}
开发者ID:nianxing,项目名称:Telemanipulation_Atlas_Using_Razer_Hydra,代码行数:9,代码来源:hydra_move_group.cpp
示例3: QuaternionToExponentialMap
Eigen::Vector3d QuaternionToExponentialMap(const Eigen::Quaterniond& quaternion)
{
Eigen::Vector3d vec = quaternion.vec();
if (vec.norm() < ITOMP_EPS)
return Eigen::Vector3d::Zero();
double theta = 2.0 * std::acos(quaternion.w());
vec.normalize();
return theta * vec;
}
开发者ID:Chpark,项目名称:itomp,代码行数:10,代码来源:exponential_map.cpp
示例4: KDLToEigen
Eigen::Isometry3d KDLToEigen(KDL::Frame tf)
{
Eigen::Isometry3d tf_out;
tf_out.setIdentity();
tf_out.translation() << tf.p[0], tf.p[1], tf.p[2];
Eigen::Quaterniond q;
tf.M.GetQuaternion(q.x(), q.y(), q.z(), q.w());
tf_out.rotate(q);
return tf_out;
}
开发者ID:tkoolen,项目名称:oh-distro,代码行数:10,代码来源:lcm2ros_ihmc.cpp
示例5: inputOdom
void GlobalOptimization::inputOdom(double t, Eigen::Vector3d OdomP, Eigen::Quaterniond OdomQ)
{
mPoseMap.lock();
vector<double> localPose{OdomP.x(), OdomP.y(), OdomP.z(),
OdomQ.w(), OdomQ.x(), OdomQ.y(), OdomQ.z()};
localPoseMap[t] = localPose;
Eigen::Quaterniond globalQ;
globalQ = WGPS_T_WVIO.block<3, 3>(0, 0) * OdomQ;
Eigen::Vector3d globalP = WGPS_T_WVIO.block<3, 3>(0, 0) * OdomP + WGPS_T_WVIO.block<3, 1>(0, 3);
vector<double> globalPose{globalP.x(), globalP.y(), globalP.z(),
globalQ.w(), globalQ.x(), globalQ.y(), globalQ.z()};
globalPoseMap[t] = globalPose;
lastP = globalP;
lastQ = globalQ;
geometry_msgs::PoseStamped pose_stamped;
pose_stamped.header.stamp = ros::Time(t);
pose_stamped.header.frame_id = "world";
pose_stamped.pose.position.x = lastP.x();
pose_stamped.pose.position.y = lastP.y();
pose_stamped.pose.position.z = lastP.z();
pose_stamped.pose.orientation.x = lastQ.x();
pose_stamped.pose.orientation.y = lastQ.y();
pose_stamped.pose.orientation.z = lastQ.z();
pose_stamped.pose.orientation.w = lastQ.w();
global_path.header = pose_stamped.header;
global_path.poses.push_back(pose_stamped);
mPoseMap.unlock();
}
开发者ID:greck2908,项目名称:VINS-Fusion,代码行数:32,代码来源:globalOpt.cpp
示例6: q
/*!
* \brief affine3d2UrdfPose converts an Eigen affine 4x4 matrix o represent the pose into a urdf pose
* vparam pose eigen Affine3d pose
* \return urdf pose with position and rotation.
*/
RCS::Pose Affine3d2UrdfPose(const Eigen::Affine3d &pose) {
RCS::Pose p;
p.getOrigin().setX(pose.translation().x());
p.getOrigin().setY(pose.translation().y());
p.getOrigin().setZ(pose.translation().z());
Eigen::Quaterniond q (pose.rotation());
tf::Quaternion qtf(q.x(),q.y(),q.z(),q.w());
//std::cout << "Affine3d2UrdfPose Quaterion = \n" << q.x() << ":" << q.y() << ":" << q.z() << ":" << q.w() << std::endl;
p.setRotation(qtf);
//std::cout << "After Affine3d2UrdfPose Quaterion = \n" << p.getRotation().x() << ":" << p.getRotation().y() << ":" << p.getRotation().z() << ":" << p.getRotation().w() << std::endl;
#if 0
MatrixEXd m = pose.rotation();
Eigen::Quaterniond q = EMatrix2Quaterion(m);
Eigen::Quaterniond q(pose.rotation());
p.getRotation().setX(q.x());
p.getRotation().setY(q.y());
p.getRotation().setZ(q.z());
p.getRotation().setW(q.w());
#endif
return p;
}
开发者ID:usnistgov,项目名称:el-robotics-core,代码行数:30,代码来源:Conversions.cpp
示例7: updateVizMarker
void updateVizMarker(int id, Eigen::Vector3d _pose, Eigen::Quaterniond _attitude)
{
marker[id].pose.position.x = _pose(0);
marker[id].pose.position.y = _pose(1);
marker[id].pose.position.z = _pose(2);
marker[id].pose.orientation.x = _attitude.x();
marker[id].pose.orientation.y = _attitude.y();
marker[id].pose.orientation.z = _attitude.z();
marker[id].pose.orientation.w = _attitude.w();
pub_body[id].publish( marker[id] );
}
开发者ID:OpenHero,项目名称:toy_code,代码行数:11,代码来源:test_quad.cpp
示例8: allRotationsToAxisCB
/**
* Recursion method to be used with traverseTreeTopDown() and recursion parameters
* of type *Vector3RecursionParams*.
*
* Re-arranges the joint-transform of the recursion link's *parent joint*, along with
* the link's visual/collision/intertial rotations, such that all joints rotate around the axis
* given in the recursion parameters vector.
*/
int allRotationsToAxisCB(urdf_traverser::RecursionParamsPtr& p)
{
urdf_traverser::LinkPtr link = p->getLink();
if (!link)
{
ROS_ERROR("allRotationsToAxis: NULL link passed");
return -1;
}
Vector3RecursionParams::Ptr param = baselib_binding_ns::dynamic_pointer_cast<Vector3RecursionParams>(p);
if (!param)
{
ROS_ERROR("Wrong recursion parameter type");
return -1;
}
urdf_traverser::JointPtr joint = link->parent_joint;
if (!joint)
{
ROS_INFO_STREAM("allRotationsToAxis: Joint for link " << link->name << " is NULL, so this must be the root joint");
return 1;
}
Eigen::Vector3d axis = param->vec;
Eigen::Quaterniond alignAxis;
if (urdf_traverser::jointTransformForAxis(joint, axis, alignAxis))
{
Eigen::Vector3d rotAxis(joint->axis.x, joint->axis.y, joint->axis.z);
// ROS_INFO_STREAM("Transforming axis "<<rotAxis<<" for joint "<<joint->name<<" with transform "<<urdf_traverser::EigenTransform(alignAxis));
urdf_traverser::applyTransform(joint, urdf_traverser::EigenTransform(alignAxis), false);
// the link has to receive the inverse transform, so it stays at the original position
Eigen::Quaterniond alignAxisInv = alignAxis.inverse();
urdf_traverser::applyTransform(link, urdf_traverser::EigenTransform(alignAxisInv), true);
// we also have to fix the child joint's (1st order child joints) transform
// to correct for this transformation.
for (std::vector<urdf_traverser::JointPtr>::iterator pj = link->child_joints.begin();
pj != link->child_joints.end(); pj++)
{
urdf_traverser::applyTransform(*pj, urdf_traverser::EigenTransform(alignAxisInv), true);
}
// finally, set the rotation axis to the target
joint->axis.x = axis.x();
joint->axis.y = axis.y();
joint->axis.z = axis.z();
}
// all good, indicate that recursion can continue
return 1;
}
开发者ID:JenniferBuehler,项目名称:urdf-tools-pkgs,代码行数:62,代码来源:AlignRotationAxis.cpp
示例9: quaternion_get_yaw
double quaternion_get_yaw(const Eigen::Quaterniond &q)
{
// to match equation from:
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
const double &q0 = q.w();
const double &q1 = q.x();
const double &q2 = q.y();
const double &q3 = q.z();
return std::atan2(2. * (q0*q3 + q1*q2), 1. - 2. * (q2*q2 + q3*q3));
}
开发者ID:liang-tang,项目名称:pendulum,代码行数:11,代码来源:ftf_quaternion_utils.cpp
示例10: angular_distance
double igl::angular_distance(
const Eigen::Quaterniond & A,
const Eigen::Quaterniond & B)
{
using namespace igl;
assert(fabs(A.norm()-1)<FLOAT_EPS && "A should be unit norm");
assert(fabs(B.norm()-1)<FLOAT_EPS && "B should be unit norm");
//// acos is always in [0,2*pi)
//return acos(fabs(A.dot(B)));
return fmod(2.*acos(A.dot(B)),2.*PI);
}
开发者ID:azer89,项目名称:BBW,代码行数:11,代码来源:angular_distance.cpp
示例11:
Eigen::Quaterniond UpperBodyPlanner::Rmat2Quaternion(const Eigen::Matrix3d& inMat_) {
Eigen::Quaterniond outQ;
KDL::Rotation convert;
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
convert.data[3 * i + j] = inMat_(i,j);
}
}
convert.GetQuaternion(outQ.x(), outQ.y(), outQ.z(), outQ.w());
return outQ;
}
开发者ID:nianxing,项目名称:Telemanipulation_Atlas_Using_Razer_Hydra,代码行数:11,代码来源:hydra_move_group.cpp
示例12: o
void planning_models::Transforms::setTransform(const geometry_msgs::TransformStamped &transform)
{
if (transform.child_frame_id.rfind(target_frame_) == transform.child_frame_id.length() - target_frame_.length())
{
Eigen::Translation3d o(transform.transform.translation.x, transform.transform.translation.y, transform.transform.translation.z);
Eigen::Quaterniond q;
quatFromMsg(transform.transform.rotation, q);
setTransform(Eigen::Affine3d(o*q.toRotationMatrix()), transform.header.frame_id);
} else {
ROS_ERROR("Given transform is to frame '%s', but frame '%s' was expected.", transform.child_frame_id.c_str(), target_frame_.c_str());
}
}
开发者ID:jonbinney,项目名称:moveit-core,代码行数:12,代码来源:transforms.cpp
示例13: create_data
void create_data(Eigen::MatrixXd &pa, Eigen::MatrixXd &pb) {
int n_data = 7;
Eigen::MatrixXd pa0(3, n_data);
Eigen::MatrixXd pb0(3, n_data);
pb0 << -0.7045189014502934,0.31652495664145264,-0.8913587885243552,0.4196143278053829,0.33125081405575785,-1.148712511573519,-0.7211957446166447,-0.4204243223315903,-0.8922857301575797,0.41556308950696674,-0.36760757371251074,-1.1630155401570457,-0.12535642300333297,0.26708755761917147,1.5095344824450356,0.9968448409012386,0.27593113974268946,1.2189108175890786,-0.28095118914331707,-0.40276257201497045,1.3669272703783852;
Eigen::Quaterniond q = Eigen::Quaterniond(2.86073, 0.0378363, 3.59752, 0.4211619).normalized();
std::cout << "groundtruth-quaternion. w: " << q.w() << " x " << q.x() << " y: " << q.y() << " z " << q.z() << std::endl;
pa = q.toRotationMatrix()*pb0;
pb = pb0;
}
开发者ID:skanti,项目名称:libcmaes,代码行数:12,代码来源:main.cpp
示例14: o
void robot_state::Transforms::setTransform(const geometry_msgs::TransformStamped &transform)
{
if (transform.child_frame_id.rfind(target_frame_) == transform.child_frame_id.length() - target_frame_.length())
{
Eigen::Translation3d o(transform.transform.translation.x, transform.transform.translation.y, transform.transform.translation.z);
Eigen::Quaterniond q;
tf::quaternionMsgToEigen(transform.transform.rotation, q);
setTransform(Eigen::Affine3d(o*q.toRotationMatrix()), transform.header.frame_id);
} else {
logError("Given transform is to frame '%s', but frame '%s' was expected.", transform.child_frame_id.c_str(), target_frame_.c_str());
}
}
开发者ID:mpomarlan,项目名称:moveit_puzzle_demo,代码行数:12,代码来源:transforms.cpp
示例15:
void LCM2ROS::neckPitchHandler(const lcm::ReceiveBuffer* rbuf, const std::string &channel, const drc::neck_pitch_t* msg)
{
ROS_ERROR("LCM2ROS got desired neck pitch");
ihmc_msgs::HeadOrientationPacketMessage mout;
Eigen::Quaterniond quat = euler_to_quat(0, msg->pitch, 0);
mout.trajectory_time = 1;
mout.orientation.w = quat.w();
mout.orientation.x = quat.x();
mout.orientation.y = quat.y();
mout.orientation.z = quat.z();
mout.unique_id = msg->utime;
neck_orientation_pub_.publish(mout);
}
开发者ID:tkoolen,项目名称:oh-distro,代码行数:13,代码来源:lcm2ros_ihmc.cpp
示例16:
Eigen::Matrix3d FeatureModel::dR_by_dqz(const Eigen::Quaterniond &q)
{
Eigen::Matrix3d M;
M << -2*q.z(), -2*q.w(), 2*q.x(),
2*q.w(), -2*q.z(), 2*q.y(),
2*q.x(), 2*q.y(), 2*q.z();
return M;
}
开发者ID:499704069,项目名称:SceneLib2,代码行数:10,代码来源:feature_model.cpp
示例17: UpdateXopt
inline void ICP::UpdateXopt(Eigen::VectorXd &xopt, const Eigen::Matrix4d& Tr){
Eigen::Quaterniond q (Tr.block<3,3>(0,0));
xopt(0) = q.x(); //rx
xopt(1) = q.y(); //ry
xopt(2) = q.z(); //rz
xopt(3) = q.w(); //teta
xopt(4) = Tr(0,3); //tx
xopt(5) = Tr(1,3); //ty
xopt(6) = Tr(2,3); //tz
}
开发者ID:Modasshir,项目名称:socrob-ros-pkg,代码行数:14,代码来源:ICPBlock.hpp
示例18: handleIMUVelocity
void VideoIMUFusionDevice::handleIMUVelocity(
const OSVR_TimeValue ×tamp, const OSVR_AngularVelocityReport &report) {
using namespace osvr::util::eigen_interop;
Eigen::Quaterniond q = map(report.state.incrementalRotation);
Eigen::Vector3d rot;
if (q.w() >= 1. || q.vec().isZero(1e-10)) {
rot = Eigen::Vector3d::Zero();
} else {
#if 0
auto magnitude = q.vec().blueNorm();
rot = (q.vec() / magnitude * (2. * std::atan2(magnitude, q.w()))) /
report.state.dt;
#else
auto angle = std::acos(q.w());
rot = q.vec().normalized() * angle * 2 / report.state.dt;
#endif
}
m_fusion.handleIMUVelocity(timestamp, rot);
if (m_fusion.running()) {
sendMainPoseReport();
}
sendVelocityReport();
}
开发者ID:6DB,项目名称:OSVR-Core,代码行数:25,代码来源:VideoIMUFusionDevice.cpp
示例19: createGrasp
int ShapeGraspPlanner::createGrasp(const geometry_msgs::PoseStamped& pose,
double gripper_opening,
double gripper_pitch,
double x_offset,
double z_offset,
double quality)
{
moveit_msgs::Grasp grasp;
grasp.grasp_pose = pose;
// defaults
grasp.pre_grasp_posture = makeGraspPosture(gripper_opening);
grasp.grasp_posture = makeGraspPosture(0.0);
grasp.pre_grasp_approach = makeGripperTranslation(approach_frame_,
approach_min_translation_,
approach_desired_translation_);
grasp.post_grasp_retreat = makeGripperTranslation(retreat_frame_,
retreat_min_translation_,
retreat_desired_translation_,
-1.0); // retreat is in negative x direction
// initial pose
Eigen::Affine3d p = Eigen::Translation3d(pose.pose.position.x,
pose.pose.position.y,
pose.pose.position.z) *
Eigen::Quaterniond(pose.pose.orientation.w,
pose.pose.orientation.x,
pose.pose.orientation.y,
pose.pose.orientation.z);
// translate by x_offset, 0, z_offset
p = p * Eigen::Translation3d(x_offset, 0, z_offset);
// rotate by 0, pitch, 0
p = p * quaternionFromEuler(0.0, gripper_pitch, 0.0);
// apply grasp point -> planning frame offset
p = p * Eigen::Translation3d(-tool_offset_, 0, 0);
grasp.grasp_pose.pose.position.x = p.translation().x();
grasp.grasp_pose.pose.position.y = p.translation().y();
grasp.grasp_pose.pose.position.z = p.translation().z();
Eigen::Quaterniond q = (Eigen::Quaterniond)p.linear();
grasp.grasp_pose.pose.orientation.x = q.x();
grasp.grasp_pose.pose.orientation.y = q.y();
grasp.grasp_pose.pose.orientation.z = q.z();
grasp.grasp_pose.pose.orientation.w = q.w();
grasp.grasp_quality = quality;
grasps_.push_back(grasp);
return 1;
}
开发者ID:hanhongsun,项目名称:simple_grasping,代码行数:50,代码来源:shape_grasp_planner.cpp
示例20: subject_publish_callback
static void subject_publish_callback(const ViconDriver::Subject &subject)
{
if(!running)
return;
static std::map<std::string, ros::Publisher> vicon_publishers;
std::map<std::string, ros::Publisher>::iterator it;
it = vicon_publishers.find(subject.name);
if(it == vicon_publishers.end())
{
ros::Publisher pub = nh->advertise<vicon::Subject>(subject.name, 10);
it = vicon_publishers.insert(std::make_pair(subject.name, pub)).first;
}
if(loadCalib(subject.name))
{
Eigen::Affine3d current_pose = Eigen::Affine3d::Identity();
current_pose.translate(Eigen::Vector3d(subject.translation));
current_pose.rotate(Eigen::Quaterniond(subject.rotation));
current_pose = current_pose * calib_pose[subject.name];
const Eigen::Vector3d position(current_pose.translation());
const Eigen::Quaterniond rotation(current_pose.rotation());
vicon::Subject subject_ros;
subject_ros.header.seq = subject.frame_number;
subject_ros.header.stamp = ros::Time(subject.time_usec / 1e6);
subject_ros.header.frame_id = "/vicon";
subject_ros.name = subject.name;
subject_ros.occluded = subject.occluded;
subject_ros.position.x = position.x();
subject_ros.position.y = position.y();
subject_ros.position.z = position.z();
subject_ros.orientation.x = rotation.x();
subject_ros.orientation.y = rotation.y();
subject_ros.orientation.z = rotation.z();
subject_ros.orientation.w = rotation.w();
subject_ros.markers.resize(subject.markers.size());
for(size_t i = 0; i < subject_ros.markers.size(); i++)
{
subject_ros.markers[i].name = subject.markers[i].name;
subject_ros.markers[i].subject_name = subject.markers[i].subject_name;
subject_ros.markers[i].position.x = subject.markers[i].translation[0];
subject_ros.markers[i].position.y = subject.markers[i].translation[1];
subject_ros.markers[i].position.z = subject.markers[i].translation[2];
subject_ros.markers[i].occluded = subject.markers[i].occluded;
}
it->second.publish(subject_ros);
}
}
开发者ID:FashGek,项目名称:vicon,代码行数:50,代码来源:vicon.cpp
注:本文中的eigen::Quaterniond类示例由纯净天空整理自Github/MSDocs等源码及文档管理平台,相关代码片段筛选自各路编程大神贡献的开源项目,源码版权归原作者所有,传播和使用请参考对应项目的License;未经允许,请勿转载。 |
请发表评论