C++ Model::getJoint方法代码示例

bool Kinematics::readJoints(urdf::Model &robot_model) {
    num_joints = 0;
    // get joint maxs and mins
    boost::shared_ptr<const urdf::Link> link = robot_model.getLink(tip_name);
    boost::shared_ptr<const urdf::Joint> joint;

    while (link && link->name != root_name) {
        joint = robot_model.getJoint(link->parent_joint->name);
        if (!joint) {
            ROS_ERROR("Could not find joint: %s",link->parent_joint->name.c_str());
            return false;
        }
        if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) {
            ROS_INFO( "adding joint: [%s]", joint->name.c_str() );
            num_joints++;
        }
        link = robot_model.getLink(link->getParent()->name);
    }

    joint_min.resize(num_joints);
    joint_max.resize(num_joints);
    info.joint_names.resize(num_joints);
    info.link_names.resize(num_joints);
    info.limits.resize(num_joints);

    link = robot_model.getLink(tip_name);
    unsigned int i = 0;
    while (link && i < num_joints) {
        joint = robot_model.getJoint(link->parent_joint->name);
        if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) {
            ROS_INFO( "getting bounds for joint: [%s]", joint->name.c_str() );

            float lower, upper;
            int hasLimits;
            if ( joint->type != urdf::Joint::CONTINUOUS ) {
                lower = joint->limits->lower;
                upper = joint->limits->upper;
                hasLimits = 1;
            } else {
                lower = -M_PI;
                upper = M_PI;
                hasLimits = 0;
            }
            int index = num_joints - i -1;

            joint_min.data[index] = lower;
            joint_max.data[index] = upper;
            info.joint_names[index] = joint->name;
            info.link_names[index] = link->name;
            info.limits[index].joint_name = joint->name;
            info.limits[index].has_position_limits = hasLimits;
            info.limits[index].min_position = lower;
            info.limits[index].max_position = upper;
            i++;
        }
        link = robot_model.getLink(link->getParent()->name);
    }
    return true;
}

void makeJointMarker(std::string jointName)
{
	boost::shared_ptr<const urdf::Joint> targetJoint = huboModel.getJoint(jointName);

	// The marker must be created in the parent frame so you don't get feedback when you move it
	visualization_msgs::InteractiveMarker marker;

	marker.scale = .125;
	marker.name = jointName;
	marker.header.frame_id = targetJoint->parent_link_name;

	geometry_msgs::Pose controlPose = hubo_motion_ros::toPose( targetJoint->parent_to_joint_origin_transform);
	marker.pose = controlPose;

	visualization_msgs::InteractiveMarkerControl control;

	Eigen::Quaternionf jointAxis;
	Eigen::Vector3f axisVector = hubo_motion_ros::toEVector3(targetJoint->axis);
	jointAxis.setFromTwoVectors(Eigen::Vector3f::UnitX(), axisVector);

	control.orientation.w = jointAxis.w();
	control.orientation.x = jointAxis.x();
	control.orientation.y = jointAxis.y();
	control.orientation.z = jointAxis.z();

	control.always_visible = true;
	control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
	control.orientation_mode = visualization_msgs::InteractiveMarkerControl::INHERIT;

	marker.controls.push_back(control);

	gIntServer->insert(marker);
	gIntServer->setCallback(marker.name, &processFeedback);
}

  bool getChainInfoFromRobotModel(urdf::Model &robot_model,
                                  const std::string &root_name,
                                  const std::string &tip_name,
                                  kinematics_msgs::KinematicSolverInfo &chain_info) 
  {
    // get joint maxs and mins
    boost::shared_ptr<const urdf::Link> link = robot_model.getLink(tip_name);
    boost::shared_ptr<const urdf::Joint> joint;
    while (link && link->name != root_name) 
    {
      joint = robot_model.getJoint(link->parent_joint->name);
      if (!joint) 
      {
        ROS_ERROR("Could not find joint: %s",link->parent_joint->name.c_str());
        return false;
      }
      if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) 
      {
        float lower, upper;
        int hasLimits;
        if ( joint->type != urdf::Joint::CONTINUOUS ) 
        {
          lower = joint->limits->lower;
          upper = joint->limits->upper;
          hasLimits = 1;
        } 
        else 
        {
          lower = -M_PI;
          upper = M_PI;
          hasLimits = 0;
        }
        chain_info.joint_names.push_back(joint->name);
        motion_planning_msgs::JointLimits limits;
        limits.joint_name = joint->name;
        limits.has_position_limits = hasLimits;
        limits.min_position = lower;
        limits.max_position = upper;
        chain_info.limits.push_back(limits);
      }
      link = robot_model.getLink(link->getParent()->name);
    }
    link = robot_model.getLink(tip_name);
    if(link)
      chain_info.link_names.push_back(tip_name);    

    std::reverse(chain_info.limits.begin(),chain_info.limits.end());
    std::reverse(chain_info.joint_names.begin(),chain_info.joint_names.end());

    return true;
  }

SolverInfoProcessor::SolverInfoProcessor(const urdf::Model &robot_model,
	const std::string &tip_name,
	const std::string &root_name)
{
	// setup the IK solver information which contains joint names, joint limits and so on
	boost::shared_ptr<const urdf::Link> link = robot_model.getLink(tip_name);
	while (link) {
		// check if we have already reached the final link
		if (link->name == root_name) break;

		// if we have reached the last joint the root frame is not in the chain
		// then we cannot build the chain
		if (!link->parent_joint) {
			ROS_ERROR("The provided root link is not in the chain");
			ROS_ASSERT(false);
		}

		// process the joint
		boost::shared_ptr<const urdf::Joint> joint = robot_model.getJoint(link->parent_joint->name);
		if (!joint) {
			ROS_ERROR("Could not find joint: %s", link->parent_joint->name.c_str());
			ROS_ASSERT(false);
		}

		// add the joint to the chain
		if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) {
			ROS_DEBUG("Joint axis: %f, %f, %f", joint->axis.x, joint->axis.y, joint->axis.z);

			addJointToChainInfo(link->parent_joint, _solver_info);
		}

		link = robot_model.getLink(link->getParent()->name);
        }

	_solver_info.link_names.push_back(tip_name);

	// We expect order from root to tip, so reverse the order
	std::reverse(_solver_info.limits.begin(), _solver_info.limits.end());
	std::reverse(_solver_info.joint_names.begin(), _solver_info.joint_names.end());
	std::reverse(_solver_info.link_names.begin(), _solver_info.link_names.end());
}

bool Pod::init(hardware_interface::EffortJointInterface* hw, urdf::Model urdf) {
  std::string joint_name;

  nh_.param("command_timeout", command_timeout_, command_timeout_);

  if (!nh_.getParam("joint", joint_name)) {
    ROS_ERROR("No joint given (namespace: %s)", nh_.getNamespace().c_str());
    return false;
  }

  joint_ = hw->getHandle(joint_name);
  boost::shared_ptr<const urdf::Joint> joint_urdf =  urdf.getJoint(joint_name);
  if (!joint_urdf) {
    ROS_ERROR("Could not find joint '%s' in urdf", joint_name.c_str());
    return false;
  }

  if (!pid_controller_.init(ros::NodeHandle(nh_, "pid")))
    return false;

  controller_state_publisher_.reset(new realtime_tools::RealtimePublisher<control_msgs::JointControllerState>(nh_, "state", 1));

  command_sub_ = nh_.subscribe<walrus_pod_controller::PodCommand>("command", 1, &Pod::setCommandCallback, this);
}

 inline bool modelHasMovableJoint(const urdf::Model& model, const std::string& name)
 {
   boost::shared_ptr<const urdf::Joint> joint = model.getJoint(name);
   return joint.get() && isMovingJoint(joint->type);
 }

bool ChainParser::parseChain(XmlRpc::XmlRpcValue& chain_description, Tree tree, urdf::Model& robot_model,
                               std::map<std::string, unsigned int>& joint_name_to_index,
                               std::vector<std::string>& index_to_joint_name,
                               std::vector<double>& q_min, std::vector<double>& q_max) {
    ros::NodeHandle n("~");
    std::string ns = n.getNamespace();

    std::cout << chain_description << std::endl;

    if (chain_description.getType() != XmlRpc::XmlRpcValue::TypeStruct) {
        ROS_ERROR("Chain description should be a struct containing 'root' and 'tip'. (namespace: %s)", n.getNamespace().c_str());
        return false;
    }

    XmlRpc::XmlRpcValue& v_root_link = chain_description["root"];
    if (v_root_link.getType() != XmlRpc::XmlRpcValue::TypeString) {
        ROS_ERROR("Chain description for does not contain 'root'. (namespace: %s)", n.getNamespace().c_str());
        return false;
    }
    std::string root_link_name = (std::string)v_root_link;

    XmlRpc::XmlRpcValue& v_tip_link = chain_description["tip"];
    if (v_tip_link.getType() != XmlRpc::XmlRpcValue::TypeString) {
        ROS_ERROR("Chain description for does not contain 'tip'. (namespace: %s)", n.getNamespace().c_str());
        return false;
    }
    std::string tip_link_name = (std::string)v_tip_link;

    Chain* chain = new Chain();

    ROS_INFO("Looking for chain from %s to %s", root_link_name.c_str(), tip_link_name.c_str());

    if (!tree.kdl_tree_.getChain(root_link_name, tip_link_name, chain->kdl_chain_)) {
//    if (!tree.kdl_tree_.getChain("base", tip_link_name, chain->kdl_chain_)) {
        ROS_FATAL("Could not initialize chain object");
        return false;
    }

    for(unsigned int i = 0; i < chain->kdl_chain_.getNrOfSegments(); ++i) {
        const KDL::Segment& segment = chain->kdl_chain_.getSegment(i);
        const KDL::Joint& joint = segment.getJoint();

        if (joint.getType() != KDL::Joint::None)
        {
            //cout << "Segment: " << segment.getName() << endl;
            //cout << "Joint:   " << joint.getName() << endl;

            unsigned int full_joint_index = 0;

            std::map<std::string, unsigned int>::iterator it_joint = joint_name_to_index.find(joint.getName());

            if (it_joint == joint_name_to_index.end()) {
                // joint name is not yet in map, so give it a new fresh index and add it to the map
                full_joint_index = joint_name_to_index.size();

                //cout << "    new joint, gets index " << full_joint_index << endl;
                //cout << "    type: " << joint.getTypeName() << endl;

                joint_name_to_index[joint.getName()] = full_joint_index;
                index_to_joint_name.push_back(joint.getName());

                //cout << "    Lower limit: " << robot_model.getJoint(joint.getName())->limits->lower << endl;
                //cout << "    Upper limit: " << robot_model.getJoint(joint.getName())->limits->upper << endl;

                // determine joint limits from URDF
                q_min.push_back(robot_model.getJoint(joint.getName())->limits->lower);
                q_max.push_back(robot_model.getJoint(joint.getName())->limits->upper);

            } else {
                // joint name already in the map, so look-up its index
                full_joint_index = it_joint->second;

                //cout << "    existing joint, has index: " << full_joint_index << endl;
            }

        }

    }

    return true;
}

void processFeedback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
{
  std::ostringstream s;
  s << "Feedback from marker '" << feedback->marker_name << "' "
	<< " / control '" << feedback->control_name << "'";

  switch ( feedback->event_type )
  {
  case visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN:
	  mouseInUse = true;
	  break;
  case visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP:
  {
	  // Compute FK for end effectors that have changed
	  // Call IK to get the joint states
	  moveit_msgs::GetPositionFKRequest req;
	  req.fk_link_names.push_back("RightArm");
	  req.robot_state.joint_state = planState;
	  req.header.stamp = ros::Time::now();

	  moveit_msgs::GetPositionFKResponse resp;
	  gFKinClient.call(req, resp);

	  std::cerr << "Response: " << resp.pose_stamped[0] << std::endl;

	  if (resp.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
	  {
		  joyInt.currentPose = resp.pose_stamped[0];
	  }
	  else
	  {
		  ROS_ERROR_STREAM("Failed to solve FK: " << resp.error_code.val);
	  }

	  gRPosePublisher.publish(joyInt.currentPose);
	  mouseInUse = false;
	  break;
  }

  case visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE:
  {
	  Eigen::AngleAxisf aa;
	  aa = Eigen::Quaternionf(feedback->pose.orientation.w,
							  feedback->pose.orientation.x,
							  feedback->pose.orientation.y,
							  feedback->pose.orientation.z);

	  boost::shared_ptr<const urdf::Joint> targetJoint = huboModel.getJoint(feedback->marker_name);
	  Eigen::Vector3f axisVector = hubo_motion_ros::toEVector3(targetJoint->axis);
	  float angle;
	  // Get sign of angle
	  if (aa.axis().dot(axisVector) < 0)
	  {
		  angle = -aa.angle();
	  }
	  else
	  {
		  angle = aa.angle();
	  }

	  // Trim angle to joint limits
	  if (angle > targetJoint->limits->upper)
	  {
		  angle = targetJoint->limits->upper;
	  }
	  else if (angle < targetJoint->limits->lower)
	  {
		  angle = targetJoint->limits->lower;
	  }

	  // Locate the index of the solution joint in the plan state
	  for (int j = 0; j < planState.name.size(); j++)
	  {
		  if (feedback->marker_name == planState.name[j])
		  {
			  planState.position[j] = angle;
		  }
	  }

	  prevAA = aa;

	  // Time and Frame stamps
	  planState.header.frame_id = "/Body_TSY";
	  planState.header.stamp = ros::Time::now();

	  gStatePublisher.publish(planState);
	  break;
  }
  }

  gIntServer->applyChanges();
}

		// Create an object of class Joint that corresponds to the URDF joint specified
		// by joint_name.
		shared_ptr<JointBase> getJoint(	const string& joint_name, const bool is_steer_joint,
										const NodeHandle& ctrlr_nh,
										const urdf::Model& urdf_model,
										EffortJointInterface *const eff_joint_iface,
										PositionJointInterface *const pos_joint_iface,
										VelocityJointInterface *const vel_joint_iface)
		{
			if (eff_joint_iface != NULL)
			{
				JointHandle handle;
				bool handle_found;
				try
				{
					handle = eff_joint_iface->getHandle(joint_name);
					handle_found = true;
				}
				catch (...)
				{
					handle_found = false;
				}

				if (handle_found)
				{
					const shared_ptr<const urdf::Joint> urdf_joint =
					urdf_model.getJoint(joint_name);
					if (urdf_joint == NULL)
					{
						throw runtime_error("\"" + joint_name +
						"\" was not found in the URDF data.");
					}

					shared_ptr<JointBase> joint(new PIDJoint(handle, urdf_joint, ctrlr_nh));
					return joint;
				}
			}

			if (pos_joint_iface != NULL)
			{
				JointHandle handle;
				bool handle_found;
				try
				{
					handle = pos_joint_iface->getHandle(joint_name);
					handle_found = true;
				}
				catch (...)
				{
					handle_found = false;
				}

				if (handle_found)
				{
					const shared_ptr<const urdf::Joint> urdf_joint =
					urdf_model.getJoint(joint_name);
					if (urdf_joint == NULL)
					{
						throw runtime_error("\"" + joint_name +
						"\" was not found in the URDF data.");
					}

					shared_ptr<JointBase> joint(new PosJoint(handle, urdf_joint));
					return joint;
				}
			}

			if (vel_joint_iface != NULL)
			{
				JointHandle handle;
				bool handle_found;
				try
				{
					handle = vel_joint_iface->getHandle(joint_name);
					handle_found = true;
				}
				catch (...)
				{
					handle_found = false;
				}

				if (handle_found)
				{
					const shared_ptr<const urdf::Joint> urdf_joint =
					urdf_model.getJoint(joint_name);
					if (urdf_joint == NULL)
					{
						throw runtime_error("\"" + joint_name +
						"\" was not found in the URDF data.");
					}

					if (is_steer_joint)
					{
						shared_ptr<JointBase> joint(new PIDJoint(handle, urdf_joint, ctrlr_nh));
						return joint;
					}
					shared_ptr<JointBase> joint(new VelJoint(handle, urdf_joint));
					return joint;
				}
			}
//.........这里部分代码省略.........

bool TreeKinematics::readJoints(urdf::Model &robot_model,
                                KDL::Tree &kdl_tree,
                                std::string &tree_root_name,
                                unsigned int &nr_of_jnts,
                                KDL::JntArray &joint_min,
                                KDL::JntArray &joint_max,
                                KDL::JntArray &joint_vel_max)
{
  KDL::SegmentMap segmentMap;
  segmentMap = kdl_tree.getSegments();
  tree_root_name = kdl_tree.getRootSegment()->second.segment.getName();
  nr_of_jnts = kdl_tree.getNrOfJoints();
  ROS_DEBUG( "the tree's number of joints: [%d]", nr_of_jnts );
  joint_min.resize(nr_of_jnts);
  joint_max.resize(nr_of_jnts);
  joint_vel_max.resize(nr_of_jnts);
  info_.joint_names.resize(nr_of_jnts);
  info_.limits.resize(nr_of_jnts);

  // The following walks through all tree segments, extracts their joints except joints of KDL::None and extracts
  // the information about min/max position and max velocity of the joints not of type urdf::Joint::UNKNOWN or
  // urdf::Joint::FIXED
  ROS_DEBUG("Extracting all joints from the tree, which are not of type KDL::Joint::None.");
  boost::shared_ptr<const urdf::Joint> joint;
  for (KDL::SegmentMap::const_iterator seg_it = segmentMap.begin(); seg_it != segmentMap.end(); ++seg_it)
  {
    if (seg_it->second.segment.getJoint().getType() != KDL::Joint::None)
    {
      // check, if joint can be found in the URDF model of the robot
      joint = robot_model.getJoint(seg_it->second.segment.getJoint().getName().c_str());
      if (!joint)
      {
        ROS_FATAL("Joint '%s' has not been found in the URDF robot model!", joint->name.c_str());
        return false;
      }
      // extract joint information
      if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED)
      {
        ROS_DEBUG( "getting information about joint: [%s]", joint->name.c_str() );
        double lower = 0.0, upper = 0.0, vel_limit = 0.0;
        unsigned int has_pos_limits = 0, has_vel_limits = 0;

        if ( joint->type != urdf::Joint::CONTINUOUS )
        {
          ROS_DEBUG("joint is not continuous.");
          lower = joint->limits->lower;
          upper = joint->limits->upper;
          has_pos_limits = 1;
          if (joint->limits->velocity)
          {
            has_vel_limits = 1;
            vel_limit = joint->limits->velocity;
            ROS_DEBUG("joint has following velocity limit: %f", vel_limit);
          }
          else
          {
            has_vel_limits = 0;
            vel_limit = 0.0;
            ROS_DEBUG("joint has no velocity limit.");
          }
        }
        else
        {
          ROS_DEBUG("joint is continuous.");
          lower = -M_PI;
          upper = M_PI;
          has_pos_limits = 0;
          if(joint->limits && joint->limits->velocity)
          {
            has_vel_limits = 1;
            vel_limit = joint->limits->velocity;
            ROS_DEBUG("joint has following velocity limit: %f", vel_limit);
          }
          else
          {
            has_vel_limits = 0;
            vel_limit = 0.0;
            ROS_DEBUG("joint has no velocity limit.");
          }
        }

        joint_min(seg_it->second.q_nr) = lower;
        joint_max(seg_it->second.q_nr) = upper;
        joint_vel_max(seg_it->second.q_nr) = vel_limit;
        ROS_DEBUG("pos_min = %f, pos_max = %f, vel_max = %f", lower, upper, vel_limit);

        info_.joint_names[seg_it->second.q_nr] = joint->name;
        info_.limits[seg_it->second.q_nr].joint_name = joint->name;
        info_.limits[seg_it->second.q_nr].has_position_limits = has_pos_limits;
        info_.limits[seg_it->second.q_nr].min_position = lower;
        info_.limits[seg_it->second.q_nr].max_position = upper;
        info_.limits[seg_it->second.q_nr].has_velocity_limits = has_vel_limits;
        info_.limits[seg_it->second.q_nr].max_velocity = vel_limit;
      }
    }
  }
  return true;
}

  /**
   * DistanceToLimit annotations.
   *
   * @return UIMA error id. UIMA_ERR_NONE on success.
   */
  uima::TyErrorId annotateLimits(
    uima::CAS& cas,
    const urdf::Model& model
  ) {
    uima::FSIndexRepository& index = cas.getIndexRepository();
    uima::ANIndex jsIndex = cas.getAnnotationIndex(JointState);
    uima::ANIterator jsIter = jsIndex.iterator();

    uima::AnnotationFS js, dst;
    uima::FeatureStructure jtp;

    uima::StringArrayFS jsNames;
    uima::DoubleArrayFS jtpPositions, jtpVelocities, jtpEfforts;

    boost::shared_ptr<const urdf::Joint> joint;
    boost::shared_ptr<urdf::JointLimits> limits;

    std::vector<std::string> dstNames;
    std::vector<double> upperLimits, lowerLimits, velocities, efforts;

    while (jsIter.isValid()) {
      js = jsIter.get();
      jtp = js.getFSValue(jsJtpFtr);

      // Get feature structure value arrays.
      jsNames       = js.getStringArrayFSValue(jsNameFtr);
      jtpPositions  = jtp.getDoubleArrayFSValue(jtpPosFtr);
      jtpVelocities = jtp.getDoubleArrayFSValue(jtpEffFtr);
      jtpEfforts    = jtp.getDoubleArrayFSValue(jtpVelFtr);

      // Clear storage vectors.
      dstNames.clear();
      upperLimits.clear();
      lowerLimits.clear();
      velocities.clear();
      efforts.clear();

      for (std::size_t i = 0; i < jsNames.size(); i++) {
        joint = model.getJoint(jsNames.get(i).asUTF8());
        limits = joint->limits;

        // Limits are only relevant for some joint types.
        if (
          joint->type == urdf::Joint::REVOLUTE ||
          joint->type == urdf::Joint::PRISMATIC
        ) {
          dstNames.push_back(joint->name);
          upperLimits.push_back(limits->upper - jtpPositions.get(i));
          lowerLimits.push_back(limits->lower - jtpPositions.get(i));
          velocities.push_back(limits->velocity - jtpVelocities.get(i));
          efforts.push_back(limits->effort - jtpEfforts.get(i));
        }
      }

      dst = cas.createAnnotation(DistanceToLimit,
                                 js.getBeginPosition(), js.getEndPosition());
      dst.setFSValue(dstNameFtr, utils::toStringArrayFS(cas, dstNames));
      dst.setFSValue(dstUppFtr, utils::toDoubleArrayFS(cas, upperLimits));
      dst.setFSValue(dstLowFtr, utils::toDoubleArrayFS(cas, lowerLimits));
      dst.setFSValue(dstVelFtr, utils::toDoubleArrayFS(cas, velocities));
      dst.setFSValue(dstEffFtr, utils::toDoubleArrayFS(cas, efforts));

      index.addFS(dst);
      jsIter.moveToNext();
    }

    return UIMA_ERR_NONE;
  }