本文整理汇总了C++中kdl::Rotation::RPY方法的典型用法代码示例。如果您正苦于以下问题：C++ Rotation::RPY方法的具体用法？C++ Rotation::RPY怎么用？C++ Rotation::RPY使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类kdl::Rotation的用法示例。
在下文中一共展示了Rotation::RPY方法的2个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: setHandCartesianPosLinear
controllerErrorCode ArmController::setHandCartesianPosLinear(double x, double y, double z, double roll, double pitch, double yaw, double timestep, double stepSize)
{
double pos;
//Set up controller
setMode(CONTROLLER_POSITION);
this->timeStep = timestep;
this->stepSize = stepSize;
//Calculate end position
KDL::Vector endPos(x,y,z);
KDL::Rotation endRot;
endRot = endRot.RPY(roll,pitch,yaw);
endFrame.p = endPos;
endFrame.M = endRot;
//Get current location
KDL::JntArray q = KDL::JntArray(pr2_kin.nJnts);
//Get joint positions
for(int i=0;i<ARM_MAX_JOINTS;i++){
armJointControllers[i].getPosAct(&pos);
q(i) = pos;
}
KDL::Frame f;
pr2_kin.FK(q,f);
startPosition = f.p; //Record Starting location
KDL::Vector move = endPos-startPosition; //Vector to ending position
double dist = move.Norm(); //Distance to move
moveDirection = move/dist; //Normalize movement vector
rotInterpolator.SetStartEnd(f.M, endRot);
double total_angle = rotInterpolator.Angle();
nSteps = (int)(dist/stepSize);
if(nSteps==0){
std::cout<<"ArmController.cpp: Error:: number of steps calculated to be 0"<<std::endl;
return CONTROLLER_COMPUTATION_ERROR;
}
angleStep = total_angle/nSteps;
lastTime= getTime(); //Record first time marker
stepIndex = 0; //Reset step index
linearInterpolation = true;
return CONTROLLER_ALL_OK;
}示例2: position
controllerErrorCode
ArmController::setHandCartesianPos(double x, double y, double z, double roll, double pitch, double yaw)
{
linearInterpolation = false;
//Define position and rotation
KDL::Vector position(x,y,z);
KDL::Rotation rotation;
rotation = rotation.RPY(roll,pitch,yaw);
cmdPos[0] = x;
cmdPos[1] = y;
cmdPos[2] = z;
cmdPos[3] = roll;
cmdPos[4] = pitch;
cmdPos[5] = yaw;
//Create frame based on position and rotation
KDL::Frame f;
f.p = position;
f.M = rotation;
return commandCartesianPos(f);
}