本文整理汇总了C++中kdl::JntArray::data方法的典型用法代码示例。如果您正苦于以下问题：C++ JntArray::data方法的具体用法？C++ JntArray::data怎么用？C++ JntArray::data使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类kdl::JntArray的用法示例。
在下文中一共展示了JntArray::data方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: checkLimits
void Arm_Cartesian_Control::checkLimits(
double dt, KDL::JntArray& joint_positions,
KDL::JntArray& jntVel) {
if (upper_joint_limits.size() < arm_chain->getNrOfJoints()
|| lower_joint_limits.size() < arm_chain->getNrOfJoints()) {
std::cout << "No Joint limits defined";
return;
}
double limit_range = 0.1;
jointLimitsReached = false;
for (int i=0; i<jntVel.data.rows(); i++) {
double pos = joint_positions.data(i);
double vel = jntVel.data(i);
double fpos = pos + vel * dt * 2;
double upper_limit = this->upper_joint_limits[i];
double lower_limit = this->lower_joint_limits[i];
double upper_clearance = upper_limit - fpos;
double lower_clearance = lower_limit - fpos;
double limit_vel = vel;
if (fabs(upper_clearance) < limit_range*2 && vel > 0) {
limit_vel *= upper_clearance;
}
if (fabs(lower_clearance) < limit_range*2 && vel < 0) {
limit_vel *= lower_clearance;
}
if (fpos > upper_limit - limit_range && vel > 0) {
limit_vel = 0.0;
std::cout << "Upper limit reached for joint " << i << std::endl;
jointLimitsReached = true;
} else if (fpos < lower_limit + limit_range && vel < 0) {
limit_vel = 0.0;
std::cout << "Lower limit reached for joint " << i << std::endl;
jointLimitsReached = true;
}
jntVel.data(i) = limit_vel;
}
}开发者ID:thassa2s，项目名称:NegarNemati_NiranjanDeshpande_TeenaHassan，代码行数:50，代码来源:arm_cartesian_control.cpp