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);
}
}