void Robot::calculateJointCheckboxes() { if (inChangedEnableAllLinks || !robot_loaded_) return; int links_with_geom_checked = 0; int links_with_geom_unchecked = 0; // check root link RobotLink *link = root_link_; if (link && link->hasGeometry()) { bool checked = link->getLinkProperty()->getValue().toBool(); links_with_geom_checked += checked ? 1 : 0; links_with_geom_unchecked += checked ? 0 : 1; } int links_with_geom = links_with_geom_checked + links_with_geom_unchecked; // check all child links and joints recursively std::vector<std::string>::const_iterator child_joint_it = link->getChildJointNames().begin(); std::vector<std::string>::const_iterator child_joint_end = link->getChildJointNames().end(); for ( ; child_joint_it != child_joint_end ; ++child_joint_it ) { RobotJoint* child_joint = getJoint( *child_joint_it ); if (child_joint) { int child_links_with_geom; int child_links_with_geom_checked; int child_links_with_geom_unchecked; child_joint->calculateJointCheckboxesRecursive(child_links_with_geom, child_links_with_geom_checked, child_links_with_geom_unchecked); links_with_geom_checked += child_links_with_geom_checked; links_with_geom_unchecked += child_links_with_geom_unchecked; } } links_with_geom = links_with_geom_checked + links_with_geom_unchecked; if (!links_with_geom) { setEnableAllLinksCheckbox(QVariant()); } else { setEnableAllLinksCheckbox(links_with_geom_unchecked == 0); } }
void Robot::update(const LinkUpdater& updater) { M_NameToLink::iterator link_it = links_.begin(); M_NameToLink::iterator link_end = links_.end(); for ( ; link_it != link_end; ++link_it ) { RobotLink* link = link_it->second; link->setToNormalMaterial(); Ogre::Vector3 visual_position, collision_position; Ogre::Quaternion visual_orientation, collision_orientation; if( updater.getLinkTransforms( link->getName(), visual_position, visual_orientation, collision_position, collision_orientation )) { link->setTransforms( visual_position, visual_orientation, collision_position, collision_orientation ); std::vector<std::string>::const_iterator joint_it = link->getChildJointNames().begin(); std::vector<std::string>::const_iterator joint_end = link->getChildJointNames().end(); for ( ; joint_it != joint_end ; ++joint_it ) { RobotJoint *joint = getJoint(*joint_it); if (joint) { joint->setTransforms(visual_position, visual_orientation); } } } else { link->setToErrorMaterial(); } } }
void Robot::update(const LinkUpdater& updater) { M_NameToLink::iterator link_it = links_.begin(); M_NameToLink::iterator link_end = links_.end(); for ( ; link_it != link_end; ++link_it ) { RobotLink* link = link_it->second; link->setToNormalMaterial(); Ogre::Vector3 visual_position, collision_position; Ogre::Quaternion visual_orientation, collision_orientation; if( updater.getLinkTransforms( link->getName(), visual_position, visual_orientation, collision_position, collision_orientation )) { // Check if visual_orientation, visual_position, collision_orientation, and collision_position are NaN. if (visual_orientation.isNaN()) { ROS_ERROR_THROTTLE( 1.0, "visual orientation of %s contains NaNs. Skipping render as long as the orientation is invalid.", link->getName().c_str() ); continue; } if (visual_position.isNaN()) { ROS_ERROR_THROTTLE( 1.0, "visual position of %s contains NaNs. Skipping render as long as the position is invalid.", link->getName().c_str() ); continue; } if (collision_orientation.isNaN()) { ROS_ERROR_THROTTLE( 1.0, "collision orientation of %s contains NaNs. Skipping render as long as the orientation is invalid.", link->getName().c_str() ); continue; } if (collision_position.isNaN()) { ROS_ERROR_THROTTLE( 1.0, "collision position of %s contains NaNs. Skipping render as long as the position is invalid.", link->getName().c_str() ); continue; } link->setTransforms( visual_position, visual_orientation, collision_position, collision_orientation ); std::vector<std::string>::const_iterator joint_it = link->getChildJointNames().begin(); std::vector<std::string>::const_iterator joint_end = link->getChildJointNames().end(); for ( ; joint_it != joint_end ; ++joint_it ) { RobotJoint *joint = getJoint(*joint_it); if (joint) { joint->setTransforms(visual_position, visual_orientation); } } } else { link->setToErrorMaterial(); } } }