本文整理汇总了C++中eigen::Quaternion::conjugate方法的典型用法代码示例。如果您正苦于以下问题：C++ Quaternion::conjugate方法的具体用法？C++ Quaternion::conjugate怎么用？C++ Quaternion::conjugate使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类eigen::Quaternion的用法示例。
在下文中一共展示了Quaternion::conjugate方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: handleSimulation
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);
}
}
}