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