void assertPosition(Hand& hand, Hand::jp_type innerLinkJp, double tolerance = 0.05) {
hand.update();
if (math::abs(hand.getInnerLinkPosition() - innerLinkJp).maxCoeff() > tolerance) {
std::cout << hand.getInnerLinkPosition() << " is not close enough to " << innerLinkJp << "\n"<<"moving to home"<<std::endl;
exit(2);
}
}
void BHandNode::publish(void)
{
hand->update(); // Update the hand sensors
Hand::jp_type hi = hand->getInnerLinkPosition(); // get finger positions information
Hand::jp_type ho = hand->getOuterLinkPosition();
for (size_t i = 0; i < 4; i++) // Save finger positions
bhand_joint_state.position[i] = hi[i];
for (size_t j = 0; j < 3; j++)
bhand_joint_state.position[j + 4] = ho[j];
bhand_joint_state.header.stamp = ros::Time::now(); // Set the timestamp
bhand_joint_state_pub.publish(bhand_joint_state); // Publish the BarrettHand joint states
btsleep(1.0 / PUBLISH_FREQ); // Sleep according to the specified publishing frequency
}