コード例 #1
0
double TreeIkSolverPos_Online::CartToJnt(const JntArray& q_in, const Frames& p_in, JntArray& q_out)
{
  assert(q_out.rows() == q_in.rows());
  assert(q_dot_.rows() == q_out.rows());

  q_out = q_in;

  // First check, if all elements in p_in are available
  for(Frames::const_iterator f_des_it=p_in.begin();f_des_it!=p_in.end();++f_des_it)
    if(frames_.find(f_des_it->first)==frames_.end())
      return -2;

  for (Frames::const_iterator f_des_it=p_in.begin();f_des_it!=p_in.end();++f_des_it)
  {
    // Get all iterators for this endpoint
    Frames::iterator f_it = frames_.find(f_des_it->first);
    Twists::iterator delta_twists_it = delta_twists_.find(f_des_it->first);

    fksolver_.JntToCart(q_out, f_it->second, f_it->first);
    twist_ = diff(f_it->second, f_des_it->second);

    // Checks, if the twist (twist_) exceeds the maximum translational and/or rotational velocity
    // And scales them, if necessary
    enforceCartVelLimits();

    delta_twists_it->second = twist_;
  }

  double res = iksolver_.CartToJnt(q_out, delta_twists_, q_dot_);

  // Checks, if joint velocities (q_dot_) exceed their maximum velocities and scales them, if necessary
  enforceJointVelLimits();

  // Integrate
  Add(q_out, q_dot_, q_out);

  // Limit joint positions
  for (unsigned int j = 0; j < q_min_.rows(); j++)
  {
    if (q_out(j) < q_min_(j))
      q_out(j) = q_min_(j);
    else if (q_out(j) > q_max_(j))
      q_out(j) = q_max_(j);
  }

  return res;
}
コード例 #2
0
void IECore::findSequences( const std::vector< std::string > &names, std::vector< FileSequencePtr > &sequences, size_t minSequenceSize )
{
	sequences.clear();

	/// this matches names of the form $prefix$frameNumber$suffix
	/// placing each of those in a group of the resulting match.
	/// both $prefix and $suffix may be the empty string and $frameNumber
	/// may be preceded by a minus sign.
	/// It also matches file extensions with 3 or 4 characters that contain numbers (for example: CR2, MP3 )
	boost::regex matchExpression( std::string( "^([^#]*?)(-?[0-9]+)([^0-9#]*|[^0-9#]*\\.[a-zA-Z]{2,3}[0-9])$" ) );

	/// build a mapping from ($prefix, $suffix) to a list of $frameNumbers
	typedef std::vector< std::string > Frames;
	typedef std::map< std::pair< std::string, std::string >, Frames > SequenceMap;

	SequenceMap sequenceMap;

	for ( std::vector< std::string >::const_iterator it = names.begin(); it != names.end(); ++it )
	{
		boost::smatch matches;
		if ( boost::regex_match( *it, matches, matchExpression ) )
		{
			sequenceMap[
				SequenceMap::key_type(
					std::string( matches[1].first, matches[1].second ),
					std::string( matches[3].first, matches[3].second )
				)
			].push_back( std::string( matches[2].first, matches[2].second ) );
		}
	}

	for ( SequenceMap::const_iterator it = sequenceMap.begin(); it != sequenceMap.end(); ++it )
	{
		const SequenceMap::key_type &fixes = it->first;
		const Frames &frames = it->second;
		// todo: could be more efficient by writing a custom comparison function that uses indexes 
		//	 into the const Frames vector rather than duplicating the strings and sorting them directly
		Frames sortedFrames = frames;
		std::sort( sortedFrames.begin(), sortedFrames.end() );
		
		/// in diabolical cases the elements of frames may not all have the same padding
		/// so we'll sort them out into padded and unpadded frame sequences here, by creating
		/// a map of padding->list of frames. unpadded things will be considered to have a padding
		/// of 1.
		typedef std::vector< FrameList::Frame > NumericFrames;
		typedef std::map< unsigned int, NumericFrames > PaddingToFramesMap;
		PaddingToFramesMap paddingToFrames;
		for ( Frames::const_iterator fIt = sortedFrames.begin(); fIt != sortedFrames.end(); ++fIt )
		{
			std::string frame = *fIt;
			int sign = 1;

			assert( frame.size() );
			if ( *frame.begin() == '-' )
			{
				frame = frame.substr( 1, frame.size() - 1 );
				sign = -1;
			}
			if ( *frame.begin() == '0' || paddingToFrames.find( frame.size() ) != paddingToFrames.end() )
			{
				paddingToFrames[ frame.size() ].push_back( sign * boost::lexical_cast<FrameList::Frame>( frame ) );
			}
			else
			{
				paddingToFrames[ 1 ].push_back( sign * boost::lexical_cast<FrameList::Frame>( frame ) );
			}
		}

		for ( PaddingToFramesMap::iterator pIt = paddingToFrames.begin(); pIt != paddingToFrames.end(); ++pIt )
		{
			const PaddingToFramesMap::key_type &padding = pIt->first;
			NumericFrames &numericFrames = pIt->second;
			std::sort( numericFrames.begin(), numericFrames.end() );

			FrameListPtr frameList = frameListFromList( numericFrames );

			std::vector< FrameList::Frame > expandedFrameList;
			frameList->asList( expandedFrameList );

			/// remove any sequences with less than the given minimum.
			if ( expandedFrameList.size() >= minSequenceSize )
			{
				std::string frameTemplate;
				for ( PaddingToFramesMap::key_type i = 0; i < padding; i++ )
				{
					frameTemplate += "#";
				}

				sequences.push_back(
					new FileSequence(
						fixes.first + frameTemplate + fixes.second,
						frameList
					)
				);
			}
		}
	}
}
コード例 #3
0
double TreeIkSolverPos_Online::CartToJnt_it(const JntArray& q_in, const Frames& p_in, JntArray& q_out)
{
  assert(q_out.rows() == q_in.rows());
  assert(q_out_old_.rows() == q_in.rows());
  assert(q_dot_.rows() == q_in.rows());
  assert(q_dot_old_.rows() == q_in.rows());
  assert(q_dot_new_.rows() == q_in.rows());



  q_out = q_in;
  SetToZero(q_dot_);
  SetToZero(q_dot_old_);
  SetToZero(q_dot_new_);
  twist_ = Twist::Zero();

  // First check, if all elements in p_in are available
  unsigned int nr_of_endeffectors = 0;
  nr_of_still_endeffectors_ = 0;
  low_pass_adj_factor_ = 0.0;

  for(Frames::const_iterator f_des_it=p_in.begin();f_des_it!=p_in.end();++f_des_it)
  {
    if(frames_.find(f_des_it->first)==frames_.end())
      return -2;
    else
    {
      /*
      Twists::iterator old_twists_it = old_twists_.find(f_des_it->first);
      old_twists_it->second = Twist::Zero();
      Frames::iterator f_0_it = frames_0_.find(f_des_it->first);
      */
      /*
      Twists::iterator old_twists_it = old_twists_.find(f_des_it->first);
      old_twists_it->second = diff(f_old_it->second, f_des_it->second);
      if (){};
      */
      Frames::iterator f_old_it = p_in_old_.find(f_des_it->first);
      Frames::iterator f_des_pos_l_it = frames_pos_lim_.find(f_des_it->first);
      twist_ = diff(f_old_it->second, f_des_it->second);
      /*
      if(sqrt(pow(twist_.vel.x(), 2) + pow(twist_.vel.y(), 2) + pow(twist_.vel.z(), 2)) < x_dot_trans_min_)
      {
        f_des_pos_l_it->second.p = addDelta(f_old_it->second.p, (0.1 * x_dot_trans_max_ * twist_.vel));
        std::cout << "old position is used." << std::endl;
      }
      else
        f_des_pos_l_it->second.p = f_des_it->second.p;

      if(sqrt(pow(twist_.rot.x(), 2) + pow(twist_.rot.y(), 2) + pow(twist_.rot.z(), 2)) < x_dot_rot_min_)
      {
        f_des_pos_l_it->second.M = addDelta(f_old_it->second.M, (0.1 * x_dot_rot_max_ * twist_.rot));
        std::cout << "old orientation is used." << std::endl;
      }
      else
        f_des_pos_l_it->second.M = f_des_it->second.M;
      */
      f_des_pos_l_it->second.p = f_des_it->second.p;
      f_des_pos_l_it->second.M = f_des_it->second.M;

      Frames::iterator f_des_vel_l_it = frames_vel_lim_.find(f_des_it->first);
      fksolver_.JntToCart(q_in, f_des_vel_l_it->second, f_des_it->first);
      twist_ = diff(f_des_vel_l_it->second, f_des_pos_l_it->second);



      f_des_vel_l_it->second = addDelta(f_des_vel_l_it->second, twist_);
      nr_of_endeffectors++;
    }
  }
  if(nr_of_still_endeffectors_ == nr_of_endeffectors)
  {
    small_task_space_movement_ = true;

  }
  else
    small_task_space_movement_ = false;

  unsigned int k=0;
  double res = 0.0;
  while(++k <= maxiter_)
  {
    //for (Frames::const_iterator f_des_it=p_in.begin(); f_des_it!=p_in.end(); ++f_des_it)
    for (Frames::const_iterator f_des_it=frames_vel_lim_.begin(); f_des_it!=frames_vel_lim_.end(); ++f_des_it)
    {
      // Get all iterators for this endpoint
      Frames::iterator f_it = frames_.find(f_des_it->first);
      Twists::iterator delta_twists_it = delta_twists_.find(f_des_it->first);
      Twists::iterator old_twists_it = old_twists_.find(f_des_it->first);
      Frames::iterator f_0_it = frames_vel_lim_.find(f_des_it->first);

      fksolver_.JntToCart(q_out, f_it->second, f_it->first);

      // Checks, if the current overall twist exceeds the maximum translational and/or rotational velocity.
      // If so, the velocities of the overall twist get scaled and a new current twist is calculated.
      delta_twists_it->second = diff(f_it->second, f_des_it->second);


      old_twists_it->second = diff(f_0_it->second, f_it->second);

      enforceCartVelLimits_it(old_twists_it->second, delta_twists_it->second);


    }

    res = iksolver_.CartToJnt(q_out, delta_twists_, q_dot_);

    if (res < eps_)
    {
      break;
      //return res;
    }

    // Checks, if joint velocities (q_dot_) exceed their maximum velocities and scales them, if necessary
    //Subtract(q_out, q_in, q_dot_old_);
    //enforceJointVelLimits_it(q_dot_old_, q_dot_);

    Subtract(q_out, q_in, q_dot_old_);

    Add(q_dot_old_, q_dot_, q_dot_);

    enforceJointVelLimits();

    Subtract(q_dot_, q_dot_old_, q_dot_);

    // Integrate
    Add(q_out, q_dot_, q_out);

    // Limit joint positions
    for (unsigned int j = 0; j < q_min_.rows(); ++j)
    {
      if (q_out(j) < q_min_(j))
        q_out(j) = q_min_(j);
      else if (q_out(j) > q_max_(j))
        q_out(j) = q_max_(j);
    }
  }

  Subtract(q_out, q_in, q_dot_);


  Add(q_in, q_dot_, q_out);
  filter(q_dot_, q_out, q_out_old_);



  q_out_old_ = q_out;
  p_in_old_ = p_in;


  if (k <= maxiter_)
    return res;
  else
    return -3;
}