本文整理汇总了C++中kdl::Rotation::GetRPY方法的典型用法代码示例。如果您正苦于以下问题：C++ Rotation::GetRPY方法的具体用法？C++ Rotation::GetRPY怎么用？C++ Rotation::GetRPY使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类kdl::Rotation的用法示例。
在下文中一共展示了Rotation::GetRPY方法的3个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: ik
int ComauSmartSix::ik(float x, float y,float z,float qx, float qy,float qz, float qw, float* q_in,float* q_out) {
KDL::Rotation rot = KDL::Rotation::Quaternion(qx,qy,qz,qw);
double roll,pitch,yaw;
rot.GetRPY(roll,pitch,yaw);
return this->ik(x,y,z,roll,pitch,yaw,q_in,q_out,true);
}示例2: decomposeTransform
void DetectorFilter::decomposeTransform(const geometry_msgs::PoseWithCovarianceStamped& pose,
MatrixWrapper::ColumnVector& vector)
{
assert(vector.rows() == 6);
// construct kdl rotation from quaternion, and extract RPY
KDL::Rotation rot = KDL::Rotation::Quaternion(pose.pose.pose.orientation.x,
pose.pose.pose.orientation.y,
pose.pose.pose.orientation.z,
pose.pose.pose.orientation.w);
rot.GetRPY(vector(4), vector(5), vector(6));
vector(1) = pose.pose.pose.position.x;
vector(2) = pose.pose.pose.position.y;
vector(3) = pose.pose.pose.position.z;
};示例3: update
void YouBotBaseService::update()
{
if(is_in_visualization_mode)
{
in_odometry_state.read(m_odometry_state);
simxFloat pos[3];
pos[0] = m_odometry_state.pose.pose.position.x;
pos[1] = m_odometry_state.pose.pose.position.y;
pos[2] = m_odometry_state.pose.pose.position.z;
simxSetObjectPosition(clientID,all_robot_handle,-1,pos,simx_opmode_oneshot);
double euler[3];
simxFloat euler_s[3];
//different coordinate systems, fixing it here
KDL::Rotation orientation = KDL::Rotation::Quaternion(
m_odometry_state.pose.pose.orientation.x,
m_odometry_state.pose.pose.orientation.y,
m_odometry_state.pose.pose.orientation.z,
m_odometry_state.pose.pose.orientation.w);
// Instead of transforming for the inverse of this rotation,
// we should find the right transform. (this is legacy code that
// transforms vrep coords to rviz coords)
KDL::Rotation rotation = KDL::Rotation::RPY(0,-M_PI/2,M_PI).Inverse();
orientation = orientation * rotation;
orientation.GetRPY(euler[0],euler[1],euler[2]);
euler_s[0] = euler[0];
euler_s[1] = euler[1];
euler_s[2] = euler[2];
simxSetObjectOrientation(clientID,all_robot_handle,-1,euler_s,simx_opmode_oneshot);
simxSetJointPosition(clientID,vrep_joint_handle[0],m_joint_state.position[0], simx_opmode_oneshot);
simxSetJointPosition(clientID,vrep_joint_handle[1],m_joint_state.position[1], simx_opmode_oneshot);
simxSetJointPosition(clientID,vrep_joint_handle[2],m_joint_state.position[2], simx_opmode_oneshot);
simxSetJointPosition(clientID,vrep_joint_handle[3],m_joint_state.position[3], simx_opmode_oneshot);
return;
}
Hilas::IRobotBaseService::update();
}