Esempi in C++ (Cpp) per KinematicState::getKinematicModel

Esempio n. 1

File: state_transformer.cpp Progetto: Arbart/cob_manipulation_sandbox

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

Esempio n. 2

File: model_utils.cpp Progetto: Beryl-bingqi/footstep_dynamic_planner

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