bool world_transform(std::string frame_id, const planning_models::KinematicState &state, btTransform &transform) { if (!frame_id.compare(state.getKinematicModel()->getRoot()->getParentFrameId())) { //identity transform transform.setIdentity(); return true; } if (!frame_id.compare(state.getKinematicModel()->getRoot()->getChildFrameId())) { transform = state.getRootTransform(); return true; } const planning_models::KinematicState::LinkState *link = state.getLinkState(frame_id); if (!link) { ROS_ERROR("Unable to find link %s in kinematic state", frame_id.c_str()); return false; } transform = link->getGlobalLinkTransform(); return true; }
void planning_environment::getAllKinematicStateStampedTransforms(const planning_models::KinematicState& state, std::vector<geometry_msgs::TransformStamped>& trans_vector, const ros::Time& stamp) { trans_vector.clear(); for(unsigned int i = 0; i < state.getLinkStateVector().size(); i++) { const planning_models::KinematicState::LinkState* ls = state.getLinkStateVector()[i]; geometry_msgs::TransformStamped ts; ts.header.stamp = stamp; ts.header.frame_id = state.getKinematicModel()->getRoot()->getParentFrameId(); ts.child_frame_id = ls->getName(); if(ts.header.frame_id == ts.child_frame_id) continue; tf::transformTFToMsg(ls->getGlobalLinkTransform(),ts.transform); trans_vector.push_back(ts); } }