本文整理汇总了C++中kdl::Rotation::GetRotAngle方法的典型用法代码示例。如果您正苦于以下问题：C++ Rotation::GetRotAngle方法的具体用法？C++ Rotation::GetRotAngle怎么用？C++ Rotation::GetRotAngle使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类kdl::Rotation的用法示例。
在下文中一共展示了Rotation::GetRotAngle方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: getJointState
void UrdfModelMarker::getJointState(boost::shared_ptr<const Link> link)
{
string link_frame_name_ = tf_prefix_ + link->name;
boost::shared_ptr<Joint> parent_joint = link->parent_joint;
if(parent_joint != NULL){
KDL::Frame initialFrame;
KDL::Frame presentFrame;
KDL::Rotation rot;
KDL::Vector rotVec;
KDL::Vector jointVec;
double jointAngle;
double jointAngleAllRange;
switch(parent_joint->type){
case Joint::REVOLUTE:
case Joint::CONTINUOUS:
{
linkProperty *link_property = &linkMarkerMap[link_frame_name_];
tf::poseMsgToKDL (link_property->initial_pose, initialFrame);
tf::poseMsgToKDL (link_property->pose, presentFrame);
rot = initialFrame.M.Inverse() * presentFrame.M;
jointAngle = rot.GetRotAngle(rotVec);
jointVec = KDL::Vector(link_property->joint_axis.x,
link_property->joint_axis.y,
link_property->joint_axis.z);
if( KDL::dot(rotVec,jointVec) < 0){
jointAngle = - jointAngle;
}
if(link_property->joint_angle > M_PI/2 && jointAngle < -M_PI/2){
link_property->rotation_count += 1;
}else if(link_property->joint_angle < -M_PI/2 && jointAngle > M_PI/2){
link_property->rotation_count -= 1;
}
link_property->joint_angle = jointAngle;
jointAngleAllRange = jointAngle + link_property->rotation_count * M_PI * 2;
if(parent_joint->type == Joint::REVOLUTE && parent_joint->limits != NULL){
bool changeMarkerAngle = false;
if(jointAngleAllRange < parent_joint->limits->lower){
jointAngleAllRange = parent_joint->limits->lower + 0.001;
changeMarkerAngle = true;
}
if(jointAngleAllRange > parent_joint->limits->upper){
jointAngleAllRange = parent_joint->limits->upper - 0.001;
changeMarkerAngle = true;
}
if(changeMarkerAngle){
setJointAngle(link, jointAngleAllRange);
}
}
joint_state_.position.push_back(jointAngleAllRange);
joint_state_.name.push_back(parent_joint->name);
break;
}
case Joint::PRISMATIC:
{
KDL::Vector pos;
linkProperty *link_property = &linkMarkerMap[link_frame_name_];
tf::poseMsgToKDL (link_property->initial_pose, initialFrame);
tf::poseMsgToKDL (link_property->pose, presentFrame);
pos = presentFrame.p - initialFrame.p;
jointVec = KDL::Vector(link_property->joint_axis.x,
link_property->joint_axis.y,
link_property->joint_axis.z);
jointVec = jointVec / jointVec.Norm(); // normalize vector
jointAngle = KDL::dot(jointVec, pos);
link_property->joint_angle = jointAngle;
jointAngleAllRange = jointAngle;
if(parent_joint->type == Joint::PRISMATIC && parent_joint->limits != NULL){
bool changeMarkerAngle = false;
if(jointAngleAllRange < parent_joint->limits->lower){
jointAngleAllRange = parent_joint->limits->lower + 0.003;
changeMarkerAngle = true;
}
if(jointAngleAllRange > parent_joint->limits->upper){
jointAngleAllRange = parent_joint->limits->upper - 0.003;
changeMarkerAngle = true;
}
if(changeMarkerAngle){
setJointAngle(link, jointAngleAllRange);
}
}
joint_state_.position.push_back(jointAngleAllRange);
joint_state_.name.push_back(parent_joint->name);
break;
}
case Joint::FIXED:
break;
default:
break;
}
server_->applyChanges();
}
//.........这里部分代码省略.........